/*
FILE FINAL - TOMBOL STAR/PAUSE/STOP LANGSUNG AKSI TANPA EDIT MODE
- Saat navIndex di tombol (STAR/PAUSE/STOP/MENU), tekan encoder LANGSUNG menjalankan aksi (tidak masuk edit mode).
- Saat navIndex pada cell parameter, tekan encoder toggle edit mode.
- Highlight selalu anti dobel garis/tumpang tindih.
*/

#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <SPI.h>
#include <EEPROM.h>
#define EEPROM_SIZE 2048

#define TFT_CS   15
#define TFT_DC   2
#define TFT_RST  4

#define ENCODER_CLK 25
#define ENCODER_DT  26
#define ENCODER_SW  27

const int MOTOR_IN1 = 33;
const int MOTOR_IN2 = 32;
const int MOTOR_ENA = 13; // PWM

#define ACS_PIN A0  // Pin arus ACS712

const int cellWidth = 60;
const int cellHeight = 35;
const int cellX[5] = {10, 72, 134, 196, 258};
const int cellY[4] = {50, 90, 130, 170};
const char* paramLabels[4] = {"TIMER", "VOLT", "DIRC", "REST"};
char paramValue[5][4][8] = {
  {"00:00","0.00","N/a","00'00"},
  {"00:00","0.00","N/a","00'00"},
  {"00:00","0.00","N/a","00'00"},
  {"00:00","0.00","N/a","00'00"},
  {"00:00","0.00","N/a","00'00"}
};

const char* dircModes[] = {
  "N/a",   // 0 Tidak ada putaran
  "FF.",   // 1 Forward CW
  "SW",    // 2 Backward CCW
  "AUTO",  // 3 Otomatis bolak balik
  "F/P",   // 4 Forward perlahan ke target
  "S/P"    // 5 Backward perlahan ke target
};
const int nDircModes = 6;

const char* buttons[] = {"STAR", "PAUSE", "STOP", "MENU"};
const int buttonWidth = 70;
const int buttonHeight = 30;
const int buttonX[4] = {10, 90, 170, 250};
const int buttonY = 210;

#define NUM_PRESET 10
const int NAV_TOTAL = 25;
int navIndex = 0;     // 0 = NO:, 1..20 cell, 21..24 tombol
bool editMode = false;
char presetData[NUM_PRESET][5][4][8];
int currentPreset = 0; // 0 = NO:1

enum MotorStatus { MOTOR_STANDBY, MOTOR_RUN, MOTOR_PAUSE, MOTOR_STOP, MOTOR_REST };
MotorStatus motorStatus = MOTOR_STANDBY;

int stepAktif = 0; // 0..4, step aktif
const int totalStep = 5;
unsigned long stepMulaiMillis = 0;
long stepSisaDetik = 0;
unsigned long restMulaiMillis = 0;
long restSisaDetik = 0;
bool motorRunning = false;
bool flagJalankanMotor = false;
bool sedangRest = false; // true = timer REST sedang berjalan

char lastWaktuStr[8] = "";
char lastArusStr[8] = "";

unsigned long stopTampilMillis = 0;
bool showStopThenStandby = false;

// ==== DIALOG SIMPAN PRESET ====
bool dialogSimpanPreset = false;
bool pilihYa = true; // true=YA, false=TIDAK

// === Untuk highlight cell agar tidak tumpang tindih ===
int navIndexLast = -1;
bool editModeLast = false;

// === PAUSE/RESUME ===
bool isPaused = false;
unsigned long pauseMillis = 0;
long pauseSisaDetik = 0;

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

