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

// --- LCD ---
constexpr uint8_t I2C_ADDR = 0x27;     // zmień na 0x3F jeśli potrzeba
LiquidCrystal_I2C lcd(I2C_ADDR, 16, 2);

// --- Piny ---
constexpr int PIN_POT_GAZ      = 34;   // ADC1
constexpr int PIN_POT_SPRZEGL  = 35;   // ADC1
constexpr int PIN_LED_GREEN    = 25;
constexpr int PIN_LED_RED      = 26;
constexpr int PIN_BTN_RESET    = 27;   // do GND (INPUT_PULLUP)

// --- Pasek na LCD ---
constexpr int LCD_COLS = 16;
constexpr int PREFIX_COLS = 2;                 // "G_"/"S_"
constexpr int BAR_COLS = LCD_COLS - PREFIX_COLS; // 14
constexpr int SEG_PER_CELL = 5;
constexpr uint32_t REFRESH_MS = 50;

// --- Progi i logika ---
constexpr int CLUTCH_PRESS_PCT   = 95; // wciśnięte
constexpr int CLUTCH_RELEASE_PCT = 15; // puszczone
constexpr int GAZ_MIN_GO_PCT     = 85; // próg “mało gazu”
constexpr bool INVERT_GAZ        = false;
constexpr bool INVERT_SPRZEGL    = false;

// --- Własne znaki (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 (dla pasków) ---
constexpr float ALPHA = 0.2f;
float filtGaz = 0.0f, 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, ERROR_STATE };
State state = WAIT_CLUTCH_95;

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

// --- Pomiary i wyniki ---
uint32_t tGreenOn = 0;
uint32_t greenDurationMs = 0;
uint32_t tGreenOff = 0;
uint32_t reactionMs = 0;
int gazPctAtGreenOff = 0;
bool lowGasFlag = false;

// --- Pomocnicze ---
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 printLine16(uint8_t row, const char* text) {
  lcd.setCursor(0, row);
  for (int i = 0; i < 16; ++i) {
    char c = text[i];
    if (c == '\0') c = ' ';
    lcd.write(c);
  }
}

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) {
  lcd.setCursor(0, 0); lcd.print("G_");
  lcd.setCursor(0, 1); lcd.print("S_");

  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;

  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() {
  // Linia 1: wynik
  char line1[17];
  snprintf(line1, sizeof(line1), "Wynik: %6lu ms", (unsigned long)reactionMs);
  printLine16(0, line1);

  // Linia 2: GAZ + ewentualnie "Malo gazu!!" (przycięte do 16 znaków)
  char buf[32];
  if (lowGasFlag) {
    snprintf(buf, sizeof(buf), "GAZ: %3d%% Malo gazu!!", gazPctAtGreenOff);
  } else {
    snprintf(buf, sizeof(buf), "GAZ: %3d%%", gazPctAtGreenOff);
  }
  printLine16(1, buf);
}

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

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;
  errorKind = ERR_NONE;
  reactionMs = 0;
  gazPctAtGreenOff = 0;
  lowGasFlag = false;
  lcd.clear();
  leds(true, false); // start: czerwona ON
}

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

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

  pinMode(PIN_LED_GREEN, OUTPUT);
  pinMode(PIN_LED_RED,   OUTPUT);
  pinMode(PIN_BTN_RESET, INPUT_PULLUP);
  leds(true, false);

  // analogReadResolution(12);
  // analogSetPinAttenuation(PIN_POT_GAZ,     ADC_11db);
  // analogSetPinAttenuation(PIN_POT_SPRZEGL, ADC_11db);

  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
}

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

  // Surowe odczyty i procenty
  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
  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) resetProcedure();
    }
  }

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

    case GREEN_ON: {
      // Falstart: puszczenie <15% zanim zgaśnie zielona
      if (pctSprz < CLUTCH_RELEASE_PCT) {
        errorKind = ERR_FALSE_START;
        state = ERROR_STATE;
        leds(true, false); // czerwona ON, zielona OFF
        lcd.clear();
        showError();
        break;
      }
      // Zgaśnięcie zielonej po losowym czasie
      if ((uint32_t)(now - tGreenOn) >= greenDurationMs) {
        leds(false, false);                 // zielona OFF
        tGreenOff = now;
        gazPctAtGreenOff = pctGaz;          // zapamiętaj gaz
        lowGasFlag = (gazPctAtGreenOff < GAZ_MIN_GO_PCT);
        state = WAIT_RELEASE_15;            // zaczynamy pomiar
      }
      break;
    }

    case WAIT_RELEASE_15:
      if (pctSprz < CLUTCH_RELEASE_PCT) {
        reactionMs = now - tGreenOff;
        state = SHOW_RESULT;
        lcd.clear();
        showResult();
      }
      break;

    case SHOW_RESULT:
      // Nic nie robimy; czekamy na RESET
      break;

    case ERROR_STATE:
      // Nic nie robimy; czekamy na RESET
      break;
  }

  // Odświeżanie pasków, jeśli w trybie “przed wynikiem”
  static uint32_t lastRefresh = 0;
  if ((state == WAIT_CLUTCH_95 || state == GREEN_ON || state == WAIT_RELEASE_15) &&
      (now - lastRefresh) >= REFRESH_MS) {
    showBars(rawGaz, rawSprz);
    lastRefresh = now;
  }
}