#include "Display_ST77916.h"
#include "LVGL_Driver.h"
#include <math.h>

// BLE and OBD Libraries
#include <NimBLEDevice.h>
#include "ELMduino.h"

// ── Gauge Geometry ────────────────────────────────────────────────
#define CX          180
#define CY          180
#define SCREEN_W    360
#define SCREEN_H    360

#define ANGLE_START  120.0f   
#define ANGLE_END    380.0f   
#define GAUGE_MIN   -1.0f
#define GAUGE_MAX    3.0f

#define R_OUTER_TICK   172
#define R_INNER_ORANGE 150
#define R_TICK_MAJOR   150
#define R_TICK_MINOR   160
#define R_LABELS       125
#define R_NEEDLE_TIP   160
#define R_HUB          22

// -- Engine Load Position (Top Middle) --
#define LOAD_W         100
#define LOAD_X         (CX - (LOAD_W / 2))
#define LOAD_Y         (CY - 70) 

// ── State & OBD Variables ─────────────────────────────────────────
static float boost_target = 0.0f;
static float boost_current = -1.0f;
static float peak_boost = -1.0f;
static uint32_t peak_time = 0;

static float clt_temp = 0.0f; 
static float iat_temp = 0.0f; 
static float engine_load = 0.0f; 

// Buffers for strings (moved out of draw calls for speed)
static char pbuf[16] = "0.00 bar";
static char clt_buf[24] = "COOLANT: 0 °C";
static char iat_buf[24] = "INTAKE:   0 °C";

// Animation State
static bool startup_anim = true;
static int anim_stage = 0; 
static float anim_speed = 0.35f;

// BLE Status
static int ble_status = 1; 
static lv_obj_t *face_obj;
static lv_obj_t *needle_obj;

// OBD Hardware Objects
NimBLEClient* pClient = nullptr;
ELM327 myELM327;

// ── Helpers ───────────────────────────────────────────────────────
static float bar_to_rad(float bar) {
    float pct = (bar - GAUGE_MIN) / (GAUGE_MAX - GAUGE_MIN);
    float deg = ANGLE_START + pct * (ANGLE_END - ANGLE_START);
    return deg * (float)M_PI / 180.0f;
}

static lv_coord_t px(float r, float rad) { return (lv_coord_t)(CX + cosf(rad) * r); }
static lv_coord_t py(float r, float rad) { return (lv_coord_t)(CY + sinf(rad) * r); }