// ==================== EEPROM PRESET ====================
void saveAllPresetsToEEPROM() {
  int addr = 0;
  for (int p = 0; p < NUM_PRESET; p++) {
    for (int i = 0; i < 5; i++) {
      for (int j = 0; j < 4; j++) {
        for (int k = 0; k < 8; k++) {
          EEPROM.write(addr++, presetData[p][i][j][k]);
        }
      }
    }
  }
  EEPROM.commit();
}
void loadAllPresetsFromEEPROM() {
  int addr = 0;
  for (int p = 0; p < NUM_PRESET; p++) {
    for (int i = 0; i < 5; i++) {
      for (int j = 0; j < 4; j++) {
        for (int k = 0; k < 8; k++) {
          presetData[p][i][j][k] = EEPROM.read(addr++);
        }
      }
    }
  }
}
void savePreset(int idx) {
  for (int i=0; i<5; i++)
    for (int j=0; j<4; j++)
      strcpy(presetData[idx][i][j], paramValue[i][j]);
}
void loadPreset(int idx) {
  for (int i=0; i<5; i++)
    for (int j=0; j<4; j++)
      strcpy(paramValue[i][j], presetData[idx][i][j]);
}
void initPresetsDefault() {
  for (int p=0; p<NUM_PRESET; p++)
    for (int i=0; i<5; i++)
      for (int j=0; j<4; j++)
        strcpy(presetData[p][i][j], paramValue[i][j]);
}

// ==================== UI & LOGIKA ====================

void drawPresetInfo() {
  int boxW = 64, boxH = 18;
  int boxX = 240, boxY = 8;
  tft.fillRect(boxX, boxY, boxW, boxH, ILI9341_BLACK);
  tft.setTextColor(editMode && navIndex==0 ? ILI9341_YELLOW : ILI9341_WHITE, ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setCursor(boxX + 8, boxY + 2);
  tft.printf("NO:%d", currentPreset+1);
  uint16_t color = (editMode && navIndex==0) ? ILI9341_YELLOW : ILI9341_WHITE;
  tft.drawRect(boxX, boxY, boxW, boxH, color);
}

void drawStatusMotor() {
  tft.fillRect(10, 5, 120, 32, ILI9341_BLACK);
  tft.setTextSize(3);
  uint16_t color = ILI9341_MAGENTA;
  const char* text = "STANDBY";
  if(motorStatus == MOTOR_RUN) {
    color = ILI9341_GREEN; text = "RUN";
  } else if(motorStatus == MOTOR_PAUSE) {
    color = ILI9341_YELLOW; text = "PAUSE";
  } else if(motorStatus == MOTOR_STOP) {
    color = ILI9341_RED; text = "STOP";
  } else if(motorStatus == MOTOR_REST) {
    color = ILI9341_YELLOW; text = "REST";
  }
  tft.setTextColor(color, ILI9341_BLACK);
  tft.setCursor(10, 5);
  tft.print(text);
}

void tampilkanCountdownDanArus(long sisa, bool rest = false) {
  static char waktuStr[8], arusStr[8];
  snprintf(waktuStr, sizeof(waktuStr), "%02ld:%02ld", sisa/60, sisa%60);
  float arus = 0.0; // ganti dengan bacaArusA() jika ingin real
  snprintf(arusStr, sizeof(arusStr), "%.2f", arus);

  if (strcmp(lastWaktuStr, waktuStr) != 0) {
      tft.fillRect(130, 10, 60, 12, ILI9341_BLACK);
      tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK);
      tft.setTextSize(1);
      tft.setCursor(150, 10);
      tft.print(waktuStr);
      strcpy(lastWaktuStr, waktuStr);
  }
  if (strcmp(lastArusStr, arusStr) != 0) {
      tft.fillRect(130, 24, 60, 12, ILI9341_BLACK);
      tft.setTextColor(ILI9341_CYAN, ILI9341_BLACK);
      tft.setTextSize(1);
      tft.setCursor(150, 24);
      tft.print(arusStr);
      tft.print("A");
      strcpy(lastArusStr, arusStr);
  }
}

