// ============================================================================
// LCD1602 (I2C) + энкодер интерфейса (CLK/DT/SW) + Servo (D10)
//
// MENU:
//  — короткое нажатие SW -> MODE1
//
// MODE1:
//  — поворот энкодера изменяет длительность импульса (мкс) с фиксированным шагом
//  — короткое нажатие SW переключает метку: 0 degree / 180 degree
//  — долгое нажатие SW возвращает в MENU
// ============================================================================

enum UiState { MENU = 0, MODE1 = 1 };

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

#define SERIAL_BAUD 115200

// -------------------- ПИНЫ -------------------------------------------------
const uint8_t SERVO_PIN = 10;

const uint8_t CLK = 11;
const uint8_t DT  = 12;
const uint8_t SW  = 13;

// -------------------- НАСТРОЙКИ --------------------------------------------
const unsigned long I2C_CLOCK_HZ = 100000UL;

const uint16_t BTN_DEBOUNCE_MS = 35;
const uint16_t BTN_LONG_MS     = 700;

const uint16_t LCD_UPDATE_MS    = 120;
const uint16_t SERIAL_UPDATE_MS = 250;

// Фиксированный шаг изменения импульса (мкс)
const int SERVO_STEP_US = 10;

// Начальные значения для меток 0 degree и 180 degree (мкс)
const int SERVO_HOME_0_US   = 540;
const int SERVO_HOME_180_US = 2400;

// Расширенный диапазон регулировки (мкс)
const int SERVO_MIN_US = 200;
const int SERVO_MAX_US = 3000;

// ============================================================================
// LCD I2C
// ============================================================================
LiquidCrystal_I2C* lcdPtr = nullptr;
uint8_t lcdAddr = 0;
bool lcdReady = false;

bool i2cPing(uint8_t addr) {
  Wire.beginTransmission(addr);
  return (Wire.endTransmission() == 0);
}

uint8_t findLcdAddress() {
  if (i2cPing(0x27)) return 0x27;
  if (i2cPing(0x3F)) return 0x3F;
  for (uint8_t a = 1; a < 127; a++) {
    if (a == 0x27 || a == 0x3F) continue;
    if (i2cPing(a)) return a;
  }
  return 0;
}

void lcdFree() {
  if (lcdPtr) {
    delete lcdPtr;
    lcdPtr = nullptr;
  }
  lcdReady = false;
}

void lcdInitOnce() {
  lcdFree();

  Wire.begin();
  Wire.setClock(I2C_CLOCK_HZ);

#if defined(WIRE_HAS_TIMEOUT)
  Wire.setWireTimeout(2000, true);
#endif

  lcdAddr = findLcdAddress();

  Serial.print("LCD address: ");
  if (lcdAddr == 0) {
    Serial.println("not found");
    return;
  }
  Serial.print("0x");
  if (lcdAddr < 16) Serial.print("0");
  Serial.println(lcdAddr, HEX);

  lcdPtr = new LiquidCrystal_I2C(lcdAddr, 16, 2);
  lcdPtr->init();
  lcdPtr->backlight();
  lcdPtr->clear();
  lcdReady = true;
}

void lcdPrintPadded(uint8_t row, const char* text) {
  if (!lcdReady) return;

  char buf[17];
  for (uint8_t i = 0; i < 16; i++) buf[i] = ' ';
  buf[16] = '\0';

  for (uint8_t i = 0; i < 16 && text[i] != '\0'; i++) buf[i] = text[i];

  lcdPtr->setCursor(0, row);
  lcdPtr->print(buf);
}

// ============================================================================
// Энкодер (таблица переходов), выдаёт целые шаги
// ============================================================================
volatile int16_t uiDelta = 0;

uint8_t encPrev = 0;
int8_t  encAccum = 0;

const int8_t ENC_TAB[16] = {
   0, -1, +1,  0,
  +1,  0,  0, -1,
  -1,  0,  0, +1,
   0, +1, -1,  0
};

uint8_t encReadAB() {
  uint8_t a = (uint8_t)digitalRead(CLK);
  uint8_t b = (uint8_t)digitalRead(DT);
  return (a << 1) | b;
}

void uiEncoderInit() {
  pinMode(CLK, INPUT_PULLUP);
  pinMode(DT,  INPUT_PULLUP);
  encPrev = encReadAB();
  encAccum = 0;
}

void uiEncoderReset() {
  noInterrupts();
  uiDelta = 0;
  interrupts();
  encPrev = encReadAB();
  encAccum = 0;
}

void uiEncoderPoll() {
  uint8_t cur = encReadAB();
  uint8_t idx = (encPrev << 2) | cur;
  int8_t  dir = ENC_TAB[idx];
  encPrev = cur;

  if (dir == 0) return;

  encAccum += dir;
  if (encAccum >= 4) {
    encAccum = 0;
    uiDelta++;
  } else if (encAccum <= -4) {
    encAccum = 0;
    uiDelta--;
  }
}

int16_t takeUiDelta() {
  int16_t d;
  noInterrupts();
  d = uiDelta;
  uiDelta = 0;
  interrupts();
  return d;
}

