#include "ELMduino.h"
#include "WiFi.h"
#include <TFT_eSPI.h>

// WiFi & OBD-II 설정
const char* ssid = "WiFi_OBDII";
const char* host = "192.168.0.10";
const uint16_t port = 35000;

// TFT 디스플레이 객체 초기화
TFT_eSPI display = TFT_eSPI();

// ELM327 객체 초기화
ELM327 myELM327;
WiFiClient client;

// 상태 정의
enum State {
    CHECK_REGEN_STATUS,
    SEND_NORMAL_QUERIES,
    SHOW_REGEN_PROGRESS
};
State currentState = CHECK_REGEN_STATUS;

// PID 명령 목록
const char* regenQuery = "223224";
const char* regenProgressQuery = "223274";
const char* normalQueries[] = {
    "223275",   // DPF 포집량
    "22303B"    // DPF 온도
};

// 데이터 저장 변수
int dpfSootLevel = 68;  // 초기 값
float dpfTemp = 165.1;   // 초기 값
int regenProgress = 75;  // 초기 값
bool isRegenerating = false;

int normalQueryIndex = 0;
const char* lastCommand = nullptr; // 마지막으로 보낸 명령

// 응답 처리용 전역 변수
char responseBuffer[64] = {0};  // 배열 크기 축소
int responseBufferIndex = 0;

// 상태 플래그
bool waitingForResponse = false; // 응답 대기 상태

void setup() {
    Serial.begin(115200);
    display.init();
    display.setRotation(1);
    display.fillScreen(TFT_BLACK);
    display.setTextColor(TFT_WHITE, TFT_BLACK);
    display.setTextSize(1);
    display.setCursor(10, 120);
    display.println("Initializing...");

    WiFi.begin(ssid);
    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        display.print(".");
        yield();  // 와이파이 연결이 완료될 때까지 다른 작업 허용
    }
    display.println("\nConnected to WiFi");

    if (!client.connect(host, port)) {
        Serial.println("Couldn't connect to ELM327 device.");
        while (1);
    }

    if (!myELM327.begin(client, true, 5000)) {  // 타임아웃 증가
        Serial.println("ELM327 device couldn't connect to ECU.");
        while (1);
    }

    Serial.println("Connected to ELM327");
    display.fillScreen(TFT_BLACK);
    myELM327.sendCommand_Blocking("ATSH7E0");
}

void loop() {
    if (client.available()) {
        char c = client.read();

        if (c == '6' && responseBufferIndex == 0) {
            responseBuffer[responseBufferIndex++] = c;
        } else if (responseBufferIndex > 0 && responseBufferIndex < sizeof(responseBuffer) - 1) {
            responseBuffer[responseBufferIndex++] = c;
            responseBuffer[responseBufferIndex] = '\0';

            if (strstr(responseBuffer, "\r\r>")) {
                processResponse(responseBuffer);
                clearResponseBuffer();
                waitingForResponse = false; // 응답 처리 완료
            }
        }
    }

    if (!waitingForResponse) {
        switch (currentState) {
        case CHECK_REGEN_STATUS:
            lastCommand = regenQuery;
            myELM327.sendCommand(regenQuery);
            waitingForResponse = true; // 응답 대기 상태로 설정
            break;

        case SEND_NORMAL_QUERIES:
            lastCommand = normalQueries[normalQueryIndex];
            myELM327.sendCommand(normalQueries[normalQueryIndex]);
            normalQueryIndex++;
            if (normalQueryIndex >= 2) {
                normalQueryIndex = 0;
                currentState = CHECK_REGEN_STATUS;
            }
            waitingForResponse = true; // 응답 대기 상태로 설정
            delay(1000);               // 딜레이 추가
            break;

        case SHOW_REGEN_PROGRESS:
            lastCommand = regenProgressQuery;
            myELM327.sendCommand(regenProgressQuery);
            waitingForResponse = true; // 응답 대기 상태로 설정
            delay(1000);               // 딜레이 추가
            break;
        }
    }
}

void processResponse(const char* response) {
    char cleanResponse[64] = {0};
    strncpy(cleanResponse, response, strlen(response) - 3);

    if (lastCommand == regenQuery) {
        int A = strtol(&cleanResponse[6], nullptr, 16);
        isRegenerating = (A & 0x01);

        // 재생 여부에 따라 상태 전환
        currentState = isRegenerating ? SHOW_REGEN_PROGRESS : SEND_NORMAL_QUERIES;
    } else if (lastCommand == regenProgressQuery) {
        int A = strtol(&cleanResponse[6], nullptr, 16);
        regenProgress = (A * 100) / 255;

        // 화면 업데이트
        displayRegenProgress();

        // 상태 전환: 재생 여부 다시 확인
        currentState = CHECK_REGEN_STATUS;
    } else if (lastCommand == normalQueries[0]) {
        int A = strtol(&cleanResponse[6], nullptr, 16);
        dpfSootLevel = A;
    } else if (lastCommand == normalQueries[1]) {
        char hexValue[5] = {cleanResponse[6], cleanResponse[7], cleanResponse[8], cleanResponse[9], '\0'};
        int rawValue = strtol(hexValue, nullptr, 16);
        dpfTemp = (rawValue / 10.0) - 40.0;

        // 일반 화면 업데이트
        displayNormalData();
    }
}

void displayRegenProgress() {
    static int lastRegenProgress = -1;

    if (lastRegenProgress != regenProgress) {
        display.fillScreen(TFT_RED);

        // Regenerating 텍스트
        String regenText = "Regenerating";
        display.setTextSize(2);
        display.setCursor(10, 10);
        display.println(regenText);

        // 재생 진행률 값
        String progressText = String(regenProgress) + "%";
        display.setTextSize(4);
        display.setCursor(30, 50);
        display.println(progressText);

        lastRegenProgress = regenProgress;
    }
}

void displayNormalData() {
    static int lastDpfSootLevel = -1;
    static float lastDpfTemp = -1.0;

    // DPF Soot 텍스트
    display.setTextSize(2);
    display.setCursor(10, 10);
    display.println("DPF Soot");

    // DPF Soot 값 업데이트
    if (lastDpfSootLevel != dpfSootLevel) {
        String sootValue = String(dpfSootLevel) + "%";
        display.setTextSize(4);
        display.fillRect(10, 30, 100, 30, TFT_BLACK); // 이전 값 영역 지움
        display.setCursor(10, 30);
        display.println(sootValue);
        lastDpfSootLevel = dpfSootLevel;
    }

    // DPF Temp 텍스트
    display.setTextSize(2);
    display.setCursor(10, 70);
    display.println("DPF Temp");

    // DPF Temp 값 업데이트
    if (lastDpfTemp != dpfTemp) {
        String tempValue = String(dpfTemp, 1) + "C";
        display.setTextSize(4);
        display.fillRect(10, 90, 100, 30, TFT_BLACK); // 이전 값 영역 지움
        display.setCursor(10, 90);
        display.println(tempValue);
        lastDpfTemp = dpfTemp;
    }
}

void clearResponseBuffer() {
    memset(responseBuffer, 0, sizeof(responseBuffer));
    responseBufferIndex = 0;
}