void drawParamValue(int col, int row, const char* val) {
  int valueLen = strlen(val);
  int valuePixelWidth = valueLen * 12;
  int valueX = cellX[col] + (cellWidth - valuePixelWidth) / 2;
  int valueY = cellY[row] + (cellHeight - 16) / 2 + 8;
  tft.fillRect(cellX[col]+2, valueY, cellWidth-4, 16, ILI9341_BLACK);
  tft.setTextColor(ILI9341_CYAN, ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setCursor(valueX, valueY);
  tft.print(val);
}

void highlightCellOrButton(int idx, bool active, bool edit) {
  if(idx == 0) {
    int boxW = 64, boxH = 18;
    int boxX = 240, boxY = 8;
    tft.fillRect(boxX, boxY, boxW, boxH, ILI9341_BLACK);
    uint16_t color = (edit && active) ? ILI9341_YELLOW : (active ? ILI9341_GREEN : ILI9341_WHITE);
    tft.drawRect(boxX, boxY, boxW, boxH, color);
    tft.setTextColor(editMode && navIndex==0 ? ILI9341_YELLOW : ILI9341_WHITE, ILI9341_BLACK);
    tft.setTextSize(2);
    tft.setCursor(boxX + 8, boxY + 2);
    tft.printf("NO:%d", currentPreset+1);
  } else if (idx >= 1 && idx <= 20) {
    int i = idx - 1;
    int col = i / 4;
    int row = i % 4;
    int x = cellX[col];
    int y = cellY[row];

    // Perbaikan anti dobel garis!
    tft.fillRect(x-2, y-2, cellWidth+4, cellHeight+4, ILI9341_BLACK);
    if (!active) {
      tft.drawRect(x, y, cellWidth, cellHeight, ILI9341_WHITE);
    } else {
      uint16_t color = (col == stepAktif && motorRunning) ? ILI9341_GREEN :
        (active ? (edit ? ILI9341_GREEN : ILI9341_YELLOW) : ILI9341_WHITE);
      tft.drawRect(x-2, y-2, cellWidth+4, cellHeight+4, color);
      tft.drawRect(x, y, cellWidth, cellHeight, color);
    }
    tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
    tft.setTextSize(1);
    int labelLen = strlen(paramLabels[row]);
    int labelPixelWidth = labelLen * 6;
    int labelX = x + (cellWidth - labelPixelWidth) / 2;
    tft.setCursor(labelX, y + 2);
    tft.print(paramLabels[row]);
    drawParamValue(col, row, paramValue[col][row]);
  } else if (idx >= 21 && idx <= 24) {
    int btn = idx - 21;
    int x = buttonX[btn];
    int y = buttonY;
    // Perbaikan anti dobel garis!
    tft.fillRect(x-2, y-2, buttonWidth+4, buttonHeight+4, ILI9341_BLACK);
    tft.fillRect(x, y, buttonWidth, buttonHeight, ILI9341_BLACK);
    if (!active) {
      tft.drawRect(x, y, buttonWidth, buttonHeight, ILI9341_WHITE);
    } else {
      uint16_t color = active ? (edit ? ILI9341_GREEN : ILI9341_YELLOW) : ILI9341_WHITE;
      tft.drawRect(x-2, y-2, buttonWidth+4, buttonHeight+4, color);
      tft.drawRect(x, y, buttonWidth, buttonHeight, color);
    }
    tft.setTextColor(ILI9341_RED);
    tft.setTextSize(2);
    int textLen = strlen(buttons[btn]);
    int textPixelWidth = textLen * 12;
    int textX = x + (buttonWidth - textPixelWidth) / 2;
    int textY = y + (buttonHeight - 16) / 2;
    tft.setCursor(textX, textY);
    tft.print(buttons[btn]);
  }
}

void drawStaticUI() {
  drawStatusMotor();
  const char* stepTitles[] = {"STEP1", "STEP2", "STEP3", "STEP4", "STEP5"};
  for (int i = 0; i < 5; i++) {
    tft.setTextColor(ILI9341_YELLOW);
    tft.setTextSize(1);
    tft.setCursor(26 + i * 62, 40);
    tft.print(stepTitles[i]);
  }
  for (int col = 0; col < 5; col++) {
    for (int row = 0; row < 4; row++) {
      int x = cellX[col];
      int y = cellY[row];
      tft.fillRect(x-2, y-2, cellWidth+4, cellHeight+4, ILI9341_BLACK);
      uint16_t color = (col == stepAktif && motorRunning) ? ILI9341_GREEN : ILI9341_WHITE;
      tft.drawRect(x, y, cellWidth, cellHeight, color);
      tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      tft.setTextSize(1);
      int labelLen = strlen(paramLabels[row]);
      int labelPixelWidth = labelLen * 6;
      int labelX = x + (cellWidth - labelPixelWidth) / 2;
      tft.setCursor(labelX, y + 2);
      tft.print(paramLabels[row]);
      drawParamValue(col, row, paramValue[col][row]);
    }
  }
  for (int i = 0; i < 4; i++) {
    int x = buttonX[i];
    int y = buttonY;
    tft.fillRect(x-2, y-2, buttonWidth+4, buttonHeight+4, ILI9341_BLACK);
    tft.fillRect(x, y, buttonWidth, buttonHeight, ILI9341_BLACK);
    tft.drawRect(x, y, buttonWidth, buttonHeight, ILI9341_WHITE);
    tft.setTextColor(ILI9341_RED);
    tft.setTextSize(2);
    int textLen = strlen(buttons[i]);
    int textPixelWidth = textLen * 12;
    int textX = x + (buttonWidth - textPixelWidth) / 2;
    int textY = y + (buttonHeight - 16) / 2;
    tft.setCursor(textX, textY);
    tft.print(buttons[i]);
  }
  highlightCellOrButton(navIndex, true, editMode);
  drawPresetInfo();
}

void tampilkanDialogSimpanPreset() {
    tft.fillRect(60, 80, 200, 60, ILI9341_BLACK);
    tft.drawRect(60, 80, 200, 60, ILI9341_WHITE);
    tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
    tft.setTextSize(2);
    tft.setCursor(75, 95);
    tft.print("Simpan preset?");
    tft.setTextSize(2);
    tft.setTextColor(pilihYa ? ILI9341_YELLOW : 0x8410, ILI9341_BLACK);
    tft.setCursor(90, 120);
    tft.print("YA");
    tft.setTextColor(!pilihYa ? ILI9341_YELLOW : 0x8410, ILI9341_BLACK);
    tft.setCursor(180, 120);
    tft.print("TIDAK");
}

void drawRestStatus(bool aktif) {
    int x = 10;
    int y = buttonY + buttonHeight + 8;
    int w = 180;
    int h = 18;
    if (aktif) {
        tft.fillRect(x, y, w, h, ILI9341_BLACK);
        tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK);
        tft.setTextSize(2);
        tft.setCursor(x, y);
        tft.print("REST / ISTIRAHAT");
    } else {
        tft.fillRect(x, y, w, h, ILI9341_BLACK);
    }
}

