#include <ESP32Servo.h>

// Definimos los pines a los que se conectan los servos
int servo1_pin = 2;
int servo2_pin = 4;
int servo3_pin = 5;

// Definimos los objetos de Servo
Servo servo1;
Servo servo2;
Servo servo3;

// Definimos los pines a los que se conectan los joysticks
int joy1_x_pin = 34;
int joy1_y_pin = 35;
int joy1_button_pin = 32;
int joy2_x_pin = 27;
int joy2_y_pin = 33;
int joy2_button_pin = 12;

// Definimos el pin a utilizar para la LED
int led_pin = 13;

// Variable para almacenar el estado de la LED
bool led_state = false;

// Definimos la función para controlar la LED
void toggleLed() {
  if (led_state) {
    digitalWrite(led_pin, LOW);
    led_state = false;
  } else {
    digitalWrite(led_pin, HIGH);
    led_state = true;
  }
}

void setup() {
  // Inicializamos los objetos de Servo
  servo1.attach(servo1_pin);
  servo2.attach(servo2_pin);
  servo3.attach(servo3_pin);

  // Configuramos los pines de los joysticks como entradas
  pinMode(joy1_x_pin, INPUT);
  pinMode(joy1_y_pin, INPUT);
  pinMode(joy1_button_pin, INPUT_PULLUP);
  pinMode(joy2_x_pin, INPUT);
  pinMode(joy2_y_pin, INPUT);
  pinMode(joy2_button_pin, INPUT_PULLUP);

  // Configuramos el pin de la LED como salida
  pinMode(led_pin, OUTPUT);
}

void loop() {
  // Leemos los valores de los joysticks
  int joy1_x = analogRead(joy1_x_pin);
  int joy1_y = analogRead(joy1_y_pin);
  int joy2_x = analogRead(joy2_x_pin);
  int joy2_y = analogRead(joy2_y_pin);

  // Mapeamos los valores de los joysticks a los ángulos de los servos
  int servo1_angle = map(joy1_x, 0, 4095, 0, 180);
  int servo2_angle = map(joy1_y, 0, 4095, 0, 180);
  int servo3_angle = map(joy2_x, 0, 4095, 0, 180);

  // Movemos los servos a los ángulos correspondientes
  servo1.write(servo1_angle);
  servo2.write(servo2_angle);
  servo3.write(servo3_angle);

  // Verificamos si se ha pulsado el botón del joystick 1
  if (digitalRead(joy1_button_pin) == LOW) {
    // Se ha pulsado el botón, así que cambiamos el estado de la LED
    toggleLed();
    // Esperamos un tiempo para evitar rebotes del botón
    delay(50);
  }
}