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

bool testMode = false; // Variable para indicar si estamos en modo test
unsigned long inicioPresionado = 0; // Variable para almacenar el tiempo de inicio de la presión simultánea
const int duracionPresionado = 3000; // Duración de la presión simultánea en milisegundos

// Pines para los botones
const int botonStart = A0;
const int botonPres = A1;
const int botonTime = A2;
const int CNY70PinA = 8; // sensor a
const int CNY70PinB = 7; //sensor b

// Pines para las salidas 
const int RpinF1 = 2;
const int RpinF2 = 3;
const int RpinBrazo = 4;
const int RpinRodillo = 5;
const int servo2Pin = 6;
const int buzzerPin = 10;

// Crear objetos
LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo servo2;
unsigned long tiempoInicio;
const int finCarUmb = 900;
const int umbralLuz = 500;

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

// Variables auxiliares
bool tareaEnProgreso = false;
bool switchAPressed = false;
bool tareabrazoB = 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.init();                      
  lcd.backlight();

  // 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(RpinF1, OUTPUT);
  pinMode(RpinF2, OUTPUT);
  pinMode(RpinBrazo, OUTPUT);
  pinMode(RpinRodillo, OUTPUT);
  pinMode(CNY70PinA, INPUT); // Configurar el pin del sensor CNY70 A como entrada
  pinMode(CNY70PinB, INPUT); // Configurar el pin del sensor CNY70 B como entrad 
  
  

  digitalWrite(RpinF1, HIGH);
  digitalWrite(RpinF2, HIGH);
  digitalWrite(RpinBrazo, HIGH);
  digitalWrite(RpinRodillo, HIGH);
  
  // Adjuntar los servos a los pines correspondientes
    servo2.attach(servo2Pin);
  // posición inicial
    servo2.write(0);

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

 /*     // Leer los estados iniciales de los sensores
    bool estadoA = digitalRead(CNY70PinA);
  bool estadoB = digitalRead(CNY70PinB);

  // Combinación de estados de los sensores
  if (estadoA == LOW && estadoB == HIGH) {
    // Caso A0 y B1: ninguna instrucción, se pasa al void loop
  } else if (estadoA == LOW && estadoB == LOW) {
    // Caso A0 y B0: activar brazo hasta que A0 y B1
        digitalWrite(RpinF1, LOW);
        digitalWrite(RpinF2, LOW);
        delay (50);
        digitalWrite(RpinBrazo, LOW); 
    while (digitalRead(CNY70PinA) == LOW && digitalRead(CNY70PinB) == LOW) {
      delay(10); // Pequeño retardo para evitar un bucle demasiado rápido
    }
    digitalWrite(RpinBrazo, HIGH); // Detener el brazo cuando se alcanza A0 y B1
  } else if (estadoA == HIGH && estadoB == LOW) {
    // Caso A1 y B0: activar brazo hasta que A0 y B1
        digitalWrite(RpinF1, LOW);
        digitalWrite(RpinF2, LOW);
        delay (50);
        digitalWrite(RpinBrazo, LOW); 
    while (digitalRead(CNY70PinA) == HIGH && digitalRead(CNY70PinB) == LOW) {
      delay(10); // Pequeño retardo para evitar un bucle demasiado rápido
    }
    digitalWrite(RpinBrazo, HIGH); // Detener el brazo cuando se alcanza A0 y B1
  } else if (estadoA == HIGH && estadoB == HIGH) {
    // Caso A1 y B1: Mostrar mensaje de error y hacer sonar el buzzer
    for (int i = 0; i < 4; i++) {
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Error Posicion");
      lcd.setCursor(0, 1);
      lcd.print("Brazo");
      digitalWrite(buzzerPin, HIGH);
      delay(250);
      lcd.clear();
      digitalWrite(buzzerPin, LOW);
      delay(250);
    }
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Corregir posicion");
    lcd.setCursor(0, 1);
    lcd.print("y reiniciar prog.");
    while (true); // Detener el programa aquí
  }
  */
}

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

  // Lógica para detectar la combinación de botones para entrar al modo test
  if (presionadoPres && presionadoTime) {
    if (!testMode) { // Si no estamos ya en modo test
      inicioPresionado = millis(); // Almacenar el tiempo de inicio de la presión simultánea
    }
    while (millis() - inicioPresionado <= duracionPresionado) {
      // Esperar a que pasen 3 segundos para confirmar la presión simultánea
      if (!digitalRead(botonPres) && !digitalRead(botonTime)) {
        testMode = true; // Activar el modo test
        break;
      }
    }
    if (testMode) {
      // Entrar al modo test
      modoTest();
    }
  }

  // Lógica normal del programa
  if (!testMode) {
  
  // 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;
    tareabrazoB = true;
       
          
    lcd.clear();
    lcd.setCursor(3, 0);
    lcd.print("WORKING...");
    lcd.setCursor(6, 1);
    lcd.print("         ");
          
    digitalWrite(RpinBrazo, LOW);
    int luzAState = analogRead(CNY70PinA); // Leer el valor analógico del sensor CNY70 conectado al pin A
    int luzBState = analogRead(CNY70PinB); // Leer el valor analógico del sensor CNY70 conectado al pin B
    
    // Mover Brazo Adelante
    while (luzAState > umbralLuz) {
        digitalWrite(RpinF1, HIGH);
        digitalWrite(RpinF2, HIGH);
        delay (50);
        digitalWrite(RpinBrazo, LOW);  
        luzAState = analogRead(CNY70PinA); // Leer el valor analógico del sensor CNY70 conectado al pin A
    }  

    // Brazo se activa
    digitalWrite(RpinBrazo, HIGH);
             
    // Mover servo2 
    digitalWrite(RpinRodillo, LOW);
    int angulo2 = 0;
    switch (presValue) {
        case 1:
            angulo2 = 20;
            break;
        case 2:
            angulo2 = 45;
            break;
        case 3:
            angulo2 = 90;
            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(RpinRodillo, LOW);
            delay(500);
            digitalWrite(RpinRodillo, HIGH);
            break;
        case 2:
            digitalWrite(RpinRodillo, LOW);
            delay(2000);
            digitalWrite(RpinRodillo, HIGH);
            break;
        case 3:
            digitalWrite(RpinRodillo, LOW);
            delay(5000);
            digitalWrite(RpinRodillo, HIGH);
            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)

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

        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
    servo2.write(0);
          
    tareabrazoB = false;
    digitalWrite(RpinF1, LOW);
    digitalWrite(RpinF2, LOW);
    delay(50);

    int luzState2 = analogRead(CNY70PinB); // Leer el valor analógico del sensor CNY70 conectado al pin B
    while (luzState2 > umbralLuz && !tareabrazoB) {
        digitalWrite(RpinBrazo, LOW);
        luzState2 = analogRead(CNY70PinB); // Leer el valor analógico del sensor CNY70 conectado al pin B
    }
    digitalWrite(RpinBrazo, HIGH);
   

    tareaEnProgreso = false;
    actualizarDisplay();
    digitalWrite(RpinF1, HIGH);
    digitalWrite(RpinF2, HIGH);
    digitalWrite(RpinBrazo, HIGH);
    digitalWrite(RpinRodillo, HIGH);
}
       
}

}