void updateActiveValue(long diff) {
  if (navIndex >= 1 && navIndex <= 20) {
    int i = navIndex - 1;
    int col = i / 4;
    int row = i % 4;
    if (row == 0) {
      int menit, detik;
      sscanf(paramValue[col][row], "%2d:%2d", &menit, &detik);
      long total = menit * 60 + detik + diff * 5;
      if (total < 0) total = 0;
      if (total > 3599) total = 3599;
      menit = total / 60;
      detik = total % 60;
      sprintf(paramValue[col][row], "%02d:%02d", menit, detik);
    } else if (row == 1) {
      float v = atof(paramValue[col][row]);
      v += diff * 0.10;
      if (v < 0) v = 0;
      if (v > 12.00) v = 12.00; // Batas ke 12.00
      sprintf(paramValue[col][row], "%0.2f", v);
    } else if (row == 2) {
      int idx = 0;
      for (int i = 0; i < nDircModes; i++) {
        if (strcmp(paramValue[col][row], dircModes[i]) == 0) {
          idx = i;
          break;
        }
      }
      idx = (idx + diff + nDircModes) % nDircModes;
      strcpy(paramValue[col][row], dircModes[idx]);
    } else if (row == 3) {
      int menit, detik;
      sscanf(paramValue[col][row], "%2d'%2d", &menit, &detik);
      long total = menit * 60 + detik + diff * 10;
      if (total < 0) total = 0;
      if (total > 3599) total = 3599;
      menit = total / 60;
      detik = total % 60;
      sprintf(paramValue[col][row], "%02d'%02d", menit, detik);
    }
    drawParamValue(col, row, paramValue[col][row]);
  }
}

