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

// Konfiguracja
const int startResetPin = 9;   // START/RESET z podciągnięciem
const int clutchPin = 2;       // Sprzęgło z podciągnięciem
const int redLedPin = 3;       // LED czerwony
const int greenLedPin = 4;     // LED zielony

LiquidCrystal_I2C lcd(0x27, 16, 2); // I2C 0x27

// Stany
enum State {
  IDLE,
  PREPARE,
  HOLDING,
  MEASURING,
  RESULT
};

State currentState = IDLE;
unsigned long stageStartTime;
unsigned long lightDuration;
unsigned long reactionTime;
bool lastStartButtonState = HIGH;

void setup() {
  pinMode(startResetPin, INPUT_PULLUP);
  pinMode(clutchPin, INPUT_PULLUP);
  pinMode(redLedPin, OUTPUT);
  pinMode(greenLedPin, OUTPUT);

  lcd.init();
  lcd.backlight();
  showStartScreen();
  randomSeed(analogRead(0));
}

void loop() {
  // Poprawne wykrywanie naciśnięcia START
  bool currentStartState = digitalRead(startResetPin);
  if (lastStartButtonState == HIGH && currentStartState == LOW) {
    delay(20); // Debouncing
    if (digitalRead(startResetPin) == LOW) { // Potwierdź naciśnięcie
      handleStartButton();
    }
  }
  lastStartButtonState = currentStartState;

  // Automatyczna logika testu
  updateTest();
}

void handleStartButton() {
  if (currentState == IDLE || currentState == RESULT) {
    startTest();
  } else {
    cancelTest();
  }
  while (digitalRead(startResetPin) == LOW); // Czekaj na zwolnienie
}

void startTest() {
  currentState = PREPARE;
  digitalWrite(redLedPin, HIGH);
  digitalWrite(greenLedPin, LOW);
  lcd.clear();
  lcd.print("Przygotuj sie...");
  stageStartTime = millis();
}

void updateTest() {
  switch (currentState) {
    case PREPARE:
      if (millis() - stageStartTime > 3000) {
        startGreenPhase();
      }
      break;

    case HOLDING:
      if (digitalRead(clutchPin) == HIGH) {
        earlyRelease();
      } else if (millis() - stageStartTime > lightDuration) {
        startMeasurement();
      }
      break;

    case MEASURING:
      if (digitalRead(clutchPin) == HIGH) {
        finishTest();
      }
      break;

    case RESULT:
      if (millis() - stageStartTime > 10000) {
        showStartScreen();
        currentState = IDLE;
      }
      break;
  }
}

void startGreenPhase() {
  currentState = HOLDING;
  digitalWrite(redLedPin, LOW);
  digitalWrite(greenLedPin, HIGH);
  lcd.clear();
  lcd.print("TRZYMAJ SPRZEGLO!");
  lightDuration = random(1000, 7001);
  stageStartTime = millis();
}

void startMeasurement() {
  currentState = MEASURING;
  digitalWrite(greenLedPin, LOW);
  lcd.clear();
  lcd.print("PUSC SPRZEGLO!");
  stageStartTime = millis();
}

void earlyRelease() {
  lcd.clear();
  lcd.print("Za wczesnie!");
  lcd.setCursor(0, 1);
  lcd.print("START - ponow");
  digitalWrite(redLedPin, LOW);
  digitalWrite(greenLedPin, LOW);
  currentState = IDLE;
}

void finishTest() {
  reactionTime = millis() - stageStartTime;
  currentState = RESULT;
  lcd.clear();
  lcd.print("Czas reakcji:");
  lcd.setCursor(0, 1);
  lcd.print(String(reactionTime) + " ms");
  stageStartTime = millis();
}

void cancelTest() {
  digitalWrite(redLedPin, LOW);
  digitalWrite(greenLedPin, LOW);
  showStartScreen();
  currentState = IDLE;
}

void showStartScreen() {
  lcd.clear();
  lcd.print("TEST REFLEKSU");
  lcd.setCursor(0, 1);
  lcd.print("START - rozpoczynaj");
}