#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>

// ================= OLED =================
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// ================= SERVO =================
Servo motorServo;

// ================= POTENTIOMETERS =================
#define P1 34
#define P2 35
#define P3 32

// ================= OUTPUTS =================
#define SERVO_PIN 18
#define RELAY_PIN 19
#define BUZZER_PIN 23

#define GREEN_LED 25
#define YELLOW_LED 26
#define RED_LED 27
#define BLUE_LED 14

// ================= PUSH BUTTON =================
#define BUTTON_PIN 33

// ================= SYSTEM =================
bool systemON = false;
bool lastButtonState = HIGH;

unsigned long lastDebounceTime = 0;
const unsigned long debounceDelay = 50;

// ===================================================
// SETUP
// ===================================================

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

  // -------- GPIO --------
  pinMode(RELAY_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  pinMode(GREEN_LED, OUTPUT);
  pinMode(YELLOW_LED, OUTPUT);
  pinMode(RED_LED, OUTPUT);
  pinMode(BLUE_LED, OUTPUT);

  pinMode(BUTTON_PIN, INPUT_PULLUP);

  // -------- Initial Outputs --------
  digitalWrite(RELAY_PIN, LOW);
  digitalWrite(BUZZER_PIN, LOW);

  digitalWrite(GREEN_LED, LOW);
  digitalWrite(YELLOW_LED, LOW);
  digitalWrite(RED_LED, LOW);
  digitalWrite(BLUE_LED, LOW);

  // -------- Servo --------
  motorServo.attach(SERVO_PIN);
  motorServo.write(0);

  // -------- OLED --------
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C))
  {
    Serial.println("OLED not found!");
    while (1);
  }

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);

  display.setTextSize(2);
  display.setCursor(10, 5);
  display.println("SYSTEM");

  display.setTextSize(1);
  display.setCursor(28, 30);
  display.println("READY");

  display.display();

  delay(2000);

  showSystemOff();
}

// ===================================================
// LOOP
// ===================================================

void loop()
{
  checkButton();

  if (systemON)
  {
    readSensors();
  }
  else
  {
    // System OFF
    digitalWrite(RELAY_PIN, LOW);

    digitalWrite(GREEN_LED, LOW);
    digitalWrite(YELLOW_LED, LOW);
    digitalWrite(RED_LED, LOW);
    digitalWrite(BLUE_LED, HIGH);

    motorServo.write(0);
  }

  delay(100);
}

// ===================================================
// BUTTON FUNCTION
// ===================================================

void checkButton()
{
  bool buttonState = digitalRead(BUTTON_PIN);

  if (buttonState != lastButtonState)
  {
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay)
  {
    if (buttonState == LOW && lastButtonState == HIGH)
    {
      systemON = !systemON;

      if (systemON)
      {
        systemStart();
      }
      else
      {
        systemStop();
      }
    }
  }

  lastButtonState = buttonState;
}

// ===================================================
// SYSTEM START
// ===================================================

void systemStart()
{
  Serial.println("SYSTEM ON");

  digitalWrite(BLUE_LED, LOW);
  digitalWrite(GREEN_LED, HIGH);

  // Short beep
  digitalWrite(BUZZER_PIN, HIGH);
  delay(150);
  digitalWrite(BUZZER_PIN, LOW);

  display.clearDisplay();

  display.setTextSize(2);
  display.setCursor(10, 5);
  display.println("SYSTEM");

  display.setTextSize(2);
  display.setCursor(35, 30);
  display.println("ON");

  display.display();

  delay(1000);
}

// ===================================================
// SYSTEM STOP
// ===================================================

void systemStop()
{
  Serial.println("SYSTEM OFF");

  digitalWrite(RELAY_PIN, LOW);

  digitalWrite(GREEN_LED, LOW);
  digitalWrite(YELLOW_LED, LOW);
  digitalWrite(RED_LED, LOW);

  digitalWrite(BLUE_LED, HIGH);

  motorServo.write(0);

  digitalWrite(BUZZER_PIN, HIGH);
  delay(100);
  digitalWrite(BUZZER_PIN, LOW);

  showSystemOff();

  delay(500);
}

// ===================================================
// READ POTENTIOMETERS
// ===================================================

void readSensors()
{
  int value1 = analogRead(P1);
  int value2 = analogRead(P2);
  int value3 = analogRead(P3);

  // Convert ADC values to percentage
  int pot1 = map(value1, 0, 4095, 0, 100);
  int pot2 = map(value2, 0, 4095, 0, 100);
  int pot3 = map(value3, 0, 4095, 0, 100);

  // Limit values
  pot1 = constrain(pot1, 0, 100);
  pot2 = constrain(pot2, 0, 100);
  pot3 = constrain(pot3, 0, 100);

  Serial.print("P1: ");
  Serial.print(pot1);

  Serial.print("  P2: ");
  Serial.print(pot2);

  Serial.print("  P3: ");
  Serial.println(pot3);

  // ------------------------------------------------
  // STATUS BASED ON POTENTIOMETER 1
  // ------------------------------------------------

  if (pot1 < 30)
  {
    // LOW LEVEL
    digitalWrite(GREEN_LED, HIGH);
    digitalWrite(YELLOW_LED, LOW);
    digitalWrite(RED_LED, LOW);

    digitalWrite(RELAY_PIN, LOW);

    motorServo.write(0);

    showDisplay("LOW", pot1, pot2, pot3);
  }

  else if (pot1 >= 30 && pot1 < 70)
  {
    // MEDIUM LEVEL
    digitalWrite(GREEN_LED, LOW);
    digitalWrite(YELLOW_LED, HIGH);
    digitalWrite(RED_LED, LOW);

    digitalWrite(RELAY_PIN, HIGH);

    motorServo.write(45);

    showDisplay("MEDIUM", pot1, pot2, pot3);
  }

  else
  {
    // HIGH LEVEL
    digitalWrite(GREEN_LED, LOW);
    digitalWrite(YELLOW_LED, LOW);
    digitalWrite(RED_LED, HIGH);

    digitalWrite(RELAY_PIN, HIGH);

    motorServo.write(90);

    // Warning buzzer
    digitalWrite(BUZZER_PIN, HIGH);
    delay(100);
    digitalWrite(BUZZER_PIN, LOW);

    showDisplay("HIGH", pot1, pot2, pot3);
  }

  delay(300);
}

// ===================================================
// OLED DISPLAY
// ===================================================

void showDisplay(
  const char *status,
  int p1,
  int p2,
  int p3)
{
  display.clearDisplay();

  display.setTextColor(SSD1306_WHITE);

  // Title
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println("ESP32 CONTROL SYSTEM");

  // Status
  display.setTextSize(2);
  display.setCursor(0, 14);
  display.println(status);

  // Potentiometer values
  display.setTextSize(1);

  display.setCursor(0, 37);
  display.print("P1: ");
  display.print(p1);
  display.println("%");

  display.setCursor(64, 37);
  display.print("P2: ");
  display.print(p2);
  display.println("%");

  display.setCursor(0, 50);
  display.print("P3: ");
  display.print(p3);
  display.println("%");

  display.display();
}

// ===================================================
// SYSTEM OFF DISPLAY
// ===================================================

void showSystemOff()
{
  display.clearDisplay();

  display.setTextColor(SSD1306_WHITE);

  display.setTextSize(2);
  display.setCursor(10, 5);
  display.println("SYSTEM");

  display.setCursor(30, 28);
  display.println("OFF");

  display.setTextSize(1);
  display.setCursor(20, 52);
  display.println("Press BUTTON");

  display.display();
}