#include <Wire.h>
#include <RTClib.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>

// Initialize the RTC and LCD
RTC_DS1307 rtc;
LiquidCrystal_I2C lcd(0x27, 20, 4); // I2C address 0x27 for 20x4 LCD
Servo servo1;
Servo servo2;

// Feeding times (24-hour format)
const int feedTimes[][2] = {
  {8, 0},  // 8:00 AM
  {20, 0} // 8:00 PM
};
const int numFeedTimes = 2;
bool hasDispensedToday[numFeedTimes] = {false, false};
DateTime lastDispenseTime1;
DateTime lastDispenseTime2;

// Push button pins
const int buttonDisplayTimePin = 13;
const int buttonManualDispense1Pin = 12;
const int buttonManualDispense2Pin = 14;
bool buttonDisplayTimePressed = false;
bool buttonManualDispense1Pressed = false;
bool buttonManualDispense2Pressed = false;

// Ultrasonic sensor pins
const int trigPin1 = 32;
const int echoPin1 = 33;
const int trigPin2 = 18;
const int echoPin2 = 5;

// Food detection threshold (in centimeters)
const int foodThreshold = 30; // Change to 30 cm

// LED indicator pins
const int ledPin1 = 23;
const int ledPin2 = 19;

void setup() {
  // Start the RTC and LCD
  Wire.begin();
  lcd.begin(20, 4); // Initialize the LCD with 20 columns and 4 rows
  lcd.backlight();

  // Start the RTC
  if (!rtc.begin()) {
    lcd.setCursor(0, 0);
    lcd.print("Couldn't find RTC");
    while (1);
  }

  if (!rtc.isrunning()) {
    lcd.setCursor(0, 1);
    lcd.print("RTC is NOT running!");
    rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));
  }

  // Initialize servos
  servo1.attach(25);
  servo2.attach(26);

  // Set initial positions
  servo1.write(0);
  servo2.write(0);

  // Initialize buttons with internal pull-up resistors
  pinMode(buttonDisplayTimePin, INPUT_PULLUP);
  pinMode(buttonManualDispense1Pin, INPUT_PULLUP);
  pinMode(buttonManualDispense2Pin, INPUT_PULLUP);

  // Initialize ultrasonic sensors
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  // Initialize LED pins
  pinMode(ledPin1, OUTPUT);
  pinMode(ledPin2, OUTPUT);

  // Initialize serial for debugging
  Serial.begin(115200);

  lcd.clear();
}

void loop() {
  DateTime now = rtc.now();

  for (int i = 0; i < numFeedTimes; i++) {
    int feedHour = feedTimes[i][0];
    int feedMinute = feedTimes[i][1];

    // Check if it's time to dispense food
    if (now.hour() == feedHour && now.minute() == feedMinute && !hasDispensedToday[i]) {
      checkAndDispenseFood();
      hasDispensedToday[i] = true;
    }

    // Reset the dispensing flag if a minute has passed
    if (now.minute() != feedMinute) {
      hasDispensedToday[i] = false;
    }
  }

  // Check if the display time button is pressed
  if (digitalRead(buttonDisplayTimePin) == LOW && !buttonDisplayTimePressed) {
    Serial.println("Display time button pressed!");
    displayLastDispenseTimes();
    buttonDisplayTimePressed = true;
  }

  // Reset the display time button press flag if the button is released
  if (digitalRead(buttonDisplayTimePin) == HIGH) {
    buttonDisplayTimePressed = false;
  }

  // Check if the manual dispense button for storage 1 is pressed
  if (digitalRead(buttonManualDispense1Pin) == LOW && !buttonManualDispense1Pressed) {
    Serial.println("Manual dispense button for storage 1 pressed!");
    manualDispense(1);
    buttonManualDispense1Pressed = true;
  }

  // Reset the manual dispense button press flag for storage 1 if the button is released
  if (digitalRead(buttonManualDispense1Pin) == HIGH) {
    buttonManualDispense1Pressed = false;
  }

  // Check if the manual dispense button for storage 2 is pressed
  if (digitalRead(buttonManualDispense2Pin) == LOW && !buttonManualDispense2Pressed) {
    Serial.println("Manual dispense button for storage 2 pressed!");
    manualDispense(2);
    buttonManualDispense2Pressed = true;
  }

  // Reset the manual dispense button press flag for storage 2 if the button is released
  if (digitalRead(buttonManualDispense2Pin) == HIGH) {
    buttonManualDispense2Pressed = false;
  }

  // Check food level and control LEDs
  updateFoodLevelIndicator();

  // Display current time
  lcd.setCursor(0, 0);
  lcd.print("Current Time:       ");
  displayTime(now, 0, 1);

  delay(1000);
}

