/*
 * debug_protocol.c
 *
 *  Created on: Mar 5, 2026
 *      Author: janik
 */

#include "debug_protocol.h"
#include "main.h"
#include "cc_controller.h"
#include "mppt.h"
#include <string.h>
#include <stdio.h>

/* ---- External variables from main.c ---- */
extern volatile float vin, iin, vout, iout, vfly, etemp;
extern volatile int16_t last_tmp;
extern volatile int16_t vfly_correction;
extern uint16_t VREF;
extern float vfly_integral;
extern volatile float vfly_avg_debug;
extern float vfly_kp, vfly_ki;
extern uint16_t vfly_clamp;
extern uint16_t vfly_loop_counter_trigger;
extern volatile uint8_t vfly_active;

extern CCController cc;
extern volatile float cc_target;
extern volatile int cc_active;
extern float cc_gain;
extern float CC_MIN_STEP, CC_MAX_STEP;
extern uint16_t cc_loop_counter_trigger;

extern MPPTController mppt;
extern volatile int mppt_active;
extern uint16_t mppt_loop_counter_trigger;
extern float mppt_initial_iref;
extern float mppt_step;
extern float mppt_iref_min, mppt_iref_max;
extern float mppt_dv_threshold;
extern float mppt_deadband;

extern float vin_min_ctrl;

extern uint8_t dt_values[];

/* Raw ADC DMA buffers */
extern uint16_t DMA1BUF1;       /* adc4: vbat */
extern uint16_t DMA1BUF2[3];    /* adc1: etemp, vin, iin */
extern uint16_t DMA1BUF3[2];    /* adc2: vfly, iout */
extern uint16_t DMA1BUF4;       /* adc5: itemp */

extern COMP_HandleTypeDef hcomp1, hcomp3, hcomp4;
extern FMAC_HandleTypeDef hfmac;

/* ---- CRC8 table (poly 0x07) ---- */
static const uint8_t crc8_table[256] = {
    0x00,0x07,0x0E,0x09,0x1C,0x1B,0x12,0x15,0x38,0x3F,0x36,0x31,0x24,0x23,0x2A,0x2D,
    0x70,0x77,0x7E,0x79,0x6C,0x6B,0x62,0x65,0x48,0x4F,0x46,0x41,0x54,0x53,0x5A,0x5D,
    0xE0,0xE7,0xEE,0xE9,0xFC,0xFB,0xF2,0xF5,0xD8,0xDF,0xD6,0xD1,0xC4,0xC3,0xCA,0xCD,
    0x90,0x97,0x9E,0x99,0x8C,0x8B,0x82,0x85,0xA8,0xAF,0xA6,0xA1,0xB4,0xB3,0xBA,0xBD,
    0xC7,0xC0,0xC9,0xCE,0xDB,0xDC,0xD5,0xD2,0xFF,0xF8,0xF1,0xF6,0xE3,0xE4,0xED,0xEA,
    0xB7,0xB0,0xB9,0xBE,0xAB,0xAC,0xA5,0xA2,0x8F,0x88,0x81,0x86,0x93,0x94,0x9D,0x9A,
    0x27,0x20,0x29,0x2E,0x3B,0x3C,0x35,0x32,0x1F,0x18,0x11,0x16,0x03,0x04,0x0D,0x0A,
    0x57,0x50,0x59,0x5E,0x4B,0x4C,0x45,0x42,0x6F,0x68,0x61,0x66,0x73,0x74,0x7D,0x7A,
    0x89,0x8E,0x87,0x80,0x95,0x92,0x9B,0x9C,0xB1,0xB6,0xBF,0xB8,0xAD,0xAA,0xA3,0xA4,
    0xF9,0xFE,0xF7,0xF0,0xE5,0xE2,0xEB,0xEC,0xC1,0xC6,0xCF,0xC8,0xDD,0xDA,0xD3,0xD4,
    0x69,0x6E,0x67,0x60,0x75,0x72,0x7B,0x7C,0x51,0x56,0x5F,0x58,0x4D,0x4A,0x43,0x44,
    0x19,0x1E,0x17,0x10,0x05,0x02,0x0B,0x0C,0x21,0x26,0x2F,0x28,0x3D,0x3A,0x33,0x34,
    0x4E,0x49,0x40,0x47,0x52,0x55,0x5C,0x5B,0x76,0x71,0x78,0x7F,0x6A,0x6D,0x64,0x63,
    0x3E,0x39,0x30,0x37,0x22,0x25,0x2C,0x2B,0x06,0x01,0x08,0x0F,0x1A,0x1D,0x14,0x13,
    0xAE,0xA9,0xA0,0xA7,0xB2,0xB5,0xBC,0xBB,0x96,0x91,0x98,0x9F,0x8A,0x8D,0x84,0x83,
    0xDE,0xD9,0xD0,0xD7,0xC2,0xC5,0xCC,0xCB,0xE6,0xE1,0xE8,0xEF,0xFA,0xFD,0xF4,0xF3,
};

