#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Definicja pinów
#define START_BUTTON_PIN 34
#define CLUTCH_BUTTON_PIN 35
#define RED_LED_PIN 25
#define GREEN_LED_PIN 26

enum State { IDLE, PREPARE, READY, MEASURE, RESULT };
State currentState = IDLE;

unsigned long prepareStartTime;
unsigned long readyStartTime;
unsigned long reactionTime;
unsigned long randomDelay;
bool startButtonPressed = false;

void setup() {
  Serial.begin(115200);
  pinMode(START_BUTTON_PIN, INPUT_PULLUP);
  pinMode(CLUTCH_BUTTON_PIN, INPUT_PULLUP);
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  digitalWrite(RED_LED_PIN, LOW);
  digitalWrite(GREEN_LED_PIN, LOW);

  // Inicjalizacja wyświetlacza
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  drawIdleScreen();
}

void loop() {
  // Obsługa przycisku START
  if (digitalRead(START_BUTTON_PIN) == LOW && !startButtonPressed) {
    startButtonPressed = true;
    delay(50); // Debouncing
    
    if (currentState == IDLE) {
      startTest();
    } 
    else if (currentState == RESULT) {
      resetTest();
    }
  }
  
  if (digitalRead(START_BUTTON_PIN) == HIGH) {
    startButtonPressed = false;
  }

  // Logika stanów
  switch(currentState) {
    case PREPARE:
      handlePrepareState();
      break;
      
    case READY:
      handleReadyState();
      break;
      
    case MEASURE:
      handleMeasureState();
      break;
  }
}

void startTest() {
  currentState = PREPARE;
  prepareStartTime = millis();
  digitalWrite(RED_LED_PIN, HIGH);
  digitalWrite(GREEN_LED_PIN, LOW);
  drawPrepareScreen();
}

void handlePrepareState() {
  if (digitalRead(CLUTCH_BUTTON_PIN) == LOW) {
    currentState = READY;
    readyStartTime = millis();
    randomDelay = random(1000, 7001); // Losowe opóźnienie 1-7 sekund
    digitalWrite(RED_LED_PIN, LOW);
    digitalWrite(GREEN_LED_PIN, HIGH);
    drawReadyScreen();
  } 
  else if (millis() - prepareStartTime > 3000) {
    currentState = IDLE;
    drawErrorScreen("Nie wcisnieto sprzegla!");
    digitalWrite(RED_LED_PIN, LOW);
    delay(2000);
    drawIdleScreen();
  }
}

void handleReadyState() {
  if (digitalRead(CLUTCH_BUTTON_PIN) == HIGH) {
    currentState = IDLE;
    drawErrorScreen("Za wczesnie!");
    digitalWrite(GREEN_LED_PIN, LOW);
    delay(2000);
    drawIdleScreen();
  } 
  else if (millis() - readyStartTime > randomDelay) {
    currentState = MEASURE;
    digitalWrite(GREEN_LED_PIN, LOW);
    drawMeasureScreen();
  }
}

void handleMeasureState() {
  if (digitalRead(CLUTCH_BUTTON_PIN) == HIGH) {
    reactionTime = millis() - (readyStartTime + randomDelay);
    currentState = RESULT;
    drawResultScreen(reactionTime);
  }
}

void resetTest() {
  currentState = IDLE;
  digitalWrite(RED_LED_PIN, LOW);
  digitalWrite(GREEN_LED_PIN, LOW);
  drawIdleScreen();
}

void drawIdleScreen() {
  display.clearDisplay();
  display.setTextSize(2);
  display.setCursor(20, 10);
  display.println("TEST REAKCJI");
  display.setTextSize(1);
  display.setCursor(20, 40);
  display.println("START - rozpoczecie");
  display.display();
}

void drawPrepareScreen() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(15, 10);
  display.println("PRZYGOTUJ SIE");
  display.setCursor(15, 30);
  display.println("Wcisnij sprzeglo");
  display.setCursor(15, 50);
  display.println("Czas: 3s");
  display.display();
}

void drawReadyScreen() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(30, 20);
  display.println("GOTOWY");
  display.setCursor(10, 40);
  display.println("Czekaj na sygnal");
  display.display();
}

void drawMeasureScreen() {
  display.clearDisplay();
  display.setTextSize(2);
  display.setCursor(40, 20);
  display.println("PUSC!");
  display.display();
}

void drawResultScreen(unsigned long time) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(40, 10);
  display.println("WYNIK:");
  display.setTextSize(2);
  display.setCursor(30, 30);
  display.print(time);
  display.println(" ms");
  display.setTextSize(1);
  display.setCursor(15, 55);
  display.println("START - nowy test");
  display.display();
}

void drawErrorScreen(const char* message) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(20, 20);
  display.println("BLAD:");
  display.setCursor(10, 40);
  display.println(message);
  display.display();
}