void checkAndDispenseFood() {
  bool storage1Available = isFoodAvailable(trigPin1, echoPin1);
  bool storage2Available = isFoodAvailable(trigPin2, echoPin2);

  if (storage1Available) {
    // Rotate servo1 to dispense food from storage 1
    Serial.println("Dispensing food from storage 1...");
    servo1.write(90);  // Adjust angle as needed
    delay(1000);       // Delay to ensure food is dispensed
    servo1.write(0);   // Return to initial position
    lastDispenseTime1 = rtc.now();
  } else {
    displayStorageMessage(1);
  }

  if (storage2Available) {
    // Rotate servo2 to dispense food from storage 2
    Serial.println("Dispensing food from storage 2...");
    servo2.write(90);  // Adjust angle as needed
    delay(1000);       // Delay to ensure food is dispensed
    servo2.write(0);   // Return to initial position
    lastDispenseTime2 = rtc.now();
  } else {
    displayStorageMessage(2);
  }

  if (storage1Available && storage2Available) {
    lcd.setCursor(0, 3);
    lcd.print("Food Dispensed      ");
  }
}

bool isFoodAvailable(int trigPin, int echoPin) {
  return measureDistance(trigPin, echoPin) < foodThreshold;
}

long measureDistance(int trigPin, int echoPin) {
  // Send ultrasonic pulse
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read echo time
  long duration = pulseIn(echoPin, HIGH);

  // Calculate distance in centimeters
  long distance = duration * 0.034 / 2;

  return distance;
}

void displayTime(DateTime time, int col, int row) {
  lcd.setCursor(col, row);
  lcd.print(time.hour() < 10 ? "0" : "");
  lcd.print(time.hour());
  lcd.print(':');
  lcd.print(time.minute() < 10 ? "0" : "");
  lcd.print(time.minute());
  lcd.print(':');
  lcd.print(time.second() < 10 ? "0" : "");
  lcd.print(time.second());
}

void displayStorageMessage(int storageNumber) {
  if (storageNumber == 1) {
    lcd.setCursor(0, 2);
    lcd.print("Check storage 1     ");
  } else if (storageNumber == 2) {
    lcd.setCursor(0, 3);
    lcd.print("Check storage 2     ");
  }
}

void displayLastDispenseTimes() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Last Dispense S1:   ");
  displayTime(lastDispenseTime1, 0, 1);
  lcd.setCursor(0, 2);
  lcd.print("Last Dispense S2:   ");
  displayTime(lastDispenseTime2, 0, 3);
  delay(5000); // Display for 5 seconds
  lcd.clear();
}

void updateFoodLevelIndicator() {
  bool isLow1 = measureDistance(trigPin1, echoPin1) >= foodThreshold;
  bool isLow2 = measureDistance(trigPin2, echoPin2) >= foodThreshold;

  digitalWrite(ledPin1, isLow1 ? HIGH : LOW);
  digitalWrite(ledPin2, isLow2 ? HIGH : LOW);

  if (isLow1) {
    displayStorageMessage(1);
  } else {
    lcd.setCursor(0, 2);
    lcd.print("                    "); // Clear the line if storage 1 level is okay
  }

  if (isLow2) {
    displayStorageMessage(2);
  } else {
    lcd.setCursor(0, 3);
    lcd.print("                    "); // Clear the line if storage 2 level is okay
  }
}

void manualDispense(int storageNumber) {
  if (storageNumber == 1) {
    if (isFoodAvailable(trigPin1, echoPin1)) {
      // Rotate servo1 to dispense food from storage 1
      Serial.println("Manually dispensing food from storage 1...");
      servo1.write(90);  // Adjust angle as needed
      delay(1000);       // Delay to ensure food is dispensed
      servo1.write(0);   // Return to initial position
      lastDispenseTime1 = rtc.now();
    } else {
      displayStorageMessage(1);
    }
  } else if (storageNumber == 2) {
    if (isFoodAvailable(trigPin2, echoPin2)) {
      // Rotate servo2 to dispense food from storage 2
      Serial.println("Manually dispensing food from storage 2...");
      servo2.write(90);  // Adjust angle as needed
      delay(1000);       // Delay to ensure food is dispensed
      servo2.write(0);   // Return to initial position
      lastDispenseTime2 = rtc.now();
    } else {
      displayStorageMessage(2);
    }
  }
}