#include <U8g2lib.h>
#include <Wire.h>
#include <EEPROM.h>

// ============================================
// PIN DEFINITIONS
// ============================================
#define HEATER_PIN        A1         // PA1 - TIM2_CH2
#define THERMOCOUPLE_PIN  PA0        // ADC1_IN0
#define VDC_PIN           PB1        // ADC1_IN9
#define UP_BUTTON_PIN     PB3
#define DOWN_BUTTON_PIN   PB4
#define SELECT_BUTTON_PIN PB5
#define IDLE_SW_PIN       PB8

#define SCREEN_WIDTH  128
#define SCREEN_HEIGHT 64
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE, PB6, PB7);

// ============================================
// CONSTANTS
// ============================================
#define ADC_MAX              4095
#define TEMP_MIN             100
#define TEMP_MAX             400
#define TEMP_STEP            10
#define TIME_STEP            10
#define VOLTAGE_OFFSET_STEP  100
#define VOLTAGE_OFFSET_MIN  -1200
#define VOLTAGE_OFFSET_MAX   1200
#define EEPROM_ADDR          0
#define HEATER_MAX_PERIOD    255
#define V_REF                3300   // mV
#define MENU_TIMEOUT         10000  // ms
#define EEPROM_SAVE_DELAY    5000   // ms

#define THERMOCOUPLE_GAIN    100
#define MV_PER_C_NT115       0.041
#define MV_PER_C_C210        0.040

#define R1 10000
#define R2 1000
#define VOLTAGE_DIVIDER      ((R1 + R2) / (float)R2)

// ============================================
// ENUMS & STRUCTURES
// ============================================
enum MenuState {
  MENU_OFF,
  MENU_MAIN,
  MENU_HANDLE,
  MENU_RAW,
  MENU_TEMP,
  MENU_TEMP_STARTUP,
  MENU_TEMP_OFFSET,
  MENU_TEMP_STANDBY,
  MENU_PID,                 // New: PID Tuning main menu
  MENU_PID_NT115,           // NT115 PID submenu
  MENU_PID_NT115_KP,
  MENU_PID_NT115_KI,
  MENU_PID_NT115_KD,
  MENU_PID_C210,            // C210 PID submenu
  MENU_PID_C210_KP,
  MENU_PID_C210_KI,
  MENU_PID_C210_KD,
  MENU_VOLTAGE,
  MENU_VOLTAGE_FIX,
  MENU_TIMERS,
  MENU_TIMERS_STANDBY,
  MENU_TIMERS_SLEEP
};

enum HandleType {
  HANDLE_NT115,
  HANDLE_C210
};

struct Settings {
  int      targetTemp;
  int      standbyTemp;
  int      tempOffset;
  int      voltageOffset;
  uint16_t standbyTime;
  uint16_t sleepTime;
  HandleType handleType;

  float    kp_nt115;
  float    ki_nt115;
  float    kd_nt115;

  float    kp_c210;
  float    ki_c210;
  float    kd_c210;
};

// ============================================
// GLOBALS
// ============================================
Settings settings = {300, 150, 0, 0, 5, 10, HANDLE_NT115,
                     5.0f, 2.0f, 0.3f,     // NT115 defaults (AxxSolder-inspired)
                     7.0f, 4.0f, 0.3f};   // C210 defaults (AxxSolder-inspired)

int      currentTemp1000     = 0;
uint8_t  currentDuty         = 0;
uint8_t  currentDutySmoothed = 0;
bool     heaterOn            = false;
bool     idleState           = false;

unsigned long lastButtonTime   = 0;
unsigned long lastIdleTime     = 0;
unsigned long lastActivityTime = 0;
unsigned long lastEepromSave   = 0;

const unsigned long debounceDelay = 50;
unsigned long selectPressTime = 0;
bool selectPressed = false;

MenuState menuState = MENU_OFF;
int menuIndex    = 0;
int submenuIndex = 0;

float integral  = 0;
float lastError = 0;

