#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <SD.h>

// Piny dla wyświetlacza
#define TFT_CS   5
#define TFT_DC   4
#define TFT_RST  22
#define TFT_MOSI 23
#define TFT_CLK  18
#define TFT_MISO 19

// Pin dla karty SD
#define SD_CS    15

// Inicjalizacja wyświetlacza
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

// Piny dla potencjometrów
#define POT_GAZ      34
#define POT_SPRZEGLO 35

// Piny dla diod
#define LED_CZERWONY 25
#define LED_ZIELONY  26

// Piny dla przycisków
#define PRZYCISK_START  27
#define PRZYCISK_LEWO   32
#define PRZYCISK_PRAWO  33
#define PRZYCISK_OK     21
#define PRZYCISK_REKORDY 12 // >>> NOWY PRZYCISK DLA REKORDÓW

// Zmienne dla obsługi przycisków
bool poprzedniStanLewo = HIGH;
bool poprzedniStanPrawo = HIGH;
bool poprzedniStanOK = HIGH;
bool poprzedniStanStart = HIGH;
bool poprzedniStanRekordy = HIGH;
unsigned long czasOstatniegoNacisniecia = 0;
const unsigned long OPOZNIENIE_PRZYCISKOW = 200; // 200ms debounce

// Zmienne globalne
int wartoscGazu = 0;
int wartoscSprzegla = 0;

// Zmienne dla zarządzania zawodnikami
#define MAX_ZAWODNIKOW 10
#define MAX_DLUGOSC_NICKU 10 // >>> ZMIANA: Długość nicku na 10
String aktualnyZawodnik = "GOSC";
String listaZawodnikow[MAX_ZAWODNIKOW];
int liczbaZawodnikow = 0;
String nazwaPlikuZawodnikow = "/zawodnicy.txt";

// Tablica dostępnych znaków (dodana spacja)
const char DOSTEPNE_ZNAKI[] = " ABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789";
const int LICZBA_ZNAKOW = sizeof(DOSTEPNE_ZNAKI) - 1;
int indeksZnaku = 0; // >>> ZMIANA: Start od spacji (indeks 0)

// Zmienne do obsługi menu
int pozycjaMenu = 0;
int pozycjaKursora = 0;
char wybranyZnak = ' ';
String nowyNick = "";

// Poprzednie wartości do porównania - zapobiega migotaniu
int poprzedniGaz = -1;
int poprzednieSprzeglo = -1;

// Tryby pracy
#define TRYB_POWITALNY 0
#define TRYB_TEST 1
#define TRYB_REKORDY 2
#define TRYB_MENU_ZAWODNIKA 3
#define TRYB_WYBOR_ZAWODNIKA 4
#define TRYB_DODAJ_ZAWODNIKA 5
#define TRYB_DIAGNOSTYKA 6 // >>> NOWY TRYB
#define TRYB_REKORDY_POTWIERDZENIE_USUNIECIA 7 // >>> NOWY TRYB

int tryb = TRYB_POWITALNY;

// Stan testu
int stanTestu = 0;

// Zmienne dla testu reakcji
unsigned long czasStartu = 0;
unsigned long czasReakcji = 0;
unsigned long czasFazy = 0;
int procentGazuWyniku = 0;

// Zmienne dla obsługi karty SD
bool sdDostepna = false;
int numerTestu = 0;
String nazwaPlikuWynikow = "/wyniki.csv";

// Najlepszy wynik
unsigned long najlepszyWynik = 9999;

// Struktura do sortowania wyników
struct Wynik {
  String nick;
  unsigned long czas;
};

// Zmienne dla ekranu rekordów
int pozycjaPrzewijaniaRekordow = 0;
const int WIERSZY_NA_EKRANIE = 10;
int maksymalnaLiczbaRekordow = 0;

// Wbudowany prosty obraz logo - flaga szachownicy
const uint16_t LOGO_KOLORY[] PROGMEM = {
  0xFFFF, 0x0000
};

// --- DEKLARACJE FUNKCJI (dla przejrzystości) ---
void rysujEkranPowitalny();
void wyswietlEkranRekordow();
void odczytajPosortujIWyswietlRekordy();
void usunWszystkieRekordy();
void wyswietlPotwierdzenieUsuniecia();
void wyswietlEkranDiagnostyki();
void aktualizujEkranDiagnostyki(int procentGazu, int procentSprzegla);

// =================================================================
// SETUP
// =================================================================
void setup() {
  Serial.begin(115200);
  
  tft.begin();
  tft.setRotation(1);
  
  pinMode(LED_CZERWONY, OUTPUT);
  pinMode(LED_ZIELONY, OUTPUT);
  pinMode(PRZYCISK_START, INPUT_PULLUP);
  pinMode(PRZYCISK_LEWO, INPUT_PULLUP);
  pinMode(PRZYCISK_PRAWO, INPUT_PULLUP);
  pinMode(PRZYCISK_OK, INPUT_PULLUP);
  pinMode(PRZYCISK_REKORDY, INPUT_PULLUP); // >>> Inicjalizacja nowego przycisku
  
  digitalWrite(LED_CZERWONY, LOW);
  digitalWrite(LED_ZIELONY, LOW);
  
  sdDostepna = inicjujKarteSD();
  
  if (sdDostepna) {
    wczytajZawodnikow();
    najlepszyWynik = odczytajNajlepszyWynik();
  }
  
  wyswietlLogoStartowe();
  delay(3000);
  
  randomSeed(analogRead(39));
  
  rysujEkranPowitalny();
}

