#include "hc_06.h"
#include <string.h>
#include <stdio.h>
#include <math.h>
#include "IKW1281Connection.h"
#include "kline_fsm.h"

extern UART_HandleTypeDef huart3;

/* =========================
   TX delay / non-blocking queue
   =========================
   Goal: delay every binary reply by ~10ms without blocking the CPU.
   Implementation: enqueue TX buffers with a due timestamp, then send using
   HAL_UART_Transmit_IT() when due, chaining via TxCplt callback.

   Notes:
   - Only binary frames (HC06_SendFrame / HC06_SendDataReply) use this queue.
   - ASCII helpers remain blocking (debug only).
*/
#define HC06_TX_DELAY_MS      10u
#define HC06_TX_QUEUE_LEN     8u
#define HC06_TX_BUF_MAX       80u   // enough for max CMD_DATA_REQUEST reply (~70B)

typedef struct {
    uint32_t due_ms;
    uint16_t len;
    uint8_t  buf[HC06_TX_BUF_MAX];
} hc06_tx_item_t;

static volatile uint8_t s_tx_head  = 0;
static volatile uint8_t s_tx_tail  = 0;
static volatile uint8_t s_tx_count = 0;
static volatile uint8_t s_tx_busy  = 0;
static hc06_tx_item_t   s_tx_q[HC06_TX_QUEUE_LEN];

static void HC06_TxReset(void)
{
    s_tx_head  = 0;
    s_tx_tail  = 0;
    s_tx_count = 0;
    s_tx_busy  = 0;
}

static HAL_StatusTypeDef HC06_TxEnqueue(const uint8_t *data, uint16_t len)
{
    if (!data || len == 0u || len > HC06_TX_BUF_MAX) return HAL_ERROR;
    if (s_tx_count >= HC06_TX_QUEUE_LEN) return HAL_BUSY;

    hc06_tx_item_t *it = &s_tx_q[s_tx_tail];
    memcpy(it->buf, data, len);
    it->len    = len;
    it->due_ms = HAL_GetTick() + HC06_TX_DELAY_MS;

    s_tx_tail = (uint8_t)((s_tx_tail + 1u) % HC06_TX_QUEUE_LEN);
    s_tx_count++;
    return HAL_OK;
}

static void HC06_TxKick(void)
{
    if (s_tx_busy)    return;
    if (s_tx_count == 0u) return;

    hc06_tx_item_t *it = &s_tx_q[s_tx_head];
    if ((int32_t)(HAL_GetTick() - it->due_ms) < 0) return;

    if (HAL_UART_Transmit_IT(&huart3, it->buf, it->len) == HAL_OK)
        s_tx_busy = 1u;
}

void HC06_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
    if (!huart) return;
    if (huart->Instance != huart3.Instance) return;
    if (!s_tx_busy) return;

    if (s_tx_count > 0u) {
        s_tx_head = (uint8_t)((s_tx_head + 1u) % HC06_TX_QUEUE_LEN);
        s_tx_count--;
    }
    s_tx_busy = 0u;
    HC06_TxKick();
}

/* =========================
   Project-specific blocking helpers
   =========================
   These still call into the old kline.c functions (ReadDfi, WriteDfi, etc.)
   that perform synchronous KWP transactions. They will need to be ported to
   FSM async commands in a future step. For now they are preserved as-is.
   DO NOT call them unless KLine_FSM_IsConnected() returns true.
*/
extern float   ReadDfi(void);
extern int     WriteDfi(float dfi, int version);
extern int     ClearFaultCodes(void);
extern uint8_t ReadAudiPin(uint16_t *pin);
extern uint8_t ReadVoltage(uint16_t *volt);

static void HC06_ForceRearmRx(void);

static void BT_InitPSG5Comm(void);
static void BT_ReadDfi(void);
static void BT_WriteDfi(void);
static void BT_ReadDTC(void);
static void BT_ClearDTC(void);
static void BT_ReadAudiPin(void);

/* =========================
   USER SETTINGS (edit)
   ========================= */
#define HC06_DO_AT_SETUP_AT_BOOT   1
#define HC06_NAME                 "HC - BPDT - Adapter"

#define HC06_TRY_SET_PIN           0
#define HC06_PIN_STR              "1234"