bool lastUpButtonState     = HIGH;
bool lastDownButtonState   = HIGH;
bool lastSelectButtonState = HIGH;
bool lastIdleState         = HIGH;

// ============================================
// SETUP
// ============================================
void setup() {
  RCC->APB2ENR |= RCC_APB2ENR_AFIOEN;
  AFIO->MAPR   |= AFIO_MAPR_SWJ_CFG_JTAGDISABLE;

  pinMode(HEATER_PIN,       OUTPUT);
  pinMode(THERMOCOUPLE_PIN, INPUT);
  pinMode(VDC_PIN,          INPUT);
  pinMode(UP_BUTTON_PIN,    INPUT_PULLUP);
  pinMode(DOWN_BUTTON_PIN,  INPUT_PULLUP);
  pinMode(SELECT_BUTTON_PIN,INPUT_PULLUP);
  pinMode(IDLE_SW_PIN,      INPUT_PULLUP);

  analogReadResolution(12);

  Wire.setSDA(PB7);
  Wire.setSCL(PB6);
  Wire.begin();

  u8g2.begin();
  analogWriteFrequency(20000);

  EEPROM.get(EEPROM_ADDR, settings);
  if (settings.targetTemp < TEMP_MIN || settings.targetTemp > TEMP_MAX ||
      settings.voltageOffset < VOLTAGE_OFFSET_MIN || settings.voltageOffset > VOLTAGE_OFFSET_MAX ||
      settings.standbyTime > 60 || settings.sleepTime > 120 ||
      settings.kp_nt115 < 0.1f || settings.kp_nt115 > 100.0f ||
      settings.kp_c210 < 0.1f || settings.kp_c210 > 100.0f) {
    settings = {300, 150, 0, 0, 5, 10, HANDLE_NT115,
                5.0f, 2.0f, 0.3f,
                7.0f, 4.0f, 0.3f};
    EEPROM.put(EEPROM_ADDR, settings);
  }

  lastActivityTime = millis();
  lastEepromSave   = millis();
}

// ============================================
// MAIN LOOP
// ============================================
void loop() {
  static unsigned long lastPID = 0;
  unsigned long now = millis();

  handleButtons();
  readIdleSwitch();
  handleTimers(now);

  if (menuState == MENU_MAIN && now - lastActivityTime > MENU_TIMEOUT) {
    menuState = MENU_OFF;
  }

  currentTemp1000 = readThermocouple();
  uint32_t vdc = readVoltage();

  if (now - lastPID >= 100) {
    lastPID = now;
    if (!idleState && heaterOn) {
      controlHeater();
    } else {
      analogWrite(HEATER_PIN, 0);
      currentDuty = 0;
      integral = 0;
    }
  }

  currentDutySmoothed = (currentDutySmoothed * 4 + currentDuty) / 5;

  updateDisplay(vdc);
  delay(20);
}

// ============================================
// TIMERS & IDLE
// ============================================
void handleTimers(unsigned long now) {
  if (!idleState) {
    lastActivityTime = now;
    return;
  }
  if (!heaterOn) return;

  if (now - lastActivityTime > settings.standbyTime * 60000UL) {
    if (settings.targetTemp != settings.standbyTemp) {
      settings.targetTemp = settings.standbyTemp;
      scheduleEepromSave();
    }
  }
  // Sleep still commented
  //if (now - lastActivityTime > settings.sleepTime * 60000UL) {
  //  heaterOn = false;
  //}
}

void readIdleSwitch() {
  bool current = digitalRead(IDLE_SW_PIN);
  unsigned long now = millis();
  if (current != lastIdleState && now - lastIdleTime > debounceDelay) {
    idleState = !current;
    lastIdleTime = now;
    lastActivityTime = now;
  }
  lastIdleState = current;
}

