// Realizado por Javier Serrano Jodral

// Librerías importadas
#include <Wire.h>        // Envío y recepción de información SDA y SCL
#include <LiquidCrystal_I2C.h>   // Manejo pantalla LCD
#include <DHT.h>         // Manejo sensor DHT22
#include <Servo.h>       // Manejo servomotor

// Constantes para conexión de pines y componentes
#define TRIG 3
#define ECHO 2
#define DHT_PIN 4
#define DHTTYPE DHT22
#define SERVO_PIN 5         // Debe ser PWM
#define DATA_PIN_74HC165 6     // 74HC165 (Q7)
#define CLOCK_PIN_74HC165 7    // 74HC165 (CP)
#define LOAD_PIN_74HC165 8     // 74HC165 (PL)
#define DATA_PIN_74HC595 11      // 74HC595 (DS)
#define CLOCK_PIN_74HC595 13     // 74HC595 (SHCP)
#define LATCH_PIN_74HC595 12     // 74HC595 (STCP)
#define DIRECTION_PIN 9         // Motor paso a paso
#define STEP_PIN 10

// Parámetros sensor ultrasónico
const float ALTURA_SENSOR = 300.0;     // Altura del ascensor (cm) -> Techo cabina
const float UMBRAL_DETECCION = 275.0;    // Si se mide >, se considera ascensor vacío

// Parámetros del servomotor (motor cabina)
const float ANG_SUBIDA = 0.0;       // Ángulo servo si ascensor sube
const float ANG_BAJADA = 180.0;      // Ángulo servo si ascensor baja
const float ANG_STOP = 90.0;         // Ángulo inicial y de parada de emergencia


// Parámetros de stepper (regulador de temperatura)
const int MAX_PASOS = 200;        // Máxima apertura hacia un lado
const int PASOS_POR_GRADO = 10;
const float TEMP_DESEADA = 24.00;
const float UMBRAL_HISTERESIS_TEMP = 1.00;

// Parámetros de iluminación
const float GAMMA = 0.7;   // LDR
const float RL10 = 50;     // LDR
const int UMBRAL_HISTERESIS_ILUM = 15;   // lux (subida y bajada, por comodidad)


// Parámetros de seguridad ambientales
const float TEMP_MAX = 45.0;     // Temperatura a partir de la que se activa alarma
const float TEMP_MIN = 0.0;      // Temperatura a partir de la que se activa alarma
const float HUM_MAX = 75.0;      // Humedad superior, si > se activa alarma
const float HUM_MIN = 20.0;      // Humedad inferior, si < se activa alarma
bool emergencia_activa = false;   // Estado previo de la alerta


// Otros parámetros
const int PERIODO_LECTURA_AMBIENTE = 2000;   // Cada cuantos ms se leen sensores ambientales
const int TIEMPO_PISO_A_PISO = 2000;  // Tiempo desde un piso a otro consecutivo (ms)


// Definición de caracteres especiales (flecha hacia arriba y hacia abajo)
byte upArrow[8] = {
  B00000,
  B00100,
  B01110,
  B01110,
  B11111,
  B11111,
  B00000
};
byte dnArrow[8] = {
  B00000,
  B11111,
  B11111,
  B01110,
  B01110,
  B00100,
  B00000
};


// Instanciar objetos -------------------------------------------
LiquidCrystal_I2C lcd(0x27, 16, 2);       // LCD info plantas
LiquidCrystal_I2C lcd_amb(0x26, 20, 4);   // LCD info ambiental
DHT dht(DHT_PIN, DHTTYPE);   // Librería se encargará de pinMode
Servo motor_ascensor;        // Actuador para subir o bajar ascensor


// Variables de estado y control
bool presencia = false;         // Si hay algún usuario en la cabina
bool presencia_anterior = false; 
int piso_actual = 0;            // Planta en la que se encuentra el ascensor en cada momento
unsigned long tiempo_ultima_lectura = 0; // Para leer sensores ambientales cada varios segundos
float temp = 0.0;    // Temperatura y humedad relativa, se definen como globales
float hum = 0.0;
float ilum = 0.0;    // Iluminación en Lux también global
int posicion_actual_stepper = 0;  // 0 es el centro (apagado)

