/**
   Proyecto: Papelera inteligente que se abre por aproximación
   Autor: José Guillermo
*/

//------------------------ Librerías -------------------------//
#include <Wire.h> //include Wire.h library
#include <Servo.h> // Incluímos la librería para poder controlar el servo
#include <SoftwareSerial.h> //Para permitir la comunicación serie en otros pines digitales, a parte de los pines 0 y 1
#include <LiquidCrystal_I2C.h> // Para usar el LCD conectado al dispositivo I2C
#include <Wire.h> // Nos permite la comunicación con el dispositivo I2C


//----------------------- Constantes -------------------------//

#define CARDBOARD 0 // Asignamos un valor númerico a la papelera de cartón
#define PLASTIC 1 // Asignamos un valor numérico a la papelera de plástico
#define LIMIT 40 // Distancia desde la cual se reconoce por proximidad (cm)
#define SECOND 1000 // 1 segundo para utilizarlo en la cuenta atrás del lcd
#define TIMEOPEN 3000 // Tiempo que mantenemos la tapa abierta (3 segundos)


//------------------------ Servomotor ------------------------//

// Declaramos las variables para controlar los 2 servos
Servo servoMotor1; // Servo de la papelera de cartón
Servo servoMotor2; // Servo de la papelera de plástico



//-------------------------- LCD -----------------------------//

LiquidCrystal_I2C lcd(0x27, 16, 2); // Creamos objeto que representa el LCD: dirección 0x27 y 16 columnas x 2 filas


//------------------- Sensor ultrasónico ---------------------//

const int echoPin = 13; // Echo del sensor de cartón en pin digital 4
const int triggerPin = 12; // Trigger del sensor de cartón en pin digital 5


//-- Variables para la apertura y cerrado de las papeleras ---//

unsigned long initMillisCardboardOpen = 0; // Momento en el que se abre la tapa de la papelera de cartón
unsigned long currentMillisCardboardOpen = 0; // Para comprobar si ha pasado el tiempo para cerrar la tapa de la papelera de cartón
unsigned long lcdMillisCardboardOpen = 0; // Instante para comprobar cuando podemos reducir la cuenta atrás en la papelera de cartón

unsigned long initMillisPlasticOpen = 0; // Momento en el que se abre la tapa de la papelera de plástico
unsigned long currentMillisPlasticOpen = 0; // Para comprobar si ha pasado el tiempo para cerrar la tapa de la papelera de plástico
unsigned long lcdMillisPlasticOpen = 0; // Instante para comprobar cuando podemos reducir la cuenta atrás en la papelera de plástico

boolean isCardboardOpen = false; // Variable booleana para saber si la tapa de cartón está abierta
boolean isPlasticOpen = false; // Variable booleana para saber si la tapa de plástico está abierta

int countdownCardboard = 0; // Cuenta atrás de la tapa abierta de la papelera del cartón
int countdownPlastic = 0; // Cuenta atrás de la tapa abierta de la papelera del plástico



void setup() {
  // Iniciamos el servo correspondiente a la papelera del cartón con el pin 7
  servoMotor1.attach(2);
  // Iniciamos el servo correspondiente a la papelera del plástico con el pin 8
  servoMotor2.attach(4);

  // Comprobamos que las dos papeleras están cerradas en el momento inicial
  closeLid(servoMotor1);
  closeLid(servoMotor2);

  pinMode(triggerPin, OUTPUT); // TriggerPin1 se configura como salida
  pinMode(echoPin, INPUT); // EchoPin1 se configura como entrada

  // Se inicializa el LCD (y el I2C)
  lcd.init();

  // Se enciende la luz de fondo del LCD
  lcd.backlight();

  // Escribimos en el LCD
  lcd.print("Carton:cerrado");
  lcd.setCursor(0, 1); // Ubicamos el cursor del LCD en la primera posición (columna:0) de la segunda línea (fila:1)
  lcd.print("Plastico:cerrado");
  Wire.begin(); // Wire communication begin
  Serial.begin(9600); // The baudrate of Serial monitor is set in 9600
  while (!Serial); // Waiting for Serial Monitor
  Serial.println("\nI2C address Scanner CircuitSchools.com");
}


void loop() {
  // Comprobar si tenemos que cerrar la tapa de la papelera de cartón
  checkCloseCardboard();

  // Comprobar si tenemos que cerrar la tapa de la papelera de plástico
  checkClosePlastic();

  // Reconocer por proximidad si tenemos que abrir alguna de las papeleras
  openByProximity();
}

