#include <ESP32Servo.h>
#include <Wire.h>
#include <LiquidCrystal.h>

// Constants for LED pins
const int led1rPin = 25;
const int led1gPin = 33;
const int led1bPin = 32;
const int led2rPin = 13;
const int led2gPin = 12;
const int led2bPin = 14;

// Constants for Button pins
const int buttonPin1 = 34;
const int buttonPin2 = 35;
const int buttonPin3 = 26;

// Constants for servo pins
const int servoPin1 = 16;
const int servoPin2 = 17;

// Constants for servo position
const int doorOpen = 0;
const int doorClose = 90;

// Vars for door static
bool door1Open = false;
bool door2Open = false;

// IR receiver pin
const int irReceiverPin = 27;

// Servo objects
Servo servo1;
Servo servo2;

// LCD I2C address (usually 0x27 or 0x3F)
LiquidCrystal lcd(21, 19, 18, 5, 4, 0);

// Buzzer
const int buzzerPin = 27;


// Variables to store the state of the LEDs and buttons
bool lastButtonState1 = LOW;
bool lastButtonState2 = LOW;

int button1Count = 0;
int button2Count = 0;

void setup() {
  lcd.begin(16, 2);
  lcd.print("System starting...");

  Serial.begin(9600);
  // Initialize the buttons and LEDs
  pinMode(led1rPin, OUTPUT);
  pinMode(led1gPin, OUTPUT);
  pinMode(led1bPin, OUTPUT);
  
  pinMode(led2rPin, OUTPUT);
  pinMode(led2gPin, OUTPUT);
  pinMode(led2bPin, OUTPUT);

  pinMode(buttonPin1, INPUT);
  pinMode(buttonPin2, INPUT);
  pinMode(buttonPin3, INPUT);

  pinMode(buzzerPin, OUTPUT);


  // Attach the servos to their respective pins
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);

  // Initialize servos to 0 degrees
  servo1.write(doorClose);
  servo2.write(doorClose);

  // Set doors to closed
  door1Open = false;
  door2Open = false;

  // Set the LEDs to red
  digitalWrite(led1rPin, LOW);
  digitalWrite(led1gPin, HIGH);
  digitalWrite(led1bPin, HIGH);
  digitalWrite(led2rPin, LOW);
  digitalWrite(led2gPin, HIGH);
  digitalWrite(led2bPin, HIGH);

  delay(2000);
    // Initialize LCD
  lcd.begin(16, 2);
  lcd.setCursor(0, 0);
  lcd.print("M:      0  ");
  lcd.setCursor(0, 1);
  lcd.print("C:      0  ");
}

void loop() {
  // Read the current state of buttons
  bool currentButtonState1 = digitalRead(buttonPin1);
  bool currentButtonState2 = digitalRead(buttonPin2);
  bool currentButtonState3 = digitalRead(buttonPin3);

  // Check for button 1 press
  if (currentButtonState1 == HIGH && lastButtonState1 == LOW) {
    digitalWrite(led1gPin, !door1Open ? LOW : HIGH);
    digitalWrite(led1rPin, !door1Open ? HIGH : LOW);

    servo1.write(door1Open ? doorClose : doorOpen);
    !door1Open ? button1Count = button1Count + 1 : button1Count = button1Count;
    door1Open = !door1Open;
    lcd.setCursor(8, 0);
    lcd.print(button1Count);
  }

  // Check for button 2 press
  if (currentButtonState2 == HIGH && lastButtonState2 == LOW) {
    digitalWrite(led2gPin, !door2Open ? LOW : HIGH);
    digitalWrite(led2rPin, !door2Open ? HIGH : LOW);
    servo2.write(door2Open ? doorClose : doorOpen);
    !door2Open ? button2Count = button2Count + 1 : button2Count = button2Count;
    door2Open = !door2Open;
    lcd.setCursor(8, 1);
    lcd.print(button2Count);
  }
  
  if (currentButtonState3 == HIGH) {
    lcd.setCursor(0, 0);
    lcd.print("Resetting...   ");
    lcd.setCursor(0, 1);
    lcd.print("                ");
    digitalWrite(led1rPin, HIGH);
    digitalWrite(led2rPin, HIGH);
    digitalWrite(led1gPin, HIGH);
    digitalWrite(led2gPin, HIGH);
    digitalWrite(led1bPin, LOW);
    digitalWrite(led2bPin, LOW);
    servo1.write(doorClose);
    servo2.write(doorClose);
    button1Count = 0;
    button2Count = 0;
    delay(3000);
    lcd.setCursor(0, 0);
    lcd.print("M:      0  ");
    lcd.setCursor(0, 1);
    lcd.print("C:      0  ");
    door1Open = false;
    door2Open = false;
    digitalWrite(led1bPin, HIGH);
    digitalWrite(led2bPin, HIGH);
     delay(500);
         digitalWrite(led1bPin, LOW);
    digitalWrite(led2bPin, LOW);
     delay(500);
         digitalWrite(led1bPin, HIGH);
    digitalWrite(led2bPin, HIGH);
     delay(500);
         digitalWrite(led1bPin, LOW);
    digitalWrite(led2bPin, LOW);
     delay(500);
         digitalWrite(led1bPin, HIGH);
    digitalWrite(led2bPin, HIGH);
     delay(500);
    digitalWrite(led1rPin, LOW);
    digitalWrite(led2rPin, LOW);
  }

    // Update the last button states
  lastButtonState1 = currentButtonState1;
  lastButtonState2 = currentButtonState2;

}