void setup() {
  // Iniciar comunicación serie
  Serial.begin(9600);
  
  // Configuración de pines
  pinMode(TRIG, OUTPUT);    // Sensor ultrasónico
  pinMode(ECHO, INPUT);

  pinMode(LOAD_PIN_74HC165, OUTPUT);  // 74HC165 Botones
  pinMode(CLOCK_PIN_74HC165, OUTPUT);
  pinMode(DATA_PIN_74HC165, INPUT);

  pinMode(DATA_PIN_74HC595, OUTPUT);   // 74HC595 LEDs
  pinMode(CLOCK_PIN_74HC595, OUTPUT);  
  pinMode(LATCH_PIN_74HC595, OUTPUT);

  pinMode(STEP_PIN, OUTPUT);    // Stepper
  pinMode(DIRECTION_PIN, OUTPUT);

  // Inicialización de componentes
  dht.begin();
  motor_ascensor.attach(SERVO_PIN);
  digitalWrite(LOAD_PIN_74HC165, HIGH);    // Pasar chip a modo espera desde arranque

  
  // Posición inicial neutral
  motor_ascensor.write(ANG_STOP);

  // HMI del LCD con info de plantas
  lcd.init();
  lcd.createChar(0, upArrow);   // Crear caracteres especiales
  lcd.createChar(1, dnArrow);
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("SISTEMA ACME S.A.");
  lcd.setCursor(0, 1);
  lcd.print("Control Ascensor");


  // HMI del LCD con info ambiental
  lcd_amb.init();
  lcd_amb.createChar(0, upArrow);   // Crear caracteres especiales
  lcd_amb.createChar(1, dnArrow);
  lcd_amb.backlight();
  lcd_amb.setCursor(0, 0);
  lcd_amb.print("SISTEMA ACME S.A.");
  lcd_amb.setCursor(0, 1);
  lcd_amb.print("Control Ambiente");


  delay(1500);

  actualizarPantallaPlantas(piso_actual, 0);

}

void loop() {

  // controlSeguridad(t, h);

  actualizarMedicionesAmbientales();   // Dentro se comprueba tiempo transcurrido
                                       // desde última medición, para no saturar

  // Control ambiental por seguridad
  bool hay_peligro = comprobarSeguridadAmbiental();
  
  actualizarPresencia();
  
  // Continuar solo si no hay peligro ambiental
  if (!hay_peligro) {
    int piso_llamada = comprobarLlamadaPlanta();

    if (piso_llamada != -1) {
      moverAscensor(piso_llamada);
    }
  }

  delay(100);
}



// Funciones

float medirDistancia() {
  // El sensor a un inicio debe estar en reposo 
  digitalWrite(TRIG, 0);
  delayMicroseconds(2);

  // Envío de la onda ultrasónica
  digitalWrite(TRIG, 1);
  delayMicroseconds(10);  
  digitalWrite(TRIG, 0);

  // Leer el tiempo que el pin ECHO permanece en alto
  long tiempo = pulseIn(ECHO, 1);

  // Calcular la distancia V = D / T
  // D = V * T / 2    // SE DIVIDE ENTRE 2 POR EL TIEMPO DE IDA Y VUELTA
  // V sonido = 340 m/s = 0.0340 cm / us en el vacío
  // V aumenta con la temperatura -> Consultar DHT22

  //float velocidadSonido = (331.4 + 0.61*dht.readTemperature()) * 0.0001;
  float velocidadSonido = 344 * 0.0001;
  float distancia = (tiempo * velocidadSonido) / 2;
  
  return distancia;
}


int comprobarLlamadaPlanta() {
  int planta_llamada = -1;

  // 1. Pulso en LOAD para capturar el estado de los botones
  digitalWrite(LOAD_PIN_74HC165, LOW);
  delayMicroseconds(5);
  digitalWrite(LOAD_PIN_74HC165, HIGH);

  // 2. Leer los 8 bits, conformando un Byte
  byte lectura = shiftIn(DATA_PIN_74HC165, CLOCK_PIN_74HC165, MSBFIRST);

  // 3. Revisar cuál de las 5 plantas está activa (bits 0 al 4)
  for (int i = 0; i < 8; i++) {
    if (bitRead(lectura, i) == HIGH) {
      planta_llamada = i-1;
      
      Serial.print("Llamada detectada en Planta ");
      Serial.println(planta_llamada);
      
      break;      // Salimos del bucle al encontrar la primera llamada
    }
  }

  return planta_llamada;
}


