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

// Definicje 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;

// Inicjalizacja LCD (adres 0x27, 16 kolumn, 2 wiersze)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Zmienne globalne
enum State {
  WELCOME,
  IDLE,
  WAIT_CLUTCH,
  RED_LIGHT,
  GREEN_LIGHT,
  MEASURE,
  SHOW_RESULT
};

State currentState = WELCOME;
unsigned long stateTimer = 0;
unsigned long reactionStartTime = 0;
unsigned long reactionTime = 0;
unsigned long greenLightDuration = 0;
int throttlePercent = 0;
bool clutchPressed = false;
bool startPressed = false;
bool lastStartState = HIGH;
bool lastClutchState = HIGH;

void setup() {
  Serial.begin(115200);
  
  // Konfiguracja pinów
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(BTN_START, INPUT_PULLUP);
  pinMode(BTN_CLUTCH, INPUT_PULLUP);
  
  // Inicjalizacja LCD
  lcd.init();
  lcd.backlight();
  
  // Inicjalizacja generatora liczb losowych
  randomSeed(analogRead(35));
  
  // Wyłączenie diod
  digitalWrite(LED_RED, LOW);
  digitalWrite(LED_GREEN, LOW);
  
  // Start z ekranem powitalnym
  showWelcome();
}

void loop() {
  // Odczyt przycisków z debouncingiem
  checkButtons();
  
  // Odczyt potencjometru
  int potValue = analogRead(POT_PIN);
  throttlePercent = map(potValue, 0, 4095, 0, 100);
  
  // Maszyna stanów
  switch(currentState) {
    case WELCOME:
      if (millis() - stateTimer > 2000) {
        showIdle();
      }
      break;
      
    case IDLE:
      if (startPressed) {
        showWaitClutch();
      }
      break;
      
    case WAIT_CLUTCH:
      if (clutchPressed) {
        showRedLight();
      } else if (millis() - stateTimer > 5000) {
        showRedLight();
      }
      break;
      
    case RED_LIGHT:
      updateRedLightDisplay();
      if (millis() - stateTimer > 3000) {
        showGreenLight();
      }
      break;
      
    case GREEN_LIGHT:
      updateGreenLightDisplay();
      if (millis() - stateTimer > greenLightDuration) {
        startMeasurement();
      }
      break;
      
    case MEASURE:
      if (!clutchPressed) {
        reactionTime = millis() - reactionStartTime;
        showResult();
      }
      updateMeasureDisplay();
      break;
      
    case SHOW_RESULT:
      if (startPressed) {
        resetSystem();
      }
      break;
  }
}

void checkButtons() {
  // Odczyt przycisku START z debouncingiem
  bool currentStartState = digitalRead(BTN_START);
  if (currentStartState == LOW && lastStartState == HIGH) {
    delay(50); // debouncing
    if (digitalRead(BTN_START) == LOW) {
      startPressed = true;
    }
  } else {
    startPressed = false;
  }
  lastStartState = currentStartState;
  
  // Odczyt przycisku SPRZĘGŁO
  bool currentClutchState = digitalRead(BTN_CLUTCH);
  if (currentClutchState == LOW) {
    clutchPressed = true;
  } else {
    clutchPressed = false;
  }
  lastClutchState = currentClutchState;
}

void showWelcome() {
  lcd.clear();
  lcd.setCursor(1, 0);
  lcd.print("--GooMoo#312--");
  lcd.setCursor(2, 1);
  lcd.print("Test Refleksu");
  stateTimer = millis();
  currentState = WELCOME;
}

void showIdle() {
  lcd.clear();
  lcd.setCursor(1, 0);
  lcd.print("Nacisnij START");
  currentState = IDLE;
}

void showWaitClutch() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Wcisnij sprzeglo");
  lcd.setCursor(3, 1);
  lcd.print("nakroc gaz");
  stateTimer = millis();
  currentState = WAIT_CLUTCH;
}

void showRedLight() {
  digitalWrite(LED_RED, HIGH);
  digitalWrite(LED_GREEN, LOW);
  stateTimer = millis();
  currentState = RED_LIGHT;
}

void updateRedLightDisplay() {
  lcd.clear();
  
  // Pierwsza linia - status sprzęgła
  lcd.setCursor(0, 0);
  if (clutchPressed) {
    lcd.print("Sprzeglo: OK");
  } else {
    lcd.print("Sprzeglo: BRAK");
  }
  
  // Druga linia - poziom gazu graficznie
  lcd.setCursor(0, 1);
  lcd.print("Gaz:");
  int barLength = map(throttlePercent, 0, 100, 0, 12);
  for (int i = 0; i < barLength; i++) {
    lcd.print((char)255); // Pełny blok
  }
}

void showGreenLight() {
  digitalWrite(LED_RED, LOW);
  digitalWrite(LED_GREEN, HIGH);
  greenLightDuration = random(1000, 7001); // 1-7 sekund
  stateTimer = millis();
  currentState = GREEN_LIGHT;
}

void updateGreenLightDisplay() {
  // Kontynuuj wyświetlanie tego samego co przy czerwonej
  updateRedLightDisplay();
}

void startMeasurement() {
  digitalWrite(LED_GREEN, LOW);
  reactionStartTime = millis();
  currentState = MEASURE;
}

void updateMeasureDisplay() {
  lcd.clear();
  lcd.setCursor(0, 0);
  if (clutchPressed) {
    lcd.print("Czekam...");
  } else {
    lcd.print("Reakcja: ");
    lcd.print(reactionTime);
    lcd.print("ms");
  }
  
  lcd.setCursor(0, 1);
  lcd.print("Gaz: ");
  lcd.print(throttlePercent);
  lcd.print("%");
}

void showResult() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Czas: ");
  lcd.print(reactionTime);
  lcd.print(" ms");
  
  lcd.setCursor(0, 1);
  lcd.print("Gaz: ");
  lcd.print(throttlePercent);
  lcd.print("%");
  
  currentState = SHOW_RESULT;
}

void resetSystem() {
  digitalWrite(LED_RED, LOW);
  digitalWrite(LED_GREEN, LOW);
  reactionTime = 0;
  showWelcome();
}