#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Preferences.h>
#if defined(ESP32)
  #include "esp_system.h"
#endif

// PWM via LEDC: 1 = LEDC (ESP32), 0 = fallback analogWrite
#define USE_LEDC 0
#if defined(ESP32) && USE_LEDC
  #include "esp32-hal-ledc.h"
#endif

// --- LCD ---
constexpr uint8_t I2C_ADDR = 0x27;
LiquidCrystal_I2C lcd(I2C_ADDR, 16, 2); // lcd.begin() w setup

// --- Piny ---
constexpr int PIN_POT_GAZ      = 34;   // ADC1
constexpr int PIN_POT_SPRZEGL  = 35;   // ADC1
// RGB CC
constexpr int PIN_RGB_R        = 26;   // czerwony
constexpr int PIN_RGB_G        = 25;   // zielony
constexpr int PIN_RGB_B        = 33;   // niebieski
constexpr int PIN_BTN_RESET    = 27;   // do GND (INPUT_PULLUP)

// --- PWM / LEDC ---
constexpr int PWM_FREQ = 5000;
constexpr int PWM_RES  = 8;            // 8-bit (0..255)
constexpr int CH_R = 0, CH_G = 1, CH_B = 2;
constexpr uint8_t BRIGHT_R = 160;
constexpr uint8_t BRIGHT_G = 200;
constexpr uint8_t BRIGHT_B = 180;

uint8_t curR = 0, curG = 0, curB = 0;

#if USE_LEDC
inline void applyRGB() { ledcWrite(CH_R, curR); ledcWrite(CH_G, curG); ledcWrite(CH_B, curB); }
void pwmBegin() {
  ledcSetup(CH_R, PWM_FREQ, PWM_RES);
  ledcSetup(CH_G, PWM_FREQ, PWM_RES);
  ledcSetup(CH_B, PWM_FREQ, PWM_RES);
  ledcAttachPin(PIN_RGB_R, CH_R);
  ledcAttachPin(PIN_RGB_G, CH_G);
  ledcAttachPin(PIN_RGB_B, CH_B);
  applyRGB();
}
#else
inline void applyRGB() { analogWrite(PIN_RGB_R, curR); analogWrite(PIN_RGB_G, curG); analogWrite(PIN_RGB_B, curB); }
void pwmBegin() {
  pinMode(PIN_RGB_R, OUTPUT);
  pinMode(PIN_RGB_G, OUTPUT);
  pinMode(PIN_RGB_B, OUTPUT);
  applyRGB();
}
#endif

inline void setRGB(uint8_t r, uint8_t g, uint8_t b) { curR=r; curG=g; curB=b; applyRGB(); }
inline void leds(bool redOn, bool greenOn) {
  curR = redOn  ? BRIGHT_R : 0;
  curG = greenOn? BRIGHT_G : 0;
  curB = 0;
  applyRGB();
}

// --- Pasek + procenty na LCD ---
constexpr int LCD_COLS = 16;
constexpr int PREFIX_COLS = 2;            // "S_" / "G_"
constexpr int PCT_WIDTH  = 4;             // "XXX%" (np. " 97%","100%")
constexpr int BAR_COLS   = LCD_COLS - PREFIX_COLS - PCT_WIDTH; // 16-2-4=10
constexpr int SEG_PER_CELL = 5;
constexpr uint32_t REFRESH_MS = 50;

// --- Progi/logika ---
constexpr int CLUTCH_PRESS_PCT   = 95;
constexpr int CLUTCH_RELEASE_PCT = 15;
constexpr int GAZ_MIN_GO_PCT     = 85;
constexpr bool INVERT_GAZ        = false;
constexpr bool INVERT_SPRZEGL    = false;

// --- Stabilizacja (histereza czasowa) ---
constexpr uint32_t STABLE_PRESS_MS      = 80; // >=95%
constexpr uint32_t STABLE_RELEASE_MS    = 50; // <15% (koniec pomiaru)
constexpr uint32_t STABLE_FALSESTART_MS = 50; // <15% (falstart)
uint32_t clutchAbovePressSince = 0;
uint32_t clutchBelowReleaseSince = 0;

