#include <Servo.h>
#include <LiquidCrystal.h>

#define BUTTON_PIN_3 2
#define BUTTON_PIN_4 3
#define TIMER_INTERVAL 10000

volatile bool button3Pressed = false;
volatile bool button4Pressed = false;
bool timerActive = false;
unsigned long lastButtonPressTime = 0;
int initialPosition = 0;

Servo servoRotar;
Servo servoEstirar;
Servo servoRecoger;
Servo servoGarra;
LiquidCrystal_I2C lcd(0x27, 16, 2);  // Cambia la dirección I2C si es necesario

void setup() {
  pinMode(BUTTON_PIN_3, INPUT_PULLUP);
  pinMode(BUTTON_PIN_4, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(BUTTON_PIN_3), button3Interrupt, CHANGE);
  attachInterrupt(digitalPinToInterrupt(BUTTON_PIN_4), button4Interrupt, CHANGE);

  servoRotar.attach(5);    // Cambia el pin si es necesario
  servoEstirar.attach(6);  // Cambia el pin si es necesario
  servoRecoger.attach(7);  // Cambia el pin si es necesario
  servoGarra.attach(8);    // Cambia el pin si es necesario

  lcd.begin(16, 2);
  lcd.print("Proyecto Final:");
  lcd.setCursor(0,1);
  lcd.print("Brazo Mecanico");
  delay(3000);
  lcd.clear();
  lcd.print("      Por:");
  delay(2000);
  lcd.clear();
  lcd.print("Cristian Alonzo");
  lcd.setCursor(0,1);
  lcd.print("y Adriana Giron");
  delay(5000);
  lcd.clear();

  // Inicializar comunicación serial
  Serial.begin(9600);
}

void loop() {
  if(timerActive) {
    unsigned long currentTime = millis();
    if(currentTime - lastButtonPressTime >= TIMER_INTERVAL) {
      resetServosToInitialPosition();
      timerActive = false;
    }
  }

  // Leer instrucciones por comunicación serial
  if (Serial.available() > 0) {
    int instruction = Serial.read();
    processInstruction(instruction);
  }
}

void button3Interrupt() {
  if(digitalRead(BUTTON_PIN_3) == HIGH) {
    if(!button3Pressed) {
      servoRotar.write(80);
      servoEstirar.write(90);
      servoRecoger.write(120);
      servoGarra.write(45);
      delay(3000);
      button3Pressed = true;

      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Accion:");
      lcd.setCursor(0,1);
      lcd.print("Posicion 3");
      delay(2000);

      resetTimer();

      // Enviar instrucción por comunicación serial
      Serial.write(3);
    }
    else {
      resetServosToInitialPosition();
      delay(3000);
      button3Pressed = false;

      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Accion: Reinicio");
      delay(2000);

      // Enviar instrucción por comunicación serial
      Serial.write(0);
    }
  }
}

void button4Interrupt() {
  if(digitalRead(BUTTON_PIN_4) == HIGH) {
    if(!button4Pressed) {
      servoRotar.write(60);
      servoEstirar.write(87);
      servoRecoger.write(65);
      servoGarra.write(30);
      delay(3000);
      button4Pressed = true;

      lcd.clear();
      lcd.setCursor(0,0),
      lcd.print("Accion:");
      lcd.setCursor(0,1);
      lcd.print("Posicion 4");
      delay(2000);

      resetTimer();

      // Enviar instrucción por comunicación serial
      Serial.write(4);
    }
    else{
      resetServosToInitialPosition();
      delay(3000);
      button4Pressed = false;

      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Accion: Reinicio");
      delay(2000);

      // Enviar instrucción por comunicación serial
      Serial.write(0);
    }
  } 
}

void resetServosToInitialPosition() {
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Inactividad");
  lcd.setCursor(0,1);
  lcd.print("Detectada");
  servoRotar.write(initialPosition);
  servoEstirar.write(initialPosition);
  servoRecoger.write(initialPosition);
  servoGarra.write(initialPosition);
}

void resetTimer() {
  lastButtonPressTime = millis();
  timerActive = true;
}

void processInstruction(int instruction) {
  // Procesar instrucción recibida
  switch (instruction) {
    case 1:
      // Mover a posición 1
      // ...
      break;
    case 2:
      // Mover a posición 2
      // ...
      break;
    // Agrega más casos según las posiciones que desees implementar

    default:
      // Instrucción desconocida
      break;
  }
}