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

// EEPROM Config
#define EEPROM_SIZE 16
#define EEPROM_ADDR_TARGET 0
#define EEPROM_ADDR_START_ANG 4
#define EEPROM_ADDR_RUN_ANG 8
#define EEPROM_ADDR_STOP_ANG 12

// Hardware Pins
const int PIN_SERVO = 18;
const int PIN_POT = 34;
const int PIN_REED = 19;     // Simulates the magnetic reed count sensor
const int PIN_RELAY = 5;
const int PIN_BUZZER = 4;

// Button Pins (Active LOW)
const int BTN_START = 13;   
const int BTN_INC = 12;     
const int BTN_X10 = 14;     
const int BTN_DEC = 27;     
const int BTN_RESET = 26;   
const int BTN_PADDLE = 33;  
const int SW_MODE = 25;     

// Peripheral Drivers
LiquidCrystal_I2C lcd(0x27, 16, 2); 
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
Servo myServo;

// Global States
int targetTurns = 100;
volatile int currentTurns = 0;
bool isRunning = false;

// Parameters (Loaded from virtual EEPROM)
int startAngle = 20;  
int runAngle = 90;    
int stopAngle = 0;    
int currentMenuPage = 0; 
bool inMenuMode = false;

// Debounce & timing checks
unsigned long btnStartPressedTime = 0;
unsigned long btnResetPressedTime = 0;
bool startPressed = false;
bool resetPressed = false;

volatile unsigned long lastInterruptTime = 0;
void IRAM_ATTR reedPulseISR() {
  unsigned long interruptTime = millis();
  if (interruptTime - lastInterruptTime > 150) { // Debounce window
    if (isRunning) {
      currentTurns++;
    }
    lastInterruptTime = interruptTime;
  }
}

void setup() {
  Serial.begin(115200);
  EEPROM.begin(EEPROM_SIZE);
  loadSettings();

  pinMode(PIN_REED, INPUT_PULLUP);
  pinMode(PIN_RELAY, OUTPUT);
  pinMode(PIN_BUZZER, OUTPUT);
  digitalWrite(PIN_RELAY, LOW);

  pinMode(BTN_START, INPUT_PULLUP);
  pinMode(BTN_INC, INPUT_PULLUP);
  pinMode(BTN_X10, INPUT_PULLUP);
  pinMode(BTN_DEC, INPUT_PULLUP);
  pinMode(BTN_RESET, INPUT_PULLUP);
  pinMode(SW_MODE, INPUT_PULLUP);
  pinMode(BTN_PADDLE, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(PIN_REED), reedPulseISR, FALLING);

  lcd.init();
  lcd.backlight();
  
  if(!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("OLED Setup Failed"));
  }
  oled.clearDisplay();
  oled.display();

  myServo.attach(PIN_SERVO);
  myServo.write(stopAngle);

  beep(100); 
  updateLCD();
  updateOLED(0);
}

void loop() {
  int modeSwitch = digitalRead(SW_MODE); 
  bool paddlePressed = (digitalRead(BTN_PADDLE) == LOW);

  handleLongPresses(modeSwitch);

  if (modeSwitch == HIGH) { 
    // RUN MODE
    inMenuMode = false; 
    currentMenuPage = 0;

    if (currentTurns >= targetTurns && isRunning) {
      isRunning = false;
      digitalWrite(PIN_RELAY, LOW);
      myServo.write(stopAngle);
      beepCompletion();
      saveSettings(); 
    }

    if (isRunning) {
      digitalWrite(PIN_RELAY, HIGH); 
      int dynamicTargetAngle = runAngle;
      int potMaxAngle = map(analogRead(PIN_POT), 0, 4095, 0, 180);
      dynamicTargetAngle = min(dynamicTargetAngle, potMaxAngle);

      if (paddlePressed) {
        if (currentTurns < 4) {
          float progress = (float)currentTurns / 4.0;
          dynamicTargetAngle = startAngle + (progress * (dynamicTargetAngle - startAngle));
        } 
        else if (targetTurns - currentTurns <= 4) {
          float progress = (float)(targetTurns - currentTurns) / 4.0;
          dynamicTargetAngle = stopAngle + (progress * (dynamicTargetAngle - stopAngle));
        }
        
        myServo.write(dynamicTargetAngle);
        int percent = map(dynamicTargetAngle, 0, 180, 0, 100);
        updateOLED(percent);
      } else {
        myServo.write(startAngle);
        updateOLED(map(startAngle, 0, 180, 0, 100));
      }
    } else {
      digitalWrite(PIN_RELAY, LOW);
      myServo.write(stopAngle);
      updateOLED(0);
    }
  } else {
    // SETUP MODE
    digitalWrite(PIN_RELAY, LOW); 
    myServo.write(stopAngle);
    updateOLED(0);

    if (digitalRead(BTN_INC) == LOW) {
      delay(50); beep(50);
      if (inMenuMode) adjustMenuVariables(1);
      else targetTurns++;
      updateLCD();
      while(digitalRead(BTN_INC) == LOW);
    }

    if (digitalRead(BTN_X10) == LOW) {
      delay(50); beep(50);
      if (inMenuMode) adjustMenuVariables(10);
      else targetTurns += 10;
      updateLCD();
      while(digitalRead(BTN_X10) == LOW);
    }

    if (digitalRead(BTN_DEC) == LOW) {
      delay(50); beep(50);
      if (inMenuMode) adjustMenuVariables(-1);
      else if (targetTurns > 0) targetTurns--;
      updateLCD();
      while(digitalRead(BTN_DEC) == LOW);
    }
  }

  static unsigned long lastUIUpdate = 0;
  if (millis() - lastUIUpdate > 150) {
    updateLCD();
    lastUIUpdate = millis();
  }
}