// ============================================
// BUTTON HANDLING
// ============================================
void handleButtons() {
  bool up = digitalRead(UP_BUTTON_PIN);
  bool down = digitalRead(DOWN_BUTTON_PIN);
  bool select = digitalRead(SELECT_BUTTON_PIN);
  unsigned long now = millis();

  if (up != lastUpButtonState && now - lastButtonTime > debounceDelay) {
    if (up == LOW) {
      handleUpButton();
      lastActivityTime = now;
    }
    lastButtonTime = now;
  }

  if (down != lastDownButtonState && now - lastButtonTime > debounceDelay) {
    if (down == LOW) {
      handleDownButton();
      lastActivityTime = now;
    }
    lastButtonTime = now;
  }

  if (select != lastSelectButtonState && now - lastButtonTime > debounceDelay) {
    if (select == LOW) {
      selectPressTime = now;
      selectPressed = true;
    } else {
      if (selectPressed) {
        if (now - selectPressTime < 3000) {
          handleSelectShortPress();
          lastActivityTime = now;
        }
        selectPressed = false;
      }
    }
    lastButtonTime = now;
  }

  if (selectPressed && now - selectPressTime >= 3000 && menuState == MENU_OFF) {
    menuState = MENU_MAIN;
    menuIndex = 0;
    lastActivityTime = now;
    selectPressed = false;
  }

  lastUpButtonState     = up;
  lastDownButtonState   = down;
  lastSelectButtonState = select;
}

// ============================================
// BUTTON ACTIONS
// ============================================
void handleUpButton() {
  switch (menuState) {
    case MENU_OFF:          settings.targetTemp = min(settings.targetTemp + TEMP_STEP, TEMP_MAX); scheduleEepromSave(); break;
    case MENU_MAIN:         menuIndex = max(menuIndex - 1, 0); break;
    case MENU_HANDLE:       settings.handleType = (settings.handleType == HANDLE_NT115) ? HANDLE_C210 : HANDLE_NT115; scheduleEepromSave(); break;
    case MENU_TEMP:         submenuIndex = max(submenuIndex - 1, 0); break;
    case MENU_PID:          submenuIndex = max(submenuIndex - 1, 0); break;
    case MENU_VOLTAGE:      submenuIndex = max(submenuIndex - 1, 0); break;
    case MENU_TIMERS:       submenuIndex = max(submenuIndex - 1, 0); break;

    case MENU_TEMP_STARTUP:   settings.targetTemp  = min(settings.targetTemp  + TEMP_STEP, TEMP_MAX); scheduleEepromSave(); break;
    case MENU_TEMP_OFFSET:    settings.tempOffset  = min(settings.tempOffset  + 1, 50); scheduleEepromSave(); break;
    case MENU_TEMP_STANDBY:   settings.standbyTemp = min(settings.standbyTemp + TEMP_STEP, TEMP_MAX); scheduleEepromSave(); break;
    case MENU_VOLTAGE_FIX:    settings.voltageOffset = min(settings.voltageOffset + VOLTAGE_OFFSET_STEP, VOLTAGE_OFFSET_MAX); scheduleEepromSave(); break;
    case MENU_TIMERS_STANDBY: settings.standbyTime = min(settings.standbyTime + TIME_STEP, 30); scheduleEepromSave(); break;
    case MENU_TIMERS_SLEEP:   settings.sleepTime   = min(settings.sleepTime   + TIME_STEP, 60); scheduleEepromSave(); break;

    // NT115 PID
    case MENU_PID_NT115_KP: settings.kp_nt115 = min(settings.kp_nt115 + 0.1f, 100.0f); scheduleEepromSave(); break;
    case MENU_PID_NT115_KI: settings.ki_nt115 = min(settings.ki_nt115 + 0.1f, 50.0f);  scheduleEepromSave(); break;
    case MENU_PID_NT115_KD: settings.kd_nt115 = min(settings.kd_nt115 + 0.1f, 50.0f);  scheduleEepromSave(); break;

    // C210 PID
    case MENU_PID_C210_KP:  settings.kp_c210  = min(settings.kp_c210  + 0.1f, 100.0f); scheduleEepromSave(); break;
    case MENU_PID_C210_KI:  settings.ki_c210  = min(settings.ki_c210  + 0.1f, 50.0f);  scheduleEepromSave(); break;
    case MENU_PID_C210_KD:  settings.kd_c210  = min(settings.kd_c210  + 0.1f, 50.0f);  scheduleEepromSave(); break;
  }
}