// --- Własne znaki paska (sloty 0..4) ---
byte bar1[8] = {0b10000,0b10000,0b10000,0b10000,0b10000,0b10000,0b10000,0b10000};
byte bar2[8] = {0b11000,0b11000,0b11000,0b11000,0b11000,0b11000,0b11000,0b11000};
byte bar3[8] = {0b11100,0b11100,0b11100,0b11100,0b11100,0b11100,0b11100,0b11100};
byte bar4[8] = {0b11110,0b11110,0b11110,0b11110,0b11110,0b11110,0b11110,0b11110};
byte bar5[8] = {0b11111,0b11111,0b11111,0b11111,0b11111,0b11111,0b11111,0b11111};

// --- Wygładzanie paska (LCD) ---
constexpr float ALPHA = 0.2f;
float filtGaz = 0.0f, filtSprz = 0.0f;

// --- Przycisk: debounce/longpress ---
constexpr uint32_t DEBOUNCE_MS   = 30;
constexpr uint32_t LONGPRESS_MS  = 1200;     // podglad BEST
constexpr uint32_t VERY_LONGPRESS_MS = 5000; // kasowanie BEST
bool btnLast = HIGH, btnStable = HIGH;
uint32_t btnLastChange = 0;
uint32_t btnStableLowSince = 0;
bool eraseDone = false;

// --- Miganie RED<->BLUE (naprzemiennie) ---
constexpr uint32_t BLINK_MS = 300;
bool blinkerActive = false;
bool blinkBluePhase = true;      // true=BLUE ON, false=RED ON
uint32_t lastBlinkToggle = 0;

// --- Stany ---
enum State { START_SCREEN, COUNTDOWN, RED_ON, GREEN_ON, WAIT_RELEASE_15, SHOW_RESULT, ERROR_STATE, SHOW_BEST };
State state = START_SCREEN;

// --- Błędy ---
enum ErrorKind { ERR_NONE, ERR_FALSE_START };
ErrorKind errorKind = ERR_NONE;

// --- Czasy i wyniki ---
uint32_t tRedOn = 0, redDurationMs = 0;
uint32_t tGreenOn = 0, greenDurationMs = 0, tGreenOff = 0;
uint32_t reactionMs = 0;
int gazPctAtGreenOff = 0;
bool lowGasFlag = false;

// --- COUNTDOWN po resecie ze sprzeglem ---
int countdownVal = 4;
uint32_t countdownLastTick = 0;

// --- NVS: rekord ---
Preferences prefs;
uint32_t bestTimeMs = 0;  // 0 = brak rekordu

// --- Splash rekordu na starcie ---
uint32_t startEnteredAt = 0;
bool startBestShown = false;
bool startBestShowing = false;
uint32_t startBestHideAt = 0;

// --- Pomocnicze ---
static inline int toPercent(int raw, bool invert=false) {
  long v = (long)raw * 100 + 2047; // zaokr.
  int p = v / 4095;
  if (p < 0) p = 0; if (p > 100) p = 100;
  return invert ? (100 - p) : p;
}

void printLine16(uint8_t row, const char* text) {
  lcd.setCursor(0, row);
  size_t len = 0; while (text[len] != '\0' && len < 16) ++len;
  for (size_t i = 0; i < 16; ++i) lcd.write(i < len ? text[i] : ' ');
}
void printCentered16(uint8_t row, const char* text) {
  char buf[17]; for (int i=0;i<16;i++) buf[i]=' '; buf[16]='\0';
  size_t len=0; while (text[len] != '\0' && len < 16) ++len;
  size_t start = (16 - len) / 2;
  for (size_t i=0; i<len; ++i) buf[start+i] = text[i];
  printLine16(row, buf);
}

void printPercentRight(uint8_t row, int pct) {
  if (pct < 0) pct = 0; if (pct > 100) pct = 100;
  char buf[5]; snprintf(buf, sizeof(buf), "%3d%%", pct); // " 97%" / "100%"
  lcd.setCursor(LCD_COLS - PCT_WIDTH, row); // kol. 12..15
  for (int i = 0; i < PCT_WIDTH; ++i) lcd.write(buf[i]);
}

void displayStartScreen() {
  lcd.clear();
  printLine16(0, "---GooMoo#312---");
  printLine16(1, "WCISNIJ SPRZEGLO");
}
void enterStartScreen(uint32_t now) {
  displayStartScreen();
  state = START_SCREEN;
  startEnteredAt = now;
  startBestShown = false;
  startBestShowing = false;
}

// Stabilizacja progow sprzegla
void updateClutchStability(int pctSprz, uint32_t now) {
  if (pctSprz >= CLUTCH_PRESS_PCT) {
    if (!clutchAbovePressSince) clutchAbovePressSince = now;
  } else {
    clutchAbovePressSince = 0;
  }
  if (pctSprz < CLUTCH_RELEASE_PCT) {
    if (!clutchBelowReleaseSince) clutchBelowReleaseSince = now;
  } else {
    clutchBelowReleaseSince = 0;
  }
}