int calculateDistance(int TriggerPin, int EchoPin) {
  long duration, distanceCm;

  digitalWrite(TriggerPin, LOW);  // Se genera un pulso corto de 5µs + 10µs
  delayMicroseconds(5);
  digitalWrite(TriggerPin, HIGH); // El HC-SR04 entra en modo de ecolocalización enviando señales a 40kHz
  delayMicroseconds(10);          // Se espera 10µs para recibir la señal rebotada
  digitalWrite(TriggerPin, LOW);

  duration = pulseIn(EchoPin, HIGH);  // Se obtiene la duración del rebote de la señal del dispositivo en µs

  /*
     Fórmula de referencia:

     Velocidad del sonido = 343m/s
     343m/s * 100cm/m * 1/1000000s/µs = 1/29.1cm/µs

     Distancia = Tiempo * Velocidad / 2;

     Distancia(cm) = Tiempo(µs)/29.1*2 = (Tiempo(µs) / 2) / 29.1;
  */
  distanceCm = (duration / 2) / 29.1;   // Convertimos a distancia en cm
  return distanceCm;
}

unsigned long tiempoInicio;
bool cuentaTiempo = false;

void openByProximity() {
  int cm1 = calculateDistance(triggerPin, echoPin);
  if (cm1 < LIMIT) {
    cuentaTiempo = true;
  } else {
    cuentaTiempo = false;
  }
  if (cuentaTiempo == true) {
    if (tiempoInicio == 0) tiempoInicio = millis();
  } else {
    if (tiempoInicio != 0) {
      unsigned long tiempoMedido = millis() - tiempoInicio;
      if (tiempoMedido > 800 && tiempoMedido < 1800) {
        // Abrir papelera de cartón
        openCardboard();
      } else if (tiempoMedido >=2500) {
        // Abrir papelera de plástico
        openPlastic();
      }
      tiempoInicio = 0;
    }
  }
}

/*
  void openByProximity() {

  //  Obtenemos la distancia a la que se encuentra la persona o el objeto del sensor ultrasónico.
  //  Si se encuentra a una distancia menor de 10 cm entonces abriremos la papelera correspondiente.


  int cm1 = calculateDistance(triggerPin, echoPin);
  if (cm1 < LIMIT) {
    // Abrir papelera de cartón
    openCardboard();
  }
  int cm2 = calculateDistance(triggerPin, echoPin);
  if (cm2 < LIMIT) {
    // Abrir papelera de plástico
    openPlastic();
  }
  }
*/
void openCardboard() {
  /*
    Abrimos la papelera de cartón.
    - En caso de que no esté la tapa levantada, declaramos en el código que la tapa
      esté como abierta, llamamos a la función que abre la tapa,
      e iniciamos el tiempo que la tapa estará abierta, junto a la cuenta atrás del LCD.
    - En caso de que la tapa ya estuviera abierta antes, resetearemos el tiempo de la apertura
      de la papelera para que se establezca otra vez en los 3 segundos,
      de igual manera, resetemos la cuenta atrás del LCD.
  */
  if (!isCardboardOpen) {
    isCardboardOpen = true; // Tapa abierta
    openLid(servoMotor1); // Llamamos a la función que abrirá la tapa correspondiente
  }
  initMillisCardboardOpen = millis(); // Momento inicial en el que se abre la papelera de cartón
  lcdMillisCardboardOpen = initMillisCardboardOpen; // Para usarlo en la cuenta atrás y actualizar el LCD cada segundo que pase
  countdownCardboard = 3; // Inicializamos la cuenta atrás
  printCountdown(0, 0, "Carton:", countdownCardboard); // Llamamos a la función que actualizará el LCD
  delay(150); // Tiempo de seguridad
}

void openPlastic() {
  /*
    Abrimos la papelera de plástico.
    - En caso de que no esté la tapa levantada, declaramos en el código que la tapa
      esté como abierta, llamamos a la función que abre la tapa,
      e iniciamos el tiempo que la tapa estará abierta, junto a la cuenta atrás del lcd.
    - En caso de que la tapa ya estuviera abierta antes, resetearemos el tiempo de la apertura
      de la papelera para que se establezca otra vez en los 3 segundos,
      de igual manera, resetemos la cuenta atrás del lcd.
  */
  if (!isPlasticOpen) {
    isPlasticOpen = true; // Tapa abierta
    openLid(servoMotor2); // Llamamos a la función que abrirá la tapa correspondiente
  }
  initMillisPlasticOpen = millis(); // Momento inicial en el que se abre la papelera de cartón
  lcdMillisPlasticOpen = initMillisPlasticOpen; // Para usarlo en la cuenta atrás y actualizar el LCD cada segundo que pase
  countdownPlastic = 3; // Inicializamos la cuenta atrás
  printCountdown(0, 1, "Plastico:", countdownPlastic); // Llamamos a la función que actualizará el LCD
  delay(150); // Tiempo de seguridad
}