// =================================================================
// GŁÓWNA PĘTLA
// =================================================================
void loop() {
  // --- Obsługa przycisków ---
  if (millis() - czasOstatniegoNacisniecia > OPOZNIENIE_PRZYCISKOW) {
    bool nacisnieto = false;
    bool stanStart = digitalRead(PRZYCISK_START);
    bool stanLewo = digitalRead(PRZYCISK_LEWO);
    bool stanPrawo = digitalRead(PRZYCISK_PRAWO);
    bool stanOK = digitalRead(PRZYCISK_OK);
    bool stanRekordy = digitalRead(PRZYCISK_REKORDY);

    // Przycisk START
    if (stanStart == LOW && poprzedniStanStart == HIGH) {
      nacisnieto = true;
      if (tryb == TRYB_POWITALNY) {
        tryb = TRYB_TEST;
        stanTestu = 0;
        tft.fillScreen(ILI9341_BLACK);
        tft.setTextSize(3);
        tft.setTextColor(ILI9341_YELLOW);
        tft.setCursor(65, 80);
        tft.print("PODJEDZ POD");
        tft.setCursor(120, 120);
        tft.print("TASME");
        czasStartu = millis();
        czasFazy = random(2000, 3000);
      } else if (tryb == TRYB_TEST && (stanTestu == 4 || stanTestu == 5)) {
        tryb = TRYB_POWITALNY;
        rysujEkranPowitalny();
      } else if (tryb == TRYB_REKORDY) {
        tryb = TRYB_REKORDY_POTWIERDZENIE_USUNIECIA;
        wyswietlPotwierdzenieUsuniecia();
      } else if (tryb == TRYB_DIAGNOSTYKA || tryb == TRYB_MENU_ZAWODNIKA || tryb == TRYB_WYBOR_ZAWODNIKA || tryb == TRYB_DODAJ_ZAWODNIKA) {
         tryb = TRYB_POWITALNY;
         rysujEkranPowitalny();
      }
    }
    
    // Przycisk REKORDY
    if (stanRekordy == LOW && poprzedniStanRekordy == HIGH) {
      nacisnieto = true;
      if (tryb == TRYB_POWITALNY) {
        tryb = TRYB_REKORDY;
        pozycjaPrzewijaniaRekordow = 0;
        wyswietlEkranRekordow();
      } else if (tryb == TRYB_REKORDY || tryb == TRYB_REKORDY_POTWIERDZENIE_USUNIECIA) {
        tryb = TRYB_POWITALNY;
        rysujEkranPowitalny();
      }
    }

    // Przycisk LEWO
    if (stanLewo == LOW && poprzedniStanLewo == HIGH) {
      nacisnieto = true;
      if (tryb == TRYB_POWITALNY) {
        tryb = TRYB_DIAGNOSTYKA;
        wyswietlEkranDiagnostyki();
      } else if (tryb == TRYB_DIAGNOSTYKA) {
        tryb = TRYB_POWITALNY;
        rysujEkranPowitalny();
      } else if (tryb == TRYB_REKORDY_POTWIERDZENIE_USUNIECIA) {
        tryb = TRYB_REKORDY;
        wyswietlEkranRekordow();
      } else {
        obslugaMenuNawigacja(-1); // -1 dla lewo/góra
      }
    }
    
    // Przycisk PRAWO
    if (stanPrawo == LOW && poprzedniStanPrawo == HIGH) {
      nacisnieto = true;
      if (tryb == TRYB_REKORDY_POTWIERDZENIE_USUNIECIA) {
        usunWszystkieRekordy();
        tryb = TRYB_REKORDY;
        wyswietlEkranRekordow();
      } else {
        obslugaMenuNawigacja(1); // 1 dla prawo/dół
      }
    }

    // Przycisk OK
    if (stanOK == LOW && poprzedniStanOK == HIGH) {
      nacisnieto = true;
      obslugaMenuOK();
    }
    
    if(nacisnieto) czasOstatniegoNacisniecia = millis();
    poprzedniStanStart = stanStart;
    poprzedniStanLewo = stanLewo;
    poprzedniStanPrawo = stanPrawo;
    poprzedniStanOK = stanOK;
    poprzedniStanRekordy = stanRekordy;
  }
  
  // --- Główna logika trybów ---
  if (tryb == TRYB_TEST) {
    obslugaTestu();
  } else if (tryb == TRYB_REKORDY) {
    obslugaPrzewijaniaRekordow();
  } else if (tryb == TRYB_DIAGNOSTYKA) {
    wartoscGazu = analogRead(POT_GAZ);
    wartoscSprzegla = analogRead(POT_SPRZEGLO);
    int procentGazu = map(wartoscGazu, 0, 4095, 0, 100);
    int procentSprzegla = map(wartoscSprzegla, 0, 4095, 0, 100);
    aktualizujEkranDiagnostyki(procentGazu, procentSprzegla);
  }
  
  delay(10);
}