void drawBarAt(uint8_t colStart, uint8_t row, uint8_t cols, int steps) {
  const int maxSteps = cols * SEG_PER_CELL;
  if (steps < 0) steps = 0; if (steps > maxSteps) steps = maxSteps;
  lcd.setCursor(colStart, row);
  for (int i = 0; i < cols; i++) {
    int seg = steps - i * SEG_PER_CELL;
    if (seg <= 0)       lcd.write(' ');
    else if (seg >= 5)  lcd.write(byte(4));
    else                lcd.write(byte(seg - 1));
  }
}
void showBars(int rawGaz, int rawSprz) {
  // Etykiety
  lcd.setCursor(0, 0); lcd.print("S_");
  lcd.setCursor(0, 1); lcd.print("G_");

  // Inwersje i wygladzanie tylko do wyswietlania
  int rawGazDisp  = INVERT_GAZ     ? (4095 - rawGaz)  : rawGaz;
  int rawSprzDisp = INVERT_SPRZEGL ? (4095 - rawSprz) : rawSprz;

  filtGaz  = ALPHA * rawGazDisp  + (1.0f - ALPHA) * filtGaz;
  filtSprz = ALPHA * rawSprzDisp + (1.0f - ALPHA) * filtSprz;

  // Pasek (10 kolumn = 50 segmentow)
  const int maxSteps = BAR_COLS * SEG_PER_CELL; // 10*5=50
  int stepsS = map((int)filtSprz, 0, 4095, 0, maxSteps);
  int stepsG = map((int)filtGaz,  0, 4095, 0, maxSteps);
  drawBarAt(PREFIX_COLS, 0, BAR_COLS, stepsS);
  drawBarAt(PREFIX_COLS, 1, BAR_COLS, stepsG);

  // Procenty po prawej (stabilne, z filtrem)
  int pctS = (int)((filtSprz * 100.0f) / 4095.0f + 0.5f);
  int pctG = (int)((filtGaz  * 100.0f) / 4095.0f + 0.5f);
  if (pctS < 0) pctS = 0; if (pctS > 100) pctS = 100;
  if (pctG < 0) pctG = 0; if (pctG > 100) pctG = 100;
  printPercentRight(0, pctS);
  printPercentRight(1, pctG);
}

// Linia 2 dla "malo gazu" (ASCII, do 16 znaków)
void printLowGasLineASCII(int pct) {
  char line2[17];
  if (pct == 100) snprintf(line2, sizeof(line2), "100%% gaz-ZA MALO");
  else            snprintf(line2, sizeof(line2), "%d%% gazu-ZA MALO", pct);
  printLine16(1, line2);
}

void showResult() {
  char timeStr[16];
  snprintf(timeStr, sizeof(timeStr), "czas: %lu ms", (unsigned long)reactionMs);
  printCentered16(0, timeStr);

  if (lowGasFlag) {
    printLowGasLineASCII(gazPctAtGreenOff);
  } else {
    char line2[17];
    snprintf(line2, sizeof(line2), "GAZ: %3d%%", gazPctAtGreenOff);
    printLine16(1, line2);
  }
}

void showError() {
  printLine16(0, "Blad: Falstart!");
  printLine16(1, "Wcisnij RESET   ");
}

void showBestFullScreen() {
  lcd.clear();
  char l1[17];
  if (bestTimeMs > 0) snprintf(l1, sizeof(l1), "BEST: %lu ms", (unsigned long)bestTimeMs);
  else                snprintf(l1, sizeof(l1), "BEST: ----");
  printCentered16(0, l1);
  printCentered16(1, "Release RESET");
}

void startRedPhase(uint32_t now) {
  redDurationMs = random(2000, 4001); // 2..4 s
  tRedOn = now;
  leds(true, false);  // czerwona ON
  lcd.clear();
  state = RED_ON;
}

void eraseBestAndShow(uint32_t now) {
  bestTimeMs = 0;
  prefs.putULong("best_ms", bestTimeMs);
  lcd.clear();
  printCentered16(0, "BEST cleared");
  printCentered16(1, "Release RESET");
  state = SHOW_BEST; // czekamy na puszczenie
}

