#include <ESP32Servo.h>
#include <Ps3Controller.h>

#define ESC1_PIN 26
#define ESC2_PIN 27
#define ESC3_PIN 14  // Define pin for the third motor

Servo esc1;
Servo esc2;
Servo esc3;  // Define the third motor

void notify()
{
    int lx = Ps3.data.analog.stick.lx;
    int ly = Ps3.data.analog.stick.ly;

    // Forward speed based on vertical joystick movement
    int speed = map(ly, 0, 255, 1000, 2000);

    // Adjust ESC speeds for turning
    int esc1Speed = speed;
    int esc2Speed = speed;

    if (lx < 255) {  // Move left
        esc1Speed = map(lx, 0, 128, 1000, speed);
        esc2Speed = speed;
    } else if (lx > 255) {  // Move right
        esc1Speed = speed;
        esc2Speed = map(lx, 0, 255, speed, 1000);
    }

    // Set the ESC speeds
    esc1.writeMicroseconds(esc1Speed);
    esc2.writeMicroseconds(esc2Speed);

    // Control the third motor with the triangle button
    if (Ps3.data.button.triangle) {
        esc3.writeMicroseconds(2000);  // Full speed forward
    } else {
        esc3.writeMicroseconds(1000);  // Stop the motor
    }

    Serial.print("Left stick: x="); Serial.print(lx); Serial.print(" y="); Serial.println(ly);
    Serial.print("ESC1 speed: "); Serial.println(esc1Speed);
    Serial.print("ESC2 speed: "); Serial.println(esc2Speed);
    Serial.print("ESC3 speed: "); Serial.println(Ps3.data.button.triangle ? 2000 : 1000);
}

void onConnect(){
    Serial.println("Connected.");
}

void setup() {
    Serial.begin(115200);

    // Initialize ESCs
    esc1.attach(ESC1_PIN);
    esc2.attach(ESC2_PIN);
    esc3.attach(ESC3_PIN);  // Initialize the third motor

    // Set all ESCs to neutral (1000 microseconds)
    esc1.writeMicroseconds(1000);
    esc2.writeMicroseconds(1000);
    esc3.writeMicroseconds(1000);

    Ps3.attach(notify);
    Ps3.attachOnConnect(onConnect);
    Ps3.begin("");

    Serial.println("Ready.");
}

void loop() {
    if (!Ps3.isConnected())
        return;
}