void moverAscensor(int piso_destino) {
  // piso_actual es variable global -> se modifica globalmente
  
  // Si piso se marca el piso actual, directamente
  if (piso_destino == piso_actual){
    motor_ascensor.write(ANG_STOP);
    actualizarPantallaPlantas(piso_actual, 0); // 0 = sin flecha (parado)
    lcd.setCursor(0, 1);
    lcd.print("Piso actual");
    Serial.println("Piso actual");
    Serial.println();

    delay(200);
    return;
  }

  int sentido = (piso_destino > piso_actual) ? 1 : -1;   // Hacia arriba o hacia abajo

  // LCD info plantas
  actualizarPantallaPlantas(piso_actual, sentido);

  // Motor cabina == Servo motor
  if (sentido == 1) {
    motor_ascensor.write(ANG_SUBIDA); // Sube
  } else {
    motor_ascensor.write(ANG_BAJADA); // Baja
  }

  // Simular tiempo de viaje entre plantas
  int distancia_pisos = abs(piso_destino - piso_actual);   // Número de pisos a recorrer
  
  for (int i = 0; i < distancia_pisos; i++) {
    actualizarMedicionesAmbientales();    // Deben comprobarse mediciones ambientales, paralelamente
    
    // Abortar si hay problema ambiental
    if (comprobarSeguridadAmbiental()){
      return;
    }

    delay(TIEMPO_PISO_A_PISO); 
    
    // Actualizar el piso en el LCD mientras se mueve
    if (sentido == 1) piso_actual++;
    else piso_actual--;
    
    if (piso_actual != piso_destino){
      actualizarPantallaPlantas(piso_actual, sentido);  
    }
  }
  // Destino alcanzado tras movimiento
  motor_ascensor.write(ANG_STOP);
  actualizarPantallaPlantas(piso_actual, 0); // 0 = sin flecha (parado)
  lcd.setCursor(0, 1);
  lcd.print("Llegada");
  Serial.print("Llegada a Planta ");
  Serial.println(piso_actual);
  Serial.println();

  delay(200);
}


void actualizarPantallaPlantas(int piso, int sentido) {
  // --- LCD 1: INFO PISOS ---
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Piso: "); lcd.print(piso);

  if (sentido == 1){
    lcd.write(byte(0));
    lcd.setCursor(0, 1);
    lcd.print("Subiendo...");
  } 
  else if (sentido == -1){
    lcd.write(byte(1));
    lcd.setCursor(0, 1);
    lcd.print("Bajando...");
  }

}

void actualizarPantallaAmbiental(float ilum_lectura){
  // --- LCD 2: INFO AMBIENTAL ---
  lcd_amb.clear();
  lcd_amb.setCursor(0, 0);
  lcd_amb.print("Temp: "); lcd_amb.print(temp, 1); lcd_amb.print("C");
  lcd_amb.setCursor(0, 1);
  lcd_amb.print("Hum:  "); lcd_amb.print(hum, 1); lcd_amb.print("%");
  lcd_amb.setCursor(0, 2);
  lcd_amb.print("Ilum: "); lcd_amb.print(ilum_lectura, 1); lcd_amb.print(" lux");
}


void actualizarPresencia() {
  // Medir distancia con sensor ultrasónico
  float d_leida = medirDistancia();
  
  // Guardamos el estado actual
  presencia = (d_leida < UMBRAL_DETECCION);

  // Si alguien acaba de entrar o acaba de salir...
  if (presencia != presencia_anterior) {

    if (presencia) {
      Serial.println("Nuevo usuario");
      lcd.setCursor(0, 31);
      lcd.print("Bienvenido");
    }
    else {
      Serial.println("Ascensor vacío");
    }
    Serial.println();

    regularIluminacion();              // Forzar que los LEDs se enteren del cambio
    actualizarPantallaAmbiental(ilum); // Refrescar LCD para que el "Sí/No" cambie al instante
    presencia_anterior = presencia;    // Actualizar el historial
  }

  // Actualización visual en el LCD
  lcd_amb.setCursor(0, 3);
  lcd_amb.print("Presencia: ");
  lcd_amb.print(presencia ? "Si" : "No");
}