static uint8_t crc8(const uint8_t *data, uint16_t len)
{
    uint8_t crc = 0x00;
    for (uint16_t i = 0; i < len; i++)
        crc = crc8_table[crc ^ data[i]];
    return crc;
}

/* ---- Parameter table ---- */
#define PARAM_COUNT 27

static ParamEntry param_table[PARAM_COUNT];

static void param_table_init(void)
{
    int i = 0;
    /* Vfly params */
    param_table[i++] = (ParamEntry){PID_VFLY_KP,                   PTYPE_FLOAT,  &vfly_kp,                   -10.0f,   10.0f};
    param_table[i++] = (ParamEntry){PID_VFLY_KI,                   PTYPE_FLOAT,  &vfly_ki,                   -10.0f,   10.0f};
    param_table[i++] = (ParamEntry){PID_VFLY_CLAMP,                PTYPE_UINT16, &vfly_clamp,                0,        10000};
    param_table[i++] = (ParamEntry){PID_VFLY_LOOP_COUNTER_TRIGGER, PTYPE_UINT16, &vfly_loop_counter_trigger, 1,        10000};
    param_table[i++] = (ParamEntry){PID_VFLY_ACTIVE,               PTYPE_UINT8,  (void*)&vfly_active,        0,        1};

    /* CC params */
    param_table[i++] = (ParamEntry){PID_CC_TARGET,                 PTYPE_FLOAT,  (void*)&cc_target,          0,        60000};
    param_table[i++] = (ParamEntry){PID_CC_GAIN,                   PTYPE_FLOAT,  &cc_gain,                   -1.0f,    1.0f};
    param_table[i++] = (ParamEntry){PID_CC_MIN_STEP,               PTYPE_FLOAT,  &CC_MIN_STEP,               -1000,    0};
    param_table[i++] = (ParamEntry){PID_CC_MAX_STEP,               PTYPE_FLOAT,  &CC_MAX_STEP,               0,        1000};
    param_table[i++] = (ParamEntry){PID_CC_LOOP_COUNTER_TRIGGER,   PTYPE_UINT16, &cc_loop_counter_trigger,   1,        10000};
    param_table[i++] = (ParamEntry){PID_CC_ACTIVE,                 PTYPE_INT32,  (void*)&cc_active,          0,        1};
    param_table[i++] = (ParamEntry){PID_VREF,                      PTYPE_UINT16, &VREF,                      3100,     3700};

    /* MPPT params */
    param_table[i++] = (ParamEntry){PID_MPPT_STEP,                 PTYPE_FLOAT,  &mppt_step,                 0,        10000};
    param_table[i++] = (ParamEntry){PID_MPPT_IREF_MIN,             PTYPE_FLOAT,  &mppt_iref_min,             0,        60000};
    param_table[i++] = (ParamEntry){PID_MPPT_IREF_MAX,             PTYPE_FLOAT,  &mppt_iref_max,             0,        60000};
    param_table[i++] = (ParamEntry){PID_MPPT_DV_THRESHOLD,         PTYPE_FLOAT,  &mppt_dv_threshold,         0,        10000};
    param_table[i++] = (ParamEntry){PID_MPPT_LOOP_COUNTER_TRIGGER, PTYPE_UINT16, &mppt_loop_counter_trigger, 1,        10000};
    param_table[i++] = (ParamEntry){PID_MPPT_ACTIVE,               PTYPE_INT32,  (void*)&mppt_active,        0,        1};
    param_table[i++] = (ParamEntry){PID_MPPT_INITIAL_IREF,         PTYPE_FLOAT,  &mppt_initial_iref,         0,        60000};
    param_table[i++] = (ParamEntry){PID_MPPT_DEADBAND,            PTYPE_FLOAT,  &mppt_deadband,             0,        1.0f};

    /* Global */
    param_table[i++] = (ParamEntry){PID_VIN_MIN_CTRL,              PTYPE_FLOAT,  &vin_min_ctrl,              0,        90000};

    /* Deadtime segments */
    param_table[i++] = (ParamEntry){PID_DT_SEG0, PTYPE_UINT8, &dt_values[0], DT_HARD_MIN, 200};
    param_table[i++] = (ParamEntry){PID_DT_SEG1, PTYPE_UINT8, &dt_values[1], DT_HARD_MIN, 200};
    param_table[i++] = (ParamEntry){PID_DT_SEG2, PTYPE_UINT8, &dt_values[2], DT_HARD_MIN, 200};
    param_table[i++] = (ParamEntry){PID_DT_SEG3, PTYPE_UINT8, &dt_values[3], DT_HARD_MIN, 200};
    param_table[i++] = (ParamEntry){PID_DT_SEG4, PTYPE_UINT8, &dt_values[4], DT_HARD_MIN, 200};
    param_table[i++] = (ParamEntry){PID_DT_SEG5, PTYPE_UINT8, &dt_values[5], DT_HARD_MIN, 200};
}