// ============================================================================
// Кнопка SW (короткое/долгое)
// ============================================================================
struct BtnState {
  bool lastStable = true;
  bool lastRead   = true;
  unsigned long lastChangeMs = 0;
  unsigned long pressStartMs = 0;
  bool longFired = false;
};

BtnState btn;
bool evShort = false;
bool evLong  = false;

void btnInit() {
  pinMode(SW, INPUT_PULLUP);
  btn.lastStable = digitalRead(SW);
  btn.lastRead = btn.lastStable;
  btn.lastChangeMs = millis();
}

void btnUpdate() {
  evShort = false;
  evLong  = false;

  bool r = digitalRead(SW);
  unsigned long now = millis();

  if (r != btn.lastRead) {
    btn.lastRead = r;
    btn.lastChangeMs = now;
  }

  if ((now - btn.lastChangeMs) >= BTN_DEBOUNCE_MS && btn.lastStable != btn.lastRead) {
    btn.lastStable = btn.lastRead;

    if (btn.lastStable == false) {
      btn.pressStartMs = now;
      btn.longFired = false;
    } else {
      unsigned long held = now - btn.pressStartMs;
      if (!btn.longFired && held >= 30 && held < BTN_LONG_MS) evShort = true;
    }
  }

  if (btn.lastStable == false && !btn.longFired) {
    if ((now - btn.pressStartMs) >= BTN_LONG_MS) {
      btn.longFired = true;
      evLong = true;
    }
  }
}

// ============================================================================
// Servo
// ============================================================================
Servo servo;

uint8_t targetIndex = 0; // 0 -> 0 degree, 1 -> 180 degree
int pulseUs[2] = { SERVO_HOME_0_US, SERVO_HOME_180_US };

void applyServo() {
  int v = pulseUs[targetIndex];
  v = constrain(v, SERVO_MIN_US, SERVO_MAX_US);
  pulseUs[targetIndex] = v;
  servo.writeMicroseconds(v);
}

// ============================================================================
// Экран
// ============================================================================
void drawMenu() {
  lcdPrintPadded(0, "Select mode:");
  lcdPrintPadded(1, ">MODE 1");
}

void drawMode1() {
  char l0[17], l1[17];

  const char* degText = (targetIndex == 0) ? "0 degree" : "180 degree";
  int us = pulseUs[targetIndex];

  // Пример: "0 degree  540us"
  snprintf(l0, sizeof(l0), "%-9s %4dus", degText, us);

  // Пример: "step:10us"
  snprintf(l1, sizeof(l1), "step:%dus", SERVO_STEP_US);

  lcdPrintPadded(0, l0);
  lcdPrintPadded(1, l1);
}

// ============================================================================
// Serial
// ============================================================================
unsigned long lastSerMs = 0;

void serialTick(UiState st) {
  unsigned long now = millis();
  if (now - lastSerMs < SERIAL_UPDATE_MS) return;
  lastSerMs = now;

  if (st == MENU) {
    Serial.println("MENU mode=MODE1");
    return;
  }

  Serial.print("MODE1 target=");
  Serial.print((targetIndex == 0) ? "0 degree" : "180 degree");
  Serial.print(" us=");
  Serial.print(pulseUs[targetIndex]);
  Serial.print(" step=");
  Serial.println(SERVO_STEP_US);
}

// ============================================================================
// Главный цикл
// ============================================================================
UiState state = MENU;
unsigned long lastLcdMs = 0;

void setup() {
  Serial.begin(SERIAL_BAUD);
  Serial.println("START");

  uiEncoderInit();
  btnInit();

  lcdInitOnce();
  if (lcdReady) drawMenu();

  servo.attach(SERVO_PIN);

  // Начальная установка: активна метка 0 degree
  targetIndex = 0;
  servo.writeMicroseconds(pulseUs[0]);
}

void loop() {
  uiEncoderPoll();
  btnUpdate();

  if (state == MENU) {
    // В MENU поворот энкодера не используется
    (void)takeUiDelta();

    if (evShort) {
      state = MODE1;
      uiEncoderReset();
      lastLcdMs = 0;
      applyServo();
    }

    unsigned long now = millis();
    if (lcdReady && (now - lastLcdMs >= LCD_UPDATE_MS)) {
      lastLcdMs = now;
      drawMenu();
    }

    serialTick(MENU);
    return;
  }

  if (state == MODE1) {
    if (evShort) {
      targetIndex ^= 1;
      applyServo();
      lastLcdMs = 0;
    }

    if (evLong) {
      state = MENU;
      lastLcdMs = 0;
      return;
    }

    int16_t d = takeUiDelta();
    if (d != 0) {
      long next = (long)pulseUs[targetIndex] + (long)d * (long)SERVO_STEP_US;
      if (next < SERVO_MIN_US) next = SERVO_MIN_US;
      if (next > SERVO_MAX_US) next = SERVO_MAX_US;
      pulseUs[targetIndex] = (int)next;

      applyServo();
      lastLcdMs = 0;
    }

    unsigned long now = millis();
    if (lcdReady && (now - lastLcdMs >= LCD_UPDATE_MS)) {
      lastLcdMs = now;
      drawMode1();
    }

    serialTick(MODE1);
    return;
  }
}