// Funkcja rysująca proste logo - flagę szachownicy wyścigową
void wyswietlLogoStartowe() {
  tft.fillScreen(ILI9341_BLACK);
  int rozmiarKwadratu = 30;
  int startX = (tft.width() - rozmiarKwadratu * 8) / 2;
  int startY = (tft.height() - rozmiarKwadratu * 4) / 2;
  for (int row = 0; row < 4; row++) {
    for (int col = 0; col < 8; col++) {
      uint16_t kolor = LOGO_KOLORY[(row + col) % 2];
      tft.fillRect(startX + col * rozmiarKwadratu, startY + row * rozmiarKwadratu, rozmiarKwadratu, rozmiarKwadratu, kolor);
    }
  }
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_YELLOW);
  tft.setCursor(startX + 10, startY - 40);
  tft.print("TESTER REFLEKSU");
  tft.setTextSize(1);
  tft.setTextColor(ILI9341_CYAN);
  tft.setCursor(startX + 35, startY + rozmiarKwadratu * 4 + 20);
  tft.print("GooMoo#312 - 2025");
}
void wczytajZawodnikow() {
  if (!sdDostepna) return;
  if (!SD.exists(nazwaPlikuZawodnikow)) {
    File plik = SD.open(nazwaPlikuZawodnikow, FILE_WRITE);
    if (plik) plik.close();
    return;
  }
  File plik = SD.open(nazwaPlikuZawodnikow, FILE_READ);
  if (plik) {
    liczbaZawodnikow = 0;
    while (plik.available() && liczbaZawodnikow < MAX_ZAWODNIKOW) {
      String linia = plik.readStringUntil('\n');
      linia.trim();
      if (linia.length() > 0) {
        listaZawodnikow[liczbaZawodnikow] = linia;
        liczbaZawodnikow++;
      }
    }
    plik.close();
  }
}
void zapiszNowegoZawodnika(String nick) {
  if (!sdDostepna || liczbaZawodnikow >= MAX_ZAWODNIKOW) return;
  for (int i = 0; i < liczbaZawodnikow; i++) {
    if (listaZawodnikow[i] == nick) return;
  }
  listaZawodnikow[liczbaZawodnikow] = nick;
  liczbaZawodnikow++;
  File plik = SD.open(nazwaPlikuZawodnikow, FILE_WRITE);
  if (plik) {
    for (int i = 0; i < liczbaZawodnikow; i++) {
      plik.println(listaZawodnikow[i]);
    }
    plik.close();
  }
}
void wyswietlMenuZawodnika(int wybranaPozycja) {
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_YELLOW);
  tft.setCursor(50, 30);
  tft.print("MENU ZAWODNIKA");
  int startY = 80;
  int spacing = 40;
  tft.setTextSize(2);
  tft.setTextColor(wybranaPozycja == 0 ? ILI9341_CYAN : ILI9341_WHITE);
  tft.setCursor(60, startY);
  tft.print("Wybierz Zawodnika");
  tft.setTextColor(wybranaPozycja == 1 ? ILI9341_CYAN : ILI9341_WHITE);
  tft.setCursor(60, startY + spacing);
  tft.print("Dodaj Zawodnika");
  tft.setTextColor(wybranaPozycja == 2 ? ILI9341_CYAN : ILI9341_WHITE);
  tft.setCursor(60, startY + 2 * spacing);
  tft.print("Kontynuuj jako GOSC");
  tft.setTextColor(wybranaPozycja == 3 ? ILI9341_CYAN : ILI9341_WHITE);
  tft.setCursor(60, startY + 3 * spacing);
  tft.print("Powrot");
  tft.setTextSize(1);
  tft.setTextColor(ILI9341_CYAN);
  tft.setCursor(20, 220);
  tft.print("LEWO/PRAWO: wybor | OK: zatwierdz | START: powrot");
}
void wyswietlListeZawodnikow(int wybranyIndeks) {
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_YELLOW);
  tft.setCursor(40, 30);
  tft.print("WYBIERZ ZAWODNIKA");
  int startY = 80;
  int spacing = 30;
  tft.setTextSize(2);
  tft.setTextColor(wybranyIndeks == 0 ? ILI9341_CYAN : ILI9341_WHITE);
  tft.setCursor(60, startY);
  tft.print("GOSC");
  for (int i = 0; i < liczbaZawodnikow; i++) {
    tft.setTextColor(wybranyIndeks == i + 1 ? ILI9341_CYAN : ILI9341_WHITE);
    tft.setCursor(60, startY + (i + 1) * spacing);
    tft.print(listaZawodnikow[i]);
  }
  tft.setTextSize(1);
  tft.setTextColor(ILI9341_CYAN);
  tft.setCursor(20, 220);
  tft.print("LEWO/PRAWO: wybor | OK: zatwierdz | START: powrot");
}
void wyswietlDodawanieZawodnika(String aktualnyNick, int pozycjaKursora, char wybranyZnak) {
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_YELLOW);
  tft.setCursor(40, 30);
  tft.print("DODAJ ZAWODNIKA");
  tft.setTextSize(2); // Mniejszy rozmiar dla 10 znaków
  tft.setTextColor(ILI9341_WHITE);
  tft.setCursor(40, 80);
  for (int i = 0; i < MAX_DLUGOSC_NICKU; i++) {
    if (i < aktualnyNick.length()) {
      tft.setTextColor(i == pozycjaKursora ? ILI9341_CYAN : ILI9341_WHITE);
      tft.print(aktualnyNick[i]);
    } else if (i == pozycjaKursora) {
      tft.setTextColor(ILI9341_CYAN);
      tft.print(wybranyZnak);
    } else {
      tft.setTextColor(ILI9341_WHITE);
      tft.print("_");
    }
  }
  tft.setTextColor(ILI9341_WHITE);
  if (pozycjaKursora >= MAX_DLUGOSC_NICKU) {
    tft.setTextSize(2);
    tft.setCursor(80, 140);
    tft.setTextColor(wybranyZnak == 'Z' ? ILI9341_CYAN : ILI9341_WHITE);
    tft.print("ZAPISZ");
    tft.setCursor(80, 170);
    tft.setTextColor(wybranyZnak == 'P' ? ILI9341_CYAN : ILI9341_WHITE);
    tft.print("POWROT");
  } else {
    tft.setTextSize(1);
    tft.setTextColor(ILI9341_CYAN);
    tft.setCursor(20, 140);
    tft.print("LEWO/PRAWO: zmiana znaku | OK: zatwierdz");
    tft.setCursor(80, 160);
    tft.setTextColor(ILI9341_WHITE);
    for (int i = -2; i <= 2; i++) {
      int idx = (indeksZnaku + i + LICZBA_ZNAKOW) % LICZBA_ZNAKOW;
      if (i == 0) {
        tft.setTextColor(ILI9341_CYAN);
        tft.print("[");
        tft.print(DOSTEPNE_ZNAKI[idx]);
        tft.print("]");
        tft.setTextColor(ILI9341_WHITE);
      } else {
        tft.print(" ");
        tft.print(DOSTEPNE_ZNAKI[idx]);
        tft.print(" ");
      }
    }
  }
  tft.setTextSize(1);
  tft.setTextColor(ILI9341_CYAN);
  tft.setCursor(20, 220);
  tft.print("OK: zatwierdz | START: powrot do menu");
}
void wyswietlEkranRekordow() {
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_YELLOW);
  tft.setCursor(90, 10);
  tft.print("REKORDY");
  tft.setTextSize(1);
  tft.setTextColor(ILI9341_WHITE);
  tft.setCursor(10, 220);
  tft.print("GAZ/SPRZEGLO: przewijanie");
  tft.setCursor(10, 230);
  tft.print("START: usun | REKORDY: powrot");
  odczytajPosortujIWyswietlRekordy();
}
// >>> NOWA, PRZEBUDOWANA FUNKCJA DO WYŚWIETLANIA REKORDÓW
void odczytajPosortujIWyswietlRekordy() {
  if (!sdDostepna) {
    tft.setTextSize(2);
    tft.setTextColor(ILI9341_RED);
    tft.setCursor(40, 100);
    tft.print("Brak karty SD!");
    return;
  }
  
  File plikWynikow = SD.open(nazwaPlikuWynikow, FILE_READ);
  if (!plikWynikow) {
    tft.setTextSize(2);
    tft.setTextColor(ILI9341_RED);
    tft.setCursor(40, 100);
    tft.print("Blad pliku!");
    return;
  }
  
  // 1. Wczytaj wszystkie wyniki do pamięci
  const int MAX_WYNIKOW = 100; // Maksymalna liczba wyników do posortowania
  Wynik wyniki[MAX_WYNIKOW];
  int liczbaWynikow = 0;

  plikWynikow.readStringUntil('\n'); // Pomiń nagłówek

  while (plikWynikow.available() && liczbaWynikow < MAX_WYNIKOW) {
    String linia = plikWynikow.readStringUntil('\n');
    if (linia.length() > 0) {
      // Parsowanie: Nr,Data,Czas,Gaz,Status,Zawodnik
      int poz1 = linia.indexOf(',');
      int poz2 = linia.indexOf(',', poz1 + 1);
      int poz3 = linia.indexOf(',', poz2 + 1);
      int poz4 = linia.indexOf(',', poz3 + 1);
      int poz5 = linia.indexOf(',', poz4 + 1);
      
      if (poz5 > 0) {
        String status = linia.substring(poz4 + 1, poz5);
        if (status.startsWith("OK")) {
          wyniki[liczbaWynikow].czas = linia.substring(poz2 + 1, poz3).toInt();
          wyniki[liczbaWynikow].nick = linia.substring(poz5 + 1);
          liczbaWynikow++;
        }
      }
    }
  }
  plikWynikow.close();
  maksymalnaLiczbaRekordow = liczbaWynikow;

  // 2. Posortuj wyniki (Bubble Sort)
  for (int i = 0; i < liczbaWynikow - 1; i++) {
    for (int j = 0; j < liczbaWynikow - i - 1; j++) {
      if (wyniki[j].czas > wyniki[j + 1].czas) {
        Wynik temp = wyniki[j];
        wyniki[j] = wyniki[j + 1];
        wyniki[j + 1] = temp;
      }
    }
  }

  // 3. Wyświetl posortowaną tabelę
  tft.fillRect(0, 40, tft.width(), 170, ILI9341_BLACK); // Wyczyść obszar tabeli
  
  // Nagłówki tabeli
  tft.setTextSize(1);
  tft.setTextColor(ILI9341_CYAN);
  tft.setCursor(10, 40);
  tft.print("MIEJSCE");
  tft.setCursor(80, 40);
  tft.print("NICK");
  tft.setCursor(240, 40);
  tft.print("WYNIK [ms]");

  if (liczbaWynikow == 0) {
    tft.setTextSize(2);
    tft.setTextColor(ILI9341_CYAN);
    tft.setCursor(70, 100);
    tft.print("Brak rekordow");
    return;
  }
  
  tft.setTextSize(2);
  int y = 60;
  for (int i = 0; i < WIERSZY_NA_EKRANIE; i++) {
    int indeks = i + pozycjaPrzewijaniaRekordow;
    if (indeks >= liczbaWynikow) break;
    
    tft.setTextColor(ILI9341_WHITE);
    // Miejsce
    tft.setCursor(20, y);
    tft.print(indeks + 1);
    tft.print(".");
    // Nick
    tft.setCursor(80, y);
    tft.print(wyniki[indeks].nick);
    // Czas
    tft.setCursor(240, y);
    tft.print(wyniki[indeks].czas);

    y += 16;
  }
  
  if (pozycjaPrzewijaniaRekordow > 0) {
    tft.fillTriangle(160, 30, 150, 40, 170, 40, ILI9341_WHITE);
  }
  if (pozycjaPrzewijaniaRekordow + WIERSZY_NA_EKRANIE < maksymalnaLiczbaRekordow) {
    tft.fillTriangle(160, 215, 150, 205, 170, 205, ILI9341_WHITE);
  }
}
void usunWszystkieRekordy() {
  if (!sdDostepna) return;
  SD.remove(nazwaPlikuWynikow);
  File plikWynikow = SD.open(nazwaPlikuWynikow, FILE_WRITE);
  if (plikWynikow) {
    plikWynikow.println("Nr,Data,Czas_ms,Gaz_%,Status,Zawodnik");
    plikWynikow.close();
  }
  numerTestu = 0;
  pozycjaPrzewijaniaRekordow = 0;
  maksymalnaLiczbaRekordow = 0;
  najlepszyWynik = 9999;
  
  tft.fillRect(50, 100, 220, 40, ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_RED);
  tft.setCursor(60, 110);
  tft.print("Rekordy usuniete!");
  delay(1500);
}
unsigned long odczytajNajlepszyWynik() {
  unsigned long najlepszy = 9999;
  if (!sdDostepna) return najlepszy;
  File plikWynikow = SD.open(nazwaPlikuWynikow, FILE_READ);
  if (!plikWynikow) return najlepszy;
  plikWynikow.readStringUntil('\n'); // Pomiń nagłówek
  while (plikWynikow.available()) {
    String linia = plikWynikow.readStringUntil('\n');
    int poz1 = linia.indexOf(',');
    int poz2 = linia.indexOf(',', poz1 + 1);
    int poz3 = linia.indexOf(',', poz2 + 1);
    int poz4 = linia.indexOf(',', poz3 + 1);
    if (poz4 > 0) {
      String status = linia.substring(poz4 + 1, linia.indexOf(',', poz4 + 1));
      if (status.startsWith("OK")) {
        unsigned long czas = linia.substring(poz2 + 1, poz3).toInt();
        if (czas > 0 && czas < najlepszy) najlepszy = czas;
      }
    }
  }
  plikWynikow.close();
  return najlepszy;
}
bool inicjujKarteSD() {
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);
  tft.setCursor(20, 100);
  tft.print("Inicjalizacja karty SD...");
  if (!SD.begin(SD_CS)) {
    tft.fillScreen(ILI9341_BLACK);
    tft.setTextColor(ILI9341_RED);
    tft.setCursor(40, 100);
    tft.print("Blad karty SD!");
    tft.setCursor(40, 130);
    tft.print("Kontynuuje bez zapisu.");
    delay(2000);
    return false;
  }
  if (!SD.exists(nazwaPlikuWynikow)) {
    File plikWynikow = SD.open(nazwaPlikuWynikow, FILE_WRITE);
    if (plikWynikow) {
      plikWynikow.println("Nr,Data,Czas_ms,Gaz_%,Status,Zawodnik");
      plikWynikow.close();
    }
  }
  File plikWynikow = SD.open(nazwaPlikuWynikow, FILE_READ);
  if (plikWynikow) {
    String linia = "";
    while (plikWynikow.available()) {
      if (plikWynikow.read() == '\n') numerTestu++;
    }
    plikWynikow.close();
    if (numerTestu > 0) numerTestu--;
  }
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_GREEN);
  tft.setCursor(40, 100);
  tft.print("Karta SD gotowa!");
  delay(1000);
  return true;
}
void zapiszWynik(unsigned long czas, int poziomGazu, String status) {
  if (!sdDostepna) return;
  numerTestu++;
  File plikWynikow = SD.open(nazwaPlikuWynikow, FILE_APPEND);
  if (plikWynikow) {
    String dane = String(numerTestu) + "," + String(millis()) + "," + String(czas) + "," + String(poziomGazu) + "," + status + "," + aktualnyZawodnik;
    plikWynikow.println(dane);
    plikWynikow.close();
    if (status.startsWith("OK") && czas > 0 && czas < najlepszyWynik) {
      najlepszyWynik = czas;
    }
  }
}
void obslugaMenuNawigacja(int kierunek) {
  if (tryb == TRYB_MENU_ZAWODNIKA) {
    pozycjaMenu += kierunek;
    if (pozycjaMenu < 0) pozycjaMenu = 3;
    if (pozycjaMenu > 3) pozycjaMenu = 0;
    wyswietlMenuZawodnika(pozycjaMenu);
  } else if (tryb == TRYB_WYBOR_ZAWODNIKA) {
    int maxIndeks = liczbaZawodnikow;
    pozycjaMenu += kierunek;
    if (pozycjaMenu < 0) pozycjaMenu = maxIndeks;
    if (pozycjaMenu > maxIndeks) pozycjaMenu = 0;
    wyswietlListeZawodnikow(pozycjaMenu);
  } else if (tryb == TRYB_DODAJ_ZAWODNIKA) {
    if (pozycjaKursora < MAX_DLUGOSC_NICKU) {
      indeksZnaku = (indeksZnaku + kierunek + LICZBA_ZNAKOW) % LICZBA_ZNAKOW;
      wybranyZnak = DOSTEPNE_ZNAKI[indeksZnaku];
    } else {
      wybranyZnak = (wybranyZnak == 'Z') ? 'P' : 'Z';
    }
    wyswietlDodawanieZawodnika(nowyNick, pozycjaKursora, wybranyZnak);
  }
}
void obslugaMenuOK() {
  if (tryb == TRYB_POWITALNY) {
    tryb = TRYB_MENU_ZAWODNIKA;
    pozycjaMenu = 0;
    wyswietlMenuZawodnika(pozycjaMenu);
  } else if (tryb == TRYB_MENU_ZAWODNIKA) {
    switch (pozycjaMenu) {
      case 0:
        tryb = TRYB_WYBOR_ZAWODNIKA;
        pozycjaMenu = 0;
        wyswietlListeZawodnikow(pozycjaMenu);
        break;
      case 1:
        tryb = TRYB_DODAJ_ZAWODNIKA;
        pozycjaKursora = 0;
        indeksZnaku = 0; // >>> ZMIANA: Start od spacji
        wybranyZnak = DOSTEPNE_ZNAKI[indeksZnaku];
        nowyNick = "";
        wyswietlDodawanieZawodnika(nowyNick, pozycjaKursora, wybranyZnak);
        break;
      case 2:
        aktualnyZawodnik = "GOSC";
        tryb = TRYB_POWITALNY;
        rysujEkranPowitalny();
        break;
      case 3:
        tryb = TRYB_POWITALNY;
        rysujEkranPowitalny();
        break;
    }
  } else if (tryb == TRYB_WYBOR_ZAWODNIKA) {
    aktualnyZawodnik = (pozycjaMenu == 0) ? "GOSC" : listaZawodnikow[pozycjaMenu - 1];
    tryb = TRYB_POWITALNY;
    rysujEkranPowitalny();
  } else if (tryb == TRYB_DODAJ_ZAWODNIKA) {
    if (pozycjaKursora < MAX_DLUGOSC_NICKU) {
      if (nowyNick.length() <= pozycjaKursora) nowyNick += wybranyZnak;
      else nowyNick.setCharAt(pozycjaKursora, wybranyZnak);
      pozycjaKursora++;
      if (pozycjaKursora >= MAX_DLUGOSC_NICKU) {
        wybranyZnak = 'Z';
      } else {
        indeksZnaku = 0; // >>> ZMIANA: Następny znak domyślnie spacja
        wybranyZnak = DOSTEPNE_ZNAKI[indeksZnaku];
      }
    } else {
      if (wybranyZnak == 'Z') {
        nowyNick.trim(); // Usuń ewentualne spacje na końcu
        if (nowyNick.length() > 0) {
          zapiszNowegoZawodnika(nowyNick);
          aktualnyZawodnik = nowyNick;
        }
        tryb = TRYB_POWITALNY;
        rysujEkranPowitalny();
      } else {
        tryb = TRYB_MENU_ZAWODNIKA;
        pozycjaMenu = 0;
        wyswietlMenuZawodnika(pozycjaMenu);
      }
    }
    wyswietlDodawanieZawodnika(nowyNick, pozycjaKursora, wybranyZnak);
  }
}
void rysujEkranPowitalny() {
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(3);
  tft.setTextColor(ILI9341_YELLOW);
  tft.setCursor(40, 40);
  tft.print("TEST REAKCJI");
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_CYAN);
  tft.setCursor(90, 100);
  tft.print("GooMoo#312");
  if (najlepszyWynik < 9999) {
    tft.setTextSize(2);
    tft.setTextColor(ILI9341_GREEN);
    tft.setCursor(60, 130);
    tft.print("Rekord: ");
    tft.print(najlepszyWynik);
    tft.print(" ms");
  }
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_MAGENTA);
  tft.setCursor(80, 160);
  tft.print("Zawodnik: ");
  tft.print(aktualnyZawodnik);
  tft.setTextSize(1);
  tft.setCursor(250, 10);
  tft.setTextColor(sdDostepna ? ILI9341_GREEN : ILI9341_RED);
  tft.print(sdDostepna ? "SD OK" : "Brak SD");
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);
  tft.setCursor(85, 200);
  tft.print("Nacisnij START");
  tft.setTextSize(1);
  tft.setTextColor(ILI9341_CYAN);
  tft.setCursor(10, 230);
  tft.print("OK:Menu | REKORDY:Tabela | LEWO:Diagnostyka");
}
void aktualizujWyswietlacz(int procentGazu, int procentSprzegla) {
  int szerokoscGazu = map(procentGazu, 0, 100, 0, 278);
  int szerokoscSprzegla = map(procentSprzegla, 0, 100, 0, 278);
  if (procentGazu != poprzedniGaz) {
    int popSzer = map(poprzedniGaz, 0, 100, 0, 278);
    if (szerokoscGazu > popSzer) {
      tft.fillRect(21 + popSzer, 71, szerokoscGazu - popSzer, 28, ILI9341_GREEN);
    } else {
      tft.fillRect(21 + szerokoscGazu, 71, 278 - szerokoscGazu, 28, ILI9341_BLACK);
    }
    tft.fillRect(150, 80, 50, 10, ILI9341_BLACK);
    tft.setCursor(150, 80);
    tft.setTextSize(1);
    tft.setTextColor(ILI9341_WHITE);
    tft.print(procentGazu);
    tft.print("%");
    poprzedniGaz = procentGazu;
  }
  if (procentSprzegla != poprzednieSprzeglo) {
    int popSzer = map(poprzednieSprzeglo, 0, 100, 0, 278);
    if (szerokoscSprzegla > popSzer) {
      tft.fillRect(21 + popSzer, 141, szerokoscSprzegla - popSzer, 28, ILI9341_RED);
    } else {
      tft.fillRect(21 + szerokoscSprzegla, 141, 278 - szerokoscSprzegla, 28, ILI9341_BLACK);
    }
    tft.fillRect(150, 150, 50, 10, ILI9341_BLACK);
    tft.setCursor(150, 150);
    tft.setTextSize(1);
    tft.setTextColor(ILI9341_WHITE);
    tft.print(procentSprzegla);
    tft.print("%");
    poprzednieSprzeglo = procentSprzegla;
  }
}
void wyswietlPotwierdzenieUsuniecia() {
    tft.fillRect(40, 80, 240, 80, ILI9341_DARKGREY);
    tft.drawRect(40, 80, 240, 80, ILI9341_WHITE);
    tft.setTextSize(2);
    tft.setTextColor(ILI9341_YELLOW);
    tft.setCursor(65, 100);
    tft.print("USUNAC WYNIKI?");
    tft.setTextSize(1);
    tft.setTextColor(ILI9341_WHITE);
    tft.setCursor(50, 140);
    tft.print("LEWO: NIE | PRAWO: TAK | REKORDY: ANULUJ");
}
void wyswietlEkranDiagnostyki() {
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_YELLOW);
  tft.setCursor(80, 20);
  tft.print("DIAGNOSTYKA");

  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);
  tft.setCursor(20, 70);
  tft.println("GAZ:");
  tft.drawRect(20, 100, 280, 30, ILI9341_WHITE);
  
  tft.setCursor(20, 150);
  tft.print("SPRZEGLO:");
  tft.drawRect(20, 180, 280, 30, ILI9341_WHITE);

  tft.setTextSize(1);
  tft.setTextColor(ILI9341_CYAN);
  tft.setCursor(80, 220);
  tft.print("LEWO lub START: Powrot");
  
  poprzedniGaz = -1;
  poprzednieSprzeglo = -1;
}
void aktualizujEkranDiagnostyki(int procentGazu, int procentSprzegla) {
  int szerGazu = map(procentGazu, 0, 100, 0, 278);
  int szerSprzegla = map(procentSprzegla, 0, 100, 0, 278);

  if (procentGazu != poprzedniGaz) {
    int popSzer = map(poprzedniGaz, 0, 100, 0, 278);
    if (szerGazu > popSzer) tft.fillRect(21 + popSzer, 101, szerGazu - popSzer, 28, ILI9341_GREEN);
    else tft.fillRect(21 + szerGazu, 101, 278 - szerGazu, 28, ILI9341_BLACK);
    tft.fillRect(140, 110, 60, 16, ILI9341_BLACK);
    tft.setCursor(140, 110);
    tft.setTextSize(2);
    tft.setTextColor(ILI9341_WHITE);
    tft.print(procentGazu);
    tft.print("%");
    poprzedniGaz = procentGazu;
  }
  if (procentSprzegla != poprzednieSprzeglo) {
    int popSzer = map(poprzednieSprzeglo, 0, 100, 0, 278);
    if (szerSprzegla > popSzer) tft.fillRect(21 + popSzer, 181, szerSprzegla - popSzer, 28, ILI9341_RED);
    else tft.fillRect(21 + szerSprzegla, 181, 278 - szerSprzegla, 28, ILI9341_BLACK);
    tft.fillRect(140, 190, 60, 16, ILI9341_BLACK);
    tft.setCursor(140, 190);
    tft.setTextSize(2);
    tft.setTextColor(ILI9341_WHITE);
    tft.print(procentSprzegla);
    tft.print("%");
    poprzednieSprzeglo = procentSprzegla;
  }
}
void obslugaPrzewijaniaRekordow() {
  wartoscGazu = analogRead(POT_GAZ);
  wartoscSprzegla = analogRead(POT_SPRZEGLO);
  int procentGazu = map(wartoscGazu, 0, 4095, 0, 100);
  int procentSprzegla = map(wartoscSprzegla, 0, 4095, 0, 100);
  
  static unsigned long ostatniePrzewiniecie = 0;
  if (millis() - ostatniePrzewiniecie > 200) {
    bool przewinieto = false;
    if (procentGazu > 90 && pozycjaPrzewijaniaRekordow < maksymalnaLiczbaRekordow - WIERSZY_NA_EKRANIE) {
      pozycjaPrzewijaniaRekordow++;
      przewinieto = true;
    }
    if (procentSprzegla > 90 && pozycjaPrzewijaniaRekordow > 0) {
      pozycjaPrzewijaniaRekordow--;
      przewinieto = true;
    }
    if (przewinieto) {
      odczytajPosortujIWyswietlRekordy();
      ostatniePrzewiniecie = millis();
    }
  }
}
void obslugaTestu() {
    wartoscGazu = analogRead(POT_GAZ);
    wartoscSprzegla = analogRead(POT_SPRZEGLO);
    int procentGazu = map(wartoscGazu, 0, 4095, 0, 100);
    int procentSprzegla = map(wartoscSprzegla, 0, 4095, 0, 100);
    unsigned long aktualnyMoment = millis();

    switch (stanTestu) {
        case 0: // "PODJEDZ POD TASME"
            if (aktualnyMoment - czasStartu >= czasFazy) {
                stanTestu = 1;
                tft.fillScreen(ILI9341_BLACK);
                tft.setTextSize(3); tft.setTextColor(ILI9341_RED); tft.setCursor(90, 40); tft.print("PRZYGOTUJ");
                tft.setCursor(145, 80); tft.print("SIE");
                tft.setTextSize(2); tft.setCursor(75, 120); tft.print("WCISNIJ SPRZEGLO");
                tft.setTextSize(1); tft.setTextColor(ILI9341_WHITE); tft.setCursor(20, 160); tft.println("Gaz:");
                tft.drawRect(20, 170, 280, 25, ILI9341_WHITE);
                tft.setCursor(20, 200); tft.print("Sprzeglo:");
                tft.drawRect(20, 210, 280, 25, ILI9341_WHITE);
                poprzedniGaz = -1; poprzednieSprzeglo = -1;
                digitalWrite(LED_CZERWONY, HIGH);
                czasStartu = aktualnyMoment;
                czasFazy = random(2000, 5000);
            }
            break;

        case 1: // "PRZYGOTUJ SIE - WCISNIJ SPRZEGLO"
            aktualizujEkranDiagnostyki(procentGazu, procentSprzegla); // Używamy tej samej funkcji do rysowania pasków na dole
            if (aktualnyMoment - czasStartu >= czasFazy) {
                if (procentSprzegla < 90) {
                    stanTestu = 5;
                    digitalWrite(LED_CZERWONY, LOW);
                    tft.fillScreen(ILI9341_BLACK);
                    tft.setTextSize(3); tft.setTextColor(ILI9341_RED); tft.setCursor(90, 80); tft.print("TASMA!!!");
                    tft.setTextSize(2); tft.setCursor(30, 140); tft.print("NIE WCISNALES SPRZEGLA");
                    zapiszWynik(0, procentGazu, "TASMA_SPRZEGLO");
                } else {
                    stanTestu = 2;
                    tft.fillScreen(ILI9341_BLACK);
                    tft.setTextSize(2); tft.setTextColor(ILI9341_WHITE); tft.setCursor(20, 10); tft.println("Test reakcji - CZEKAJ");
                    tft.setTextSize(1); tft.setCursor(20, 50); tft.println("Gaz:");
                    tft.setCursor(20, 120); tft.print("Sprzeglo:");
                    tft.drawRect(20, 70, 280, 30, ILI9341_WHITE);
                    tft.drawRect(20, 140, 280, 30, ILI9341_WHITE);
                    digitalWrite(LED_CZERWONY, LOW);
                    digitalWrite(LED_ZIELONY, HIGH);
                    czasStartu = aktualnyMoment;
                    czasFazy = random(2000, 6000);
                    poprzedniGaz = -1; poprzednieSprzeglo = -1;
                }
            }
            break;
        
        case 2: // Czekanie na zgaśnięcie zielonej diody
            aktualizujWyswietlacz(procentGazu, procentSprzegla);
            if (procentSprzegla < 80) {
                stanTestu = 5;
                digitalWrite(LED_ZIELONY, LOW);
                tft.fillScreen(ILI9341_BLACK);
                tft.setTextSize(3); tft.setTextColor(ILI9341_RED); tft.setCursor(90, 80); tft.print("TASMA!!!");
                tft.setTextSize(2); tft.setCursor(35, 140); tft.print("ZA WCZESNIE - SKUP SIE");
                zapiszWynik(0, procentGazu, "TASMA_ZA_WCZESNIE");
            } else if (aktualnyMoment - czasStartu >= czasFazy) {
                stanTestu = 3;
                digitalWrite(LED_ZIELONY, LOW);
                tft.fillRect(20, 10, 280, 30, ILI9341_BLACK);
                tft.setTextSize(2); tft.setTextColor(ILI9341_RED); tft.setCursor(20, 10); tft.println("PUSC SPRZEGLO TERAZ!");
                czasStartu = aktualnyMoment;
            }
            break;
            
        case 3: // Oczekiwanie na puszczenie sprzęgła
            aktualizujWyswietlacz(procentGazu, procentSprzegla);
            if (procentSprzegla < 10) {
                czasReakcji = aktualnyMoment - czasStartu;
                procentGazuWyniku = procentGazu;
                stanTestu = 4;
                tft.fillScreen(ILI9341_BLACK);
                tft.setTextSize(3); tft.setTextColor(ILI9341_GREEN); tft.setCursor(120, 20); tft.print("WYNIK");
                tft.setTextSize(2); tft.setTextColor(ILI9341_MAGENTA); tft.setCursor(60, 55); tft.print("Zawodnik: "); tft.print(aktualnyZawodnik);
                tft.setTextSize(3); tft.setTextColor(ILI9341_WHITE); tft.setCursor(95, 85); tft.print(czasReakcji); tft.print(" ms");
                tft.setTextSize(2); tft.setTextColor(ILI9341_CYAN); tft.setCursor(40, 120); tft.print("Najlepszy: "); tft.print(najlepszyWynik); tft.print(" ms");
                tft.setTextSize(2); tft.setTextColor(ILI9341_YELLOW); tft.setCursor(60, 150); tft.print("Poziom gazu: "); tft.print(procentGazuWyniku); tft.print("%");
                if (sdDostepna) {
                    tft.setTextSize(1); tft.setTextColor(ILI9341_CYAN); tft.setCursor(210, 20); tft.print("SD: #"); tft.print(numerTestu + 1);
                }
                if (procentGazuWyniku < 75) {
                    tft.setTextSize(2); tft.setTextColor(ILI9341_RED); tft.setCursor(80, 175); tft.print("Troche malo -");
                    tft.setTextColor(ILI9341_ORANGE); tft.setCursor(50, 200); tft.print("NAKREC TEGO GAZU!");
                    zapiszWynik(czasReakcji, procentGazuWyniku, "OK_MALO_GAZU");
                } else {
                    zapiszWynik(czasReakcji, procentGazuWyniku, "OK");
                }
            } else if (aktualnyMoment - czasStartu > 2000) {
                stanTestu = 5;
                tft.fillScreen(ILI9341_BLACK);
                tft.setTextSize(3); tft.setTextColor(ILI9341_RED); tft.setCursor(40, 100); tft.print("BRAK REAKCJI!");
                zapiszWynik(2000, procentGazu, "BRAK_REAKCJI");
            }
            break;
    }

    if (stanTestu == 4 || stanTestu == 5) {
        tft.setTextSize(1);
        tft.setTextColor(ILI9341_WHITE);
        tft.setCursor(35, 230);
        tft.print("Nacisnij START aby powrocic");
    }
}