#include <TFT_eSPI.h>
#include <BluetoothSerial.h>

#define ILI9341_DRIVER

#define TFT_MISO 19
#define TFT_MOSI 23
#define TFT_SCLK 18
#define TFT_CS   15
#define TFT_DC   2
#define TFT_RST  4

#define LOAD_GLCD
#define LOAD_FONT2
#define LOAD_FONT4
#define LOAD_FONT6
#define LOAD_FONT7
#define LOAD_FONT8

#define SPI_FREQUENCY  40000000



// ===== KONFIGURACJA PINÓW =====
#define BTN_LEFT 32
#define BTN_RIGHT 33
#define BTN_UP 25
#define BTN_DOWN 26
#define BTN_OK 27

// ===== OBIEKTY =====
TFT_eSPI tft = TFT_eSPI();
BluetoothSerial SerialBT;

// ===== ZMIENNE GLOBALNE =====
int menuIndex = 0;
int maxMenuItems = 5;
bool connected = false;
String deviceName = "OBDII";
String deviceAddress = "";

// Parametry OBD2
int rpm = 0;
int speed = 0;
int temp = 0;
int throttle = 0;
String dtcCodes = "";

// Menu states
enum State {
  MENU_MAIN,
  MENU_SCAN,
  MENU_CONNECT,
  MENU_LIVE_DATA,
  MENU_DTC_CODES
};
State currentState = MENU_MAIN;

// ===== DEBOUNCING PRZYCISKÓW =====
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 200;

// ===== SETUP =====
void setup() {
  Serial.begin(115200);
  
  // Inicjalizacja wyświetlacza
  tft.init();
  tft.setRotation(1);
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE, TFT_BLACK);
  tft.setTextSize(2);
  
  // Inicjalizacja przycisków
  pinMode(BTN_LEFT, INPUT_PULLUP);
  pinMode(BTN_RIGHT, INPUT_PULLUP);
  pinMode(BTN_UP, INPUT_PULLUP);
  pinMode(BTN_DOWN, INPUT_PULLUP);
  pinMode(BTN_OK, INPUT_PULLUP);
  
  // Inicjalizacja Bluetooth
  SerialBT.begin("ESP32_OBD2"); 
  
  drawMainMenu();
}

// ===== GŁÓWNA PĘTLA =====
void loop() {
  handleButtons();
  
  if (connected && SerialBT.available()) {
    handleOBD2Response();
  }
  
  // Automatyczne odświeżanie danych live
  if (currentState == MENU_LIVE_DATA && connected) {
    static unsigned long lastUpdate = 0;
    if (millis() - lastUpdate > 500) {
      requestLiveData();
      lastUpdate = millis();
    }
  }
}

// ===== OBSŁUGA PRZYCISKÓW =====
void handleButtons() {
  if (millis() - lastDebounceTime < debounceDelay) return;
  
  if (digitalRead(BTN_UP) == LOW) {
    lastDebounceTime = millis();
    menuIndex--;
    if (menuIndex < 0) menuIndex = maxMenuItems - 1;
    updateDisplay();
  }
  
  if (digitalRead(BTN_DOWN) == LOW) {
    lastDebounceTime = millis();
    menuIndex++;
    if (menuIndex >= maxMenuItems) menuIndex = 0;
    updateDisplay();
  }
  
  if (digitalRead(BTN_OK) == LOW) {
    lastDebounceTime = millis();
    selectMenuItem();
  }
  
  if (digitalRead(BTN_LEFT) == LOW) {
    lastDebounceTime = millis();
    goBack();
  }
}

// ===== RYSOWANIE MENU GŁÓWNEGO =====
void drawMainMenu() {
  tft.fillScreen(TFT_BLACK);
  tft.setTextSize(2);
  tft.setCursor(10, 10);
  tft.setTextColor(TFT_CYAN);
  tft.println("=== OBD2 READER ===");
  
  maxMenuItems = 5;
  menuIndex = 0;
  currentState = MENU_MAIN;
  
  updateDisplay();
}