void motorStop() {
  digitalWrite(MOTOR_IN1, LOW);
  digitalWrite(MOTOR_IN2, LOW);
  analogWrite(MOTOR_ENA, 0);
}
void motorCW(uint8_t speed) {
  digitalWrite(MOTOR_IN1, HIGH);
  digitalWrite(MOTOR_IN2, LOW);
  analogWrite(MOTOR_ENA, speed);
}
void motorCCW(uint8_t speed) {
  digitalWrite(MOTOR_IN1, LOW);
  digitalWrite(MOTOR_IN2, HIGH);
  analogWrite(MOTOR_ENA, speed);
}

float bacaArusA() {
  int adc = analogRead(ACS_PIN);
  float Vout = adc * (5.0 / 1023.0);
  float Sensitivity = 0.185;
  float Zero = 2.5;
  float I = (Vout - Zero) / Sensitivity;
  if (fabs(I) < 0.08) I = 0.0;
  if (I < 0) I = -I;
  if (I > 5.0) I = 5.00;
  return I;
}

void jalankanModeDIRC(const char* mode) {
  static unsigned long t0 = 0;
  static int substate = 0;
  unsigned long now = millis();
  motorStatus = MOTOR_RUN;

  float volt_set = atof(paramValue[stepAktif][1]);
  if (volt_set < 0) volt_set = 0;
  if (volt_set > 12.0) volt_set = 12.0; // Batas ke 12.0
  int pwm_value = (volt_set / 12.0) * 255; // Rasio ke 12V
  if (pwm_value > 255) pwm_value = 255;
  if (pwm_value < 0) pwm_value = 0;

  mode = paramValue[stepAktif][2];

  if (strcmp(mode, "N/a") == 0) {
    motorStop();
    substate = 0;
    return;
  }
  else if (strcmp(mode, "FF.") == 0) {
    motorCW(pwm_value);
    substate = 0;
  }
  else if (strcmp(mode, "SW") == 0) {
    motorCCW(pwm_value);
    substate = 0;
  }
  else if (strcmp(mode, "AUTO") == 0) {
    switch (substate) {
      case 0:
        motorCW(pwm_value);
        if (now - t0 > 10000) { t0 = now; substate = 1; }
        break;
      case 1:
        motorStop();
        if (now - t0 > 2000) { t0 = now; substate = 2; }
        break;
      case 2:
        motorCCW(pwm_value);
        if (now - t0 > 10000) { t0 = now; substate = 3; }
        break;
      case 3:
        motorStop();
        if (now - t0 > 2000) { t0 = now; substate = 0; }
        break;
    }
  }
 else if (strcmp(mode, "F/P") == 0) {
  switch (substate) {
    case 0: {
      int pwm_min = (1.0 / 12.0) * 255;  // PWM untuk 1 volt, sesuai 12V
      int ramp_pwm = map(now - t0, 0, 5000, pwm_min, pwm_value);
      if ((now - t0) >= 5000) {
        ramp_pwm = pwm_value;
        t0 = now;
        substate = 1;
      }
      if (ramp_pwm > pwm_value) ramp_pwm = pwm_value;
      if (ramp_pwm < pwm_min) ramp_pwm = pwm_min;
      motorCW(ramp_pwm);
      break;
    }
    case 1:
      motorStop();
      if (now - t0 > 2000) {
        t0 = now;
        substate = 0;
      }
      break;
  }
}
else if (strcmp(mode, "S/P") == 0) {
  switch (substate) {
    case 0: {
      int pwm_min = (1.0 / 12.0) * 255;  // PWM untuk 1 volt, sesuai 12V
      int ramp_pwm = map(now - t0, 0, 5000, pwm_min, pwm_value);
      if ((now - t0) >= 5000) {
        ramp_pwm = pwm_value;
        t0 = now;
        substate = 1;
      }
      if (ramp_pwm > pwm_value) ramp_pwm = pwm_value;
      if (ramp_pwm < pwm_min) ramp_pwm = pwm_min;
      motorCCW(ramp_pwm);
      break;
    }
    case 1:
      motorStop();
      if (now - t0 > 2000) {
        t0 = now;
        substate = 0;
      }
      break;
  }
}

}

