#include <Servo.h>

// Crear objetos Servo para los cuatro servomotores
Servo myServo1;
Servo myServo2;
Servo myServo3;
Servo myServo4;

// Definir los pines de los potenciómetros
const int potPin1 = A0;
const int potPin2 = A1;

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

const int buttonLedPin = 6;

// 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

const int pinPotenciometro = A2; // Potenciometro direccion

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

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


// 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;

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

void setup() {
  
  // Asignar los pines de los servomotores
  myServo1.attach(9);
  myServo2.attach(10);
  myServo3.attach(11);
  myServo4.attach(12);

  pinMode(leftLedPin, OUTPUT);
  pinMode(rightLedPin, OUTPUT);
  pinMode(pinPotenciometro, INPUT);

  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 amarillos como salidas
  pinMode(yellowLed1, OUTPUT);
  pinMode(yellowLed2, OUTPUT);

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

void loop() {

  int valorPotenciometro = analogRead(pinPotenciometro); // Leer el valor del potenciómetro
  
  // Leer el valor del primer potenciómetro
  int potValue1 = analogRead(potPin1);
  // Leer el valor del segundo potenciómetro
  int potValue2 = analogRead(potPin2);

  // Mapear el valor del primer potenciómetro (0 a 1023) a ángulos de servomotor (0 a 90)
  int angle1 = map(potValue1, 0, 1023, 10, 90);
  int angle2 = map(potValue1, 0, 1023, 170, 90);  // Invertir el ángulo para el segundo servomotor

  // Mapear el valor del segundo potenciómetro (0 a 1023) a ángulos de servomotor (0 a 90)
  int angle3 = map(potValue2, 0, 1023, 10, 90);
  int angle4 = map(potValue2, 0, 1023, 170, 90);  // Invertir el ángulo para el cuarto servomotor

  // Mover los servomotores al ángulo calculado
  myServo1.write(angle1);
  myServo2.write(angle2);
  myServo3.write(angle3);
  myServo4.write(angle4);

  // Pequeño retardo para estabilizar el movimiento
  delay(15);

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

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

  // 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
  }
  // Si el valor del potenciómetro es menor que 512, se enciende el LED izquierdo
  // Si es mayor o igual a 512, se enciende el LED derecho
  if (valorPotenciometro < 512) { 
    digitalWrite(leftLedPin, HIGH);
    digitalWrite(rightLedPin, LOW); 
    } else {  
    digitalWrite(leftLedPin, LOW); 
    digitalWrite(rightLedPin, HIGH); 
    }
}