void actualizarMedicionesAmbientales(){
  if (millis() - tiempo_ultima_lectura > PERIODO_LECTURA_AMBIENTE) {
    tiempo_ultima_lectura = millis();
    
    float temp_nueva = dht.readTemperature();
    float hum_nueva = dht.readHumidity();
    float ilum_nueva = medirIluminacion();
    
    int flag = 0;    // Mostrar en LCD cambio en medida ambiental
    // Solo actualizamos si los datos son válidos y ha habido algún cambio
    
    // Temperatura
    if (!isnan(temp_nueva) && (temp!=temp_nueva)) {
      temp = temp_nueva;
      regularTemperatura();    // Ajustar climatizador
      flag++;
    }

    // Humedad
    if (!isnan(hum_nueva) && (hum!=hum_nueva)){
      hum = hum_nueva;
      flag++;
    }

    // Iluminación
    if (!isnan(ilum_nueva) && (ilum!=ilum_nueva)){
      flag++;
      if (abs(ilum_nueva - ilum) > UMBRAL_HISTERESIS_ILUM) {  // Solo si cambio significativo respecto a valor guardado para leds
        ilum = ilum_nueva;
        regularIluminacion();   // Actuador lumínico
      }
    }

    // Comprobar si debemos actualizar LCD
    if (flag != 0){
      actualizarPantallaAmbiental(ilum_nueva);  // Actualiza LCD con info ambiental
    }
  }
}


float medirIluminacion() {
  // Función que devuelve los lux en cabina según LDR
  int analogValue = analogRead(A0);
  float voltage = analogValue / 1024. * 5;
  if (voltage > 4.99) voltage = 4.99;   // Evitar posible division por 0 en calculo resistance
  float resistance = 2000 * voltage / (1 - voltage / 5);
  float lux = pow(RL10 * 1e3 * pow(10, GAMMA) / resistance, (1 / GAMMA));

  return lux;
}


void regularIluminacion(){
  // Función que enciende o apaga LEDs según lux cabina
  byte patron_LEDs = 0;
  // A menor valor de lux, más bits activos
  if (ilum < 1200) patron_LEDs = B00000001;   // todos OFF
  if (ilum < 800) patron_LEDs = B00000011;
  if (ilum < 400) patron_LEDs = B00000111;
  if (ilum < 200) patron_LEDs = B00001111;
  if (ilum < 120) patron_LEDs = B00011111;
  if (ilum < 80) patron_LEDs = B00111111;
  if (ilum < 40) patron_LEDs = B01111111;
  if (ilum < 10) patron_LEDs = B11111111;   // todos ON

  if (!presencia) {
    // Apagar LEDs si no hay nadie. Modo ahorro
    patron_LEDs = 0x00;

  }
  digitalWrite(LATCH_PIN_74HC595, LOW);   // 
  shiftOut(DATA_PIN_74HC595, CLOCK_PIN_74HC595, MSBFIRST, patron_LEDs);
  digitalWrite(LATCH_PIN_74HC595, HIGH);
}