long getStepWaktu(int step) {
  int menit, detik;
  sscanf(paramValue[step][0], "%2d:%2d", &menit, &detik);
  return menit*60 + detik;
}
long getStepRest(int step) {
  int menit, detik;
  sscanf(paramValue[step][3], "%2d'%2d", &menit, &detik);
  return menit*60 + detik;
}
bool stepValid(int step) {
  long waktu = getStepWaktu(step);
  float volt = atof(paramValue[step][1]);
  const char* dirc = paramValue[step][2];
  return (waktu > 0) && (volt > 0.0) && (strcmp(dirc, "N/a") != 0);
}

void mulaiDariStep0() {
  stepAktif = 0;
  sedangRest = false;
  while(stepAktif < totalStep && !stepValid(stepAktif)) stepAktif++;
  if(stepAktif < totalStep) {
    stepSisaDetik = getStepWaktu(stepAktif);
    stepMulaiMillis = millis();
    motorRunning = true;
    motorStatus = MOTOR_RUN;
    flagJalankanMotor = true;
    sedangRest = false;
    drawStaticUI();
  } else {
    motorRunning = false;
    motorStatus = MOTOR_STANDBY;
    drawStaticUI();
    navIndexLast = -1;
    editModeLast = false;
  }
}

// ===== POLA ANTI DOBEL GARIS: update highlight di setiap navIndex/editMode berubah =====
void updateHighlight() {
  if(navIndex != navIndexLast || editMode != editModeLast) {
    if(navIndexLast >= 0)
      highlightCellOrButton(navIndexLast, false, editModeLast);
    highlightCellOrButton(navIndex, true, editMode);
    navIndexLast = navIndex;
    editModeLast = editMode;
  }
}