// ── Drawing Functions ─────────────────────────────────────────────
static void draw_gauge_face(lv_event_t *e) {
    lv_draw_ctx_t *draw_ctx = lv_event_get_draw_ctx(e);
    lv_point_t center = {CX, CY};

    // 1. Inner Orange Arc
    lv_draw_arc_dsc_t orange_arc;
    lv_draw_arc_dsc_init(&orange_arc);
    orange_arc.color = lv_color_make(0xFF, 0x44, 0x00);
    orange_arc.width = 2;
    lv_draw_arc(draw_ctx, &orange_arc, &center, R_INNER_ORANGE, (int)ANGLE_START, (int)ANGLE_END);

    // 2. Ticks
    for (int i = 0; i <= 40; i++) {
        float val = GAUGE_MIN + (i * 0.1f);
        float rad = bar_to_rad(val);
        bool is_major = (fmod(val + 0.01f, 0.5f) < 0.05f);
        lv_draw_line_dsc_t tick_dsc;
        lv_draw_line_dsc_init(&tick_dsc);
        tick_dsc.color = lv_color_white();
        tick_dsc.width = is_major ? 3 : 1;
        lv_point_t p_out = {px(R_OUTER_TICK, rad), py(R_OUTER_TICK, rad)};
        lv_point_t p_in  = {px(is_major ? R_TICK_MAJOR : R_TICK_MINOR, rad), py(is_major ? R_TICK_MAJOR : R_TICK_MINOR, rad)};
        lv_draw_line(draw_ctx, &tick_dsc, &p_in, &p_out);
    }

    // 3. Scale Labels
    lv_draw_label_dsc_t label_dsc;
    lv_draw_label_dsc_init(&label_dsc);
    label_dsc.color = lv_color_white();
    label_dsc.font = &lv_font_montserrat_14; 
    float labels[] = {-1.0, -0.5, 0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0};
    for (float l : labels) {
        float rad = bar_to_rad(l);
        char l_buf[8];
        sprintf(l_buf, "%g", (l < 0 ? fabsf(l) : l));
        int lx = px(R_LABELS, rad) - 15;
        int ly = py(R_LABELS, rad) - 10;
        lv_area_t area = {lx, ly, lx + 40, ly + 20};
        lv_draw_label(draw_ctx, &label_dsc, &area, l_buf, NULL);
    }

    // 4. ENGINE LOAD SECTION
    lv_draw_rect_dsc_t load_bg_dsc;
    lv_draw_rect_dsc_init(&load_bg_dsc);
    load_bg_dsc.bg_color = lv_color_make(0x1A, 0x1A, 0x1A);
    load_bg_dsc.radius = 2;
    lv_area_t load_bg_area = {LOAD_X, LOAD_Y, LOAD_X + LOAD_W, LOAD_Y + 5};
    lv_draw_rect(draw_ctx, &load_bg_dsc, &load_bg_area);

    lv_draw_rect_dsc_t load_fill_dsc;
    lv_draw_rect_dsc_init(&load_fill_dsc);
    load_fill_dsc.bg_color = lv_color_make(0x00, 0xFF, 0x00); 
    int fill_w = (int)((engine_load / 100.0f) * LOAD_W);
    if(fill_w > LOAD_W) fill_w = LOAD_W;
    lv_area_t load_fill_area = {LOAD_X, LOAD_Y, LOAD_X + fill_w, LOAD_Y + 5};
    lv_draw_rect(draw_ctx, &load_fill_dsc, &load_fill_area);

    lv_draw_label_dsc_t load_lbl_dsc;
    lv_draw_label_dsc_init(&load_lbl_dsc);
    load_lbl_dsc.color = lv_color_make(150, 150, 150);
    load_lbl_dsc.font = &lv_font_montserrat_10;
    lv_area_t load_txt_area = {LOAD_X, LOAD_Y + 8, LOAD_X + LOAD_W, LOAD_Y + 20};
    lv_draw_label(draw_ctx, &load_lbl_dsc, &load_txt_area, "ENGINE LOAD", NULL);

    // 5. FUTURISTIC DATA HUB
    lv_draw_rect_dsc_t box_dsc;
    lv_draw_rect_dsc_init(&box_dsc);
    box_dsc.bg_color = lv_color_make(0x1A, 0x1A, 0x1A);
    box_dsc.bg_opa = 200; 
    box_dsc.radius = 4;
    lv_area_t data_box = {CX - 35, CY + 55, CX + 115, CY + 145};
    lv_draw_rect(draw_ctx, &box_dsc, &data_box);

    lv_draw_rect_dsc_t accent_dsc;
    lv_draw_rect_dsc_init(&accent_dsc);
    accent_dsc.bg_color = lv_color_make(0xFF, 0x44, 0x00);
    accent_dsc.bg_opa = (ble_status != 2 && !startup_anim) ? (150 + 100 * sin(millis() / 200.0)) : 255;
    lv_area_t accent_bar = {CX - 33, CY + 60, CX - 30, CY + 140};
    lv_draw_rect(draw_ctx, &accent_dsc, &accent_bar);

    label_dsc.color = lv_color_make(180, 180, 180);
    label_dsc.font = &lv_font_montserrat_12;
    lv_area_t pk_label_area = {CX - 20, CY + 62, CX + 100, CY + 75};
    lv_draw_label(draw_ctx, &label_dsc, &pk_label_area, "PEAK BOOST", NULL);

    label_dsc.color = lv_color_make(0xFF, 0xAA, 0x00);
    label_dsc.font = &lv_font_montserrat_22;
    lv_area_t peak_val_area = {CX - 20, CY + 76, CX + 110, CY + 102};
    lv_draw_label(draw_ctx, &label_dsc, &peak_val_area, pbuf, NULL);

    lv_draw_line_dsc_t line_dsc;
    lv_draw_line_dsc_init(&line_dsc);
    line_dsc.color = lv_color_make(0x33, 0x33, 0x33);
    line_dsc.width = 1;
    lv_point_t lp1 = {CX - 20, CY + 105}, lp2 = {CX + 100, CY + 105};
    lv_draw_line(draw_ctx, &line_dsc, &lp1, &lp2);

    label_dsc.font = &lv_font_montserrat_14;
    label_dsc.color = lv_color_make(220, 220, 220);
    lv_area_t clt_area = {CX - 20, CY + 110, CX + 110, CY + 125};
    lv_draw_label(draw_ctx, &label_dsc, &clt_area, clt_buf, NULL);
    lv_area_t iat_area = {CX - 20, CY + 127, CX + 110, CY + 142};
    lv_draw_label(draw_ctx, &label_dsc, &iat_area, iat_buf, NULL);
}