void modoTest() {
  
  char opcionesMenu[][10] = {"Rodillo", "Brazo", "Sensores", "Servo", "Display", "Modo 5", "Modo 6", "Exit"};
  int opcionSeleccionada = 0;


  unsigned long inicioPresionadoStart = 0; // Variable para almacenar el inicio de la presión del botón Start
  bool ejecutandoTest = false; // Bandera para indicar si se está ejecutando un test

  while (true) {
   
     lcd.setCursor(0, 0);
     lcd.print("   Modo Test    "); // Alineación centrada

     lcd.setCursor(0, 1);
    lcd.print("TEST: ");

    lcd.setCursor(6, 1); // Posición fija para el nombre del modo
    lcd.print(opcionesMenu[opcionSeleccionada]);
    lcd.print("            "); // Limpiamos cualquier remanente de texto anterior

    bool presionadoPres = !digitalRead(botonPres);
    bool presionadoTime = !digitalRead(botonTime);
    bool presionadoStart = !digitalRead(botonStart);

    if (presionadoStart) {
      ejecutarTest(opcionSeleccionada);
          ejecutandoTest = true;
      delay(50); // Delay opcional para evitar múltiples incrementos
    }

    // Navegación por el menú
    if (presionadoPres) {
      opcionSeleccionada = (opcionSeleccionada - 1 + 8) % 8;
      delay(50); // Delay opcional para evitar múltiples incrementos
    }

    if (presionadoTime) {
      opcionSeleccionada = (opcionSeleccionada + 1) % 8;
      delay(50); // Delay opcional para evitar múltiples incrementos
    }
  }
}
// Funciones para cada test
void testRodillo() {
  delay(200);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("  Test Rodillo");

  while (true) {
    bool presionadoPres = !digitalRead(botonPres);
    bool presionadoTime = !digitalRead(botonTime);
    bool presionadoStart = !digitalRead(botonStart);

    // Lógica para activar/desactivar el rodillo según el botón presionado
    if (presionadoTime) {
      digitalWrite(RpinRodillo, LOW); // Mandar un 0 en RpinRodillo si se presiona el botón Time
    } else if (presionadoPres) {
      digitalWrite(RpinRodillo, HIGH); // Mandar un 1 en RpinRodillo si se presiona el botón Pres
    }

    // Salir del bucle y volver al menú principal si se presiona el botón Start
    if (presionadoStart) {
      digitalWrite(RpinRodillo, HIGH); // Mandar un 1 en RpinRodillo antes de salir
      break; // Salir del bucle "Test Rodillo"
    }
  }
}