// ========== ENCODER DAN NAVIGASI ==========
void updateEncoder() {
  int clkState = digitalRead(ENCODER_CLK);
  int dtState  = digitalRead(ENCODER_DT);
  static int lastClkTmp = HIGH;
  static bool lastSw = HIGH;
  bool swState = digitalRead(ENCODER_SW);

  if (dialogSimpanPreset) {
    if (clkState != lastClkTmp && clkState == LOW) {
        if (dtState != clkState) pilihYa = true;
        else pilihYa = false;
        tampilkanDialogSimpanPreset();
    }
    if (lastSw == HIGH && swState == LOW) {
        dialogSimpanPreset = false;
        tft.fillRect(60, 80, 200, 60, ILI9341_BLACK);
        if (pilihYa) {
            savePreset(currentPreset);
            saveAllPresetsToEEPROM();
            tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK);
            tft.setTextSize(1);
            tft.setCursor(200, 30);
            tft.print("Preset tersimpan!");
            delay(500);
            tft.fillRect(200, 30, 120, 12, ILI9341_BLACK);
        }
        mulaiDariStep0();
    }
    lastClkTmp = clkState;
    lastSw = swState;
    return;
  }

  if (clkState != lastClkTmp && clkState == LOW) {
    long diff = (dtState != clkState) ? 1 : -1;
    if(editMode) {
      if(navIndex == 0) {
        int prevPreset = currentPreset;
        currentPreset += diff;
        if(currentPreset < 0) currentPreset = NUM_PRESET-1;
        if(currentPreset >= NUM_PRESET) currentPreset = 0;
        if(prevPreset != currentPreset) {
          loadPreset(currentPreset);
          drawStaticUI();
        }
      } else if(navIndex >= 1 && navIndex <= 20) {
        updateActiveValue(diff);
      }
    } else {
      navIndex += diff;
      if (navIndex < 0) navIndex = NAV_TOTAL-1;
      if (navIndex >= NAV_TOTAL) navIndex = 0;
    }
  }
  lastClkTmp = clkState;

  // --- INI BAGIAN BENAHAN TOMBOL! ---
  if (lastSw == HIGH && swState == LOW) {
    // Jika highlight di tombol, LANGSUNG aksi tanpa masuk edit mode!
    if (navIndex >= 21 && navIndex <= 24) {
        if (navIndex == 21 && !dialogSimpanPreset) { // STAR
            if (isPaused && motorStatus == MOTOR_PAUSE) {
                motorStatus = MOTOR_RUN;
                isPaused = false;
                if (!sedangRest) {
                    stepSisaDetik = pauseSisaDetik;
                    stepMulaiMillis = millis();
                } else {
                    restSisaDetik = pauseSisaDetik;
                    restMulaiMillis = millis();
                }
                motorRunning = true;
                drawStatusMotor();
            } else if (!motorRunning) {
                dialogSimpanPreset = true;
                pilihYa = true;
                tampilkanDialogSimpanPreset();
            }
        }
        else if (navIndex == 22) { // PAUSE
            if (motorRunning && !isPaused) {
                motorStatus = MOTOR_PAUSE;
                motorStop();
                isPaused = true;
                pauseMillis = millis();
                if (!sedangRest) {
                    pauseSisaDetik = stepSisaDetik - ((millis() - stepMulaiMillis) / 1000);
                } else {
                    pauseSisaDetik = restSisaDetik - ((millis() - restMulaiMillis) / 1000);
                }
                drawStatusMotor();
            }
        }
        else if (navIndex == 23) { // STOP
            motorStatus = MOTOR_STOP;
            motorRunning = false;
            sedangRest = false;
            motorStop();
            isPaused = false;
            drawStatusMotor();
        }
        // Tombol MENU (24) bisa Anda isi sendiri jika ingin fungsi khusus
        updateHighlight();
        lastSw = swState;
        return; // JANGAN MASUK MODE EDIT
    }
    // Jika highlight di cell parameter, baru toggle edit mode
    editMode = !editMode;
  }
  lastSw = swState;

  updateHighlight();
}

void setup() {
  tft.begin();
  tft.setRotation(1);
  tft.fillScreen(ILI9341_BLACK);
  EEPROM.begin(EEPROM_SIZE);
  if (EEPROM.read(0) == 0xFF) {
    initPresetsDefault();
    saveAllPresetsToEEPROM();
  } else {
    loadAllPresetsFromEEPROM();
  }
  loadPreset(currentPreset);
  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);
  pinMode(ENCODER_SW, INPUT_PULLUP);
  pinMode(MOTOR_IN1, OUTPUT);
  pinMode(MOTOR_IN2, OUTPUT);
  pinMode(MOTOR_ENA, OUTPUT);
  pinMode(ACS_PIN, INPUT);
  Serial.begin(115200);
  drawStaticUI();
  drawPresetInfo();
}