void handleDownButton() {
  switch (menuState) {
    case MENU_OFF:          settings.targetTemp = max(settings.targetTemp - TEMP_STEP, TEMP_MIN); scheduleEepromSave(); break;
    case MENU_MAIN:         menuIndex = min(menuIndex + 1, 6); break;  // 0..6 now
    case MENU_HANDLE:       settings.handleType = (settings.handleType == HANDLE_NT115) ? HANDLE_C210 : HANDLE_NT115; scheduleEepromSave(); break;
    case MENU_TEMP:         submenuIndex = min(submenuIndex + 1, 3); break;
    case MENU_PID:          submenuIndex = min(submenuIndex + 1, 2); break;  // NT115, C210, Back
    case MENU_VOLTAGE:      submenuIndex = min(submenuIndex + 1, 1); break;
    case MENU_TIMERS:       submenuIndex = min(submenuIndex + 1, 2); break;

    case MENU_TEMP_STARTUP:   settings.targetTemp  = max(settings.targetTemp  - TEMP_STEP, TEMP_MIN); scheduleEepromSave(); break;
    case MENU_TEMP_OFFSET:    settings.tempOffset  = max(settings.tempOffset  - 1, -50); scheduleEepromSave(); break;
    case MENU_TEMP_STANDBY:   settings.standbyTemp = max(settings.standbyTemp - TEMP_STEP, TEMP_MIN); scheduleEepromSave(); break;
    case MENU_VOLTAGE_FIX:    settings.voltageOffset = max(settings.voltageOffset - VOLTAGE_OFFSET_STEP, VOLTAGE_OFFSET_MIN); scheduleEepromSave(); break;
    case MENU_TIMERS_STANDBY: settings.standbyTime = max(settings.standbyTime - TIME_STEP, 1); scheduleEepromSave(); break;
    case MENU_TIMERS_SLEEP:   settings.sleepTime   = max(settings.sleepTime   - TIME_STEP, 1); scheduleEepromSave(); break;

    // NT115 PID
    case MENU_PID_NT115_KP: settings.kp_nt115 = max(settings.kp_nt115 - 0.1f, 0.1f); scheduleEepromSave(); break;
    case MENU_PID_NT115_KI: settings.ki_nt115 = max(settings.ki_nt115 - 0.1f, 0.0f); scheduleEepromSave(); break;
    case MENU_PID_NT115_KD: settings.kd_nt115 = max(settings.kd_nt115 - 0.1f, 0.0f); scheduleEepromSave(); break;

    // C210 PID
    case MENU_PID_C210_KP:  settings.kp_c210  = max(settings.kp_c210  - 0.1f, 0.1f); scheduleEepromSave(); break;
    case MENU_PID_C210_KI:  settings.ki_c210  = max(settings.ki_c210  - 0.1f, 0.0f); scheduleEepromSave(); break;
    case MENU_PID_C210_KD:  settings.kd_c210  = max(settings.kd_c210  - 0.1f, 0.0f); scheduleEepromSave(); break;
  }
}

