#include <Servo.h>

// Crear objetos Servo para los cuatro servomotores y el de dirección
Servo servo1, servo2, servo3, servo4, servoDireccion;

// Definir los pines de los servomotores
const int servoPin1 = 8;
const int servoPin2 = 9;
const int servoPin3 = 10;
const int servoPin4 = 11;
const int servoDireccionPin = 12; // Servo de dirección

// Definir los pines de los botones
const int buttonPin1 = 2;
const int buttonPin2 = 3;
const int buttonPin3 = 4;
const int buttonPin4 = 5;
const int buttonLedPin = 6;

// Botones para los LEDs amarillos
const int yellowButton1 = 29; // Botón LED amarillo 1
const int yellowButton2 = 30; // Botón LED amarillo 2

// Definir el pin del potenciómetro
const int potPin = A0;

// Definir los pines de los LEDs RGB
const int ledRedPin = 22;
const int ledGreenPin = 23;
const int ledBluePin = 24;

// Pines de los LEDs de giro
const int leftLedPin = 27;  // LED para giro a la izquierda
const int rightLedPin = 28; // LED para giro a la derecha

// Pines de los LEDs amarillos
const int yellowLed1 = 25; // LED amarillo 1
const int yellowLed2 = 26; // LED amarillo 2

// Pines del joystick
const int joyXPin = A1; // Eje X
const int joySWPin = 7; // Pulsador

// Variables para el joystick
int joyXValue = 0;

// Variables para LEDs y botones
int buttonState1 = 0, buttonState2 = 0, buttonState3 = 0, buttonState4 = 0;
int buttonLedState = 0;
bool ledsOn = false;

// Variables para la dirección
int direccionAngulo = 90; // Ángulo inicial del servo de dirección

// Estados de los LEDs amarillos
bool yellow1Blinking = false;
bool yellow2Blinking = false;

// Estado de los botones amarillos (para detección de pulsaciones únicas)
int lastYellowButton1State = HIGH;
int lastYellowButton2State = HIGH;

void setup() {
  // Asignar los pines de los servomotores
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);
  servo4.attach(servoPin4);
  servoDireccion.attach(servoDireccionPin);

  // Configurar los pines de los botones como entradas con resistencia pull-up
  pinMode(buttonPin1, INPUT_PULLUP);
  pinMode(buttonPin2, INPUT_PULLUP);
  pinMode(buttonPin3, INPUT_PULLUP);
  pinMode(buttonPin4, INPUT_PULLUP);
  pinMode(buttonLedPin, INPUT_PULLUP);

  // Configurar los botones para los LEDs amarillos
  pinMode(yellowButton1, INPUT_PULLUP);
  pinMode(yellowButton2, INPUT_PULLUP);

  // Configurar los pines de los LEDs RGB y de giro como salidas
  pinMode(ledRedPin, OUTPUT);
  pinMode(ledGreenPin, OUTPUT);
  pinMode(ledBluePin, OUTPUT);
  pinMode(leftLedPin, OUTPUT);
  pinMode(rightLedPin, OUTPUT);

  // Configurar los pines de los LEDs amarillos como salidas
  pinMode(yellowLed1, OUTPUT);
  pinMode(yellowLed2, OUTPUT);

  // Inicializar LEDs apagados
  digitalWrite(ledRedPin, LOW);
  digitalWrite(ledGreenPin, LOW);
  digitalWrite(ledBluePin, LOW);
  digitalWrite(leftLedPin, LOW);
  digitalWrite(rightLedPin, LOW);
  digitalWrite(yellowLed1, LOW);
  digitalWrite(yellowLed2, LOW);
}

void loop() {
  // Leer el valor del potenciómetro
  int potValue = analogRead(potPin);
  int potAngle = map(potValue, 0, 1023, 0, 90);

  // Leer el estado de los botones
  buttonState1 = digitalRead(buttonPin1);
  buttonState2 = digitalRead(buttonPin2);
  buttonState3 = digitalRead(buttonPin3);
  buttonState4 = digitalRead(buttonPin4);
  buttonLedState = digitalRead(buttonLedPin);

  // Leer los estados de los botones amarillos
  int yellowButton1State = digitalRead(yellowButton1);
  int yellowButton2State = digitalRead(yellowButton2);

  // Detectar cambios en el botón del LED amarillo 1
  if (yellowButton1State == LOW && lastYellowButton1State == HIGH) {
    yellow1Blinking = !yellow1Blinking; // Cambiar estado de parpadeo
  }
  lastYellowButton1State = yellowButton1State;

  // Detectar cambios en el botón del LED amarillo 2
  if (yellowButton2State == LOW && lastYellowButton2State == HIGH) {
    yellow2Blinking = !yellow2Blinking; // Cambiar estado de parpadeo
  }
  lastYellowButton2State = yellowButton2State;

  // Controlar el parpadeo del LED amarillo 1
  if (yellow1Blinking) {
    digitalWrite(yellowLed1, millis() % 500 < 250 ? HIGH : LOW); // Parpadea cada 500 ms
  } else {
    digitalWrite(yellowLed1, LOW); // Apagado
  }

  // Controlar el parpadeo del LED amarillo 2
  if (yellow2Blinking) {
    digitalWrite(yellowLed2, millis() % 500 < 250 ? HIGH : LOW); // Parpadea cada 500 ms
  } else {
    digitalWrite(yellowLed2, LOW); // Apagado
  }

  // Controlar los servos de suspensión con los botones
  if (buttonState1 == LOW) servo1.write(90); else servo1.write(potAngle);
  if (buttonState2 == LOW) servo2.write(90); else servo2.write(potAngle);
  if (buttonState3 == LOW) servo3.write(90); else servo3.write(potAngle);
  if (buttonState4 == LOW) servo4.write(90); else servo4.write(potAngle);

  // Controlar los LEDs con el botón
  if (buttonLedState == LOW) ledsOn = !ledsOn;
  digitalWrite(ledRedPin, LOW);
  digitalWrite(ledGreenPin, LOW);
  digitalWrite(ledBluePin, ledsOn ? HIGH : LOW);

  // Leer valores del joystick
  joyXValue = analogRead(joyXPin);

  // Controlar el servo de dirección (Eje X)
  direccionAngulo = map(joyXValue, 0, 1023, 0, 180); // Convertir el eje X a ángulo
  servoDireccion.write(direccionAngulo);

  // Controlar los LEDs de giro
  if (direccionAngulo < 85) {
    digitalWrite(leftLedPin, HIGH);
    digitalWrite(rightLedPin, LOW);
  } else if (direccionAngulo > 95) {
    digitalWrite(leftLedPin, LOW);
    digitalWrite(rightLedPin, HIGH);
  } else {
    digitalWrite(leftLedPin, LOW);
    digitalWrite(rightLedPin, LOW);
  }

  delay(15);
}