void adjustMenuVariables(int amount) {
  if (currentMenuPage == 1) startAngle = constrain(startAngle + amount, 0, 180);
  if (currentMenuPage == 2) runAngle = constrain(runAngle + amount, 0, 180);
  if (currentMenuPage == 3) stopAngle = constrain(stopAngle + amount, 0, 180);
}

void handleLongPresses(int modeSwitch) {
  if (digitalRead(BTN_START) == LOW) {
    if (!startPressed) {
      btnStartPressedTime = millis();
      startPressed = true;
    }
    if (modeSwitch == LOW && (millis() - btnStartPressedTime > 2000) && !inMenuMode) {
      inMenuMode = true;
      currentMenuPage = 1;
      beep(250);
      updateLCD();
      while(digitalRead(BTN_START) == LOW); 
    }
  } else {
    if (startPressed) {
      unsigned long pressDuration = millis() - btnStartPressedTime;
      startPressed = false;
      if (pressDuration < 2000) { 
        beep(50);
        if (modeSwitch == HIGH) {
          isRunning = !isRunning; 
        } else if (inMenuMode) {
          currentMenuPage++;
          if (currentMenuPage > 3) {
            inMenuMode = false;
            currentMenuPage = 0;
            saveSettings(); 
          }
          updateLCD();
        }
      }
    }
  }

  if (digitalRead(BTN_RESET) == LOW) {
    if (!resetPressed) {
      btnResetPressedTime = millis();
      resetPressed = true;
    }
    if (millis() - btnResetPressedTime > 2000) {
      beepCancel();
      currentTurns = 0;
      updateLCD();
      while(digitalRead(BTN_RESET) == LOW);
      resetPressed = false;
    }
  } else {
    resetPressed = false;
  }
}

void updateLCD() {
  lcd.clear();
  if (inMenuMode) {
    lcd.setCursor(0, 0);
    lcd.print("--- SETTINGS ---");
    lcd.setCursor(0, 1);
    if (currentMenuPage == 1) { lcd.print("Start Ang: "); lcd.print(startAngle); lcd.print((char)223); }
    if (currentMenuPage == 2) { lcd.print("Run Ang:   "); lcd.print(runAngle); lcd.print((char)223); }
    if (currentMenuPage == 3) { lcd.print("Stop Ang:  "); lcd.print(stopAngle); lcd.print((char)223); }
  } else {
    lcd.setCursor(0, 0);
    lcd.print("Target: ");
    lcd.print(targetTurns);
    lcd.setCursor(0, 1);
    lcd.print("Count:  ");
    lcd.print(currentTurns);
    if (digitalRead(SW_MODE) == HIGH) {
      lcd.setCursor(12, 0);
      lcd.print(isRunning ? "[RUN]" : "[PS]");
    } else {
      lcd.setCursor(12, 0);
      lcd.print("[SET]");
    }
  }
}

void updateOLED(int percent) {
  oled.clearDisplay();
  oled.setTextSize(1);
  oled.setTextColor(SSD1306_WHITE);
  oled.setCursor(0, 4);
  oled.print("Motor Speed: ");
  oled.setTextSize(2);
  oled.setCursor(0, 18);
  oled.print(percent);
  oled.print("%");
  
  oled.drawRect(0, 42, 128, 16, SSD1306_WHITE);
  int barWidth = map(percent, 0, 100, 0, 124);
  if (barWidth > 0) {
    oled.fillRect(2, 44, barWidth, 12, SSD1306_WHITE);
  }
  oled.display();
}

void beep(int duration) {
  digitalWrite(PIN_BUZZER, HIGH);
  delay(duration);
  digitalWrite(PIN_BUZZER, LOW);
}

void beepCompletion() {
  for(int i = 0; i < 3; i++) {
    digitalWrite(PIN_BUZZER, HIGH);
    delay(100);
    digitalWrite(PIN_BUZZER, LOW);
    delay(80);
  }
}

void beepCancel() {
  digitalWrite(PIN_BUZZER, HIGH);
  delay(500);
  digitalWrite(PIN_BUZZER, LOW);
}

void saveSettings() {
  EEPROM.put(EEPROM_ADDR_TARGET, targetTurns);
  EEPROM.put(EEPROM_ADDR_START_ANG, startAngle);
  EEPROM.put(EEPROM_ADDR_RUN_ANG, runAngle);
  EEPROM.put(EEPROM_ADDR_STOP_ANG, stopAngle);
  EEPROM.commit();
}

void loadSettings() {
  int tempTarget, tempStart, tempRun, tempStop;
  EEPROM.get(EEPROM_ADDR_TARGET, tempTarget);
  EEPROM.get(EEPROM_ADDR_START_ANG, tempStart);
  EEPROM.get(EEPROM_ADDR_RUN_ANG, tempRun);
  EEPROM.get(EEPROM_ADDR_STOP_ANG, tempStop);

  if(tempTarget >= 0 && tempTarget < 10000) targetTurns = tempTarget;
  if(tempStart >= 0 && tempStart <= 180) startAngle = tempStart;
  if(tempRun >= 0 && tempRun <= 180) runAngle = tempRun;
  if(tempStop >= 0 && tempStop <= 180) stopAngle = tempStop;
}