#include <Servo.h>
#include <LiquidCrystal.h> // <-- Usamos esta librería para LCD estándar (no I2C)

// LCD: RS, E, D4, D5, D6, D7
LiquidCrystal lcd(9, 10, A2, A3, A4, A5);

//----------------------- Constantes -------------------------//
#define CARDBOARD 0
#define PLASTIC 1
#define LIMIT 10
#define SECOND 1000
#define TIMEOPEN 3000

//------------------------------------------------------------//
Servo servoMotor1;
Servo servoMotor2;

//------------------- Sensor ultrasónico ---------------------//
const int echoPin1 = 4;
const int triggerPin1 = 5;
const int echoPin2 = 11;
const int triggerPin2 = 12;

//------------------- Sensores IR ----------------------------//
const int irSensorCardboard = A0;
const int irSensorPlastic = A1;

//---------------- Variables de control de tapa -------------//
unsigned long initMillisCardboardOpen = 0;
unsigned long currentMillisCardboardOpen = 0;
unsigned long lcdMillisCardboardOpen = 0;

unsigned long initMillisPlasticOpen = 0;
unsigned long currentMillisPlasticOpen = 0;
unsigned long lcdMillisPlasticOpen = 0;

boolean isCardboardOpen = false;
boolean isPlasticOpen = false;

int countdownCardboard = 0;
int countdownPlastic = 0;

void setup() {
  Serial.begin(115200);

  servoMotor1.attach(7);
  servoMotor2.attach(8);

  closeLid(servoMotor1);
  closeLid(servoMotor2);

  pinMode(triggerPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(triggerPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  pinMode(irSensorCardboard, INPUT);
  pinMode(irSensorPlastic, INPUT);

  lcd.begin(16, 2); // Inicializamos el LCD estándar
  lcd.setCursor(0, 0);
  lcd.print("Carton:cerrado");
  lcd.setCursor(0, 1);
  lcd.print("Plastico:cerrado");
}

void loop() {
  checkCloseCardboard();
  checkClosePlastic();
  openByProximity();
}

int calculateDistance(int TriggerPin, int EchoPin) {
  long duration, distanceCm;
  digitalWrite(TriggerPin, LOW);
  delayMicroseconds(5);
  digitalWrite(TriggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(TriggerPin, LOW);
  duration = pulseIn(EchoPin, HIGH);
  distanceCm = (duration / 2) / 29.1;
  return distanceCm;
}

void openByProximity() {
  int cm1 = calculateDistance(triggerPin1, echoPin1);
  if (cm1 < LIMIT && !isCardboardFull()) {
    openCardboard();
  }

  int cm2 = calculateDistance(triggerPin2, echoPin2);
  if (cm2 < LIMIT && !isPlasticFull()) {
    openPlastic();
  }
}

bool isCardboardFull() {
  int irValue = analogRead(irSensorCardboard);
  return irValue > 500;
}

bool isPlasticFull() {
  int irValue = analogRead(irSensorPlastic);
  return irValue > 500;
}

void openCardboard() {
  if (!isCardboardOpen) {
    isCardboardOpen = true;
    openLid(servoMotor1);
  }
  initMillisCardboardOpen = millis();
  lcdMillisCardboardOpen = initMillisCardboardOpen;
  countdownCardboard = 3;
  printCountdown(0, 0, "Carton:", countdownCardboard);
  delay(150);
}

void openPlastic() {
  if (!isPlasticOpen) {
    isPlasticOpen = true;
    openLid(servoMotor2);
  }
  initMillisPlasticOpen = millis();
  lcdMillisPlasticOpen = initMillisPlasticOpen;
  countdownPlastic = 3;
  printCountdown(0, 1, "Plastico:", countdownPlastic);
  delay(150);
}

void checkCloseCardboard() {
  if (isCardboardOpen) {
    currentMillisCardboardOpen = millis();
    if ((currentMillisCardboardOpen - initMillisCardboardOpen) >= TIMEOPEN) {
      isCardboardOpen = false;
      closeLid(servoMotor1);
      printCountdown(0, 0, "Carton:", -1);
      delay(150);
    } else if ((currentMillisCardboardOpen - lcdMillisCardboardOpen) >= SECOND) {
      lcdMillisCardboardOpen = millis();
      countdownCardboard--;
      printCountdown(0, 0, "Carton:", countdownCardboard);
    }
  }
}

void checkClosePlastic() {
  if (isPlasticOpen) {
    currentMillisPlasticOpen = millis();
    if ((currentMillisPlasticOpen - initMillisPlasticOpen) >= TIMEOPEN) {
      isPlasticOpen = false;
      closeLid(servoMotor2);
      printCountdown(0, 1, "Plastico:", -1);
      delay(150);
    } else if ((currentMillisPlasticOpen - lcdMillisPlasticOpen) >= SECOND) {
      lcdMillisPlasticOpen = millis();
      countdownPlastic--;
      printCountdown(0, 1, "Plastico:", countdownPlastic);
    }
  }
}

void printCountdown(int col, int row, String typeText, int countdown) {
  lcd.setCursor(col, row);
  lcd.print("                "); // Limpiar línea
  lcd.setCursor(col, row);
  String lcdText = (countdown < 0) ? typeText + "cerrado" : typeText + " " + countdown + " sg";
  lcd.print(lcdText);
}

void closeLid(Servo servoMotor) {
  servoMotor.write(0);
}

void openLid(Servo servoMotor) {
  servoMotor.write(180);
}