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

// =====================================================
// LCD I2C
// =====================================================
LiquidCrystal_I2C lcd(0x27, 16, 2);

// =====================================================
// TECLADO 4x4
// =====================================================
const byte FILAS = 4;
const byte COLUMNAS = 4;

const byte pinesFilas[FILAS] = {6, 7, 8, 9};
const byte pinesColumnas[COLUMNAS] = {10, 11, 12, 13};

const char mapaTeclas[FILAS][COLUMNAS] = {
    {'1', '2', '3', 'A'},
    {'4', '5', '6', 'B'},
    {'7', '8', '9', 'C'},
    {'*', '0', '#', 'D'}
};

// =====================================================
// SALIDAS
// =====================================================
const byte LED_VERDE_1 = 14;
const byte LED_VERDE_2 = 15;
const byte LED_ROJO_1  = 16;
const byte LED_ROJO_2  = 17;
const byte PIN_SERVO   = 18;

Servo servoPuerta;

// =====================================================
// CONTRASEÑA
// =====================================================
const char CLAVE_CORRECTA[] = "#*B1";

char claveIngresada[5] = "";
byte cantidadCaracteres = 0;

const int POSICION_INICIAL = 90;
const int POSICION_CORRECTA = 120;
const int POSICION_INCORRECTA = 45;

// =====================================================
// LECTURA MANUAL DEL TECLADO
// =====================================================
char leerTeclado() {
    for (byte fila = 0; fila < FILAS; fila++) {

        // Todas las filas quedan en HIGH
        for (byte i = 0; i < FILAS; i++) {
            digitalWrite(pinesFilas[i], HIGH);
        }

        // Activar solamente la fila actual
        digitalWrite(pinesFilas[fila], LOW);

        delayMicroseconds(10);

        for (byte columna = 0; columna < COLUMNAS; columna++) {
            if (digitalRead(pinesColumnas[columna]) == LOW) {

                char tecla = mapaTeclas[fila][columna];

                // Antirrebote
                delay(30);

                // Esperar a que se suelte la tecla
                while (digitalRead(pinesColumnas[columna]) == LOW) {
                    delay(5);
                }

                delay(30);
                return tecla;
            }
        }
    }

    return '\0';
}

// =====================================================
// CONTROL DEL SISTEMA
// =====================================================
void apagarLeds() {
    digitalWrite(LED_VERDE_1, LOW);
    digitalWrite(LED_VERDE_2, LOW);
    digitalWrite(LED_ROJO_1, LOW);
    digitalWrite(LED_ROJO_2, LOW);
}

void pantallaInicial() {
    cantidadCaracteres = 0;
    claveIngresada[0] = '\0';

    apagarLeds();
    servoPuerta.write(POSICION_INICIAL);

    lcd.clear();
    lcd.setCursor(1, 0);
    lcd.print("INGRESAR CLAVE");

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

void accesoCorrecto() {
    digitalWrite(LED_VERDE_1, HIGH);
    digitalWrite(LED_VERDE_2, HIGH);
    digitalWrite(LED_ROJO_1, LOW);
    digitalWrite(LED_ROJO_2, LOW);

    servoPuerta.write(POSICION_CORRECTA);

    lcd.clear();
    lcd.setCursor(1, 0);
    lcd.print("CLAVE CORRECTA");
    lcd.setCursor(0, 1);
    lcd.print("Acceso permitido");

    delay(3000);
    pantallaInicial();
}

void accesoIncorrecto() {
    digitalWrite(LED_VERDE_1, LOW);
    digitalWrite(LED_VERDE_2, LOW);
    digitalWrite(LED_ROJO_1, HIGH);
    digitalWrite(LED_ROJO_2, HIGH);

    servoPuerta.write(POSICION_INCORRECTA);

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("CLAVE INCORRECTA");
    lcd.setCursor(0, 1);
    lcd.print("Acceso denegado");

    delay(3000);
    pantallaInicial();
}

void comprobarClave() {
    if (strcmp(claveIngresada, CLAVE_CORRECTA) == 0) {
        accesoCorrecto();
    } else {
        accesoIncorrecto();
    }
}

// =====================================================
// SETUP
// =====================================================
void setup() {
    Serial.begin(115200);

    // Filas como salidas
    for (byte i = 0; i < FILAS; i++) {
        pinMode(pinesFilas[i], OUTPUT);
        digitalWrite(pinesFilas[i], HIGH);
    }

    // Columnas con resistencias internas pull-up
    for (byte i = 0; i < COLUMNAS; i++) {
        pinMode(pinesColumnas[i], INPUT_PULLUP);
    }

    pinMode(LED_VERDE_1, OUTPUT);
    pinMode(LED_VERDE_2, OUTPUT);
    pinMode(LED_ROJO_1, OUTPUT);
    pinMode(LED_ROJO_2, OUTPUT);

    // Pines I2C de la Raspberry Pi Pico
    Wire.setSDA(4);
    Wire.setSCL(5);
    Wire.begin();

    lcd.init();
    lcd.backlight();

    servoPuerta.attach(PIN_SERVO);
    pantallaInicial();

    Serial.println("Sistema iniciado");
    Serial.println("Ingrese la clave #*B1");
}

// =====================================================
// LOOP
// =====================================================
void loop() {
    char tecla = leerTeclado();

    if (tecla == '\0') {
        return;
    }

    Serial.print("Tecla detectada: ");
    Serial.println(tecla);

    // D borra la contraseña
    if (tecla == 'D') {
        pantallaInicial();
        return;
    }

    if (cantidadCaracteres < 4) {
        claveIngresada[cantidadCaracteres] = tecla;
        cantidadCaracteres++;

        // Terminación necesaria para comparar cadenas
        claveIngresada[cantidadCaracteres] = '\0';

        lcd.setCursor(6 + cantidadCaracteres, 1);
        lcd.print("*");
    }

    if (cantidadCaracteres == 4) {
        delay(200);
        comprobarClave();
    }
}