static void draw_needle(lv_event_t *e) {
    lv_draw_ctx_t *draw_ctx = lv_event_get_draw_ctx(e);
    float rad = bar_to_rad(boost_current);
    lv_draw_line_dsc_t needle_dsc;
    lv_draw_line_dsc_init(&needle_dsc);
    needle_dsc.color = lv_color_make(0xFF, 0x22, 0x00);
    needle_dsc.width = 5;
    needle_dsc.round_end = 1;
    lv_point_t p0 = {px(R_HUB - 5, rad), py(R_HUB - 5, rad)};
    lv_point_t p1 = {px(R_NEEDLE_TIP, rad), py(R_NEEDLE_TIP, rad)};
    lv_draw_line(draw_ctx, &needle_dsc, &p0, &p1);

    lv_draw_rect_dsc_t hub_dsc;
    lv_draw_rect_dsc_init(&hub_dsc);
    hub_dsc.bg_color = lv_color_make(0x11, 0x11, 0x11);
    hub_dsc.radius = LV_RADIUS_CIRCLE;
    hub_dsc.border_width = 2;
    hub_dsc.border_color = lv_color_make(0x66, 0x66, 0x66);
    lv_area_t hub_area = {CX - R_HUB, CY - R_HUB, CX + R_HUB, CY + R_HUB};
    lv_draw_rect(draw_ctx, &hub_dsc, &hub_area);
}

void connect_to_vlink() {
    NimBLEScan* pScan = NimBLEDevice::getScan();
    pScan->setActiveScan(true);
    pScan->start(1, false); 
    auto results = pScan->getResults();
    for (auto it = results.begin(); it != results.end(); ++it) {
        NimBLEAdvertisedDevice* device = *it;
        if (device->getName().find("V-LINK") != std::string::npos || device->getName().find("OBDII") != std::string::npos) {
            pClient = NimBLEDevice::createClient();
            if (pClient->connect(device)) {
                if (myELM327.begin(Serial1, false, 2000)) {
                    ble_status = 2; 
                    return;
                }
            }
        }
    }
}