void loop() {
  updateEncoder();
  updateHighlight();

  if(flagJalankanMotor) {
    flagJalankanMotor = false;
    motorStatus = MOTOR_RUN;
    motorRunning = true;
    isPaused = false;
    drawStatusMotor();
  }

  if(isPaused && motorStatus == MOTOR_PAUSE) {
    if(!sedangRest) {
      tampilkanCountdownDanArus(pauseSisaDetik, false);
    } else {
      tampilkanCountdownDanArus(pauseSisaDetik, true);
    }
    delay(2);
    return;
  }

  if(motorRunning && (motorStatus == MOTOR_RUN || motorStatus == MOTOR_REST)) {
    unsigned long now = millis();
    if(!sedangRest) {
      if (motorStatus == MOTOR_REST) {
        motorStatus = MOTOR_RUN;
        drawStatusMotor();
      }
      drawRestStatus(false);
      long elapsed = (now - stepMulaiMillis) / 1000;
      long sisa = stepSisaDetik - elapsed;
      if(sisa < 0) sisa = 0;
      tampilkanCountdownDanArus(sisa, false);
      jalankanModeDIRC(paramValue[stepAktif][2]);
      if(sisa == 0) {
        motorStop();
        long restDuration = getStepRest(stepAktif);
        if(restDuration > 0) {
          sedangRest = true;
          restSisaDetik = restDuration;
          restMulaiMillis = millis();
        } else {
          sedangRest = false;
          stepAktif++;
          while(stepAktif < totalStep && !stepValid(stepAktif)) stepAktif++;
          if(stepAktif >= totalStep || !stepValid(stepAktif)) {
            motorStop();
            motorStatus = MOTOR_STOP;
            motorRunning = false;
            sedangRest = false;
            stepAktif = 0;
            isPaused = false;
            drawStaticUI();
            drawStatusMotor();
            lastWaktuStr[0]=0; lastArusStr[0]=0;
            stopTampilMillis = millis();
            showStopThenStandby = true;
          } else {
            stepSisaDetik = getStepWaktu(stepAktif);
            stepMulaiMillis = millis();
            drawStaticUI();
            lastWaktuStr[0]=0; lastArusStr[0]=0;
          }
        }
      }
    } else {
      if (motorStatus != MOTOR_REST) {
        motorStatus = MOTOR_REST;
        drawStatusMotor();
      }
      drawRestStatus(true);
      long elapsed = (now - restMulaiMillis) / 1000;
      long sisa = restSisaDetik - elapsed;
      if(sisa < 0) sisa = 0;
      tampilkanCountdownDanArus(sisa, true);
      if(sisa == 0) {
        sedangRest = false;
        drawRestStatus(false);
        if (motorStatus == MOTOR_REST) {
          motorStatus = MOTOR_RUN;
          drawStatusMotor();
        }
        stepAktif++;
        while(stepAktif < totalStep && !stepValid(stepAktif)) stepAktif++;
        if(stepAktif >= totalStep || !stepValid(stepAktif)) {
          motorStop();
          motorStatus = MOTOR_STOP;
          motorRunning = false;
          sedangRest = false;
          isPaused = false;
          stepAktif = 0;
          drawStaticUI();
          drawStatusMotor();
          lastWaktuStr[0]=0; lastArusStr[0]=0;
          stopTampilMillis = millis();
          showStopThenStandby = true;
        } else {
          stepSisaDetik = getStepWaktu(stepAktif);
          stepMulaiMillis = millis();
          drawStaticUI();
          lastWaktuStr[0]=0; lastArusStr[0]=0;
        }
      }
    }
  } else {
    drawRestStatus(false);
  }

  if(showStopThenStandby) {
    if(millis() - stopTampilMillis > 2000) {
      motorStatus = MOTOR_STANDBY;
      drawStatusMotor();
      showStopThenStandby = false;
      navIndexLast = -1;
      editModeLast = false;
      isPaused = false;
      drawStaticUI();
    }
  }

  delay(2);
}