/* main.c — ESP32-S3 BALANCE app_main (bd-66hx)
 *
 * Initializes Orin serial and VESC CAN TWAI, creates tasks:
 *   orin_rx   — parse incoming Orin commands
 *   orin_tx   — transmit queued serial frames
 *   vesc_rx   — receive VESC CAN telemetry, proxy to Orin
 *   telem     — periodic TELEM_STATUS to Orin @ 10 Hz
 *   drive     — apply Orin drive commands to VESCs via CAN
 */

#include "orin_serial.h"
#include "vesc_can.h"
#include "gc9a01.h"
#include "config.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "esp_log.h"
#include "esp_timer.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

static const char *TAG = "main";

static QueueHandle_t s_orin_tx_q;

/* ── HUD task: SAULT brand + battery voltage on GC9A01 display ── */
static void hud_task(void *arg)
{
    /* Draw static "SAULT" header in orange — scale=3: each char 15×21 px */
    const int hs = 3;
    int hx = (240 - 5 * 6 * hs) / 2;
    display_draw_string_s(hx, 16, "SAULT", COL_ORANGE, COL_BG, hs);

    char prev[16] = "";
    for (;;) {
        vTaskDelay(pdMS_TO_TICKS(1000));

        int16_t v10 = g_vesc[0].voltage_x10;
        char vstr[16];
        if (v10 <= 0) {
            snprintf(vstr, sizeof(vstr), " --.-V");
        } else {
            snprintf(vstr, sizeof(vstr), "%2d.%dV", v10 / 10, abs(v10 % 10));
        }

        if (strcmp(vstr, prev) != 0) {
            const int vs = 4;  /* scale=4: each char 20×28 px */
            int nch = (int)strlen(vstr);
            int vx  = (240 - nch * 6 * vs) / 2;
            display_fill_rect(0, 95, 240, 7 * vs + 2, COL_BG);
            display_draw_string_s(vx, 97, vstr, COL_WHITE, COL_BG, vs);
            strncpy(prev, vstr, sizeof(prev) - 1);
        }
    }
}

/* ── Telemetry task: sends TELEM_STATUS to Orin at 10 Hz ── */
static void telem_task(void *arg)
{
    for (;;) {
        vTaskDelay(pdMS_TO_TICKS(TELEM_STATUS_PERIOD_MS));

        uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
        bool hb_timeout = (now_ms - g_orin_ctrl.hb_last_ms) > HB_TIMEOUT_MS;

        /* Determine balance state for telemetry */
        bal_state_t state;
        if (g_orin_ctrl.estop) {
            state = BAL_ESTOP;
        } else if (!g_orin_ctrl.armed) {
            state = BAL_DISARMED;
        } else {
            state = BAL_ARMED;
        }

        /* flags: bit0=estop_active, bit1=heartbeat_timeout, bit2=twai_bus_off */
        uint8_t flags = (g_orin_ctrl.estop ? 0x01u : 0x00u) |
                        (hb_timeout         ? 0x02u : 0x00u) |
                        (g_twai_bus_off     ? 0x04u : 0x00u);

        /* Battery voltage from VESC_ID_A STATUS_5 (V×10 → mV) */
        uint16_t vbat_mv = (uint16_t)((int32_t)g_vesc[0].voltage_x10 * 100);

        orin_send_status(s_orin_tx_q,
                         0,      /* pitch_x10: stub — full IMU in future bead */
                         0,      /* motor_cmd: stub */
                         vbat_mv,
                         state,
                         flags);
    }
}