void gauge_create(void) {
    lv_obj_t *scr = lv_scr_act();
    lv_obj_clean(scr); 
    lv_obj_set_style_bg_color(scr, lv_color_black(), 0);
    
    // Background Layer (Labels, Ticks)
    face_obj = lv_obj_create(scr);
    lv_obj_set_size(face_obj, SCREEN_W, SCREEN_H);
    lv_obj_set_style_bg_opa(face_obj, 0, 0);
    lv_obj_set_style_border_width(face_obj, 0, 0);
    lv_obj_add_event_cb(face_obj, draw_gauge_face, LV_EVENT_DRAW_MAIN, NULL);
    
    // Foreground Layer (Needle)
    needle_obj = lv_obj_create(scr);
    lv_obj_set_size(needle_obj, SCREEN_W, SCREEN_H);
    lv_obj_set_style_bg_opa(needle_obj, 0, 0);
    lv_obj_set_style_border_width(needle_obj, 0, 0);
    lv_obj_add_event_cb(needle_obj, draw_needle, LV_EVENT_DRAW_MAIN, NULL);
}

void DriverTask(void *parameter) {
    while (1) {
        if (!startup_anim) {
            if (ble_status != 2) {
                connect_to_vlink();
                vTaskDelay(pdMS_TO_TICKS(1000));
            } else {
                float t_clt = myELM327.engineCoolantTemp();
                if (myELM327.nb_rx_state == ELM_SUCCESS) clt_temp = t_clt;
                float t_iat = myELM327.intakeAirTemp();
                if (myELM327.nb_rx_state == ELM_SUCCESS) iat_temp = t_iat;
                float t_load = myELM327.engineLoad();
                if (myELM327.nb_rx_state == ELM_SUCCESS) engine_load = t_load;

                float map_kpa = myELM327.manifoldPressure();
                if (myELM327.nb_rx_state == ELM_SUCCESS) {
                    boost_target = (map_kpa / 100.0f) - 1.01f;
                }
                if (boost_target > peak_boost) {
                    peak_boost = boost_target;
                    peak_time = millis();
                }
                
                // Update text buffers outside of drawing thread
                sprintf(pbuf, "%.2f bar", peak_boost < 0 ? 0.00 : peak_boost);
                sprintf(clt_buf, "COOLANT: %.0f °C", clt_temp);
                sprintf(iat_buf, "INTAKE:   %.0f °C", iat_temp);

                vTaskDelay(pdMS_TO_TICKS(50));
            }
        } else {
            vTaskDelay(pdMS_TO_TICKS(100));
        }
    }
}

void setup() {
    Serial.begin(115200);
    Serial1.begin(38400); 
    TCA9554PWR_Init(0x00);
    Backlight_Init(); LCD_Init(); Lvgl_Init();
    NimBLEDevice::init("ESP32_Gauge");
    gauge_create();
    xTaskCreatePinnedToCore(DriverTask, "DriverTask", 8192, NULL, 2, NULL, 1);
}

void loop() {
    static uint32_t last_face_update = 0;

    if (startup_anim) {
        if (anim_stage == 0) {
            boost_current += anim_speed;
            if (boost_current >= GAUGE_MAX) {
                boost_current = GAUGE_MAX;
                anim_stage = 1;
            }
        } else if (anim_stage == 1) {
            float diff = boost_current - 0.0f;
            float step = diff * 0.07f; 
            if (step < 0.01f) step = 0.01f;
            boost_current -= step;
            if (boost_current <= 0.01f) {
                boost_current = 0.0f;
                boost_target = 0.0f;
                startup_anim = false;
                anim_stage = 2;
            }
        }
        vTaskDelay(pdMS_TO_TICKS(10)); 
    } else {
        boost_current += (boost_target - boost_current) * 0.15f;
        if (millis() - peak_time > 10000) {
            peak_boost = boost_current;
            peak_time = millis();
        }
    }

    // CRITICAL OPTIMIZATION: 
    // Always update the needle (fast)
    lv_obj_invalidate(needle_obj); 

    // Update the face (labels/load) only every 100ms (10Hz) to save FPS
    if (millis() - last_face_update > 100) {
        lv_obj_invalidate(face_obj);
        last_face_update = millis();
    }

    Lvgl_Loop();
    vTaskDelay(pdMS_TO_TICKS(16)); // Target ~60 FPS
}