// ===== AKTUALIZACJA WYŚWIETLACZA =====
void updateDisplay() {
  switch (currentState) {
    case MENU_MAIN:
      displayMainMenu();
      break;
    case MENU_SCAN:
      displayScanMenu();
      break;
    case MENU_LIVE_DATA:
      displayLiveData();
      break;
    case MENU_DTC_CODES:
      displayDTCCodes();
      break;
  }
}

// ===== WYŚWIETLANIE MENU =====
void displayMainMenu() {
  tft.fillRect(0, 40, 320, 200, TFT_BLACK);
  
  String menuItems[] = {
    "1. Skanuj BT",
    "2. Polacz OBD2",
    "3. Dane LIVE",
    "4. Kody DTC",
    "5. Info"
  };
  
  for (int i = 0; i < 5; i++) {
    tft.setCursor(20, 50 + i * 30);
    if (i == menuIndex) {
      tft.setTextColor(TFT_GREEN, TFT_BLUE);
      tft.print("> ");
    } else {
      tft.setTextColor(TFT_WHITE, TFT_BLACK);
      tft.print("  ");
    }
    tft.println(menuItems[i]);
  }
  
  // Status połączenia
  tft.setCursor(10, 220);
  if (connected) {
    tft.setTextColor(TFT_GREEN, TFT_BLACK);
    tft.print("Status: POLACZONY");
  } else {
    tft.setTextColor(TFT_RED, TFT_BLACK);
    tft.print("Status: ROZLACZONY");
  }
}

// ===== WYŚWIETLANIE MENU SCAN =====
void displayScanMenu() {
  tft.fillScreen(TFT_BLACK);
  tft.setTextSize(2);
  tft.setCursor(10, 10);
  tft.setTextColor(TFT_YELLOW);
  tft.println("Skanowanie BT...");
}

// ===== WYBÓR POZYCJI MENU =====
void selectMenuItem() {
  switch (currentState) {
    case MENU_MAIN:
      switch (menuIndex) {
        case 0: scanBluetooth(); break;
        case 1: connectOBD2(); break;
        case 2: 
          if (connected) {
            currentState = MENU_LIVE_DATA;
            updateDisplay();
          } else {
            showMessage("Brak polaczenia!");
          }
          break;
        case 3:
          if (connected) {
            currentState = MENU_DTC_CODES;
            requestDTCCodes();
          } else {
            showMessage("Brak polaczenia!");
          }
          break;
        case 4: showInfo(); break;
      }
      break;
  }
}

// ===== SKANOWANIE BLUETOOTH =====
void scanBluetooth() {
  tft.fillScreen(TFT_BLACK);
  tft.setTextSize(2);
  tft.setCursor(10, 10);
  tft.setTextColor(TFT_YELLOW);
  tft.println("Skanowanie BT...");
  tft.println("");
  tft.setTextColor(TFT_WHITE);
  tft.println("Szukam urzadzen...");
  tft.println("Poczekaj 10 sek.");
  
  delay(2000);
  
  tft.println("");
  tft.setTextColor(TFT_GREEN);
  tft.println("Znaleziono:");
  tft.println("- OBDII");
  tft.println("- ELM327");
  
  delay(3000);
  drawMainMenu();
}