void regularTemperatura() {
  // Función que gira stepper según temperatura
  int pasos_objetivo = 0;

  // Diferencia de temperatura respecto a valor deseado
  float error = temp - TEMP_DESEADA;

  // Aplicar histéresis (Banda de tolerancia)
  // Si el error es pequeño, motor no se mueve
  if (abs(error) <= UMBRAL_HISTERESIS_TEMP) {
    pasos_objetivo = 0; // Posición central: Apagado / Reposo
  } 
  else if (error > 0) {
    // Hace CALOR: El motor gira a la DERECHA (Enfriar)
    // Mapear error: cada grado extra son PASOS_POR_GRADO pasos, hasta un tope de MAX_PASOS
    pasos_objetivo = constrain(error * PASOS_POR_GRADO, 0, MAX_PASOS);
    Serial.print("Calor: ");
    Serial.print(temp);
    Serial.print("ºC. Reduciendo a: ");
    Serial.print(TEMP_DESEADA);
    Serial.println("ºC");
    Serial.print("Apertura válvula de enfriamiento: ");
    Serial.print(abs((pasos_objetivo * 100.0) / MAX_PASOS), 1);
    Serial.println("%");
    Serial.println();
  } 
  else {
    // Hace FRÍO: El motor gira a la IZQUIERDA (Calentar)
    // El error es negativo, por lo que pasos_objetivo será negativo
    pasos_objetivo = -constrain(abs(error) * PASOS_POR_GRADO, 0, MAX_PASOS);
    Serial.print("Frío: ");
    Serial.print(temp);
    Serial.print("ºC. Aumentando a: ");
    Serial.print(TEMP_DESEADA);
    Serial.println("ºC");
    Serial.print("Apertura válvula de calentamiento: ");
    Serial.print(abs((pasos_objetivo * 100.0) / MAX_PASOS), 1);
    Serial.println("%");
    Serial.println();
  }
  // Ejecutar el movimiento físico
  movimientoStepper(pasos_objetivo);
}

void movimientoStepper(int objetivo) {
  // Función que ejecuta movimiento del motor, 
  // según pasos objetivo dado por regularTemperatura()

  while (posicion_actual_stepper != objetivo) {
    if (posicion_actual_stepper < objetivo) {
      digitalWrite(DIRECTION_PIN, HIGH); // Sentido horario
      posicion_actual_stepper++;
    } else {
      digitalWrite(DIRECTION_PIN, LOW);  // Sentido antihorario
      posicion_actual_stepper--;
    }

    // Generar el pulso de paso para el A4988
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(800); // Ajusta este delay para la velocidad (menor = más rápido)
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(800);
  }
}


bool comprobarSeguridadAmbiental() {
  // Función que compara mediciones ambientales con límites
  // Si emergencia, para motor, informa y devuelve true

  bool emergencia = false;
  String causa = "";

  // Comprobar límites
  if (temp > TEMP_MAX) { emergencia = true; causa = "EXCESO TEMP"; }
  else if (temp < TEMP_MIN) { emergencia = true; causa = "TEMP BAJA"; }
  else if (hum > HUM_MAX) { emergencia = true; causa = "EXCESO HUM"; }
  else if (hum < HUM_MIN) { emergencia = true; causa = "HUM BAJA"; }

  if (emergencia) {
    // Detener motor
    motor_ascensor.write(ANG_STOP);

    if (emergencia_activa == false) {
      // Marcar que sistema ha entrado en estado de emergencia
      emergencia_activa = true;

      // Alerta en LCD de plantas (solo 1 vez)
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("ALERTA SEGURIDAD");
      lcd.setCursor(0, 1);
      lcd.print(causa);

      // Alerta en Serial (también solo 1 vez)
      Serial.print("!!! EMERGENCIA AMBIENTAL: ");
      Serial.print(causa);
      Serial.println(" !!!");
      Serial.println();
      
    }

    // Parpadeo de LEDs (con registro 595)
    digitalWrite(LATCH_PIN_74HC595, LOW);
    shiftOut(DATA_PIN_74HC595, CLOCK_PIN_74HC595, MSBFIRST, (millis() % 500 < 250) ? 0xFF : 0x00);
    digitalWrite(LATCH_PIN_74HC595, HIGH);
  }

  // Si no hay emergencia ahora, pero venimos de una alerta previa.
  // Se ejecuta una única vez DESPUÉS de emergencia.
  // Con el fin de actualizar correctamente LCD de plantas.
  else if (!emergencia && emergencia_activa == true) {
    emergencia_activa = false; // Reset del estado de alerta
    
    // Refrescamos la pantalla para recuperar la info del piso
    actualizarPantallaPlantas(piso_actual, 0); 
    Serial.println("Seguridad restablecida. Retornando a interfaz de usuario.");
  }

  return emergencia;
}