#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <Servo.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <DHT.h>

// OLED display setup
#define OLED_RESET -1
Adafruit_SSD1306 display(128, 64, &Wire, OLED_RESET);

// LCD setup
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Keypad setup
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
byte rowPins[ROWS] = {10, 11, 12, 13};  // Pin changes for Mega
byte colPins[COLS] = {A0, A1, A2, A3}; // Pin changes for Mega
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// Servos
Servo servo1, servo2, servo3, servo4;
Servo servoOnPin2; // Servo motor for pin 2

// Ultrasonic sensor
const int trigPin = 3;   // Pin change for Mega
const int echoPin = 4;   // Pin change for Mega

// PIR sensor
const int pirPin = 22;   // Pin change for Mega

// Slide switch pin
const int slideSwitchPin = 5;  // Pin connected to the slide switch

// DHT22 sensor setup
#define DHT_PIN 7        // DHT22 connected to pin 7
DHT dht(DHT_PIN, DHT22);   // Create DHT sensor object

// Variables
String inputPassword = "";
const String password = "1234";
bool accessGranted = false;
bool motionDetected = false;
bool isSlideSwitchOnLast = false;  // Variable to track the last slide switch state

void setup() {
  // LCD initialization
  lcd.begin(16, 2); // Specify 16x2 dimensions
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("SWITCH OFF");
  stopServos();
  
  // OLED initialization
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.display();
  delay(2000);  // Pause for 2 seconds

  // Clear display
  display.clearDisplay();
  display.setTextSize(1);      // Normal 1:1 pixel size
  display.setTextColor(SSD1306_WHITE);  // Set text color to white

  // Initialize DHT22 sensor
  dht.begin();

  // Servo initialization
  servo1.attach(48);  // Pin change for Mega
  servo2.attach(49);  // Pin change for Mega
  servo3.attach(50);  // Pin change for Mega
  servo4.attach(51);  // Pin change for Mega
  servoOnPin2.attach(2); // Initialize servo motor on pin 2
  
  // Ultrasonic and PIR setup
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(pirPin, INPUT);
  
  // Slide switch setup
  pinMode(slideSwitchPin, INPUT_PULLUP);  // Assuming the slide switch connects to ground when "on"
  
  // Set servos to initial position
  stopServos();
  servoOnPin2.write(0); // Initial position for servo on pin 2
}

void loop() {
  // Check if the slide switch is "on"
  bool isSlideSwitchOn = digitalRead(slideSwitchPin) == LOW;  // LOW means "on" if the switch is connected to ground
  
  if (isSlideSwitchOn != isSlideSwitchOnLast) {  // Detect state change
    if (isSlideSwitchOn) {
      // Slide switch turned on
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Enter Password:");
      inputPassword = ""; // Reset input when slide switch is on
      accessGranted = false; // Reset access status
    } else {
      // Slide switch turned off
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("SWITCH OFF");
      stopServos();
      servoOnPin2.write(0); // Reset servo on pin 2

      // Update OLED display
      display.clearDisplay();
      display.setCursor(0, 0);
      display.print("System Offline"); // Display default message
      display.display();
    }
    isSlideSwitchOnLast = isSlideSwitchOn;  // Update last state
  }
  
  if (!isSlideSwitchOn) {
    return;  // If switch is off, skip the rest of the loop and don't process any further
  }
  
  if (!accessGranted) {
    handlePasswordInput();
  } else {
    lcd.setCursor(0, 0);
    lcd.print("Motion Status:     ");

    motionDetected = digitalRead(pirPin);
    if (motionDetected) {
      lcd.setCursor(0, 1);
      lcd.print("Motion Detected   ");
      
      long distance = getUltrasonicDistance();
      if (distance < 20) {
        rotateServos();
      } 
     else {
      lcd.setCursor(0, 1);
      lcd.print("No Motion         ");
      stopServos();
      }
    }

    // Read DHT22 sensor and display on OLED
    float temperature = dht.readTemperature();
    float humidity = dht.readHumidity();

    if (isnan(temperature) || isnan(humidity)) {
      display.setCursor(0, 0);
      display.print("Failed to read DHT");
    } else {
      display.clearDisplay();
      display.setCursor(0, 0);
      display.print("Temp: ");
      display.print(temperature);
      display.print(" C");

      display.setCursor(0, 20);
      display.print("Humidity: ");
      display.print(humidity);
      display.print(" %");
    }
    display.display();

    delay(100); // Small delay for stability
  }
}

void handlePasswordInput() {
  char key = keypad.getKey();
  if (key) {
    if (key == '#') {
      if (inputPassword == password) {
        accessGranted = true;
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Access Granted! ");
        delay(2000);
        lcd.clear();
      } else {
        lcd.setCursor(0, 1);
        lcd.print("Wrong Password  ");
        delay(2000);
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Enter Password: ");
      }
      inputPassword = ""; // Reset input
    } else if (key == '*') {
      inputPassword = ""; // Clear input
      lcd.setCursor(0, 1);
      lcd.print("                "); // Clear line
    } else {
      inputPassword += key;
      lcd.setCursor(0, 1);
      lcd.print(inputPassword);
    }
  }
}

long getUltrasonicDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  return duration / 58; // Convert to cm
}

void rotateServos() {
  for (int i = 0; i < 3; i++) { // Repeat 3 cycles (simulate continuous rotation)
    servo1.write(0);  // Move to 0°
    servo2.write(0);
    servo3.write(0);
    servo4.write(0);
    servoOnPin2.write(0);
    delay(500);       // Wait for movement
    servo1.write(180); // Move to 180°
    servo2.write(180);
    servo3.write(180);
    servo4.write(180);
    servoOnPin2.write(180);
    delay(500);       // Wait for movement
  }
}

void stopServos() {
  servo1.write(90); // Stop
  servo2.write(90); // Stop
  servo3.write(90); // Stop
  servo4.write(90); // Stop
  servoOnPin2.write(90); // Stop
}