#define HC06_AT_USE_CRLF           1

#define HC06_AT_WAIT_MS_AT         600
#define HC06_AT_WAIT_MS_NAME       800
#define HC06_AT_WAIT_MS_PIN        800

/* =========================
   Globals
   ========================= */
volatile uint8_t g_hc06_frame_ready = 0;
hc06_frame_t     g_hc06_frame;

static uint8_t   rx_byte;
uint8_t          commandPending = 0;

/* =========================
   Mode
   ========================= */
typedef enum {
    HC06_MODE_AT  = 0,
    HC06_MODE_BIN = 1
} hc06_mode_t;

static volatile hc06_mode_t s_mode = HC06_MODE_AT;

/* =========================
   AT capture
   ========================= */
static volatile char    s_at_buf[64];
static volatile uint8_t s_at_len  = 0;
static volatile uint8_t s_at_done = 0;

static void AT_Clear(void)
{
    s_at_len  = 0;
    s_at_done = 0;
    memset((void*)s_at_buf, 0, sizeof(s_at_buf));
}

static void AT_Wait(uint32_t timeout_ms)
{
    uint32_t t0 = HAL_GetTick();
    while (!s_at_done && (HAL_GetTick() - t0) < timeout_ms) {}
}

/* =========================
   Binary frame parser
   ========================= */
typedef enum {
    ST_SYNC = 0,
    ST_CMD,
    ST_REQ_ID,
    ST_REQ_CRC,
    ST_D0, ST_D1, ST_D2, ST_D3, ST_D4, ST_D5,
    ST_CRC
} hc06_rx_state_t;

static hc06_rx_state_t st = ST_SYNC;
static uint8_t payload[7];
static uint8_t pi  = 0;
static uint8_t crc = 0;

static uint16_t u16_le(uint8_t lo, uint8_t hi)
{
    return (uint16_t)lo | ((uint16_t)hi << 8);
}

static void u16_to_le(uint16_t v, uint8_t *lo, uint8_t *hi)
{
    *lo = (uint8_t)(v & 0xFF);
    *hi = (uint8_t)((v >> 8) & 0xFF);
}

static uint8_t crc8_update(uint8_t c, uint8_t data)
{
    c ^= data;
    for (int i = 0; i < 8; i++)
        c = (c & 0x80) ? (uint8_t)((c << 1) ^ 0x07) : (uint8_t)(c << 1);
    return c;
}

/* =========================
   Public: DFI scaling
   ========================= */
int16_t HC06_DfiFloatToS16(float dfi)
{
    float s = dfi * (256.0f / 3.0f);
    if (s >  32767.0f) s =  32767.0f;
    if (s < -32768.0f) s = -32768.0f;
    return (int16_t)lroundf(s);
}

float HC06_DfiS16ToFloat(int16_t s)
{
    return ((float)s) * (3.0f / 256.0f);
}

/* =========================
   TX helpers
   ========================= */
HAL_StatusTypeDef HC06_SendAscii(const char *s)
{
    if (!s) return HAL_ERROR;
    return HAL_UART_Transmit(&huart3, (uint8_t*)s, (uint16_t)strlen(s), 200);
}

HAL_StatusTypeDef HC06_SendAsciiLn(const char *s)
{
    HAL_StatusTypeDef a = HC06_SendAscii(s);
    HAL_StatusTypeDef b = HC06_SendAscii("\r\n");
    return (a == HAL_OK && b == HAL_OK) ? HAL_OK : HAL_ERROR;
}

HAL_StatusTypeDef HC06_SendFrame(uint8_t cmd, uint16_t w1, uint16_t w2, uint16_t w3)
{
    uint8_t buf[9];
    uint8_t c = 0;

    buf[0] = HC06_SYNC;
    buf[1] = cmd;

    uint8_t lo, hi;
    u16_to_le(w1, &lo, &hi); buf[2] = lo; buf[3] = hi;
    u16_to_le(w2, &lo, &hi); buf[4] = lo; buf[5] = hi;
    u16_to_le(w3, &lo, &hi); buf[6] = lo; buf[7] = hi;

    for (int i = 1; i <= 7; i++) c = crc8_update(c, buf[i]);
    buf[8] = c;

    HAL_StatusTypeDef s = HC06_TxEnqueue(buf, (uint16_t)sizeof(buf));
    HC06_TxKick();
    return s;
}

