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

extern UART_HandleTypeDef huart2;

/* =========================
   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];

    // wait until due timestamp
    if ((int32_t)(HAL_GetTick() - it->due_ms) < 0) return;

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

// Call this from HAL_UART_TxCpltCallback() for USART2
void HC06_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
    if (!huart) return;
    if (huart->Instance != huart2.Instance) return;
    if (!s_tx_busy) return;

    // pop the item that just finished
    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;

    // send next (if already due)
    HC06_TxKick();
}

/* =========================
   Your project functions
   ========================= */
extern void  InitPSG5Comm(void);
extern float ReadDfi(void);
extern int  WriteDfi(float dfi, int version);
extern int  ClearFaultCodes(void);
extern uint8_t ReadAudiPin(uint16_t* pin);
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 - dFi Tool v1"

// Many HC-06 firmwares do NOT support PIN change; leave disabled unless proven.
#define HC06_TRY_SET_PIN           0
#define HC06_PIN_STR              "1234"

// AT command send style: you already confirmed AT\r\n works.
#define HC06_AT_USE_CRLF           1

// AT wait timeouts (ms)
#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;

/* 1-byte RX for everything */
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) {
        // idle
    }
}

/* =========================
   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]; // [0]=cmd, [1..6]=data bytes
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);
}

/* CRC-8 poly 0x07, init 0x00, over [cmd + 6 data bytes] */
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(&huart2, (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]; // sync + cmd + 6 data + crc
    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;

    // Non-blocking + delayed TX
    HAL_StatusTypeDef st = HC06_TxEnqueue(buf, (uint16_t)sizeof(buf));
    HC06_TxKick();
    return st;
}

/* =========================
   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);
}

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

/* Call once at boot (optional). Module must be NOT connected (LED blinking). */
void HC06_AT_BootSetup(void)
{
#if HC06_DO_AT_SETUP_AT_BOOT
    // Ensure we're in AT mode and buffer is clean
    s_mode = HC06_MODE_AT;
    AT_Clear();

    // 1) Basic AT check
#if HC06_AT_USE_CRLF
    AT_SendRaw("AT\r\n");
#else
    AT_SendRaw("AT");
#endif
    AT_Wait(HC06_AT_WAIT_MS_AT);
    // Expect "OK" in s_at_buf (you saw OKsetname for name)

    // 2) Set name
    AT_Clear();
    {
        char cmd[64];
        snprintf(cmd, sizeof(cmd), "NAME%s", HC06_NAME);
        AT_SendCmd(cmd);
    }
    AT_Wait(HC06_AT_WAIT_MS_NAME);
    // Expect "OKsetname"

#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

    // Switch to BIN mode after setup
    s_mode = HC06_MODE_BIN;
#else
    // Directly start in binary mode
    s_mode = HC06_MODE_BIN;
#endif
    HC06_ForceRearmRx();
}

// Variable-length reply for CMD 0x84: SYNC, CMD, STATUS, LEN(u16 LE), PAYLOAD, CRC
// CRC is over [CMD, STATUS, LENlo, LENhi, PAYLOAD]
#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;

    // Non-blocking + delayed TX
    HAL_StatusTypeDef st = HC06_TxEnqueue(buf, idx);
    HC06_TxKick();
    return st;
}

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

    uint8_t x = rx_byte;

    if (s_mode == HC06_MODE_AT)
    {
        char c = (char)x;

        // Capture until newline or full
        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;
        }

        // continue RX
        HAL_UART_Receive_IT(&huart2, &rx_byte, 1);
        return;
    }

    // Binary frame parsing (robust resync + CRC)
    switch (st)
    {
        case ST_SYNC:
            if (x == HC06_SYNC) {
                st = ST_CMD;
                pi = 0;
                crc = 0;
            }
            break;

        case ST_CMD:
            payload[pi++] = x;      // cmd
            crc = crc8_update(crc, x);
            if (x == CMD_DATA_REQUEST) {
                // short request: SYNC, CMD(0x04), REQ_ID, CRC
                st = ST_REQ_ID;
            } else {
                st = ST_D0;
            }
            break;

        case ST_REQ_ID:
            payload[pi++] = x;      // req_id
            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]; // REQ_ID in low byte
                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;      // data bytes
            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; // resync always
            break;

        default:
            st = ST_SYNC;
            break;
    }

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

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

// -------------------------------
// Data Request placeholder handlers
// -------------------------------
static uint8_t HC06_HandleReq_FwVersion(uint8_t *out, uint16_t *outLen)  { (void)out; *outLen = 0; return HC06_STATUS_OK; }

// These must exist somewhere in your project (or change to match your actual symbols)
extern char identStr[11];
extern uint8_t connectionAlive;
//extern uint8_t BitBang;

static uint8_t HC06_HandleReq_Ident(uint8_t *out, uint16_t *outLen)
{
    // Send exactly 11 bytes (no null terminator expected/required)
	uint8_t empty = 1;
	for (uint16_t i = 0; i < 11; i++) {
		if((uint8_t)identStr[i]){
			empty = 0;
			break;
		}
	}
	if(empty){
		IdentifyEcu();
	}
    for (uint16_t i = 0; i < 11; i++) {
        out[i] = (uint8_t)identStr[i];
    }
    *outLen = 11;
    return HC06_STATUS_OK;
}

static uint8_t HC06_HandleReq_Status(uint8_t *out, uint16_t *outLen)
{
    out[0] = connectionAlive;
    out[1] = BitBang;
    *outLen = 2;
    return HC06_STATUS_OK;
}

int N_fc = 0;
FaultCode fault_codes[16];