/* ---- Protocol context ---- */
ProtoCtx proto;

/* ---- Internal helpers ---- */

static void start_rx(void)
{
    HAL_UART_Receive_IT(proto.huart, &proto.rx_byte, 1);
}

static void uart_recover(void)
{
    /* Clear error flags and reset UART state so TX/RX work again */
    __HAL_UART_CLEAR_FLAG(proto.huart, UART_CLEAR_OREF | UART_CLEAR_NEF | UART_CLEAR_PEF | UART_CLEAR_FEF);
    proto.huart->gState = HAL_UART_STATE_READY;
    proto.huart->RxState = HAL_UART_STATE_READY;
    proto.huart->ErrorCode = HAL_UART_ERROR_NONE;
    /* Re-arm RX */
    start_rx();
}

static void send_frame(uint8_t cmd, const uint8_t *payload, uint8_t len)
{
    /* Pick the inactive buffer */
    uint8_t buf_idx = proto.tx_active ^ 1;
    uint8_t *buf = proto.tx_buf[buf_idx];
    uint16_t total = PROTO_HEADER_SIZE + len + 1;

    buf[0] = PROTO_SYNC_BYTE;
    buf[1] = cmd;
    buf[2] = len;
    if (len > 0)
        memcpy(&buf[3], payload, len);
    buf[3 + len] = crc8(buf, 3 + len);

    proto.tx_active = buf_idx;
    if (HAL_UART_Transmit(proto.huart, buf, total, 5) != HAL_OK)
    {
        proto.uart_errors++;
        uart_recover();
    }
}

static ParamEntry* find_param(uint8_t id)
{
    for (int i = 0; i < PARAM_COUNT; i++)
    {
        if (param_table[i].id == id)
            return &param_table[i];
    }
    return NULL;
}

static void apply_param_write(const ParamWritePayload *pw)
{
    ParamEntry *p = find_param(pw->param_id);
    if (!p) return;

    /* Extract value and clamp */
    float fval;
    switch (p->type)
    {
    case PTYPE_FLOAT:
        fval = pw->value.f;
        if (fval < p->min_val) fval = p->min_val;
        if (fval > p->max_val) fval = p->max_val;
        *(float*)p->ptr = fval;
        break;
    case PTYPE_UINT16:
        fval = (float)pw->value.u16;
        if (fval < p->min_val) fval = p->min_val;
        if (fval > p->max_val) fval = p->max_val;
        *(uint16_t*)p->ptr = (uint16_t)fval;
        break;
    case PTYPE_UINT8:
        fval = (float)pw->value.u8;
        if (fval < p->min_val) fval = p->min_val;
        if (fval > p->max_val) fval = p->max_val;
        *(uint8_t*)p->ptr = (uint8_t)fval;
        break;
    case PTYPE_INT32:
        fval = (float)pw->value.i32;
        if (fval < p->min_val) fval = p->min_val;
        if (fval > p->max_val) fval = p->max_val;
        *(int32_t*)p->ptr = (int32_t)fval;
        break;
    }

    /* Side effects: VREF update -> ADC3 offset + CC output */
    if (pw->param_id == PID_VREF)
    {
        ADC3->OFR1 = (ADC3->OFR1 & ~0xFFFU) | (VREF & 0xFFF);
        cc.output_f = (float)VREF;
    }
    /* Side effects for CC params: sync struct fields */
    else if (pw->param_id == PID_CC_GAIN)
        cc.gain = cc_gain;
    else if (pw->param_id == PID_CC_MIN_STEP)
        cc.min_step = CC_MIN_STEP;
    else if (pw->param_id == PID_CC_MAX_STEP)
        cc.max_step = CC_MAX_STEP;
    /* Side effects for MPPT params: sync struct fields */
    else if (pw->param_id == PID_MPPT_STEP)
        mppt.step = mppt_step;
    else if (pw->param_id == PID_MPPT_IREF_MIN)
        mppt.iref_min = mppt_iref_min;
    else if (pw->param_id == PID_MPPT_IREF_MAX)
        mppt.iref_max = mppt_iref_max;
    else if (pw->param_id == PID_MPPT_DV_THRESHOLD)
        mppt.dv_min = mppt_dv_threshold;
    else if (pw->param_id == PID_MPPT_DEADBAND)
        mppt.deadband = mppt_deadband;
    else if (pw->param_id == PID_MPPT_ACTIVE && mppt_active)
        cc_active = 1;

    /* Send ACK: echo back the write payload */
    send_frame(CMD_PARAM_WRITE_ACK, (const uint8_t*)pw, sizeof(ParamWritePayload));
}