void handleSelectShortPress() {
  switch (menuState) {
    case MENU_OFF:
      heaterOn = !heaterOn;
      break;

    case MENU_MAIN:
      switch (menuIndex) {
        case 0: menuState = MENU_OFF;      break;
        case 1: menuState = MENU_HANDLE;   break;
        case 2: menuState = MENU_RAW;      break;
        case 3: menuState = MENU_TEMP;     submenuIndex = 0; break;
        case 4: menuState = MENU_PID;      submenuIndex = 0; break;
        case 5: menuState = MENU_VOLTAGE;  submenuIndex = 0; break;
        case 6: menuState = MENU_TIMERS;   submenuIndex = 0; break;
      }
      break;

    case MENU_PID:
      switch (submenuIndex) {
        case 0: menuState = MENU_PID_NT115; submenuIndex = 0; break;
        case 1: menuState = MENU_PID_C210;  submenuIndex = 0; break;
        case 2: menuState = MENU_MAIN;      break;
      }
      break;

    case MENU_PID_NT115:
      switch (submenuIndex) {
        case 0: menuState = MENU_PID_NT115_KP; break;
        case 1: menuState = MENU_PID_NT115_KI; break;
        case 2: menuState = MENU_PID_NT115_KD; break;
        case 3: menuState = MENU_PID;          break;
      }
      break;

    case MENU_PID_C210:
      switch (submenuIndex) {
        case 0: menuState = MENU_PID_C210_KP; break;
        case 1: menuState = MENU_PID_C210_KI; break;
        case 2: menuState = MENU_PID_C210_KD; break;
        case 3: menuState = MENU_PID;         break;
      }
      break;

    case MENU_PID_NT115_KP:
    case MENU_PID_NT115_KI:
    case MENU_PID_NT115_KD:
    case MENU_PID_C210_KP:
    case MENU_PID_C210_KI:
    case MENU_PID_C210_KD:
      menuState = (menuState <= MENU_PID_NT115_KD) ? MENU_PID_NT115 : MENU_PID_C210;
      saveEepromNow();
      break;

    case MENU_TEMP:
      switch (submenuIndex) {
        case 0: menuState = MENU_TEMP_STARTUP; break;
        case 1: menuState = MENU_TEMP_OFFSET; break;
        case 2: menuState = MENU_TEMP_STANDBY; break;
        case 3: menuState = MENU_MAIN; break;
      }
      break;

    case MENU_VOLTAGE:
      switch (submenuIndex) {
        case 0: menuState = MENU_VOLTAGE_FIX; break;
        case 1: menuState = MENU_MAIN; break;
      }
      break;

    case MENU_TIMERS:
      switch (submenuIndex) {
        case 0: menuState = MENU_TIMERS_STANDBY; break;
        case 1: menuState = MENU_TIMERS_SLEEP; break;
        case 2: menuState = MENU_MAIN; break;
      }
      break;

    case MENU_RAW:
    case MENU_HANDLE:
      menuState = MENU_MAIN;
      break;

    case MENU_TEMP_STARTUP:
    case MENU_TEMP_OFFSET:
    case MENU_TEMP_STANDBY:
      menuState = MENU_TEMP;
      saveEepromNow();
      break;

    case MENU_VOLTAGE_FIX:
      menuState = MENU_VOLTAGE;
      saveEepromNow();
      break;

    case MENU_TIMERS_STANDBY:
    case MENU_TIMERS_SLEEP:
      menuState = MENU_TIMERS;
      saveEepromNow();
      break;
  }
}

// ============================================
// EEPROM
// ============================================
void scheduleEepromSave() {
  lastEepromSave = millis() - EEPROM_SAVE_DELAY + 1000;
}

void saveEepromNow() {
  EEPROM.put(EEPROM_ADDR, settings);
  lastEepromSave = millis();
}

// ============================================
// SENSORS
// ============================================
int readThermocouple() {
  uint32_t sum = 0;
  for (uint8_t i = 0; i < 16; i++) sum += analogRead(THERMOCOUPLE_PIN);
  int adc = sum >> 4;

  float voltage = adc * V_REF / (float)ADC_MAX;
  float tc_mv   = voltage / THERMOCOUPLE_GAIN;

  float mv_per_c = (settings.handleType == HANDLE_NT115) ? MV_PER_C_NT115 : MV_PER_C_C210;
  int temp = (int)((tc_mv / mv_per_c) * 1000.0f);

  temp += 25000 + settings.tempOffset * 1000;
  return temp;
}

