#include <Wire.h>
#include <LiquidCrystal_I2C.h>

#define ENCODER_A 2
#define ENCODER_B 3
#define SigmaAuto 5
#define RelayOut 8

#define BTN_MENU 6
#define BTN_UP   9
#define BTN_DOWN 10

int PPR = 120;
float wheelCirc = 100.0;
int maxLength = 2000;

volatile long pulseCount = 0;
bool isCounting = false;
bool isLimitReached = false;
bool lastSigmaAutoState = HIGH;

LiquidCrystal_I2C lcd(0x27, 16, 2);

int menuIndex = 0;
int lastMenuIndex = -1;
unsigned long menuTimeout = 0;

bool lastBtnMenuState = false;
bool lastBtnUpState = false;
bool lastBtnDownState = false;

unsigned long upHoldStart = 0;
unsigned long downHoldStart = 0;

unsigned long lastBoostTime = 0;
bool upHeldBoosting = false;
bool downHeldBoosting = false;

unsigned long lastRefreshTime = 0;

void readEncoder() {
  if (!isCounting || isLimitReached) return;

  if (digitalRead(ENCODER_A) == HIGH) {
    if (digitalRead(ENCODER_B) == HIGH) {
      pulseCount++;
    } else {
      pulseCount--;
    }
  }
}

bool readButtonOnce(int pin, bool &lastState) {
  bool stateNow = digitalRead(pin) == LOW;
  bool pressed = stateNow && !lastState;
  lastState = stateNow;
  return pressed;
}

void adjustValue(int direction, int boost) {
  switch (menuIndex) {
    case 1: PPR = constrain(PPR + direction * boost, 1, 1000); break;
    case 2: wheelCirc = constrain(wheelCirc + direction * boost, 1.0, 1000.0); break;
    case 3: maxLength = constrain(maxLength + direction * 10 * boost, 100, 10000); break;
  }
  lastMenuIndex = -1;
}

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

  pinMode(ENCODER_A, INPUT);
  pinMode(ENCODER_B, INPUT);
  pinMode(SigmaAuto, INPUT);
  pinMode(RelayOut, OUTPUT);

  pinMode(BTN_MENU, INPUT_PULLUP);
  pinMode(BTN_UP, INPUT_PULLUP);
  pinMode(BTN_DOWN, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(ENCODER_A), readEncoder, CHANGE);

  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Program Load...");
  lcd.setCursor(0, 1);
  lcd.print("by Jikun MTC SKF");
  delay(3000);

  Serial.println("Program dimulai...");
}

void loop() {

  

  bool SigmaAutoState = digitalRead(SigmaAuto);

  if (!SigmaAutoState && lastSigmaAutoState) {
    Serial.println("SigmaAuto OFF: Reset panjang ke 0 mm");
    pulseCount = 0;
    isLimitReached = false;
    isCounting = false;
    digitalWrite(RelayOut, LOW);
  }

  if (SigmaAutoState && !lastSigmaAutoState) {
    Serial.println("SigmaAuto ON: Hitungan dimulai...");
    isCounting = true;
  }

  lastSigmaAutoState = SigmaAutoState;

  int length_mm = (int)((pulseCount / (float)PPR) * wheelCirc);

  if (length_mm >= maxLength && !isLimitReached) {
    isLimitReached = true;
    isCounting = false;
    digitalWrite(RelayOut, HIGH);
    Serial.println("Mencapai batas, RelayOut ON!");
  }

  // === Tombol dan menu ===
  bool menuPressed  = readButtonOnce(BTN_MENU, lastBtnMenuState);
  bool upPressed    = readButtonOnce(BTN_UP, lastBtnUpState);
  bool downPressed  = readButtonOnce(BTN_DOWN, lastBtnDownState);

  if (menuPressed) {
    menuIndex = (menuIndex + 1) % 4;
    menuTimeout = millis();
    lastMenuIndex = -1;
  }

  // Handle single press
  if (upPressed) {
    adjustValue(+1, 1);
    menuTimeout = millis();
    upHoldStart = millis();
    upHeldBoosting = false;
  }

  if (downPressed) {
    adjustValue(-1, 1);
    menuTimeout = millis();
    downHoldStart = millis();
    downHeldBoosting = false;
  }

  // Handle hold for boost
  bool upHeld = digitalRead(BTN_UP) == LOW;
  bool downHeld = digitalRead(BTN_DOWN) == LOW;

  if (upHeld && (millis() - upHoldStart > 3000)) {
    if (!upHeldBoosting || millis() - lastBoostTime >= 200) {
      adjustValue(+1, 10);
      menuTimeout = millis();
      lastBoostTime = millis();
      upHeldBoosting = true;
    }
  }

  if (downHeld && (millis() - downHoldStart > 3000)) {
    if (!downHeldBoosting || millis() - lastBoostTime >= 200) {
      adjustValue(-1, 10);
      menuTimeout = millis();
      lastBoostTime = millis();
      downHeldBoosting = true;
    }
  }

  if (!upHeld) upHoldStart = 0;
  if (!downHeld) downHoldStart = 0;

  if (millis() - menuTimeout > 10000) {
    menuIndex = 0;
  }

  // === Tampilan LCD ===
  if (menuIndex != lastMenuIndex) {
    lcd.clear();
    lastMenuIndex = menuIndex;

    switch (menuIndex) {
      case 0:
        if (millis() - lastRefreshTime > 200) {  // Update tiap 200ms
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("SigmaAuto: ");
          lcd.print(SigmaAutoState ? "ON " : "OFF");

          lcd.setCursor(0, 1);
          lcd.print("P: ");
          lcd.print(length_mm);
          lcd.print(" mm");

          lastRefreshTime = millis();
        }
        break;


      case 1:
        lcd.setCursor(0, 0);
        lcd.print("Set PPR:");
        lcd.setCursor(0, 1);
        lcd.print(PPR);
        break;

      case 2:
        lcd.setCursor(0, 0);
        lcd.print("WheelCirc (mm):");
        lcd.setCursor(0, 1);
        lcd.print(wheelCirc, 1);
        break;

      case 3:
        lcd.setCursor(0, 0);
        lcd.print("MaxLength (mm):");
        lcd.setCursor(0, 1);
        lcd.print(maxLength);
        break;
    }
  }

  delay(50);
}