static void send_param_value(const ParamEntry *p)
{
    ParamWritePayload pv;
    pv.param_id = p->id;
    pv.param_type = p->type;
    pv._pad[0] = 0; pv._pad[1] = 0;
    pv.value.u32 = 0;
    switch (p->type)
    {
    case PTYPE_FLOAT:  pv.value.f   = *(float*)p->ptr;    break;
    case PTYPE_UINT16: pv.value.u16 = *(uint16_t*)p->ptr; break;
    case PTYPE_UINT8:  pv.value.u8  = *(uint8_t*)p->ptr;  break;
    case PTYPE_INT32:  pv.value.i32 = *(int32_t*)p->ptr;  break;
    }
    send_frame(CMD_PARAM_VALUE, (const uint8_t*)&pv, sizeof(ParamWritePayload));
}

static void send_all_params(void)
{
    for (int i = 0; i < PARAM_COUNT; i++)
        send_param_value(&param_table[i]);
}

static void process_rx_frame(uint8_t cmd, const uint8_t *payload, uint8_t len)
{
    switch (cmd)
    {
    case CMD_PARAM_WRITE:
        if (len >= sizeof(ParamWritePayload))
            apply_param_write((const ParamWritePayload*)payload);
        break;
    case CMD_PARAM_READ_ALL:
        send_all_params();
        break;
    case CMD_PING:
        send_frame(CMD_PONG, NULL, 0);
        break;
    default:
        break;
    }
}

/* ---- Public API ---- */

void Proto_Init(UART_HandleTypeDef *huart)
{
    memset(&proto, 0, sizeof(proto));
    proto.huart = huart;
    proto.rx_state = RX_WAIT_SYNC;
    param_table_init();
    start_rx();
}

void Proto_SendTelemetry(void)
{
    TelemetryPayload t;
    t.vin = vin;
    t.vout = vout;
    t.iin = iin;
    t.iout = iout;
    t.vfly = vfly;
    t.etemp = etemp;
    t.last_tmp = last_tmp;
    t.VREF = VREF;
    t.vfly_correction = vfly_correction;
    t._pad0 = 0;
    t.vfly_integral = vfly_integral;
    t.vfly_avg_debug = vfly_avg_debug;
    t.cc_output_f = cc.output_f;
    t.mppt_iref = mppt.iref;
    t.mppt_last_vin = mppt.last_vin;
    t.mppt_last_iin = mppt.last_iin;
    t.p_in = vin * (-iin) / 1e6f;   /* mV * mA → W */
    t.p_out = vout * iout / 1e6f;   /* mV * mA → W */
    t.seq = proto.seq++;
    t._pad1[0] = 0; t._pad1[1] = 0; t._pad1[2] = 0;

    send_frame(CMD_TELEMETRY, (const uint8_t*)&t, sizeof(TelemetryPayload));
}

void Proto_SendError(const char *msg)
{
    uint8_t len = 0;
    while (msg[len] && len < PROTO_MAX_PAYLOAD) len++;
    send_frame(CMD_ERROR_MSG, (const uint8_t*)msg, len);
}