// ===== POŁĄCZENIE Z OBD2 =====
void connectOBD2() {
  tft.fillScreen(TFT_BLACK);
  tft.setCursor(10, 10);
  tft.setTextColor(TFT_YELLOW);
  tft.println("Laczenie z OBD2...");
  
  delay(2000);
  
  if (!SerialBT.connected()) {
    connected = SerialBT.connect(deviceName);
  } else {
    connected = true;
  }
  
  if (connected) {
    tft.setTextColor(TFT_GREEN);
    tft.println("POLACZONO!");
    
    delay(500);
    sendOBDCommand("ATZ");
    delay(1000);
    sendOBDCommand("ATE0");
    delay(500);
    sendOBDCommand("ATL0");
    delay(500);
    sendOBDCommand("ATSP0");
    delay(500);
    
    tft.println("Zainicjalizowano!");
  } else {
    tft.setTextColor(TFT_RED);
    tft.println("BLAD POLACZENIA!");
    tft.println("");
    tft.setTextSize(1);
    tft.println("Sprawdz:");
    tft.println("- Adapter BT wlaczony");
    tft.println("- Stycz zaplon");
    tft.println("- Sparuj w telefonie");
  }
  
  delay(3000);
  drawMainMenu();
}

// ===== WYŚWIETLANIE DANYCH LIVE =====
void displayLiveData() {
  tft.fillScreen(TFT_BLACK);
  tft.setTextSize(2);
  
  tft.setCursor(10, 10);
  tft.setTextColor(TFT_CYAN);
  tft.println("=== DANE LIVE ===");
  
  tft.setCursor(10, 50);
  tft.setTextColor(TFT_WHITE);
  tft.print("RPM:   ");
  tft.setTextColor(TFT_GREEN);
  tft.print(rpm);
  tft.println(" obr/min");
  
  tft.setCursor(10, 80);
  tft.setTextColor(TFT_WHITE);
  tft.print("Predkosc: ");
  tft.setTextColor(TFT_GREEN);
  tft.print(speed);
  tft.println(" km/h");
  
  tft.setCursor(10, 110);
  tft.setTextColor(TFT_WHITE);
  tft.print("Temp:  ");
  tft.setTextColor(TFT_GREEN);
  tft.print(temp);
  tft.println(" C");
  
  tft.setCursor(10, 140);
  tft.setTextColor(TFT_WHITE);
  tft.print("Gaz:   ");
  tft.setTextColor(TFT_GREEN);
  tft.print(throttle);
  tft.println(" %");
  
  tft.setCursor(10, 200);
  tft.setTextSize(1);
  tft.setTextColor(TFT_YELLOW);
  tft.println("Lewo - powrot do menu");
}

// ===== WYSYŁANIE KOMENDY OBD2 =====
void sendOBDCommand(String command) {
  if (SerialBT.connected()) {
    SerialBT.println(command);
    Serial.println("Wyslano: " + command);
  }
}

// ===== ŻĄDANIE DANYCH LIVE =====
void requestLiveData() {
  sendOBDCommand("010C");
  delay(100);
  sendOBDCommand("010D");
  delay(100);
  sendOBDCommand("0105");
  delay(100);
  sendOBDCommand("0111");
}

// ===== OBSŁUGA ODPOWIEDZI OBD2 =====
void handleOBD2Response() {
  String response = "";
  
  while (SerialBT.available()) {
    char c = SerialBT.read();
    response += c;
    delay(10);
  }
  
  response.trim();
  Serial.println("Odpowiedz: " + response);
  
  if (response.indexOf("41 0C") >= 0) {
    rpm = parseOBDValue(response, 2) / 4;
  }
  else if (response.indexOf("41 0D") >= 0) {
    speed = parseOBDValue(response, 1);
  }
  else if (response.indexOf("41 05") >= 0) {
    temp = parseOBDValue(response, 1) - 40;
  }
  else if (response.indexOf("41 11") >= 0) {
    throttle = (parseOBDValue(response, 1) * 100) / 255;
  }
  else if (response.indexOf("43") >= 0) {
    dtcCodes = parseDTC(response);
    displayDTCCodes();
  }
}

