#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <EEPROM.h>

// Pines para los botones
const int botonStart = A0;
const int botonPres = A1;
const int botonTime = A2;


// Pines para las salidas
const int buzzerPin = 4;
const int servo1Pin = 5;
const int servo2Pin = 6;
const int RS1 = 7;
const int RS2 = 8;
const int RM = 9;

// Crear objetos

LiquidCrystal_I2C lcd (0x27, 2, 1, 0, 4, 5, 6, 7);
Servo servo1;
Servo servo2;
unsigned long tiempoInicio;

// Variables para almacenar los valores de PRES y TIME
int presValue = 2;
int timeValue = 2;

// Variables auxiliares
bool tareaEnProgreso = false;
// Variables para almacenar las posiciones iniciales de los servos
int startPos1 = 0;
int startPos2 = 0;

void setup() {
  
 // Leer los valores almacenados en la memoria EEPROM  
  presValue = EEPROM.read(0);
  timeValue = EEPROM.read(1);
  actualizarDisplay();
 // Leer los valores almacenados en la memoria EEPROM
  int storedPresValue = EEPROM.read(0);
  int storedTimeValue = EEPROM.read(1);

 // Verificar si los valores leídos son válidos (distintos de 255)
  if (storedPresValue != 255 && storedTimeValue != 255) {
    // Los valores en la memoria EEPROM son válidos, usarlos
    presValue = storedPresValue;
    timeValue = storedTimeValue;
  } else {
    // Los valores en la memoria EEPROM no son válidos, establecer valores predeterminados
    presValue = 2; // Valor predeterminado para PRES
    timeValue = 2; // Valor predeterminado para TIME

    // Guardar los valores predeterminados en la memoria EEPROM
    EEPROM.write(0, presValue);
    EEPROM.write(1, timeValue);
  }

  actualizarDisplay();
  // Inicializar el display LCD
  lcd.setBacklightPin(3,POSITIVE);
  lcd.setBacklight(HIGH);
  lcd.begin(16, 2);
  lcd.clear();

  // Configurar los pines de los botones como entradas con resistencias pull-up
  pinMode(botonPres, INPUT_PULLUP);
  pinMode(botonTime, INPUT_PULLUP);
  pinMode(botonStart, INPUT_PULLUP);
  pinMode(buzzerPin, OUTPUT);
  pinMode(RS1, OUTPUT);
  pinMode(RS2, OUTPUT);
  pinMode(RM, OUTPUT);



  // Adjuntar los servos a los pines correspondientes
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  // posición inicial
    servo1.write(0);
    servo2.write(0);

  // Mostrar la leyenda inicial en el display
  actualizarDisplay();
}

void loop() {
  // Leer el estado de los botones (estado bajo cuando se presiona)
  bool presionadoPres = !digitalRead(botonPres);
  bool presionadoTime = !digitalRead(botonTime);
  bool presionadoStart = !digitalRead(botonStart);

  // Lógica para el botón "P" (incrementar el valor de PRES)
  if (presionadoPres) {
    presValue++;
    if (presValue > 3) {
      presValue = 1;
    }
    actualizarDisplay();
    delay(200); // Pequeño retardo para evitar rebotes
    
    // Almacenar el valor actual de presValue en la memoria EEPROM
    EEPROM.write(0, presValue);
  }

  // Lógica para el botón "T" (incrementar el valor de TIME)
  if (presionadoTime) {
    timeValue++;
    if (timeValue > 3) {
      timeValue = 1;
    }
    actualizarDisplay();
    delay(200); // Pequeño retardo para evitar rebotes
     // Almacenar el valor actual de timeValue en la memoria EEPROM
    EEPROM.write(1, timeValue);
  }

  // Lógica para el botón "S" (iniciar la tarea principal)
   if (presionadoStart && !tareaEnProgreso) {
    tareaEnProgreso = true;
    lcd.clear();
    lcd.setCursor(3, 0);
    lcd.print("WORKING...");
    lcd.setCursor(6, 1);
    lcd.print("         ");

    // Mover servo1 a 90 grados en 1 segundos
    int startPos1 = 0;
    int targetPos1 = 90;
    unsigned long startTime1 = millis();
    while (millis() - startTime1 <= 1000) {
    int pos1 = map(millis() - startTime1, 0, 1000, startPos1, targetPos1);
    servo1.write(pos1);
    delay(20); // Pequeño retardo para controlar la velocidad del movimiento
    }

    // Mover servo2 
    digitalWrite(RS1, HIGH);
    int angulo2 = 0;
    switch (presValue) {
      case 1:
        angulo2 = 20;
        break;
      case 2:
        angulo2 = 25;
        break;
      case 3:
        angulo2 = 30;
        break;
        }
    int startPos2 = 0;
    int targetPos2 = angulo2;
    unsigned long startTime2 = millis();
    while (millis() - startTime2 <= 300) {
    int pos2 = map(millis() - startTime2, 0, 300, startPos2, targetPos2);
    servo2.write(pos2);
    delay(20); // Pequeño retardo para controlar la velocidad del movimiento
    }
    
    // Activar el relay durante 'motortime' segundos
    switch (timeValue) {
      case 1:
        digitalWrite(RS1, HIGH);
        delay(500);
        digitalWrite(RS1, LOW);
        break;
      case 2:
        digitalWrite(RS1, HIGH);
        delay(1000);
        digitalWrite(RS1, LOW);
        break;
      case 3:
        digitalWrite(RS1, HIGH);
        delay(1500);
        digitalWrite(RS1, LOW);
        break;
      default:
       int presValue = 2;
       int timeValue = 2;
        // Si timeValue no es 1, 2 o 3, hacer algo aquí (opcional)
        break;
    }

   // Volver a la posición inicial de forma gradual en 1 segundo
    unsigned long startTime = millis();
    int targetPos = 0; // Definimos targetPos y lo establecemos en la posición inicial (0 grados)

    startPos1 = servo1.read(); // Actualizamos la posición inicial del servo 1
    startPos2 = servo2.read(); // Actualizamos la posición inicial del servo 2

    while (millis() - startTime <= 1000) {
      int pos1 = map(millis() - startTime, 0, 1000, startPos1, targetPos);
      int pos2 = map(millis() - startTime, 0, 500, startPos2, targetPos);

      servo1.write(pos1);
      servo2.write(pos2);

      delay(20); // Pequeño retardo para controlar la velocidad del movimiento
    }

    // Asegurarse de que los servos estén en la posición inicial (0 grados) exactamente
    servo1.write(0);
    servo2.write(0);


    lcd.clear();
    lcd.setCursor(4, 0);
    lcd.print("-FINISH-");
    digitalWrite(buzzerPin, HIGH);
    delay(1000);
    digitalWrite(buzzerPin, LOW);
    tareaEnProgreso = false;
    actualizarDisplay();
  }
}

// Función para actualizar el contenido del display LCD
void actualizarDisplay() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("PRES: ");
  lcd.print(presValue);
  lcd.setCursor(8, 0);
  lcd.print("TIME: ");
  lcd.print(timeValue);
  lcd.setCursor(5, 1);
  lcd.print("READY!");
}