// ============================================================================
// Управляющая программа с LCD1602 (I2C), интерфейсным энкодером и двумя моторами.
//
// Режим MENU:
//  — поворот энкодера выбирает MODE 1 или MODE 2;
//  — короткое нажатие выбирает режим.
//
// MODE 1:
//  — отображает счётчики импульсов энкодеров моторов;
//  — короткое нажатие сбрасывает счётчики;
//  — долгое нажатие возвращает в MENU.
//
// MODE 2:
//  — короткое нажатие запускает/останавливает моторы;
//  — при работе вычисляет среднюю частоту импульсов (имп/с) за последнюю 1 секунду;
//  — долгое нажатие останавливает моторы и возвращает в MENU.
// ============================================================================

// -------------------- ТИПЫ ДО #include -------------------------------------
enum Mode { MENU = 0, MODE1 = 1, MODE2 = 2 };

struct Sample {
  unsigned long t;
  unsigned long c1;
  unsigned long c2;
};

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

// -------------------- НАСТРОЙКИ --------------------------------------------
#define SERIAL_BAUD 115200

const uint8_t  MOTOR_PWM_MODE2      = 255;

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

const uint16_t MODE2_SAMPLE_MS      = 50;
const uint16_t MODE2_WINDOW_MS      = 1000;

const uint16_t LCD_UPDATE_MS_MENU   = 200;
const uint16_t LCD_UPDATE_MS_MODE1  = 120;
const uint16_t LCD_UPDATE_MS_MODE2  = 200;

const uint16_t MODE1_SER_UPDATE_MS  = 500;
const uint16_t MODE2_SER_UPDATE_MS  = 250;

const uint8_t  MODE2_BUF_N          = (MODE2_WINDOW_MS / MODE2_SAMPLE_MS) + 4;

const unsigned long ENC_MIN_EDGE_US = 200;

const unsigned long I2C_CLOCK_HZ    = 100000UL;

// -------------------- ПИНЫ -------------------------------------------------
// Моторы: скорость (PWM) и направление (IN)
const uint8_t MOTOR_1_SPEED = 5;
const uint8_t MOTOR_1_DIR   = A2;

const uint8_t MOTOR_2_SPEED = 6;
const uint8_t MOTOR_2_DIR   = A3;

// Интерфейсный энкодер
const uint8_t CLK = 11;  // также встречается: A / S1 / OUT1
const uint8_t DT  = 12;  // также встречается: B / S2 / OUT2
const uint8_t SW  = 13;  // также встречается: SW / KEY / BTN

// Энкодеры моторов (используется канал A)
const uint8_t ENC_1_A = 2;
const uint8_t ENC_2_A = 3;

// ============================================================================
// 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 unsigned long enc1_count = 0;
volatile unsigned long enc2_count = 0;

volatile unsigned long enc1_last_us = 0;
volatile unsigned long enc2_last_us = 0;

void isrEnc1() {
  unsigned long t = micros();
  if (t - enc1_last_us >= ENC_MIN_EDGE_US) {
    enc1_last_us = t;
    enc1_count++;
  }
}

void isrEnc2() {
  unsigned long t = micros();
  if (t - enc2_last_us >= ENC_MIN_EDGE_US) {
    enc2_last_us = t;
    enc2_count++;
  }
}

unsigned long encGet1() { noInterrupts(); unsigned long v = enc1_count; interrupts(); return v; }
unsigned long encGet2() { noInterrupts(); unsigned long v = enc2_count; interrupts(); return v; }

void encReset() {
  noInterrupts();
  enc1_count = 0;
  enc2_count = 0;
  enc1_last_us = micros();
  enc2_last_us = micros();
  interrupts();
}

// ============================================================================
// Интерфейсный энкодер
// ============================================================================
int16_t uiDelta = 0;
uint8_t uiLast = 0;

void uiEncoderInit() {
  pinMode(CLK, INPUT_PULLUP);
  pinMode(DT,  INPUT_PULLUP);
  uiLast = (digitalRead(CLK) << 1) | digitalRead(DT);
}