static uint8_t HC06_HandleReq_Error(uint8_t *out, uint16_t *outLen)
{
    for (uint16_t i = 0; i < N_fc; i++) {
        uint8_t ind = 2*i;
        out[ind] = fault_codes[i].dtc;
        out[ind+1] = fault_codes[i].status;
    }
    *outLen = 2*N_fc;
    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)
{
    // Always allow delayed TX queue to progress
    HC06_TxKick();

    if (!g_hc06_frame_ready) return;
    g_hc06_frame_ready = 0;

    const uint8_t  cmd = g_hc06_frame.cmd;
    const uint16_t w3  = g_hc06_frame.w3;

    if(commandPending){return;}

    commandPending = 1;
    switch (cmd)
    {
        case CMD_INIT_COMM:
        {
            BT_InitPSG5Comm();
        } break;

        case CMD_READ_DFI:
        {
            //first should check if connection is alive
            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();
		} break;

        default:
        {
            HC06_SendAsciiLn("ERR Unknown CMD");
            // Return error frame with original cmd in w3
            HC06_SendFrame(0xFF, 0, 0, (uint16_t)cmd);
        } break;
    }
    commandPending = 0;
}

static void HC06_ForceRearmRx(void)
{
    // Stop anything currently running on UART2
    HAL_UART_AbortReceive_IT(&huart2);
    HAL_UART_AbortReceive(&huart2);
    HAL_UART_AbortTransmit(&huart2);

    // Reset queued TX as well (avoid sending stale frames after re-arm)
    HC06_TxReset();

    // Clear UART error flags properly
    __HAL_UART_CLEAR_OREFLAG(&huart2);
    __HAL_UART_CLEAR_NEFLAG(&huart2);
    __HAL_UART_CLEAR_FEFLAG(&huart2);
    __HAL_UART_CLEAR_PEFLAG(&huart2);

    // Ensure peripheral interrupt enable bits are set
    __HAL_UART_ENABLE_IT(&huart2, UART_IT_RXNE);
    __HAL_UART_ENABLE_IT(&huart2, UART_IT_ERR);

    // Also clear any pending interrupt in NVIC (rare but helps)
    NVIC_ClearPendingIRQ(USART2_IRQn);

    // Reset parser/AT state so it doesn't wait on stale state
    st = ST_SYNC;
    pi = 0;
    crc = 0;
    AT_Clear();

    // Rearm 1-byte RX no matter what mode
    HAL_StatusTypeDef rst = HAL_UART_Receive_IT(&huart2, &rx_byte, 1);
    (void)rst;
    crc = 0;
}


/*
 *
 * API
 *
 */

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

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

void BT_InitPSG5Comm(){
    if(!connectionAlive){
        BitBang = 1;
    }else{
        HC06_SendFrame(reply_cmd(CMD_INIT_COMM), CONN_ALIVE, 0, 0);
    }
}

void BT_WriteDfi(){
    if(connectionAlive){
        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 * KLINE_ERROR, 0, 0);
    }else{
        HC06_SendFrame(reply_cmd(CMD_WRITE_DFI), CONN_DEAD, 0, 0);
    }
}

void BT_OnPSG5CommEstablished(){
    if(KeepAlive()){
        HC06_SendFrame(reply_cmd(CMD_INIT_COMM), 0, 0, 0);
    }else{
        HC06_SendFrame(reply_cmd(CMD_INIT_COMM), CONN_HEAVY, 0, 0);
    }
}

void BT_ReadDTC(){
    if(connectionAlive){
        N_fc = ReadFaultCodes(fault_codes, 16);
        int res = (N_fc == -1) ? 0 : 1;
        HC06_SendFrame(reply_cmd(CMD_READ_DTC), !res * KLINE_ERROR, 0, (uint16_t)N_fc);
    }
    else{
        HC06_SendFrame(reply_cmd(CMD_READ_DTC), CONN_DEAD, 0, 0);
    }
}

void BT_ClearDTC(){
    if(connectionAlive){
        int res = ClearFaultCodes();
        HC06_SendFrame(reply_cmd(CMD_ERASE_DTC), !res * KLINE_ERROR, 0, 0);
    }
    else{
        HC06_SendFrame(reply_cmd(CMD_ERASE_DTC), CONN_DEAD, 0, 0);
    }
}
void BT_ReadAudiPin(){
	uint16_t pin = 0;
    if(connectionAlive){
        HC06_SendFrame(reply_cmd(CMD_READ_AUDI_PIN), !ReadAudiPin(&pin) * KLINE_ERROR, 0, (uint16_t)pin);
    }
    else{
        HC06_SendFrame(reply_cmd(CMD_READ_AUDI_PIN), CONN_DEAD, 0, 0);
    }
}
void BT_WriteAudiPin(){
    /*if(connectionAlive){
        uint16_t pin = (uint16_t)g_hc06_frame.w3;
        HC06_SendFrame(reply_cmd(CMD_READ_AUDI_PIN), !WriteAudiPin(pin) * KLINE_ERROR, 0, 0);
    }
    else{
        HC06_SendFrame(reply_cmd(CMD_READ_AUDI_PIN), CONN_DEAD, 0, 0);
    }*/
}

void BT_SendCommStatus(){
	uint16_t volt = 0;
    if(connectionAlive){
        HC06_SendFrame(reply_cmd(CMD_TOOL_COMM_STATUS), !ReadVoltage(&volt) * KLINE_ERROR, 0, (uint16_t)volt);
    }
    else{
        HC06_SendFrame(reply_cmd(CMD_TOOL_COMM_STATUS), CONN_DEAD, 0, 0);
    }
}
