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

// Definicje pinów dla ESP32
#define RED_LED_PIN 25
#define GREEN_LED_PIN 26
#define CLUTCH_BUTTON_PIN 27
#define THROTTLE_POT_PIN 34

// Konfiguracja matrycy LED
#define HARDWARE_TYPE MD_MAX72XX::FC16_HW
#define MAX_DEVICES 4  // 4 moduły = 32x8 pikseli
#define CLK_PIN    14
#define DATA_PIN   13
#define CS_PIN     15

// Inicjalizacja wyświetlaczy
LiquidCrystal_I2C lcd(0x27, 16, 2);
MD_MAX72XX ledMatrix = MD_MAX72XX(HARDWARE_TYPE, DATA_PIN, CLK_PIN, CS_PIN, MAX_DEVICES);

// Zmienne stanu
enum State {
  WELCOME,
  WAIT_FOR_CLUTCH,
  PREPARE,
  REACTION_TEST,
  SHOW_RESULT
};

State currentState = WELCOME;
unsigned long stateStartTime;
unsigned long reactionStartTime;
unsigned long reactionTime;
bool clutchPressed = false;
bool lastClutchState = false;

void setup() {
  // Inicjalizacja pinów
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(CLUTCH_BUTTON_PIN, INPUT_PULLUP);
  
  // Inicjalizacja wyświetlaczy
  lcd.init();
  lcd.backlight();
  ledMatrix.begin();
  ledMatrix.control(MD_MAX72XX::INTENSITY, 5);
  ledMatrix.clear();
  
  changeState(WELCOME);
}

void loop() {
  bool currentClutchState = digitalRead(CLUTCH_BUTTON_PIN) == LOW;
  if (currentClutchState && !lastClutchState) {
    clutchPressed = true;
  }
  lastClutchState = currentClutchState;
  
  switch (currentState) {
    case WELCOME:
      if (millis() - stateStartTime >= 5000) {
        changeState(WAIT_FOR_CLUTCH);
      }
      break;
      
    case WAIT_FOR_CLUTCH:
      if (clutchPressed) {
        changeState(PREPARE);
        clutchPressed = false;
      }
      break;
      
    case PREPARE:
      if (millis() - stateStartTime >= 2000) {
        changeState(REACTION_TEST);
      }
      break;
      
    case REACTION_TEST:
      if (!currentClutchState) {
        reactionTime = millis() - reactionStartTime;
        changeState(SHOW_RESULT);
      } else {
        displayThrottle(analogRead(THROTTLE_POT_PIN));
      }
      break;
      
    case SHOW_RESULT:
      if (clutchPressed) {
        changeState(WAIT_FOR_CLUTCH);
        clutchPressed = false;
      }
      break;
  }
}

void changeState(State newState) {
  currentState = newState;
  stateStartTime = millis();
  
  switch (newState) {
    case WELCOME:
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("---GooMoo#312---");
      lcd.setCursor(0, 1);
      lcd.print("TEST REAKCJI");
      digitalWrite(RED_LED_PIN, LOW);
      digitalWrite(GREEN_LED_PIN, LOW);
      ledMatrix.clear();
      break;
      
    case WAIT_FOR_CLUTCH:
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Aby rozpoczac");
      lcd.setCursor(0, 1);
      lcd.print("Wcisnij Sprzeglo");
      digitalWrite(RED_LED_PIN, LOW);
      digitalWrite(GREEN_LED_PIN, LOW);
      ledMatrix.clear();
      break;
      
    case PREPARE:
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Przygotuj sie");
      digitalWrite(RED_LED_PIN, HIGH);
      digitalWrite(GREEN_LED_PIN, LOW);
      ledMatrix.clear();
      break;
      
    case REACTION_TEST:
      digitalWrite(RED_LED_PIN, LOW);
      digitalWrite(GREEN_LED_PIN, HIGH);
      reactionStartTime = millis() + random(1000, 7000);
      break;
      
    case SHOW_RESULT:
      digitalWrite(GREEN_LED_PIN, LOW);
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Nastepny TEST");
      lcd.setCursor(0, 1);
      lcd.print("Wcisnij sprzeglo");
      displayResult(reactionTime);
      break;
  }
}

void displayThrottle(int value) {
  int width = map(value, 0, 1023, 0, 32);
  ledMatrix.clear();
  
  // Rysowanie trójkąta od prawej
  for (int x = 0; x < width; x++) {
    int height = map(x, 0, 31, 1, 8);
    for (int y = 0; y < height; y++) {
      ledMatrix.setPoint(7 - y, 31 - x, true);
    }
  }
}

void displayResult(unsigned long timeMs) {
  ledMatrix.clear();
  
  // Konwersja czasu na string
  char timeStr[10];
  sprintf(timeStr, "%lu", timeMs);
  int length = strlen(timeStr);
  
  // Wyświetlanie wyniku
  for (int i = 0; i < length; i++) {
    ledMatrix.setChar((3 - i) * 8, timeStr[length - 1 - i]);
  }
  
  // Dodanie "ms" na końcu
  delay(500);
  ledMatrix.setChar(16, 'm');
  ledMatrix.setChar(8, 's');
}