void uiEncoderPoll() {
  uint8_t cur = (digitalRead(CLK) << 1) | digitalRead(DT);
  if (cur == uiLast) return;

  int8_t step = 0;
  if ((uiLast == 0b00 && cur == 0b10) ||
      (uiLast == 0b10 && cur == 0b11) ||
      (uiLast == 0b11 && cur == 0b01) ||
      (uiLast == 0b01 && cur == 0b00)) step = +1;
  else if ((uiLast == 0b00 && cur == 0b01) ||
           (uiLast == 0b01 && cur == 0b11) ||
           (uiLast == 0b11 && cur == 0b10) ||
           (uiLast == 0b10 && cur == 0b00)) step = -1;

  uiLast = cur;
  if (step != 0) uiDelta += step;
}

// ============================================================================
// Кнопка 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;
    }
  }
}

// ============================================================================
// Моторы
// ============================================================================
void motorSet(uint8_t motor, int pwmSigned) {
  pwmSigned = constrain(pwmSigned, -255, 255);

  uint8_t dirPin   = (motor == 1) ? MOTOR_1_DIR : MOTOR_2_DIR;
  uint8_t speedPin = (motor == 1) ? MOTOR_1_SPEED : MOTOR_2_SPEED;

  if (pwmSigned >= 0) {
    digitalWrite(dirPin, HIGH);
    analogWrite(speedPin, (uint8_t)pwmSigned);
  } else {
    digitalWrite(dirPin, LOW);
    analogWrite(speedPin, (uint8_t)(-pwmSigned));
  }
}

void motorsStop() {
  analogWrite(MOTOR_1_SPEED, 0);
  analogWrite(MOTOR_2_SPEED, 0);
}

// ============================================================================
// MODE2: средняя частота импульсов за последнюю 1 секунду
// ============================================================================
Sample buf[MODE2_BUF_N];
uint8_t bufHead  = 0;
uint8_t bufCount = 0;

bool mode2Running = false;
unsigned long lastSampleMs = 0;

float P1 = 0.0f;
float P2 = 0.0f;

void mode2BufReset() {
  bufHead = 0;
  bufCount = 0;
  lastSampleMs = millis();
  P1 = 0.0f;
  P2 = 0.0f;
}

void mode2BufPush(unsigned long t, unsigned long c1, unsigned long c2) {
  buf[bufHead].t  = t;
  buf[bufHead].c1 = c1;
  buf[bufHead].c2 = c2;

  bufHead = (uint8_t)((bufHead + 1) % MODE2_BUF_N);
  if (bufCount < MODE2_BUF_N) bufCount++;
}

bool mode2GetOldestForWindow(unsigned long now, Sample &oldest) {
  if (bufCount < 2) return false;

  int idxOldest = (int)bufHead - (int)bufCount;
  while (idxOldest < 0) idxOldest += MODE2_BUF_N;

  unsigned long target = (now >= MODE2_WINDOW_MS) ? (now - MODE2_WINDOW_MS) : 0;

  oldest = buf[idxOldest];
  for (uint8_t i = 0; i < bufCount; i++) {
    int idx = idxOldest + i;
    if (idx >= MODE2_BUF_N) idx -= MODE2_BUF_N;

    if (buf[idx].t <= target) oldest = buf[idx];
    else break;
  }
  return true;
}

void mode2UpdateAveraging() {
  unsigned long now = millis();

  if (now - lastSampleMs >= MODE2_SAMPLE_MS) {
    lastSampleMs += MODE2_SAMPLE_MS;
    mode2BufPush(now, encGet1(), encGet2());
  }

  if (bufCount < 2) return;

  int idxLast = (int)bufHead - 1;
  if (idxLast < 0) idxLast += MODE2_BUF_N;
  Sample newest = buf[idxLast];

  Sample oldest;
  if (!mode2GetOldestForWindow(now, oldest)) return;

  unsigned long dtMs = newest.t - oldest.t;
  if (dtMs < 100) return;

  unsigned long dc1 = newest.c1 - oldest.c1;
  unsigned long dc2 = newest.c2 - oldest.c2;

  float dt = dtMs / 1000.0f;
  P1 = dc1 / dt;
  P2 = dc2 / dt;
}

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

void drawMode1(unsigned long c1, unsigned long c2) {
  char l0[17], l1[17];
  snprintf(l0, sizeof(l0), "MODE1 CNT (imp)");
  snprintf(l1, sizeof(l1), "1:%lu 2:%lu", c1, c2);
  lcdPrintPadded(0, l0);
  lcdPrintPadded(1, l1);
}

