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

// Configuración del LCD 20x4 con interfaz I2C
LiquidCrystal_I2C lcd(0x27, 20, 4);

// Configuración del teclado matricial 4x4
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
byte rowPins[ROWS] = {11, 10, 8, 7};
byte colPins[COLS] = {6, 5, 4, 2};
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// Configuración del sensor infrarrojo digital
const int sensorPin = 3;
int cantidadPiezas = 0;
int piezasContadas = 0;
unsigned long tiempoAnterior = 0;
const unsigned long intervaloDeEspera = 100; // Intervalo de espera en milisegundos para evitar rebotes
bool separando = false;
String cantidadStr = "";
bool sensorAnteriorEstado = LOW; // Estado anterior del sensor para detectar cambios

Servo servo;
const int pinServo = 13;

// Estado del menú actual
enum MenuState {MAIN_MENU, CONTAR_MENU, SEPARAR_MENU, REGISTRO_MENU};
MenuState currentMenu = MAIN_MENU;

void muestro_menu() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("A. Contar Tuercas");
  lcd.setCursor(0, 1);
  lcd.print("B. Separar Tuercas");
  lcd.setCursor(0, 2);
  lcd.print("C. Registro Conteo");
}

void separandoPiezas () {
  bool sensorEstadoActual = digitalRead(sensorPin);
  //Serial.println(sensorEstadoActual ? " ON" : "OFF");
  lcd.setCursor(0, 2);
  lcd.print(sensorEstadoActual ? "true" : "false");
  if (sensorEstadoActual && !sensorAnteriorEstado) {
    piezasContadas++;
    lcd.setCursor(0, 1);
    lcd.print("Contador: ");
    lcd.print(piezasContadas);
    delay(200); // Retardo para evitar múltiples conteos por una sola pieza
    if (piezasContadas >= cantidadPiezas) {
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Separacion completada");
      delay(2000);
      separando = false;
    }
  }
  if (sensorEstadoActual != sensorAnteriorEstado) {
    Serial.println(sensorEstadoActual ? " ON" : "OFF");
    sensorAnteriorEstado = sensorEstadoActual;
  }

}

void setup() {
  Serial.begin(9600);
  pinMode(sensorPin, INPUT);
  pinMode(pinServo, OUTPUT);
  servo.attach(9);

  // Inicializar el LCD
  lcd.init();
  lcd.backlight();
  muestro_menu();
}

void loop() {
  char key = keypad.getKey();

  if (key) {
    switch (currentMenu) {
      case MAIN_MENU:
        handleMainMenu(key);
        break;

      case CONTAR_MENU:
        handleContarMenu(key);
        break;

      case SEPARAR_MENU:
        handleSepararMenu(key);
        break;

      case REGISTRO_MENU:
        handleRegistroMenu(key);
        break;
    }
  }
  //Serial.println("Key: "+ String(key));
  if (separando) {
    separandoPiezas();
  }
}

void handleMainMenu(char key) {
  muestro_menu();


  if (key == 'A') {
    currentMenu = CONTAR_MENU;
  } else if (key == 'B') {
    currentMenu = SEPARAR_MENU;
  } else if (key == 'C') {
    currentMenu = REGISTRO_MENU;
  } else if (key == 'D') {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Saliendo...");
    delay(1000);
    lcd.clear();
    currentMenu = MAIN_MENU;
  }
}

void handleContarMenu(char key) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Contando tuercas...");
  lcd.setCursor(0, 3);
  lcd.print("Press D to salir");

  piezasContadas = 0;
  tiempoAnterior = millis(); // Guardar el tiempo inicial

  while (true) {
    if (millis() - tiempoAnterior >= intervaloDeEspera) {
      bool sensorEstadoActual = digitalRead(sensorPin);
      //Serial.println(sensorEstadoActual ? " ON" : "OFF");
      if (sensorEstadoActual && !sensorAnteriorEstado) {
        piezasContadas++;
        lcd.setCursor(0, 1);
        lcd.print("Contadas: ");
        lcd.print(piezasContadas);
        tiempoAnterior = millis(); // Actualizar el tiempo anterior
      }
      sensorAnteriorEstado = sensorEstadoActual;
    }

    if (keypad.getKey() == 'D') {
      currentMenu = MAIN_MENU;
      return;
    }
  }
}

void handleSepararMenu(char key) {
  bool sensorEstadoActual;

  lcd.setCursor(0, 3);
  lcd.print(separando ? "true" : "false");
  //Serial.println(separando ? "separando =  ON" : "separando = OFF");
  if (!separando) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Ingrese cantidad:");
    if (key >= '0' && key <= '9') {
      cantidadStr += key;
      lcd.setCursor(0, 1);
      lcd.print("Cantidad: ");
      lcd.print(cantidadStr);
    } else if (key == '#') {
      cantidadPiezas = cantidadStr.toInt();
      cantidadStr = "";
      separando = true;
      piezasContadas = 0;
      //lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Separando tuercas...");
      lcd.setCursor(0, 1);
      lcd.print("Contador: ");
      lcd.print(piezasContadas);
    }
    // } else {


    //   if (key == 'D') {
    //     currentMenu = MAIN_MENU;
    //     return;
    //   }
  }
}

void handleRegistroMenu(char key) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Conteo registrado:");
  lcd.setCursor(0, 1);
  lcd.print(piezasContadas);

  if (key == 'D') {
    currentMenu = MAIN_MENU;
  }
}