#include <EEPROM.h>
#include <Encoder.h>
#include <LiquidCrystal_I2C.h>
#include <AccelStepper.h>

// Pin Definitions
#define STEP_PIN        2
#define DIR_PIN         3
#define START_BTN       4
#define EMG_STOP_BTN    5
#define CYCLE_DONE_PIN  9

#define ENCODER_CLK     6
#define ENCODER_DT      7
#define ENCODER_BTN     8

#define STEPS_PER_REV   6400

#define EEPROM_MODE_ADDR    0
#define EEPROM_DIR_ADDR     1
#define EEPROM_TARGET_ADDR  2
#define EEPROM_DELAY_ADDR   4

LiquidCrystal_I2C lcd(0x27, 16, 2);
Encoder encoder(ENCODER_CLK, ENCODER_DT);
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// Settings
bool modeAuto = true;
bool dirCW = true;
int targetDeg = 90;
int delayVal = 300;

bool settingMode = false;
int menuIndex = 0;
const int menuMax = 3;
bool editingValue = false;
long lastEncoderPos = 0;
unsigned long lastBtnPressTime = 0;
bool lastBtnState = HIGH;
const unsigned long LONG_PRESS_TIME = 1000;

void setup() {
  pinMode(START_BTN, INPUT_PULLUP);
  pinMode(EMG_STOP_BTN, INPUT_PULLUP);
  pinMode(ENCODER_BTN, INPUT_PULLUP);
  pinMode(CYCLE_DONE_PIN, OUTPUT);
  digitalWrite(CYCLE_DONE_PIN, LOW);

  lcd.init();
  lcd.backlight();

  encoder.write(0);
  lastEncoderPos = 0;

  loadSettings();

  stepper.setMaxSpeed(1000);
  stepper.setAcceleration(500);

  updateStepperParams();
  updateLCD();
}

void loop() {
  handleEncoderButton();
  handleEncoder();

  if (!settingMode) {
    if (digitalRead(START_BTN) == LOW) {
      delay(200);
      if (modeAuto) runAutoCycle();
      else runManualCycle();
    }
  }

  if (digitalRead(EMG_STOP_BTN) == LOW) {
    delay(100);
    stepper.stop();
    pulseDoneSignal();
  }
}

void handleEncoder() {
  long newPos = encoder.read() / 4;
  if (newPos != lastEncoderPos) {
    if (settingMode) {
      if (!editingValue) {
        menuIndex += (newPos > lastEncoderPos) ? 1 : -1;
        if (menuIndex > menuMax) menuIndex = 0;
        if (menuIndex < 0) menuIndex = menuMax;
        encoder.write(0);
        lastEncoderPos = 0;
      } else {
        switch (menuIndex) {
          case 0:
            modeAuto = !modeAuto;
            break;
          case 1:
            dirCW = !dirCW;
            break;
          case 2:
            targetDeg += (newPos > lastEncoderPos) ? -1 : 1;
            if (targetDeg < 0) targetDeg = 0;
            if (targetDeg > 360) targetDeg = 360;
            break;
          case 3:
            delayVal += (newPos > lastEncoderPos) ? -10 : 10;
            if (delayVal < 10) delayVal = 10;
            if (delayVal > 2000) delayVal = 2000;
            break;
        }
        encoder.write(0);
        lastEncoderPos = 0;
        updateStepperParams();
      }
    } else {
      encoder.write(0);
    }
    updateLCD();
  }
}

void handleEncoderButton() {
  bool btnState = digitalRead(ENCODER_BTN);
  if (btnState == LOW && lastBtnState == HIGH) {
    lastBtnPressTime = millis();
  }

  if (btnState == HIGH && lastBtnState == LOW) {
    unsigned long pressDuration = millis() - lastBtnPressTime;
    if (pressDuration > LONG_PRESS_TIME) {
      settingMode = !settingMode;
      editingValue = false;
      if (!settingMode) {
        saveSettings();
        encoder.write(0);
        lastEncoderPos = 0;
      }
    } else if (settingMode) {
      editingValue = !editingValue;
    }
    updateLCD();
  }
  lastBtnState = btnState;
}

void updateLCD() {
  lcd.clear();
  if (settingMode) {
    switch (menuIndex) {
      case 0:
        lcd.setCursor(0, 0);
        lcd.print("Mode: ");
        lcd.print(modeAuto ? "AUTO" : "MANUAL");
        break;
      case 1:
        lcd.setCursor(0, 0);
        lcd.print("Direction: ");
        lcd.print(dirCW ? "CW" : "CCW");
        break;
      case 2:
        lcd.setCursor(0, 0);
        lcd.print("Target: ");
        lcd.print(targetDeg);
        lcd.print(" deg");
        break;
      case 3:
        lcd.setCursor(0, 0);
        lcd.print("Speed: ");
        lcd.print(delayVal);
        lcd.print(" us");
        break;
    }
    lcd.setCursor(0, 1);
    lcd.print(editingValue ? "> Edit Value" : "> Select Menu");
  } else {
    lcd.setCursor(0, 0);
    lcd.print(modeAuto ? "MODE: AUTO   " : "MODE: MANUAL ");
    lcd.setCursor(0, 1);
    lcd.print("Tar:");
    lcd.print(targetDeg);
    lcd.write(0xDF);
    lcd.print(" Dir:");
    lcd.print(dirCW ? "CW " : "CCW");
  }
}

void updateStepperParams() {
  stepper.setMaxSpeed(1000000.0 / delayVal);
  stepper.setAcceleration(500);
  stepper.setPinsInverted(!dirCW, false); // Invert direction if CCW
  //stepper.setPinsInverted(false, false);
}

void runAutoCycle() {
  updateStepperParams();
  long stepsToMove = (long)((targetDeg / 360.0) * STEPS_PER_REV);
  stepper.moveTo(stepsToMove);
  while (stepper.distanceToGo() != 0 && digitalRead(EMG_STOP_BTN) == HIGH) {
    stepper.run();
  }
  pulseDoneSignal();
  stepper.setCurrentPosition(0);
}

void runManualCycle() {  
  stepper.setPinsInverted(false, false);
  long speed = 1000000L / delayVal;
  speed = dirCW ? speed : -speed;
  stepper.setSpeed(speed);
  while (digitalRead(EMG_STOP_BTN) == HIGH) {
    stepper.runSpeed();
  }
  stepper.stop();
  pulseDoneSignal();
}

void pulseDoneSignal() {
  digitalWrite(CYCLE_DONE_PIN, HIGH);
  delay(100);
  digitalWrite(CYCLE_DONE_PIN, LOW);
}

void loadSettings() {
  modeAuto = EEPROM.read(EEPROM_MODE_ADDR);
  dirCW = EEPROM.read(EEPROM_DIR_ADDR);
  EEPROM.get(EEPROM_TARGET_ADDR, targetDeg);
  EEPROM.get(EEPROM_DELAY_ADDR, delayVal);
  updateStepperParams();
}

void saveSettings() {
  EEPROM.update(EEPROM_MODE_ADDR, modeAuto);
  EEPROM.update(EEPROM_DIR_ADDR, dirCW);
  EEPROM.put(EEPROM_TARGET_ADDR, targetDeg);
  EEPROM.put(EEPROM_DELAY_ADDR, delayVal);
}