/* =========================
   AT send helpers
   ========================= */
static void AT_SendRaw(const char *s) { (void)HC06_SendAscii(s); }

static void AT_SendCmd(const char *cmd_no_prefix)
{
    char buf[96];
#if HC06_AT_USE_CRLF
    snprintf(buf, sizeof(buf), "AT+%s\r\n", cmd_no_prefix);
#else
    snprintf(buf, sizeof(buf), "AT+%s", cmd_no_prefix);
#endif
    AT_SendRaw(buf);
}

/* =========================
   Variable-length data reply
   ========================= */
#define HC06_MAX_PAYLOAD 64
static HAL_StatusTypeDef HC06_SendDataReply(uint8_t status, const uint8_t *payloadIn, uint16_t len)
{
    if (status != 0x00) { len = 0; payloadIn = NULL; }
    if (len > HC06_MAX_PAYLOAD) len = HC06_MAX_PAYLOAD;

    uint8_t  buf[1 + 1 + 1 + 2 + HC06_MAX_PAYLOAD + 1];
    uint16_t idx = 0;
    uint8_t  c   = 0;

    buf[idx++] = HC06_SYNC;
    buf[idx++] = 0x84;
    buf[idx++] = status;

    uint8_t lo, hi;
    u16_to_le(len, &lo, &hi);
    buf[idx++] = lo;
    buf[idx++] = hi;

    for (uint16_t i = 0; i < len; i++)
        buf[idx++] = payloadIn ? payloadIn[i] : 0;

    for (uint16_t i = 1; i < idx; i++) c = crc8_update(c, buf[i]);
    buf[idx++] = c;

    HAL_StatusTypeDef s = HC06_TxEnqueue(buf, idx);
    HC06_TxKick();
    return s;
}

/* =========================
   Init
   ========================= */
void HC06_Init(void)
{
    s_mode = HC06_MODE_AT;
    HC06_TxReset();
    g_hc06_frame_ready = 0;
    HC06_ForceRearmRx();
}

void HC06_AT_BootSetup(void)
{
#if HC06_DO_AT_SETUP_AT_BOOT
    s_mode = HC06_MODE_AT;
    AT_Clear();

    AT_SendRaw("AT\r\n");
    AT_Wait(HC06_AT_WAIT_MS_AT);

    AT_Clear();
    {
        char cmd[64];
        snprintf(cmd, sizeof(cmd), "NAME%s", HC06_NAME);
        AT_SendCmd(cmd);
    }
    AT_Wait(HC06_AT_WAIT_MS_NAME);

#if HC06_TRY_SET_PIN
    AT_Clear();
    {
        char cmd[32];
        snprintf(cmd, sizeof(cmd), "PIN%s", HC06_PIN_STR);
        AT_SendCmd(cmd);
    }
    AT_Wait(HC06_AT_WAIT_MS_PIN);
#endif

    s_mode = HC06_MODE_BIN;
#else
    s_mode = HC06_MODE_BIN;
#endif
    HC06_ForceRearmRx();
}

/* =========================
   RX callback (1 byte at a time)
   ========================= */