/* ── Drive task: applies Orin drive commands to VESCs @ 50 Hz ── */
static void drive_task(void *arg)
{
    uint32_t log_tick    = 0;
    bool     was_driving = false;

    for (;;) {
        vTaskDelay(pdMS_TO_TICKS(20));  /* 50 Hz */

        uint32_t now_ms   = (uint32_t)(esp_timer_get_time() / 1000LL);
        bool hb_timeout   = (now_ms - g_orin_ctrl.hb_last_ms) > HB_TIMEOUT_MS;
        bool drive_stale  = (now_ms - g_orin_drive.updated_ms) > DRIVE_TIMEOUT_MS;
        bool gates_ok     = g_orin_ctrl.armed && !g_orin_ctrl.estop &&
                            !hb_timeout && !drive_stale;

        int32_t left_erpm  = 0;
        int32_t right_erpm = 0;

        if (gates_ok) {
            int32_t spd = (int32_t)g_orin_drive.speed * RPM_PER_SPEED_UNIT;
            int32_t str = (int32_t)g_orin_drive.steer * RPM_PER_STEER_UNIT;
            left_erpm  = spd + str;
            right_erpm = spd - str;
            if (!was_driving) {
                ESP_LOGI(TAG, "drive ENABLED: spd=%d str=%d L=%ld R=%ld",
                         g_orin_drive.speed, g_orin_drive.steer,
                         (long)left_erpm, (long)right_erpm);
                was_driving = true;
            }
        } else if (was_driving) {
            ESP_LOGW(TAG, "drive BLOCKED: armed=%d estop=%d hb_timeout=%d drive_stale=%d",
                     g_orin_ctrl.armed, g_orin_ctrl.estop, hb_timeout, drive_stale);
            was_driving = false;
        }

        /* 1 Hz gate-state diagnostic */
        if ((now_ms - log_tick) >= 1000u) {
            log_tick = now_ms;
            ESP_LOGI(TAG, "gate: armed=%d estop=%d hb_age=%lums drv_age=%lums twai_off=%d L=%ld R=%ld",
                     g_orin_ctrl.armed, g_orin_ctrl.estop,
                     (unsigned long)(now_ms - g_orin_ctrl.hb_last_ms),
                     (unsigned long)(now_ms - g_orin_drive.updated_ms),
                     (int)g_twai_bus_off,
                     (long)left_erpm, (long)right_erpm);
        }

        /* VESC_ID_A (56) = LEFT, VESC_ID_B (68) = RIGHT per bd-wim1 protocol */
        vesc_can_send_rpm(VESC_ID_A, left_erpm);
        vesc_can_send_rpm(VESC_ID_B, right_erpm);
    }
}

void app_main(void)
{
    ESP_LOGI(TAG, "ESP32-S3 BALANCE bd-66hx starting");

    orin_serial_init();
    vesc_can_init();
    gc9a01_init();

    /* TX queue for outbound serial frames */
    s_orin_tx_q = xQueueCreate(ORIN_TX_QUEUE_DEPTH, sizeof(orin_tx_frame_t));
    configASSERT(s_orin_tx_q);

    /* Seed timers so we don't immediately trip hb_timeout or drive_stale */
    g_orin_ctrl.hb_last_ms  = (uint32_t)(esp_timer_get_time() / 1000LL);
    g_orin_drive.updated_ms = g_orin_ctrl.hb_last_ms;
    g_orin_ctrl.armed = true;  /* bypass for motor testing */

    /* Create tasks */
    xTaskCreate(orin_serial_rx_task, "orin_rx",  4096, s_orin_tx_q, 10, NULL);
    xTaskCreate(orin_serial_tx_task, "orin_tx",  2048, s_orin_tx_q,  9, NULL);
    if (!g_twai_bus_off) {
        xTaskCreate(vesc_can_rx_task,    "vesc_rx",  4096, s_orin_tx_q, 10, NULL);
        xTaskCreate(drive_task,          "drive",    4096, NULL,          8, NULL);
    } else {
        ESP_LOGW(TAG, "CAN disabled — vesc_rx and drive tasks not started");
    }
    xTaskCreate(telem_task,          "telem",    2048, NULL,          5, NULL);
    xTaskCreate(hud_task,            "hud",      4096, NULL,          3, NULL);

    ESP_LOGI(TAG, "all tasks started");
    /* app_main returns — FreeRTOS scheduler continues */
}