// klik RESET: jesli sprzeglo >=95% -> COUNTDOWN; inaczej ekran start
void resetProcedure(bool fromButton, int pctSprz, uint32_t now) {
  errorKind = ERR_NONE;
  reactionMs = 0;
  gazPctAtGreenOff = 0;
  lowGasFlag = false;

  blinkerActive = false;
  setRGB(0,0,0);

  if (fromButton && pctSprz >= CLUTCH_PRESS_PCT) {
    lcd.clear();
    printLine16(0, "---GooMoo#312---");
    countdownVal = 4;
    countdownLastTick = now - 1000; // natychmiast pokaz "4"
    char num[4]; snprintf(num, sizeof(num), "%d", countdownVal);
    printCentered16(1, num);
    state = COUNTDOWN;
  } else {
    enterStartScreen(now);
  }
}

// Miganie RED <-> BLUE (naprzemiennie) przy falstarcie lub lowGas wyniku
void updateBlinker(uint32_t now) {
  bool active = (state == ERROR_STATE) || (state == SHOW_RESULT && lowGasFlag);
  if (active) {
    if (!blinkerActive) {
      blinkerActive = true;
      blinkBluePhase = true;
      lastBlinkToggle = now;
      // start od BLUE
      curR = 0; curG = 0; curB = BRIGHT_B; applyRGB();
    } else if (now - lastBlinkToggle >= BLINK_MS) {
      lastBlinkToggle = now;
      blinkBluePhase = !blinkBluePhase;
      if (blinkBluePhase) { curR = 0;        curG = 0; curB = BRIGHT_B; }
      else                { curR = BRIGHT_R; curG = 0; curB = 0;        }
      applyRGB();
    }
  } else {
    if (blinkerActive) {
      blinkerActive = false;
      curB = 0; curR = 0; applyRGB(); // zgas R/B
    }
  }
}

void setup() {
  Wire.begin(21, 22);

  lcd.init();   // zamiast lcd.begin(16, 2)
  lcd.backlight();

  lcd.createChar(0, bar1);
  lcd.createChar(1, bar2);
  lcd.createChar(2, bar3);
  lcd.createChar(3, bar4);
  lcd.createChar(4, bar5);

  pwmBegin();

  pinMode(PIN_BTN_RESET, INPUT_PULLUP);

  int g0 = analogRead(PIN_POT_GAZ);
  int s0 = analogRead(PIN_POT_SPRZEGL);
  filtGaz  = INVERT_GAZ     ? (4095 - g0) : g0;
  filtSprz = INVERT_SPRZEGL ? (4095 - s0) : s0;

  #if defined(ESP32)
    randomSeed(esp_random());
  #else
    randomSeed(analogRead(0) ^ millis());
  #endif

  prefs.begin("goomoo", false);
  bestTimeMs = prefs.getULong("best_ms", 0);

  enterStartScreen(millis());
}