void HC06_UART_RxByteCallback(UART_HandleTypeDef *huart)
{
    if (huart->Instance != huart3.Instance) return;

    uint8_t x = rx_byte;

    if (s_mode == HC06_MODE_AT)
    {
        char c = (char)x;
        if (c == '\n' || s_at_len >= (sizeof(s_at_buf) - 1)) {
            s_at_buf[s_at_len] = 0;
            s_at_done = 1;
        } else if (c != '\r') {
            s_at_buf[s_at_len++] = c;
        }
        HAL_UART_Receive_IT(&huart3, &rx_byte, 1);
        return;
    }

    switch (st)
    {
        case ST_SYNC:
            if (x == HC06_SYNC) { st = ST_CMD; pi = 0; crc = 0; }
            break;

        case ST_CMD:
            payload[pi++] = x;
            crc = crc8_update(crc, x);
            st  = (x == CMD_DATA_REQUEST) ? ST_REQ_ID : ST_D0;
            break;

        case ST_REQ_ID:
            payload[pi++] = x;
            crc = crc8_update(crc, x);
            st  = ST_REQ_CRC;
            break;

        case ST_REQ_CRC:
            if (x == crc) {
                g_hc06_frame.cmd = payload[0];
                g_hc06_frame.w1  = (uint16_t)payload[1];
                g_hc06_frame.w2  = 0;
                g_hc06_frame.w3  = 0;
                g_hc06_frame_ready = 1;
            }
            st = ST_SYNC;
            break;

        case ST_D0: case ST_D1: case ST_D2: case ST_D3: case ST_D4: case ST_D5:
            payload[pi++] = x;
            crc = crc8_update(crc, x);
            if (pi >= 7) st = ST_CRC;
            else         st = (hc06_rx_state_t)((int)st + 1);
            break;

        case ST_CRC:
            if (x == crc) {
                g_hc06_frame.cmd = payload[0];
                g_hc06_frame.w1  = u16_le(payload[1], payload[2]);
                g_hc06_frame.w2  = u16_le(payload[3], payload[4]);
                g_hc06_frame.w3  = u16_le(payload[5], payload[6]);
                g_hc06_frame_ready = 1;
            }
            st = ST_SYNC;
            break;

        default:
            st = ST_SYNC;
            break;
    }

    HAL_UART_Receive_IT(&huart3, &rx_byte, 1);
}

/* =========================
   Command processing
   ========================= */
static uint8_t reply_cmd(uint8_t cmd) { return (uint8_t)(cmd | 0x80); }

/* --- Data request handlers --- */

static uint8_t HC06_HandleReq_FwVersion(uint8_t *out, uint16_t *outLen)
{
    (void)out;
    *outLen = 0;
    return HC06_STATUS_OK;
}

static uint8_t HC06_HandleReq_Ident(uint8_t *out, uint16_t *outLen)
{
    /*
     * Return the soft_info field from the ECU info captured during
     * connection. If not yet connected, return zeros.
     * Previously this called IdentifyEcu() synchronously — that blocking
     * call is gone. The FSM runs IdentifyEcu as part of the connection
     * sequence, so by the time BT asks for ident the data is ready.
     */
    const ControllerInfo *info = KLine_FSM_GetEcuInfo();
    for (uint16_t i = 0; i < 11u; i++)
        out[i] = (uint8_t)info->soft_info[i];
    *outLen = 11u;
    return HC06_STATUS_OK;
}

static uint8_t HC06_HandleReq_Status(uint8_t *out, uint16_t *outLen)
{
    /*
     * Previously returned connectionAlive + BitBang.
     * Now uses FSM status query — no global state needed.
     */
    out[0]  = KLine_FSM_IsConnected() ? 1u : 0u;
    out[1]  = (KLine_FSM_GetStatus() == KLINE_STATUS_CONNECTING) ? 1u : 0u;
    *outLen = 2u;
    return HC06_STATUS_OK;
}

static int             s_fault_count = 0;
static KLine_FaultCode s_fault_cache[KLINE_MAX_FAULT_CODES];

static uint8_t HC06_HandleReq_Error(uint8_t *out, uint16_t *outLen)
{
    /*
     * Return fault codes captured by the FSM during connection.
     * If a fresh read was requested (BT_ReadDTC), the cache has been
     * updated already before this handler runs.
     */
    for (int i = 0; i < s_fault_count; i++) {
        out[2 * i]     = s_fault_cache[i].dtc;
        out[2 * i + 1] = s_fault_cache[i].status;
    }
    *outLen = (uint16_t)(2 * s_fault_count);
    return HC06_STATUS_OK;
}

static uint8_t HC06_HandleReq_Config(uint8_t *out, uint16_t *outLen)
{
    (void)out; *outLen = 0;
    return HC06_STATUS_NOT_IMPLEMENTED;
}

static uint8_t HC06_HandleReq_Reserved(uint8_t *out, uint16_t *outLen)
{
    (void)out; *outLen = 0;
    return HC06_STATUS_NOT_IMPLEMENTED;
}