void testBrazo() {
  delay(200);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("   Test Brazo");

  while (true) {
    bool presionadoPres = !digitalRead(botonPres);
    bool presionadoTime = !digitalRead(botonTime);
    bool presionadoStart = !digitalRead(botonStart);

    // Lógica para controlar los pines según los botones presionados
    if (presionadoTime) {
      // Mientras se presiona el botón Time
      digitalWrite(RpinF1, HIGH);
      digitalWrite(RpinF2, HIGH);
      digitalWrite(RpinBrazo, LOW);
    } else if (presionadoPres) {
      // Mientras se presiona el botón Pres
      digitalWrite(RpinF1, LOW);
      digitalWrite(RpinF2, LOW);
      digitalWrite(RpinBrazo, LOW);
    } else {
      // Cuando no se presionan los botones
      digitalWrite(RpinF1, HIGH);
      digitalWrite(RpinF2, HIGH);
      digitalWrite(RpinBrazo, HIGH);
    }

    if (presionadoStart) {
      digitalWrite(RpinF1, HIGH);
      digitalWrite(RpinF2, HIGH);
      digitalWrite(RpinBrazo, HIGH);
      break; // Salir del bucle "Test Brazo"
    }
  }
  void modotest();
}

void testSensores() {
  delay(200);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(" Test Sensores");

  bool sensorA = true; // Variable para seguir cuál sensor se está probando
  while (true) {
    bool presionadoPres = !digitalRead(botonPres);
    bool presionadoTime = !digitalRead(botonTime);
    bool presionadoStart = !digitalRead(botonStart);

    int sensorValue = sensorA ? digitalRead(CNY70PinA) : digitalRead(CNY70PinB);
    lcd.setCursor(0, 1);
    lcd.print("Sensor ");
    lcd.print(sensorA ? "A" : "B");
    lcd.print(" = ");
    if (sensorValue == LOW) {
      lcd.print("ON ");
      digitalWrite(10, HIGH); // Activar el buzzer
    } else {
      lcd.print("OFF");
      digitalWrite(10, LOW); // Desactivar el buzzer
    }

    if (presionadoPres) {
      sensorA = false;
      delay(100);
    }

    if (presionadoTime) {
      sensorA = true;
      delay(100);
    }

    if (presionadoStart) {
      digitalWrite(10, LOW); // Asegurarse de que el buzzer esté apagado
      break; // Salir del bucle "Test Sensores"
    }
  }
}

void testServo() {
  delay(200);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("   Test Servo");
  

  int servoAngle = 0; // Inicializar el ángulo del servo a 0 grados
  servo2.write(servoAngle);
 //delay(200);
  while (true) {
    lcd.setCursor(0, 1);
    lcd.print("Pos = ");
    lcd.print(servoAngle);
    lcd.print("   "); // Limpiar caracteres restantes si hay

    
    bool presionadoPres = !digitalRead(botonPres);
    bool presionadoTime = !digitalRead(botonTime);
    bool presionadoStart = !digitalRead(botonStart);

    if (presionadoTime && servoAngle < 180) {
      servoAngle += 10;
      servo2.write(servoAngle);
      delay(50); // Pequeño retardo para evitar cambios rápidos
    }

    if (presionadoPres && servoAngle > 0) {
      servoAngle -= 10;
      servo2.write(servoAngle);
      delay(50); // Pequeño retardo para evitar cambios rápidos
    }

    if (presionadoStart) {
      servoAngle = 0; // Restaurar ángulo inicial
      servo2.write(servoAngle); // Mover servo a ángulo 0
    
      break; // Salir del bucle "Test Servo"
      
    }
  }
}

void testDisplay() {
  lcd.init();
  lcd.backlight(); // Encender retroiluminación (si está disponible)

  const int LCD_COLUMNS = 16;
  const int LCD_ROWS = 2;
  lcd.clear();
  lcd.setCursor(0, 0);

  // Llena la primera línea
  for (int i = 0; i < LCD_COLUMNS; i++) {
    lcd.print((char)255); // Carácter especial para algunos displays
  }

  // Llena la segunda línea
  lcd.setCursor(0, 1);
  for (int i = 0; i < LCD_COLUMNS; i++) {
    lcd.print((char)255); // Carácter especial para algunos displays
  }
  delay(400);

  // Apagar retroiluminación durante 1 segundo
  lcd.noBacklight();
  delay(400);

  // Encender retroiluminación durante 1 segundo
  lcd.backlight();
  delay(400);

  // Hacer un barrido de izquierda a derecha y de arriba a abajo
  lcd.clear();
  lcd.setCursor(0, 0);
  for (int i = 0; i < LCD_COLUMNS * LCD_ROWS; i++) {
    lcd.print("*");
    delay(50); // Retardo para el efecto de barrido
    if (i % LCD_COLUMNS == LCD_COLUMNS - 1) {
      lcd.setCursor(0, 1); // Cambiar a la siguiente línea
    }
  }
}

// Función para ejecutar el test según la opción seleccionada
void ejecutarTest(int opcion) {
  switch (opcion) {
    case 0:
      testRodillo();
      break;
    case 1:
      testBrazo();
      break;
    case 2:
      testSensores();
      break;
    case 3:
      testServo();
      break;
    case 4:
      testDisplay();
      break;
    case 7:
      asm volatile ("jmp 0"); // Reiniciar el Arduino
      break;
  }
}
// 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!");
}