#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#if defined(ESP32)
  #include "esp_system.h" // dla esp_random() do seedowania RNG
#endif

// --- Ustawienia LCD ---
constexpr uint8_t I2C_ADDR = 0x27;     // zmień na 0x3F jeśli Twój LCD ma inny adres
LiquidCrystal_I2C lcd(I2C_ADDR, 16, 2);

// --- Piny ESP32 ---
constexpr int PIN_POT_GAZ      = 34;   // ADC1 (wejściowy tylko)
constexpr int PIN_POT_SPRZEGL  = 35;   // ADC1 (wejściowy tylko)
constexpr int PIN_LED_GREEN    = 25;   // zielona dioda
constexpr int PIN_LED_RED      = 26;   // czerwona dioda
constexpr int PIN_BTN_RESET    = 27;   // przycisk do GND (INPUT_PULLUP)

// --- Parametry pasków na LCD ---
constexpr int LCD_COLS = 16;
constexpr int PREFIX_COLS = 2;                 // "G_" / "S_"
constexpr int BAR_COLS = LCD_COLS - PREFIX_COLS; // 14 kolumn paska
constexpr int SEG_PER_CELL = 5;                // HD44780: 5 “pikseli” poziomych/znak
constexpr uint32_t REFRESH_MS = 50;            // odświeżanie LCD

// --- Progi i logika ---
constexpr int CLUTCH_PRESS_PCT   = 95;         // sprzęgło uznane za wciśnięte
constexpr int CLUTCH_RELEASE_PCT = 15;         // “puszczone” (koniec pomiaru)
constexpr bool INVERT_GAZ        = false;      // odwróć kierunek, jeśli “rośnie” odwrotnie
constexpr bool INVERT_SPRZEGL    = false;      // j.w. dla sprzęgła

// --- Własne znaki do paska (sloty 0..4) ---
byte bar1[8] = {B10000,B10000,B10000,B10000,B10000,B10000,B10000,B10000}; // 1/5
byte bar2[8] = {B11000,B11000,B11000,B11000,B11000,B11000,B11000,B11000}; // 2/5
byte bar3[8] = {B11100,B11100,B11100,B11100,B11100,B11100,B11100,B11100}; // 3/5
byte bar4[8] = {B11110,B11110,B11110,B11110,B11110,B11110,B11110,B11110}; // 4/5
byte bar5[8] = {B11111,B11111,B11111,B11111,B11111,B11111,B11111,B11111}; // 5/5

// --- Wygładzanie do LCD (nie używamy do progów/czasu!) ---
constexpr float ALPHA = 0.2f;
float filtGaz = 0.0f;
float filtSprz = 0.0f;

// --- Debounce przycisku ---
constexpr uint32_t DEBOUNCE_MS = 30;
bool btnLast = HIGH, btnStable = HIGH;
uint32_t btnLastChange = 0;

// --- Stan maszyny ---
enum State { WAIT_CLUTCH_95, GREEN_ON, WAIT_RELEASE_15, SHOW_RESULT };
State state = WAIT_CLUTCH_95;

// --- Czas i wyniki ---
uint32_t tGreenOn = 0;
uint32_t tGreenOff = 0;
uint32_t greenDurationMs = 0;
uint32_t reactionMs = 0;
int gazPctAtGreenOff = 0;

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

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 >= SEG_PER_CELL) {
      lcd.write(byte(4));       // pełny
    } else {
      lcd.write(byte(seg - 1)); // częściowy
    }
  }
}

void showBars(int rawGaz, int rawSprz) {
  // Etykiety
  lcd.setCursor(0, 0); lcd.print("G_");
  lcd.setCursor(0, 1); lcd.print("S_");

  // Do pasków używamy surowych ADC +/- odwrócenie
  int rawGazDisp  = INVERT_GAZ     ? (4095 - rawGaz)  : rawGaz;
  int rawSprzDisp = INVERT_SPRZEGL ? (4095 - rawSprz) : rawSprz;

  // Wygładzanie tylko do pokazu
  filtGaz  = ALPHA * rawGazDisp  + (1.0f - ALPHA) * filtGaz;
  filtSprz = ALPHA * rawSprzDisp + (1.0f - ALPHA) * filtSprz;

  const int maxSteps = BAR_COLS * SEG_PER_CELL;
  int stepsG = map((int)filtGaz,  0, 4095, 0, maxSteps);
  int stepsS = map((int)filtSprz, 0, 4095, 0, maxSteps);

  drawBarAt(PREFIX_COLS, 0, BAR_COLS, stepsG);
  drawBarAt(PREFIX_COLS, 1, BAR_COLS, stepsS);
}