static uint8_t HC06_DispatchDataRequest(uint8_t reqId, uint8_t *out, uint16_t *outLen)
{
    switch (reqId) {
        case REQ_FW_VERSION: return HC06_HandleReq_FwVersion(out, outLen);
        case REQ_IDENT:      return HC06_HandleReq_Ident(out, outLen);
        case REQ_STATUS:     return HC06_HandleReq_Status(out, outLen);
        case REQ_CONFIG:     return HC06_HandleReq_Config(out, outLen);
        case REQ_ERROR:      return HC06_HandleReq_Error(out, outLen);
        case REQ_RESERVED:
        default:             return HC06_HandleReq_Reserved(out, outLen);
    }
}

void HC06_Process(void)
{
    HC06_TxKick();

    if (!g_hc06_frame_ready) return;
    g_hc06_frame_ready = 0;

    if (commandPending) return;
    commandPending = 1;

    const uint8_t  cmd = g_hc06_frame.cmd;

    switch (cmd)
    {
        case CMD_INIT_COMM:
            BT_InitPSG5Comm();
            break;

        case CMD_READ_DFI:
            BT_ReadDfi();
            break;

        case CMD_WRITE_DFI:
            BT_WriteDfi();
            break;

        case CMD_DATA_REQUEST:
        {
            uint8_t  reqId = (uint8_t)(g_hc06_frame.w1 & 0xFF);
            uint8_t  out[HC06_MAX_PAYLOAD];
            uint16_t outLen = 0;
            uint8_t  stc = HC06_DispatchDataRequest(reqId, out, &outLen);
            (void)HC06_SendDataReply(stc, out, outLen);
            break;
        }

        case CMD_READ_DTC:
            BT_ReadDTC();
            break;

        case CMD_ERASE_DTC:
            BT_ClearDTC();
            break;

        case CMD_READ_AUDI_PIN:
            BT_ReadAudiPin();
            break;

        case CMD_WRITE_AUDI_PIN:
            BT_ReadAudiPin();   /* placeholder — WriteAudiPin not yet implemented */
            break;

        default:
            HC06_SendAsciiLn("ERR Unknown CMD");
            HC06_SendFrame(0xFF, 0, 0, (uint16_t)cmd);
            break;
    }

    commandPending = 0;
}

/* =========================
   Force RX re-arm (UART3)
   ========================= */
static void HC06_ForceRearmRx(void)
{
    HAL_UART_AbortReceive_IT(&huart3);
    HAL_UART_AbortReceive(&huart3);
    HAL_UART_AbortTransmit(&huart3);
    HC06_TxReset();

    __HAL_UART_CLEAR_OREFLAG(&huart3);
    __HAL_UART_CLEAR_NEFLAG(&huart3);
    __HAL_UART_CLEAR_FEFLAG(&huart3);
    __HAL_UART_CLEAR_PEFLAG(&huart3);

    __HAL_UART_ENABLE_IT(&huart3, UART_IT_RXNE);
    __HAL_UART_ENABLE_IT(&huart3, UART_IT_ERR);

    NVIC_ClearPendingIRQ(USART2_IRQn);

    st  = ST_SYNC;
    pi  = 0;
    crc = 0;
    AT_Clear();

    HAL_UART_Receive_IT(&huart3, &rx_byte, 1);
}

/* =========================
   Public KLine event callbacks
   (called from kline_fsm.c via weak symbol override in main.c)
   ========================= */

void BT_KLINE_ERROR(void)
{
    HC06_SendFrame(reply_cmd(CMD_INIT_COMM), CONN_HEAVY, 0, 0);
}

void BT_OnPSG5CommEstablished(void)
{
    /*
     * Previously called KeepAlive() here (blocking). The FSM has already
     * verified the session is alive before firing KLine_OnConnected(), so
     * we just send the success frame directly.
     */
    HC06_SendFrame(reply_cmd(CMD_INIT_COMM), HC06_STATUS_OK, 0, 0);
}

void BT_SendCommStatus(void)
{
    /*
     * ReadVoltage() is still a synchronous KWP call (not yet ported to FSM).
     * It is safe to call here only because this is invoked from
     * KLine_OnKeepAliveTick() which fires from inside KLine_FSM_Run(),
     * meaning the session is known-good at this instant.
     * TODO: port to an async FSM command when ReadVoltage is migrated.
     */
    uint16_t volt = 0;
    if (KLine_FSM_IsConnected()) {
        uint8_t ok = ReadVoltage(&volt);
        HC06_SendFrame(reply_cmd(CMD_TOOL_COMM_STATUS),
                       ok ? HC06_STATUS_OK : (uint16_t)KLINE_ERROR,
                       0,
                       volt);
    } else {
        HC06_SendFrame(reply_cmd(CMD_TOOL_COMM_STATUS), CONN_DEAD, 0, 0);
    }
}

