/* main.c — ESP32-S3 BALANCE app_main (bd-66hx + OTA beads) */

#include "orin_serial.h"
#include "vesc_can.h"
#include "gc9a01.h"
#include "gitea_ota.h"
#include "ota_self.h"
#include "uart_ota.h"
#include "ota_display.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 <string.h>

static const char *TAG = "main";

static QueueHandle_t s_orin_tx_q;

/* ── 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 */
        uint8_t flags = (g_orin_ctrl.estop ? 0x01u : 0x00u) |
                        (hb_timeout         ? 0x02u : 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)
{
    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;

        int32_t front_erpm = 0;

        if (g_orin_ctrl.armed && !g_orin_ctrl.estop &&
            !hb_timeout && !drive_stale) {
            front_erpm = (int32_t)g_orin_drive.speed * RPM_PER_SPEED_UNIT;
        }

        vesc_can_send_rpm(VESC_ID_A, front_erpm);
    }
}

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

    /* OTA rollback health check — must be called within OTA_ROLLBACK_WINDOW_S */
    ota_self_health_check();

    /* Init peripherals — gc9a01 before vesc_can so BL/GPIO2 is high before TWAI takes it */
    gc9a01_init();
    orin_serial_init();
    vesc_can_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 heartbeat timer so we don't immediately timeout */
    g_orin_ctrl.hb_last_ms = (uint32_t)(esp_timer_get_time() / 1000LL);

    /* 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);
    xTaskCreate(vesc_can_rx_task,    "vesc_rx",  4096, s_orin_tx_q, 10, NULL);
    xTaskCreate(telem_task,          "telem",    2048, NULL,          5, NULL);
    xTaskCreate(drive_task,          "drive",    2048, NULL,          8, NULL);

    /* OTA subsystem — WiFi version checker + display overlay */
    gitea_ota_init();
    ota_display_init();

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