uint32_t readVoltage() {
  uint32_t sum = 0;
  for (uint8_t i = 0; i < 16; i++) sum += analogRead(VDC_PIN);
  uint32_t adc = sum >> 4;

  uint64_t v = ((uint64_t)adc * V_REF * VOLTAGE_DIVIDER) / ADC_MAX;
  int32_t voltage = (int32_t)v + settings.voltageOffset;
  return (voltage > 0) ? voltage : 0;
}

// ============================================
// HEATER CONTROL
// ============================================
void controlHeater() {
  float setpoint = settings.targetTemp;
  float temp     = currentTemp1000 / 1000.0f;
  float error    = setpoint - temp;

  integral += error * 0.1f;
  integral = constrain(integral, -500, 500);

  float derivative = (error - lastError) / 0.1f;
  lastError = error;

  float kp, ki, kd;
  if (settings.handleType == HANDLE_NT115) {
    kp = settings.kp_nt115;
    ki = settings.ki_nt115;
    kd = settings.kd_nt115;
  } else {
    kp = settings.kp_c210;
    ki = settings.ki_c210;
    kd = settings.kd_c210;
  }

  float output = kp * error + ki * integral + kd * derivative;
  currentDuty = constrain((int)output, 0, HEATER_MAX_PERIOD);

  if (idleState) {
    currentDuty = min(currentDuty, uint8_t(HEATER_MAX_PERIOD / 3));
  }

  analogWrite(HEATER_PIN, currentDuty);
}

// ============================================
// DISPLAY
// ============================================
void updateDisplay(uint32_t vdc) {
  if (menuState == MENU_OFF) {
    display_home(vdc);
  } else if (menuState == MENU_RAW) {
    display_raw();
  } else {
    display_menu();
  }
}

void display_home(uint32_t vdc) {
  int temp_c = currentTemp1000 / 1000;
  uint16_t temp_round = ((temp_c + 5) / 10) * 10;

  uint16_t vdc_volts = vdc / 1000;
  uint16_t vdc_decimal = (vdc % 1000) / 100;

  u8g2.clearBuffer();

  u8g2.setFont(u8g2_font_logisoso26_tf);
  char disp_buf[4];
  snprintf(disp_buf, sizeof(disp_buf), "%03d", temp_round);
  u8g2.setDrawColor(1);
  u8g2.drawStr(0, 29, disp_buf);
  u8g2.drawCircle(54, 10, 3);
  u8g2.drawStr(58, 29, "C");

  u8g2.setFont(u8g2_font_6x10_tf);
  snprintf(disp_buf, sizeof(disp_buf), "%3d", settings.targetTemp);
  u8g2.drawStr(90, 13, disp_buf);
  u8g2.drawCircle(117, 5, 1);
  u8g2.drawStr(121, 13, "C");

  char vdc_buf[8];
  snprintf(vdc_buf, sizeof(vdc_buf), "V:%u.%u", vdc_volts, vdc_decimal);
  u8g2.drawStr(90, 42, vdc_buf);

  char pwr_buf[12];
  uint8_t percent = (currentDutySmoothed * 100UL) / HEATER_MAX_PERIOD;
  snprintf(pwr_buf, sizeof(pwr_buf), "PWR:%3d%%", percent);
  u8g2.drawStr(90, 55, pwr_buf);

  const char* handle = (settings.handleType == HANDLE_NT115) ? "NT115" : "C210";
  u8g2.setDrawColor(1);
  u8g2.drawBox(90, 46, 40, 10);
  u8g2.setDrawColor(0);
  u8g2.drawStr(94, 54, handle);

  u8g2.setDrawColor(1);
  if (!heaterOn) {
    u8g2.drawStr(128 - 3*6, 30, "OFF");
  } else if (idleState) {
    u8g2.drawStr(128 - 4*6, 30, "STBY");
  } else {
    u8g2.drawStr(128 - 2*6, 30, "ON");
  }

  uint16_t pwrline = currentDutySmoothed * SCREEN_WIDTH / HEATER_MAX_PERIOD;
  if (pwrline > 0) u8g2.drawHLine(0, 62, pwrline);

  u8g2.sendBuffer();
}

