#include <ESP32Servo.h>
#include <DHT.h>
#include <Adafruit_SSD1306.h> // OLED library

// Pin Definitions
const int servo1Pin = 13;
const int servo2Pin = 12;
const int servo3Pin = 14;
const int relayMotorPin = 27;
const int load1 = 18;
const int load2 = 19;
const int dhtPin = 15;
const int MQSmokePin = 26;
const int MainsPin = 16;
const int SolarPin = 17;

// Servo Objects
Servo servo1;
Servo servo2;
Servo servo3;

// OLED Display Config
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// DHT Sensor Config
#define DHTTYPE DHT22
DHT dht(dhtPin, DHTTYPE);

// Thresholds
const float tempThreshold = 70.0;
const float humThreshold = 90.0;
const int smokeThreshold = 80;

// Servo Control Variables
bool servoActive = false;



// Setup Function
void setup() {
  // Initialize serial communication
  Serial.begin(115200);

  // Setup Servo Motors
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);
  
  // Setup Relay and Loads
  pinMode(relayMotorPin, OUTPUT);
  pinMode(load1, OUTPUT);
  pinMode(load2, OUTPUT);
  
  // Setup Power Source Switches
  pinMode(MainsPin, INPUT);
  pinMode(SolarPin, INPUT);
  
  // Setup DHT Sensor
  dht.begin();
  
  // Setup Smoke Sensor
  pinMode(MQSmokePin, INPUT);
  
  // Setup OLED Display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  displayMessage("IIOT Automation", 1000);

  // Deactivate servos initially
  controlServo(false);
  display.clearDisplay();
  display.setCursor(20, 0);display.println("IIoT Automation");

}

// Main Loop
void loop() {
  display.clearDisplay();
  display.setCursor(20, 0);display.println("IIoT Automation");

  // Read power source state
  bool mainsPower = digitalRead(MainsPin);
  bool solarPower = digitalRead(SolarPin);
  
  // Read temp and humidity
  float temperature, humidity;
  readTempAndHumidity(temperature, humidity);
  
  // Read smoke value
  int smokeDensity = readSmoke();
  
  // Check thresholds and control robotic arm and relay
  if (temperature > tempThreshold || humidity > humThreshold || smokeDensity > smokeThreshold) {
    servoActive = false;
    controlRelayAndLoads(false, false, false);
  } else {
    if (mainsPower && solarPower) {
      servoActive = true;
      controlRelayAndLoads(true, true, true);
    } else if (!mainsPower && solarPower) {
      servoActive = false;
      controlRelayAndLoads(true, true, false);
    } else if (mainsPower && !solarPower) {
      servoActive = false;
      controlRelayAndLoads(true, true, false);
    } else {
      servoActive = false;
      controlRelayAndLoads(false, false, false);
    }
  }

  

  String relayText;
  String L1Text;
  String L2Text;

  // Check relay motor pin status
  if (getPinStatus(relayMotorPin) == HIGH) {relayText = " R * ";} else {relayText = " R - ";}

  // Check load1 pin status
  if (getPinStatus(load1) == HIGH) {L1Text = " L1 * ";} else {L1Text = " L1 - ";}

  // Check load2 pin status
  if (getPinStatus(load2) == HIGH) {L2Text = " L2 * ";} else {L2Text = " L2 - ";}

  // Combine texts for display
  String displayText = relayText + L1Text + L2Text;

  // Display the result on OLED
  display.setCursor(0, 30);
  display.println(displayText);


  
  // Control servos based on active state
  controlServo(servoActive);
  
  // Small delay for debouncing
  delay(500);
}

// Function to activate/deactivate the servos
void controlServo(bool state) {
  if (state) {
    servo1.write(90);
    servo2.write(90);
    servo3.write(90);
    delay(500);
    servo1.write(45);
    servo2.write(20);
    servo3.write(10);
    delay(500);
    servo1.write(0);
    servo2.write(0);
    servo3.write(0);
  } else {
    servo1.write(0);
    servo2.write(0);
    servo3.write(0);
  }
}

// Function to display message on OLED
void displayMessage(const String &message, int delayTime) {
  display.clearDisplay();
  display.setCursor(30, 10);display.println("IIoT Automation");

  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor((SCREEN_WIDTH - message.length() * 6) / 2, SCREEN_HEIGHT / 2);
  display.print(message);
  display.display();
  delay(delayTime);
}


// Function to read temperature and humidity
void readTempAndHumidity(float &temperature, float &humidity) {
  temperature = dht.readTemperature();
  humidity = dht.readHumidity();
  
  if (isnan(temperature) || isnan(humidity)) {
    Serial.println("Failed to read from DHT sensor!");
    return;
  }
  
  Serial.print("Temp: ");
  Serial.print(temperature);
  Serial.print(" °C, Humidity: ");
  Serial.print(humidity);
  Serial.println(" %");
  
  // display.setCursor(0,10);display.println("message");
  // Display DHT data on OLED
  display.setCursor(0, 40);
  display.print("Temp: ");
  display.print(temperature);
  display.print(" C");

  display.setCursor(0, 50);
  display.print("Hum: ");
  display.print(humidity);
  display.print(" %  ");
  display.display();
}

// Function to read smoke sensor value
int readSmoke() {
  int smokeValue = analogRead(MQSmokePin);
  int mappedValue = map(smokeValue, 0, 4095, 0, 100);
  Serial.print("Smoke Density: ");Serial.println(mappedValue);
  return mappedValue;
}

// Function to control relays and loads
void controlRelayAndLoads(bool relayState, bool load1State, bool load2State) {
  digitalWrite(relayMotorPin, relayState ? HIGH : LOW);
  digitalWrite(load1, load1State ? HIGH : LOW);
  digitalWrite(load2, load2State ? HIGH : LOW);
}

int getPinStatus(int pinNumber) {
  // Read and return the pin status (HIGH/LOW)
  return digitalRead(pinNumber);
}