void loop() {
  uint32_t now = millis();

  // Odczyty
  int rawGaz  = analogRead(PIN_POT_GAZ);
  int rawSprz = analogRead(PIN_POT_SPRZEGL);
  int pctGaz  = toPercent(rawGaz, INVERT_GAZ);
  int pctSprz = toPercent(rawSprz, INVERT_SPRZEGL);

  // Aktualizacja stabilizacji progow sprzegla
  updateClutchStability(pctSprz, now);

  // Debounce + klik/zwolnienie
  bool btnRead = digitalRead(PIN_BTN_RESET);
  if (btnRead != btnLast) { btnLastChange = now; btnLast = btnRead; }
  if ((now - btnLastChange) > DEBOUNCE_MS) {
    if (btnStable != btnRead) {
      btnStable = btnRead;
      if (btnStable == LOW) {
        btnStableLowSince = now;
        resetProcedure(true, pctSprz, now); // klik -> reset procedury
      } else {
        btnStableLowSince = 0;
        eraseDone = false;
        if (state == SHOW_BEST) {
          enterStartScreen(now);
        }
      }
    }
  }

  // START_SCREEN: auto-splash BEST + long/very-long press
  if (state == START_SCREEN) {
    if (!startBestShown && (now - startEnteredAt >= 1000)) {
      char l2[17];
      if (bestTimeMs > 0) snprintf(l2, sizeof(l2), "BEST: %lu ms", (unsigned long)bestTimeMs);
      else                snprintf(l2, sizeof(l2), "BEST: ----");
      printCentered16(1, l2);
      startBestShown = true;
      startBestShowing = true;
      startBestHideAt = now + 2000;
    }
    if (startBestShowing && now >= startBestHideAt) {
      printLine16(1, "WCISNIJ SPRZEGLO");
      startBestShowing = false;
    }
    // long-press 1.2s -> ekran BEST
    if (btnStable == LOW && btnStableLowSince > 0 && (now - btnStableLowSince >= LONGPRESS_MS)) {
      showBestFullScreen();
      state = SHOW_BEST;
    }
    // very-long 5s -> kasowanie rekordu
    if (!eraseDone && btnStable == LOW && btnStableLowSince > 0 &&
        (now - btnStableLowSince >= VERY_LONGPRESS_MS)) {
      eraseBestAndShow(now);
      eraseDone = true;
    }
  }

  // SHOW_BEST: pozwol skasowac rekord 5s bez wychodzenia
  if (state == SHOW_BEST) {
    if (!eraseDone && btnStable == LOW && btnStableLowSince > 0 &&
        (now - btnStableLowSince >= VERY_LONGPRESS_MS)) {
      eraseBestAndShow(now);
      eraseDone = true;
    }
  }

  // Maszyna stanów (z użyciem stabilizacji)
  switch (state) {
    case START_SCREEN:
      if (clutchAbovePressSince && (now - clutchAbovePressSince >= STABLE_PRESS_MS)) {
        startRedPhase(now);
      }
      break;

    case COUNTDOWN:
      if ((uint32_t)(now - countdownLastTick) >= 1000) {
        countdownLastTick = now;
        char num[4]; snprintf(num, sizeof(num), "%d", countdownVal);
        printCentered16(1, num);
        if (countdownVal == 0) startRedPhase(now);
        else countdownVal--;
      }
      break;

    case RED_ON:
      // Falstart tylko gdy <15% utrzyma się >= STABLE_FALSESTART_MS
      if (clutchBelowReleaseSince && (now - clutchBelowReleaseSince >= STABLE_FALSESTART_MS)) {
        errorKind = ERR_FALSE_START;
        state = ERROR_STATE;
        lcd.clear();
        showError();
        break;
      }
      if ((uint32_t)(now - tRedOn) >= redDurationMs) {
        greenDurationMs = random(2000, 6001); // 2..6 s
        tGreenOn = now;
        leds(false, true);                    // zielona ON
        state = GREEN_ON;
      }
      break;

    case GREEN_ON:
      // Falstart: jw.
      if (clutchBelowReleaseSince && (now - clutchBelowReleaseSince >= STABLE_FALSESTART_MS)) {
        errorKind = ERR_FALSE_START;
        state = ERROR_STATE;
        lcd.clear();
        showError();
        break;
      }
      if ((uint32_t)(now - tGreenOn) >= greenDurationMs) {
        leds(false, false);                 // zielona OFF
        tGreenOff = now;
        gazPctAtGreenOff = pctGaz;
        lowGasFlag = (gazPctAtGreenOff < GAZ_MIN_GO_PCT);
        state = WAIT_RELEASE_15;
      }
      break;

    case WAIT_RELEASE_15:
      // Koniec pomiaru dopiero, gdy <15% utrzyma się >= STABLE_RELEASE_MS
      if (clutchBelowReleaseSince && (now - clutchBelowReleaseSince >= STABLE_RELEASE_MS)) {
        reactionMs = now - tGreenOff;

        // Rekord tylko gdy nie "ZA MALO"
        if (!lowGasFlag) {
          if (bestTimeMs == 0 || reactionMs < bestTimeMs) {
            bestTimeMs = reactionMs;
            prefs.putULong("best_ms", bestTimeMs);
          }
        }

        state = SHOW_RESULT;
        lcd.clear();
        showResult();
      }
      break;

    case SHOW_RESULT:
      // czekamy na RESET
      break;

    case ERROR_STATE:
      // czekamy na RESET
      break;

    case SHOW_BEST:
      // czekamy na zwolnienie RESET (obsluzone wyzej)
      break;
  }

  // Paski + procenty (poza ekranami start, countdown, wynik, blad, show_best)
  static uint32_t lastRefresh = 0;
  if ((state == RED_ON || state == GREEN_ON || state == WAIT_RELEASE_15) &&
      (now - lastRefresh) >= REFRESH_MS) {
    showBars(rawGaz, rawSprz);
    lastRefresh = now;
  }

  // Miganie naprzemienne RED <-> BLUE przy falstarcie lub niskim gazie
  updateBlinker(now);
}