void display_raw() {
  uint16_t adc_tc  = analogRead(THERMOCOUPLE_PIN);
  uint16_t adc_vdc = analogRead(VDC_PIN);
  uint32_t v33 = V_REF;
  uint32_t vdc = (uint32_t)((adc_vdc * V_REF * VOLTAGE_DIVIDER) / ADC_MAX);
  uint32_t vtc = (uint32_t)((adc_tc * ((uint32_t)V_REF * 1000) / THERMOCOUPLE_GAIN) / ADC_MAX);

  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_6x10_tf);
  char buff[32];

  snprintf(buff, sizeof(buff), "V33 %4lu mV", v33);   u8g2.drawStr(0, 10, buff);
  snprintf(buff, sizeof(buff), "VDC %4lu mV", vdc);   u8g2.drawStr(0, 20, buff);
  snprintf(buff, sizeof(buff), "TC  %4lu uV", vtc);   u8g2.drawStr(0, 30, buff);

  uint16_t pwr = currentDutySmoothed * 100 / HEATER_MAX_PERIOD;
  snprintf(buff, sizeof(buff), "P %u%%", pwr);
  u8g2.drawStr(86, 10, buff);

  u8g2.sendBuffer();
}

void display_menu() {
  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_helvR08_tr);
  u8g2.setDrawColor(1);

  const char* mainItems[] = {"Home", "Handle Type", "Raw Data", "Temperature", "PID Tuning", "Voltage", "Timers"};

  switch (menuState) {
    case MENU_MAIN: {
      int start = max(0, menuIndex - 4);
      for (int i = 0; i < 5 && start + i < 7; i++) {
        int idx = start + i;
        if (idx == menuIndex) {
          u8g2.setDrawColor(1);
          u8g2.drawBox(0, i*12, SCREEN_WIDTH, 12);
          u8g2.setDrawColor(0);
        }
        u8g2.drawStr(10, i*12 + 10, mainItems[idx]);
      }
      break;
    }

    case MENU_HANDLE: {
      const char* h[] = {"NT115", "C210"};
      int sel = (settings.handleType == HANDLE_NT115) ? 0 : 1;
      for (int i = 0; i < 2; i++) {
        if (i == sel) { u8g2.setDrawColor(1); u8g2.drawBox(0,i*12,SCREEN_WIDTH,12); u8g2.setDrawColor(0); }
        u8g2.drawStr(10, i*12 + 10, h[i]);
      }
      break;
    }

    case MENU_TEMP: {
      const char* items[] = {"Startup Temp", "Temp Offset", "Standby Temp", "<< Back"};
      for (int i = 0; i < 4; i++) {
        char b[32];
        if (i==0) snprintf(b,sizeof(b),"%s: %d C", items[i], settings.targetTemp);
        else if (i==1) snprintf(b,sizeof(b),"%s: %d C", items[i], settings.tempOffset);
        else if (i==2) snprintf(b,sizeof(b),"%s: %d C", items[i], settings.standbyTemp);
        else strcpy(b, items[i]);

        if (i == submenuIndex) { u8g2.setDrawColor(1); u8g2.drawBox(0,i*12,SCREEN_WIDTH,12); u8g2.setDrawColor(0); }
        u8g2.drawStr(10, i*12 + 10, b);
      }
      break;
    }

    case MENU_PID: {
      const char* items[] = {"NT115 PID", "C210 PID", "<< Back"};
      for (int i = 0; i < 3; i++) {
        if (i == submenuIndex) { u8g2.setDrawColor(1); u8g2.drawBox(0,i*12,SCREEN_WIDTH,12); u8g2.setDrawColor(0); }
        u8g2.drawStr(10, i*12 + 10, items[i]);
      }
      break;
    }

    case MENU_PID_NT115:
    case MENU_PID_C210: {
      float kp, ki, kd;
      const char* title = (menuState == MENU_PID_NT115) ? "NT115 PID" : "C210 PID";
      if (menuState == MENU_PID_NT115) {
        kp = settings.kp_nt115; ki = settings.ki_nt115; kd = settings.kd_nt115;
      } else {
        kp = settings.kp_c210; ki = settings.ki_c210; kd = settings.kd_c210;
      }

      const char* items[] = {"Kp", "Ki", "Kd", "<< Back"};

      // Title
      u8g2.setFont(u8g2_font_6x10_tf);
      u8g2.drawStr(10, 10, title);

      for (int i = 0; i < 4; i++) {
        char buf[32];
        if (i < 3) {
          snprintf(buf, sizeof(buf), "%s: %.1f", items[i], (i==0)?kp : (i==1)?ki : kd);
        } else {
          strcpy(buf, items[i]);
        }

        if (i == submenuIndex) {
          u8g2.setDrawColor(1);
          u8g2.drawBox(0, 12 + i*12, SCREEN_WIDTH, 12);
          u8g2.setDrawColor(0);
        }
        u8g2.drawStr(10, 22 + i*12, buf);
      }
      break;
    }

    case MENU_PID_NT115_KP: case MENU_PID_NT115_KI: case MENU_PID_NT115_KD:
    case MENU_PID_C210_KP:  case MENU_PID_C210_KI:  case MENU_PID_C210_KD: {
      float val;
      const char* lbl;
      bool isNT115 = (menuState <= MENU_PID_NT115_KD);

      if (menuState % 3 == 0) { lbl = "Kp"; val = isNT115 ? settings.kp_nt115 : settings.kp_c210; }
      else if (menuState % 3 == 1) { lbl = "Ki"; val = isNT115 ? settings.ki_nt115 : settings.ki_c210; }
      else { lbl = "Kd"; val = isNT115 ? settings.kd_nt115 : settings.kd_c210; }

      char buf[32];
      snprintf(buf, sizeof(buf), "%s: %.1f", lbl, val);

      u8g2.setFont(u8g2_font_logisoso16_tf);
      u8g2.setDrawColor(1);
      u8g2.drawBox(0, 0, SCREEN_WIDTH, 30);
      u8g2.setDrawColor(0);
      u8g2.drawStr(10, 25, buf);

      u8g2.setFont(u8g2_font_6x10_tf);
      u8g2.drawStr(10, 50, "Up/Dn: +/-0.1   Sel: Save");
      break;
    }

    case MENU_TIMERS: {
      const char* items[] = {"Standby Time", "Sleep Time", "<< Back"};
      for (int i = 0; i < 3; i++) {
        char b[32];
        if (i==0) snprintf(b,sizeof(b),"%s: %u Min", items[i], settings.standbyTime);
        else if (i==1) snprintf(b,sizeof(b),"%s: %u Min", items[i], settings.sleepTime);
        else strcpy(b, items[i]);

        if (i == submenuIndex) { u8g2.setDrawColor(1); u8g2.drawBox(0,i*12,SCREEN_WIDTH,12); u8g2.setDrawColor(0); }
        u8g2.drawStr(10, i*12 + 10, b);
      }
      break;
    }

    case MENU_VOLTAGE: {
      const char* items[] = {"Voltage Fix", "<< Back"};
      for (int i = 0; i < 2; i++) {
        char b[32];
        if (i==0) snprintf(b,sizeof(b),"%s: %d.%d V", items[i],
                           settings.voltageOffset/1000, abs(settings.voltageOffset%1000)/100);
        else strcpy(b, items[i]);

        if (i == submenuIndex) { u8g2.setDrawColor(1); u8g2.drawBox(0,i*12,SCREEN_WIDTH,12); u8g2.setDrawColor(0); }
        u8g2.drawStr(10, i*12 + 10, b);
      }
      break;
    }

    default: break;
  }

  u8g2.sendBuffer();
}