void drawMode2Idle() {
  lcdPrintPadded(0, "MODE2 SPEED P");
  lcdPrintPadded(1, "Press to START");
}

void drawMode2Run(float p1, float p2) {
  char l0[17], l1[17];
  snprintf(l0, sizeof(l0), "P avg 1s imp/s");
  snprintf(l1, sizeof(l1), "1:%d 2:%d", (int)p1, (int)p2);
  lcdPrintPadded(0, l0);
  lcdPrintPadded(1, l1);
}

// ============================================================================
// Serial
// ============================================================================
unsigned long lastMode1SerMs = 0;
unsigned long lastMode2SerMs = 0;

void serialMode1Tick() {
  unsigned long now = millis();
  if (now - lastMode1SerMs < MODE1_SER_UPDATE_MS) return;
  lastMode1SerMs = now;

  Serial.print("MODE1 c1=");
  Serial.print(encGet1());
  Serial.print(" c2=");
  Serial.println(encGet2());
}

void serialMode2Tick() {
  unsigned long now = millis();
  if (now - lastMode2SerMs < MODE2_SER_UPDATE_MS) return;
  lastMode2SerMs = now;

  Serial.print("MODE2 P1=");
  Serial.print((int)P1);
  Serial.print(" P2=");
  Serial.print((int)P2);
  Serial.print(" c1=");
  Serial.print(encGet1());
  Serial.print(" c2=");
  Serial.println(encGet2());
}

// ============================================================================
// Главный цикл
// ============================================================================
Mode mode = MENU;
uint8_t menuIndex = 0;
unsigned long lastLcdMs = 0;

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

  pinMode(MOTOR_1_DIR, OUTPUT);
  pinMode(MOTOR_2_DIR, OUTPUT);
  pinMode(MOTOR_1_SPEED, OUTPUT);
  pinMode(MOTOR_2_SPEED, OUTPUT);
  motorsStop();

  pinMode(ENC_1_A, INPUT_PULLUP);
  pinMode(ENC_2_A, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENC_1_A), isrEnc1, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC_2_A), isrEnc2, RISING);

  uiEncoderInit();
  btnInit();

  lcdInitOnce();
  if (lcdReady) drawMenu(menuIndex);
}

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

  int16_t d = uiDelta;
  if (d != 0) uiDelta = 0;

  if (mode == MENU) {
    if (d != 0) {
      if (d > 0) menuIndex = (menuIndex + 1) % 2;
      else       menuIndex = (menuIndex + 1 + 1) % 2;
      lastLcdMs = 0;
    }

    if (evShort) {
      if (menuIndex == 0) {
        mode = MODE1;
        encReset();
      } else {
        mode = MODE2;
        mode2Running = false;
        motorsStop();
      }
      lastLcdMs = 0;
    }

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

  if (mode == MODE1) {
    if (evShort) encReset();

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

    unsigned long c1 = encGet1();
    unsigned long c2 = encGet2();

    unsigned long now = millis();
    if (lcdReady && (now - lastLcdMs >= LCD_UPDATE_MS_MODE1)) {
      lastLcdMs = now;
      drawMode1(c1, c2);
    }

    serialMode1Tick();
    return;
  }

  if (mode == MODE2) {
    if (evLong) {
      motorsStop();
      mode2Running = false;
      mode = MENU;
      lastLcdMs = 0;
      return;
    }

    if (evShort) {
      if (!mode2Running) {
        mode2Running = true;

        motorSet(1, MOTOR_PWM_MODE2);
        motorSet(2, MOTOR_PWM_MODE2);

        mode2BufReset();
        mode2BufPush(millis(), encGet1(), encGet2());

        Serial.println("MODE2: START");
      } else {
        motorsStop();
        mode2Running = false;
        Serial.println("MODE2: STOP");
      }
      lastLcdMs = 0;
    }

    if (!mode2Running) {
      unsigned long now = millis();
      if (lcdReady && (now - lastLcdMs >= LCD_UPDATE_MS_MODE2)) {
        lastLcdMs = now;
        drawMode2Idle();
      }
      return;
    }

    mode2UpdateAveraging();

    unsigned long now = millis();
    if (lcdReady && (now - lastLcdMs >= LCD_UPDATE_MS_MODE2)) {
      lastLcdMs = now;
      drawMode2Run(P1, P2);
    }

    serialMode2Tick();
    return;
  }
}