void showResult() {
  lcd.setCursor(0, 0);
  char line1[17]; snprintf(line1, sizeof(line1), "Wynik: %6lu ms", (unsigned long)reactionMs);
  for (int i = 0; i < 16; ++i) lcd.write(line1[i] ? line1[i] : ' ');

  lcd.setCursor(0, 1);
  char line2[17]; snprintf(line2, sizeof(line2), "GAZ: %3d%%         ", gazPctAtGreenOff);
  for (int i = 0; i < 16; ++i) lcd.write(line2[i] ? line2[i] : ' ');
}

void leds(bool redOn, bool greenOn) {
  digitalWrite(PIN_LED_RED,   redOn   ? HIGH : LOW);
  digitalWrite(PIN_LED_GREEN, greenOn ? HIGH : LOW);
}

void resetProcedure() {
  state = WAIT_CLUTCH_95;
  reactionMs = 0;
  gazPctAtGreenOff = 0;
  leds(true, false);           // czerwona ON, zielona OFF
  lcd.clear();                 // wracamy do pasków w pętli
}

// --- Setup ---
void setup() {
  // I2C
  Wire.begin(21, 22);
  lcd.init();
  lcd.backlight();
  lcd.clear();

  // Rejestracja znaków
  lcd.createChar(0, bar1);
  lcd.createChar(1, bar2);
  lcd.createChar(2, bar3);
  lcd.createChar(3, bar4);
  lcd.createChar(4, bar5);

  // Piny
  pinMode(PIN_LED_GREEN, OUTPUT);
  pinMode(PIN_LED_RED,   OUTPUT);
  pinMode(PIN_BTN_RESET, INPUT_PULLUP);

  leds(true, false); // startowo czerwona świeci

  // ADC (opcjonalnie dopasowanie)
  // analogReadResolution(12);
  // analogSetPinAttenuation(PIN_POT_GAZ,     ADC_11db);
  // analogSetPinAttenuation(PIN_POT_SPRZEGL, ADC_11db);

  // Inicjalizacja filtrów
  int g0 = analogRead(PIN_POT_GAZ);
  int s0 = analogRead(PIN_POT_SPRZEGL);
  filtGaz  = INVERT_GAZ     ? (4095 - g0) : g0;
  filtSprz = INVERT_SPRZEGL ? (4095 - s0) : s0;

  // Losowość
  #if defined(ESP32)
    randomSeed(esp_random());
  #else
    randomSeed(analogRead(A0) ^ millis());
  #endif
}

// --- Pętla główna ---
void loop() {
  uint32_t now = millis();

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

  // Debounce przycisku i reset na klik
  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) { // klik
        resetProcedure();
      }
    }
  }

  // Maszyna stanów (progowe decyzje na surowych odczytach)
  switch (state) {
    case WAIT_CLUTCH_95:
      // Czekamy aż sprzęgło >= 95%
      if (pctSprz >= CLUTCH_PRESS_PCT) {
        leds(false, true);                    // czerwona OFF, zielona ON
        greenDurationMs = random(2000, 6001); // 2000..6000 ms
        tGreenOn = now;
        tGreenOff = tGreenOn + greenDurationMs;
        state = GREEN_ON;
      }
      break;

    case GREEN_ON:
      // Zielona świeci, czekamy na jej zgaśnięcie po losowym czasie
      if (now >= tGreenOff) {
        leds(false, false);                   // zielona OFF
        gazPctAtGreenOff = pctGaz;            // zapamiętaj GAZ w chwili zgaśnięcia
        state = WAIT_RELEASE_15;              // start pomiaru
      }
      break;

    case WAIT_RELEASE_15:
      // Mierzymy czas od zgaśnięcia zielonej do puszczenia sprzęgła < 15%
      if (pctSprz < CLUTCH_RELEASE_PCT) {
        reactionMs = now - tGreenOff;
        state = SHOW_RESULT;
        lcd.clear();
        showResult();
      }
      break;

    case SHOW_RESULT:
      // nic nie robimy, czekamy na przycisk reset
      break;
  }

  // Odświeżanie LCD (paski) jeśli nie jesteśmy w SHOW_RESULT
  static uint32_t lastRefresh = 0;
  if (state != SHOW_RESULT && (now - lastRefresh) >= REFRESH_MS) {
    showBars(rawGaz, rawSprz);
    lastRefresh = now;
  }
}