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

// --- Konfiguracja pinów ---
const int LED_RED = 25;
const int LED_GREEN = 26;
const int BTN_START = 32;
const int BTN_CLUTCH = 33;
const int POT_PIN = 34;      // ADC1_CH6 (GPIO34)

// --- Konfiguracja wyświetlacza LCD ---
// Sprawdź adres I2C swojego modułu (najczęściej 0x27 lub 0x3F)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// --- Stany programu (maszyna stanów) ---
enum State {
  WELCOME,
  IDLE,
  PREPARE,
  READY,
  SET,
  GO,
  SHOW_RESULT
};
State currentState = WELCOME;

// --- Zmienne pomocnicze ---
unsigned long stateChangeTime = 0;
unsigned long randomDelay = 0;
unsigned long reactionStartTime = 0;
long reactionTime = 0;

// Zmienne do obsługi przycisków (debounce)
bool lastClutchState = HIGH;
bool lastStartState = HIGH;
bool clutchPressed = false;
bool startPressed = false;

// Definicja własnego znaku dla paska postępu
byte customChar[] = {
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111
};

// --- Funkcja setup() - uruchamiana raz na starcie ---
void setup() {
  // Inicjalizacja pinów
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(BTN_START, INPUT_PULLUP);   // Używamy wewnętrznego rezystora podciągającego
  pinMode(BTN_CLUTCH, INPUT_PULLUP);  // Dzięki temu wciśnięty przycisk daje stan LOW
  pinMode(POT_PIN, INPUT);

  // Zgaszenie diod na starcie
  digitalWrite(LED_RED, LOW);
  digitalWrite(LED_GREEN, LOW);

  // Inicjalizacja LCD
  lcd.init();
  lcd.backlight();
  lcd.createChar(0, customChar); // Tworzymy znak o indeksie 0

  // Inicjalizacja generatora liczb losowych
  randomSeed(analogRead(POT_PIN));
}

// --- Główna pętla programu ---
void loop() {
  // Odczyt stanu przycisków z prostym debounce
  // Sprawdzamy zmianę stanu z HIGH na LOW (wciśnięcie)
  bool currentStartState = digitalRead(BTN_START);
  if (lastStartState == HIGH && currentStartState == LOW) {
    startPressed = true;
  } else {
    startPressed = false;
  }
  lastStartState = currentStartState;

  bool currentClutchState = digitalRead(BTN_CLUTCH);
  if (lastClutchState == HIGH && currentClutchState == LOW) {
    clutchPressed = true;
  }
  lastClutchState = currentClutchState;

  // Maszyna stanów
  switch (currentState) {
    case WELCOME:
      // Wyświetl ekran powitalny
      lcd.setCursor(0, 0);
      lcd.print("---GooMoo#312---");
      lcd.setCursor(0, 1);
      lcd.print(" Test Refleksu  ");
      stateChangeTime = millis();
      currentState = IDLE; // Przejdź do stanu oczekiwania po pokazaniu ekranu
      delay(2000); // Wyświetlaj przez 2 sekundy
      break;

    case IDLE:
      // Oczekiwanie na wciśnięcie START
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print(" Naci_nij START ");
      
      if (startPressed) {
        stateChangeTime = millis();
        currentState = PREPARE;
      }
      break;

    case PREPARE:
      // Oczekiwanie na wciśnięcie sprzęgła
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Wci_nij sprz_g_o");
      lcd.setCursor(0, 1);
      lcd.print("  nakr__ gaz    ");

      // Sprawdź, czy wciśnięto sprzęgło lub minęło 5 sekund
      if (clutchPressed) {
        stateChangeTime = millis();
        currentState = READY;
      } else if (millis() - stateChangeTime > 5000) {
        currentState = IDLE; // Wróć do początku, jeśli nie wciśnięto sprzęgła
      }
      break;

    case READY:
      // Czerwone światło - przygotowanie
      digitalWrite(LED_RED, HIGH);
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Sprz_g_o OK!   ");
      
      // Rysowanie paska "gazu"
      drawThrottleBar();

      // Po 3 sekundach przejdź do następnego stanu
      if (millis() - stateChangeTime > 3000) {
        digitalWrite(LED_RED, LOW);
        stateChangeTime = millis();
        randomDelay = random(1000, 7001); // Losowy czas od 1 do 7 sekund
        currentState = SET;
      }
      break;

    case SET:
      // Zielone światło - oczekiwanie
      digitalWrite(LED_GREEN, HIGH);
      drawThrottleBar(); // Cały czas pokazuj pozycję gazu

      // Czekaj losowy czas, a następnie zgaś diodę
      if (millis() - stateChangeTime > randomDelay) {
        digitalWrite(LED_GREEN, LOW);
        reactionStartTime = millis(); // Rozpocznij pomiar czasu!
        currentState = GO;
      }
      break;

    case GO:
      // Pomiar czasu reakcji - czekanie na puszczenie sprzęgła
      drawThrottleBar(); // Można też pokazywać pasek gazu tutaj

      // Sprawdzamy, czy przycisk sprzęgła został puszczony (stan HIGH)
      if (digitalRead(BTN_CLUTCH) == HIGH) {
        reactionTime = millis() - reactionStartTime;
        currentState = SHOW_RESULT;
      }
      break;

    case SHOW_RESULT:
      // Wyświetl wynik
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Czas: ");
      lcd.print(reactionTime);
      lcd.print(" ms");

      lcd.setCursor(0, 1);
      lcd.print("Gaz: ");
      // Mapowanie wartości z ADC (0-4095 dla ESP32) na procenty
      int throttlePercent = map(analogRead(POT_PIN), 0, 4095, 0, 100);
      lcd.print(throttlePercent);
      lcd.print(" %");

      // Czekaj na wciśnięcie START, aby zrestartować cykl
      if (startPressed) {
        currentState = IDLE;
      }
      break;
  }
}

// Funkcja do rysowania paska gazu na wyświetlaczu
void drawThrottleBar() {
  int potValue = analogRead(POT_PIN);
  // Mapowanie wartości ADC (12-bit, 0-4095) na 16 pozycji na LCD
  int barLength = map(potValue, 0, 4095, 0, 16);
  
  lcd.setCursor(0, 1);
  for (int i = 0; i < 16; i++) {
    if (i < barLength) {
      lcd.write((byte)0); // Wypisz nasz własny znak
    } else {
      lcd.print(" ");
    }
  }
}