void checkCloseCardboard() {
  /*
    En caso de que la papelera de cartón esté abierta:
    - Comprobamos si ya ha pasado el tiempo para cerrar la tapa de la papelera de cartón,
      en ese caso, lo pondremos en el código como cerrado, cerramos la tapa
      y ponemos en el LCD que la papelera de cartón está cerrada.
    - Cada vez que pase un segundo (si la tapa está abierta),
      vamos a reducir un segundo la cuenta atrás en el LCD.
  */
  if (isCardboardOpen) {
    currentMillisCardboardOpen = millis(); // Para saber si podemos cerrar la papelera y reducir la cuenta atrás
    // Comprobar si ha pasado el tiempo para cerrar la tapa de la papelera
    if ((currentMillisCardboardOpen - initMillisCardboardOpen) >= TIMEOPEN) {
      isCardboardOpen = false; // Tapa cerrada
      closeLid(servoMotor1); // Llamamos a la función que cerrará la tapa correspondiente
      printCountdown(0, 0, "Carton:", -1); // Llamamos a la función que actualizará el LCD
      delay(150); // Tiempo de seguridad
      // Comprobar si podemos reducir 1 segundo la cuenta atrás
    } else if ((currentMillisCardboardOpen - lcdMillisCardboardOpen) >= SECOND) {
      lcdMillisCardboardOpen = millis(); // Junto a "currentMillisCardboardOpen" para saber cuando pasará otro segundo
      countdownCardboard--; // Reducimos la cuenta atrás en un segundo
      printCountdown(0, 0, "Carton:", countdownCardboard); // Llamamos a la función que actualizará el LCD
    }
  }
}

void checkClosePlastic() {
  /*
    En caso de que la papelera de plástico esté abierta:
    - Comprobamos si ya ha pasado el tiempo para cerrar la tapa de la papelera de plástico,
      en ese caso, lo pondremos en el código como cerrado, cerramos la tapa
      y ponemos en el LCD que la papelera de plástico está cerrada.
    - Cada vez que pase un segundo (si la tapa está abierta),
      vamos a reducir un segundo la cuenta atrás en el LCD.
  */
  if (isPlasticOpen) {
    currentMillisPlasticOpen = millis(); // Para saber si podemos cerrar la papelera y reducir la cuenta atrás
    // Comprobar si ha pasado el tiempo para cerrar la tapa de la papelera
    if ((currentMillisPlasticOpen - initMillisPlasticOpen) >= TIMEOPEN) {
      isPlasticOpen = false; // Tapa cerrada
      closeLid(servoMotor2); // Llamamos a la función que cerrará la tapa correspondiente
      printCountdown(0, 1, "Plastico:", -1); // Llamamos a la función que actualizará el LCD
      delay(150); // Tiempo de seguridad
      // Comprobar si podemos reducir 1 segundo la cuenta atrás
    } else if ((currentMillisPlasticOpen - lcdMillisPlasticOpen) >= SECOND) {
      lcdMillisPlasticOpen = millis(); // Junto a "currentMillisPlasticOpen" para saber cuando pasará otro segundo
      countdownPlastic--; // Reducimos la cuenta atrás en un segundo
      printCountdown(0, 1, "Plastico:", countdownPlastic); // Llamamos a la función que actualizará el LCD
    }
  }
}

void printCountdown(int col, int row, String typeText, int countdown) {
  /*
    Escribimos en el LCD cada vez que tengamos que actualizar alguna información.
    - En caso de que "countdown" sea menor a 0, en el LCD ponemos que la papelera
      correspondiente está cerrada.
    - En caso de que "countdown" sea mayor a 0, [1,3] ponemos ese valor como la cuenta atrás
      en el LCD.
  */
 // lcd.setCursor(col, row);
  //lcd.print("                ");
  lcd.setCursor(col, row);
  String lcdText = "";
  if (countdown < 0) {
    lcdText = typeText + "cerrado";
  } else {
    lcdText = typeText + " " + countdown + " sg  ";
  }
  lcd.print(lcdText);
}

void closeLid(Servo servoMotor) {
  // Desplazamos el servo a la posición 0º
  servoMotor.write(0);
}

void openLid(Servo servoMotor) {
  // Desplazamos el servo a la posición 180º
  servoMotor.write(180);
}