/* =========================
   BT command implementations
   ========================= */

static void BT_InitPSG5Comm(void)
{
    if (KLine_FSM_IsConnected()) {
        /* Already connected — tell the app */
        HC06_SendFrame(reply_cmd(CMD_INIT_COMM), CONN_ALIVE, 0, 0);
    } else if (KLine_FSM_GetStatus() == KLINE_STATUS_CONNECTING) {
        /* Already trying — ignore duplicate request */
    } else {
        /* Kick off the FSM connection sequence */
        KLine_FSM_RequestConnect(ECU_INIT_ADDRESS);
        /* Response will come via KLine_OnConnected() / KLine_OnDisconnected() */
    }
}

static void BT_ReadDfi(void)
{
    if (KLine_FSM_IsConnected()) {
        float   dfi = ReadDfi();
        int16_t s   = HC06_DfiFloatToS16(dfi);
        HC06_SendFrame(reply_cmd(CMD_READ_DFI), HC06_STATUS_OK, 0, (uint16_t)s);
    } else {
        HC06_SendFrame(reply_cmd(CMD_READ_DFI), CONN_DEAD, 0, 0);
    }
}

static void BT_WriteDfi(void)
{
    if (KLine_FSM_IsConnected()) {
        int16_t s   = (int16_t)g_hc06_frame.w3;
        float   dfi = HC06_DfiS16ToFloat(s);
        int     res = WriteDfi(dfi, 0);
        HC06_SendFrame(reply_cmd(CMD_WRITE_DFI),
                       res ? HC06_STATUS_OK : (uint16_t)KLINE_ERROR,
                       0, 0);
    } else {
        HC06_SendFrame(reply_cmd(CMD_WRITE_DFI), CONN_DEAD, 0, 0);
    }
}

static void BT_ReadDTC(void)
{
    if (KLine_FSM_IsConnected()) {
        /*
         * Snapshot the FSM's fault code cache (populated during connection).
         * ReadFaultCodes() is a blocking call — preserved here for now,
         * to be migrated to an async FSM command in a future step.
         */
        const KLine_FaultCode *fc = KLine_FSM_GetFaultCodes(&s_fault_count);
        if (fc && s_fault_count > 0)
            memcpy(s_fault_cache, fc,
                   (size_t)s_fault_count * sizeof(KLine_FaultCode));
        int ok = (s_fault_count >= 0) ? 1 : 0;
        HC06_SendFrame(reply_cmd(CMD_READ_DTC),
                       ok ? HC06_STATUS_OK : (uint16_t)KLINE_ERROR,
                       0,
                       (uint16_t)s_fault_count);
    } else {
        HC06_SendFrame(reply_cmd(CMD_READ_DTC), CONN_DEAD, 0, 0);
    }
}

static void BT_ClearDTC(void)
{
    if (KLine_FSM_IsConnected()) {
        int res = ClearFaultCodes();   /* still blocking — to be ported */
        HC06_SendFrame(reply_cmd(CMD_ERASE_DTC),
                       res ? HC06_STATUS_OK : (uint16_t)KLINE_ERROR,
                       0, 0);
    } else {
        HC06_SendFrame(reply_cmd(CMD_ERASE_DTC), CONN_DEAD, 0, 0);
    }
}

static void BT_ReadAudiPin(void)
{
    uint16_t pin = 0;
    if (KLine_FSM_IsConnected()) {
        uint8_t ok = ReadAudiPin(&pin);
        HC06_SendFrame(reply_cmd(CMD_READ_AUDI_PIN),
                       ok ? HC06_STATUS_OK : (uint16_t)KLINE_ERROR,
                       0,
                       pin);
    } else {
        HC06_SendFrame(reply_cmd(CMD_READ_AUDI_PIN), CONN_DEAD, 0, 0);
    }
}
