#include <PS4Controller.h>
#include <sbus.h>
#include <ESP32PWM.h>
#include <ESP32Servo.h>

#define SBUS_RX 14 // 32
#define SBUS_TX 15 // 33
#define SBUS_INV true

/* SBUS object, writing SBUS */
// HardwareSerial sBusSerial(2);
bfs::SbusTx sbus_tx(&Serial1, SBUS_RX, SBUS_TX, SBUS_INV);
bfs::SbusData data;

// SBUS Channels to use with Rc_Engine_Sound_ESP32
// (corresponds to the channel defined in 2_Remote.h)
#define THROTTLE 1
#define GUN 2
#define SIREN 2
#define ENGINE 5
#define LIGHTS 3
#define SOUND1 4

// Servos pin (to control Servo directly)
#define STEERING 12 // 18
#define TURRET 2    // 19

bool ramble_trigger = false;
unsigned long ramble_duration = 0;
unsigned long flash_duration = 0;

unsigned long ramble_startmillis = 0;

Servo servoSteering;
Servo servoTurret;

void setup()
{
    Serial.begin(115200);
    PS4.begin("1a:2b:3c:01:01:01");
    sbus_tx.Begin();

    for (uint8_t i = 0; i < data.NUM_CH; i++)
    {
        data.ch[i] = mapSBUS(0);
    }

    servoSteering.attach(STEERING);
    servoTurret.attach(TURRET);

    Serial.println("Ready.");
}

void loop()
{
    if (PS4.isConnected())
    {
        // Reset values
        data.ch[THROTTLE - 1] = mapSBUS(0);
        data.ch[GUN - 1] = mapSBUS(0);
        data.ch[SIREN - 1] = mapSBUS(0);
        data.ch[ENGINE - 1] = mapSBUS(-100);
        data.ch[LIGHTS - 1] = mapSBUS(0);
        data.ch[SOUND1 - 1] = mapSBUS(-100);

        // SBUS : check every command
        throttle();
        gun();
        siren();
        engine();
        lights();
        sound1();

        // Servo direct
        steering();
        turret();

        // PS4 flash and ramble
        ramble();
        // flash();
        PS4.sendToController();

        /*
        for (uint8_t i = 0; i < data.NUM_CH; i++)
        {
            // channels[i] = (uint16_t)(((float)ain[i]) * scaleFactor + bias);
            //Serial.print(data.ch[i]); // print the channel command (172-1811)
            //Serial.print("\t");
        }
        //Serial.println();
        */
        sbus_tx.data(data);
        sbus_tx.Write();

        delay(9);
    }
}

// We map the percentage (-100% / 100% to the SBUS values (172 / 1811))
// (https://quadmeup.com/generate-s-bus-with-arduino-in-a-simple-way/)
int mapSBUS(int percent)
{

    return map(percent, -100, 100, 172, 1811);
}

// Throttle :
// 2 buttons : R2 to accelerate (0 -> 100%) / L2 to Brake (0 -> -100%)
void throttle()
{
    int val = 0;

    // Accelerate
    if (PS4.R2())
    {
        val = map(PS4.R2Value(), 0, 255, 0, 100);
    }
    // Break
    else if (PS4.L2())
    {
        val = map(PS4.L2Value(), 0, 255, 0, -100);
    }

    // Nothing pressed => we set to 0 (stop)
    if (!PS4.R2() && !PS4.L2())
    {
        val = 0;
    }

    // If we have a value
    if (val != 0)
    {
        Serial.print("Throttle : ");
        Serial.println(val);
    }

    if (ramble_duration == 0)
    {
        int rambleVal = map(val, 0, 100, 0, 128);
        PS4.setRumble(rambleVal, rambleVal);
    }

    data.ch[THROTTLE - 1] = mapSBUS(val);
}

// Gun on R1
void gun()
{

    if (PS4.R1())
    {
        Serial.println("Gun");
        data.ch[GUN - 1] = mapSBUS(-100);

        rambleStart(1000);
    }
}

// SIREN on Circle
void siren()
{

    if (PS4.Circle())
    {
        Serial.println("Siren");
        data.ch[SIREN - 1] = mapSBUS(100);
    }
}

// Engine start/Stop on Square
void engine()
{
    if (PS4.Square())
    {
        Serial.println("Engine");
        data.ch[ENGINE - 1] = mapSBUS(100);
    }
}

// Light on/off on Triangle
void lights()
{
    if (PS4.Triangle())
    {
        Serial.println("Lights");
        data.ch[LIGHTS - 1] = mapSBUS(100);
    }
}

// Sound1 on Cross
void sound1()
{
    if (PS4.Cross())
    {
        Serial.println("Sound 1");
        data.ch[SOUND1 - 1] = mapSBUS(100);
    }
}

// Steering directly controls Servo
// We map to the desired angle
void steering()
{
    int val = 90;

    if (abs(PS4.LStickX()) > 5)
    {
        val = map(PS4.LStickX(), -127, 127, 0, 180);
    }

    if (val != 90)
    {
        Serial.print("Steering : ");
        Serial.println(val);
    }

    servoSteering.write(val);
}

// Turret directly controls servo (360°)
// We map to the desired direction
void turret()
{
    int val = 90;

    if (PS4.RStickX() > 10)
    {
        val = 135;
    }
    else if (PS4.RStickX() < -10)
    {
        val = 45;
    }

    if (val != 90)
    {
        Serial.print("Turret : ");
        Serial.print(val);
    }

    servoTurret.write(val);
}

void rambleStart(unsigned long duration)
{
    ramble_trigger = true;
    ramble_duration = duration;
}

void ramble()
{
    // Start
    if (ramble_trigger)
    {
        Serial.println("Start ramble");

        ramble_startmillis = millis();
        ramble_trigger = false;
        PS4.setRumble(0, 255);
    }

    // Stop
    unsigned long currentMillis = millis(); // Get the current time
    if (ramble_duration > 0 && currentMillis - ramble_startmillis > ramble_duration)
    {
        Serial.println("Stop ramble");

        ramble_duration = 0;
        PS4.setRumble(0, 0);
    }
}