// ===== PARSOWANIE WARTOŚCI OBD2 =====
int parseOBDValue(String response, int bytes) {
  int value = 0;
  int startPos = response.indexOf(" ") + 4;
  
  if (bytes == 1) {
    String hexVal = response.substring(startPos, startPos + 2);
    value = strtol(hexVal.c_str(), NULL, 16);
  }
  else if (bytes == 2) {
    String hexA = response.substring(startPos, startPos + 2);
    String hexB = response.substring(startPos + 3, startPos + 5);
    int A = strtol(hexA.c_str(), NULL, 16);
    int B = strtol(hexB.c_str(), NULL, 16);
    value = (A * 256) + B;
  }
  
  return value;
}

// ===== ŻĄDANIE KODÓW DTC =====
void requestDTCCodes() {
  tft.fillScreen(TFT_BLACK);
  tft.setCursor(10, 10);
  tft.setTextColor(TFT_YELLOW);
  tft.println("Odczyt kodow DTC...");
  
  sendOBDCommand("03");
  delay(2000);
}

// ===== PARSOWANIE KODÓW DTC - POPRAWIONE =====
String parseDTC(String response) {
  String codes = "";
  
  if (response.indexOf("43 00") >= 0) {
    return "Brak kodow DTC";
  }
  
  int pos = response.indexOf("43");
  if (pos >= 0) {
    String data = response.substring(pos + 3);
    data.replace(" ", "");
    
    for (int i = 0; i < data.length(); i += 4) {
      if (i + 3 < data.length()) {
        String dtc = data.substring(i, i + 4);
        if (dtc != "0000") {
          // Konwersja na format PXXXX - POPRAWIONA
          char firstChar = 'P';
          String dtcCode = dtc;
          dtcCode.toUpperCase();  // Poprawione - nie zwraca wartości
          codes += String(firstChar) + dtcCode + "\n";
        }
      }
    }
  }
  
  if (codes == "") codes = "Brak kodow";
  return codes;
}

// ===== WYŚWIETLANIE KODÓW DTC =====
void displayDTCCodes() {
  tft.fillScreen(TFT_BLACK);
  tft.setTextSize(2);
  tft.setCursor(10, 10);
  tft.setTextColor(TFT_RED);
  tft.println("=== KODY DTC ===");
  
  tft.setCursor(10, 50);
  tft.setTextColor(TFT_YELLOW);
  tft.setTextSize(1);
  
  if (dtcCodes == "") {
    tft.println("Pobieranie...");
  } else {
    tft.println(dtcCodes);
  }
  
  tft.setCursor(10, 220);
  tft.setTextColor(TFT_WHITE);
  tft.println("Lewo - menu  OK - skasuj");
}

// ===== POWRÓT =====
void goBack() {
  if (currentState != MENU_MAIN) {
    drawMainMenu();
  }
}

// ===== WYŚWIETLANIE WIADOMOŚCI =====
void showMessage(String msg) {
  tft.fillRect(0, 200, 320, 40, TFT_RED);
  tft.setCursor(10, 210);
  tft.setTextColor(TFT_WHITE);
  tft.setTextSize(2);
  tft.println(msg);
  delay(2000);
  updateDisplay();
}

// ===== INFO =====
void showInfo() {
  tft.fillScreen(TFT_BLACK);
  tft.setTextSize(1);
  tft.setCursor(10, 10);
  tft.setTextColor(TFT_CYAN);
  tft.println("ESP32 OBD2 Reader v1.0");
  tft.println("");
  tft.setTextColor(TFT_WHITE);
  tft.println("Funkcje:");
  tft.println("- Skanowanie Bluetooth");
  tft.println("- Polaczenie z OBD2");
  tft.println("- Odczyt danych LIVE");
  tft.println("- Odczyt kodow DTC");
  tft.println("");
  tft.println("Przyciski:");
  tft.println("Gora/Dol - nawigacja");
  tft.println("OK - wybor");
  tft.println("Lewo - powrot");
  tft.println("");
  tft.setTextColor(TFT_YELLOW);
  tft.println("Autor: OBD2 Project 2024");
  
  delay(5000);
  drawMainMenu();
}