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

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

const int servoPin1 = 9; 
const int servoPin2 = 10; 
const int servoPin3 = 11; 
const int servoPin4 = 12;

Servo servoRotar; //Servo que rota el brazo mecánico
Servo servoEstirar; //Servo que estira el brazo mecánico
Servo servoRecoger; //Servo que hace que el suba y baje el brazo
Servo servoGarra; //Servo que controla la garra del brazo

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

LiquidCrystal lcd(4, 5, 6, 7, 8, 13);

void setup() {
  servoRotar.attach(servoPin1);
  servoEstirar.attach(servoPin2);
  servoRecoger.attach(servoPin3);
  servoGarra.attach(servoPin4);

  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.write(initialPosition);
  servoEstirar.write(initialPosition);
  servoRecoger.write(initialPosition);
  servoGarra.write(initialPosition);

  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();

}

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

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();
    }
    else {
      resetServosToInitialPosition();
      delay(3000);
      button3Pressed = false;

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

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();
  }
  else{
    resetServosToInitialPosition();
    delay(3000);
    button4Pressed = false;

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

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