void Proto_SendDiagDump(const char *reason)
{
    char buf[128];

    /* Line 1: reason */
    Proto_SendError(reason);

    /* Line 2: converted analog values (integer-only — no float printf with nano.specs) */
    snprintf(buf, sizeof(buf), "DIAG V:%lu/%lu I:%d/%lu Vfly:%lu T:%d.%u",
             (unsigned long)(vin > 0 ? (uint32_t)vin : 0),
             (unsigned long)(vout > 0 ? (uint32_t)vout : 0),
             (int)iin,
             (unsigned long)(iout > 0 ? (uint32_t)iout : 0),
             (unsigned long)(vfly > 0 ? (uint32_t)vfly : 0),
             (int)etemp, ((unsigned)(etemp > 0 ? etemp : -etemp) % 10));
    Proto_SendError(buf);

    /* Line 3: raw ADC counts */
    snprintf(buf, sizeof(buf), "RAW adc1[%u,%u,%u] adc2[%u,%u] adc4[%u] adc5[%u]",
             DMA1BUF2[0], DMA1BUF2[1], DMA1BUF2[2],
             DMA1BUF3[0], DMA1BUF3[1],
             DMA1BUF1, DMA1BUF4);
    Proto_SendError(buf);

    /* Line 4: controller state */
    snprintf(buf, sizeof(buf), "CTRL VREF:%u last_tmp:%d vfly_corr:%d cc_out:%d mppt_iref:%d",
             VREF, (int)last_tmp, (int)vfly_correction,
             (int)cc.output_f, (int)mppt.iref);
    Proto_SendError(buf);

    /* Line 5: HRTIM fault status + comparator outputs */
    uint32_t hrtim_isr = HRTIM1->sCommonRegs.ISR;
    uint32_t hrtim_oenr = HRTIM1->sCommonRegs.OENR;
    uint32_t comp_out = 0;
    if (HAL_COMP_GetOutputLevel(&hcomp1) == COMP_OUTPUT_LEVEL_HIGH) comp_out |= 1;
    if (HAL_COMP_GetOutputLevel(&hcomp3) == COMP_OUTPUT_LEVEL_HIGH) comp_out |= 2;
    if (HAL_COMP_GetOutputLevel(&hcomp4) == COMP_OUTPUT_LEVEL_HIGH) comp_out |= 4;
    snprintf(buf, sizeof(buf), "HW HRTIM_ISR:%08lX OENR:%08lX COMP:%lu FMAC_SR:%08lX",
             hrtim_isr, hrtim_oenr, comp_out, FMAC->SR);
    Proto_SendError(buf);

    /* Line 6: HRTIM duty + master compare */
    snprintf(buf, sizeof(buf), "HRTIM TE_CMP1:%lu TF_CMP1:%lu MCMP1:%lu",
             HRTIM1->sTimerxRegs[HRTIM_TIMERINDEX_TIMER_E].CMP1xR,
             HRTIM1->sTimerxRegs[HRTIM_TIMERINDEX_TIMER_F].CMP1xR,
             HRTIM1->sMasterRegs.MCMP1R);
    Proto_SendError(buf);

    /* Line 7: active flags + ADC3 offset */
    snprintf(buf, sizeof(buf), "FLAGS cc_act:%d mppt_act:%d vfly_act:%u ADC3_OFR1:%08lX",
             cc_active, mppt_active, (unsigned)vfly_active, ADC3->OFR1);
    Proto_SendError(buf);
}

/* ---- HAL Callbacks ---- */

void Proto_TxCpltCallback(UART_HandleTypeDef *huart)
{
    if (huart == proto.huart)
        proto.tx_busy = 0;
}

void Proto_ErrorCallback(UART_HandleTypeDef *huart)
{
    if (huart != proto.huart)
        return;
    proto.uart_errors++;
    uart_recover();
}

void Proto_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if (huart != proto.huart)
        return;

    uint8_t b = proto.rx_byte;

    switch (proto.rx_state)
    {
    case RX_WAIT_SYNC:
        if (b == PROTO_SYNC_BYTE)
        {
            proto.rx_buf[0] = b;
            proto.rx_state = RX_WAIT_CMD;
        }
        break;
    case RX_WAIT_CMD:
        proto.rx_cmd = b;
        proto.rx_buf[1] = b;
        proto.rx_state = RX_WAIT_LEN;
        break;
    case RX_WAIT_LEN:
        proto.rx_len = b;
        proto.rx_buf[2] = b;
        proto.rx_idx = 0;
        if (b == 0)
            proto.rx_state = RX_WAIT_CRC;
        else if (b > PROTO_MAX_PAYLOAD)
            proto.rx_state = RX_WAIT_SYNC; /* invalid, reset */
        else
            proto.rx_state = RX_WAIT_PAYLOAD;
        break;
    case RX_WAIT_PAYLOAD:
        proto.rx_buf[PROTO_HEADER_SIZE + proto.rx_idx] = b;
        proto.rx_idx++;
        if (proto.rx_idx >= proto.rx_len)
            proto.rx_state = RX_WAIT_CRC;
        break;
    case RX_WAIT_CRC:
    {
        uint8_t expected = crc8(proto.rx_buf, PROTO_HEADER_SIZE + proto.rx_len);
        if (b == expected)
            process_rx_frame(proto.rx_cmd, &proto.rx_buf[PROTO_HEADER_SIZE], proto.rx_len);
        proto.rx_state = RX_WAIT_SYNC;
        break;
    }
    }

    /* Re-arm RX */
    start_rx();
}