/**
 * @file    protocol.c
 * @brief   EDC 通讯协议实现 — CRC16 / 组帧 / 解析 / I2C / UART
 */

#include "protocol.h"
#include <string.h>
#include "esp_log.h"
#include "freertos/task.h"

static const char *TAG = "EDC_PROTO";

/* ================================================================
 * CRC-16/MODBUS
 *
 * 多项式:    0x8005 (逆序 0xA001)
 * 初始值:    0xFFFF
 * 输入/输出: 反射
 * 异或输出:  0x0000
 * 校验范围:  CMD + LEN + DATA
 * ================================================================ */

uint16_t edc_crc16(const uint8_t *data, size_t len)
{
    uint16_t crc = 0xFFFF;
    for (size_t i = 0; i < len; i++) {
        crc ^= data[i];
        for (int j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc = (crc >> 1) ^ 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    return crc;
}

/* ================================================================
 * 大端序 uint16_t 写入 / 读取
 * ================================================================ */

static inline void write_u16_be(uint8_t *buf, uint16_t val)
{
    buf[0] = (uint8_t)(val >> 8);
    buf[1] = (uint8_t)(val);
}

static inline uint16_t read_u16_be(const uint8_t *buf)
{
    return ((uint16_t)buf[0] << 8) | buf[1];
}

/* ================================================================
 * Cycle 组帧
 *
 * DATA 区: cycle_id(2B,BE) + frag_count(1B) + fragments(N×12B)
 * Fragment: src.x(2B) src.y(2B) src.angle(2B) dst.x(2B) dst.y(2B) dst.angle(2B)
 * ================================================================ */

int edc_cycle_pack(const edc_cycle_t *cycle, uint8_t *out)
{
    uint8_t count = cycle->frag_count;
    if (count < EDC_FRAG_MIN) count = EDC_FRAG_MIN;
    if (count > EDC_FRAG_MAX) count = EDC_FRAG_MAX;

    uint8_t data_len = 3 + count * EDC_FRAG_BYTES;  /* id(2) + cnt(1) + frags */

    out[0] = EDC_PROTO_SOF1;
    out[1] = EDC_PROTO_SOF2;
    out[2] = EDC_CMD_CYCLE_DATA;
    out[3] = data_len;

    /* cycle_id (大端) */
    uint8_t *p = &out[4];
    write_u16_be(p, cycle->cycle_id);
    p += 2;

    /* frag_count */
    *p++ = count;

    /* fragments */
    for (int i = 0; i < count; i++) {
        const edc_fragment_t *f = &cycle->fragments[i];
        write_u16_be(p,      f->source.x);         p += 2;
        write_u16_be(p,      f->source.y);         p += 2;
        write_u16_be(p,      f->source.angle_x10); p += 2;
        write_u16_be(p,      f->target.x);         p += 2;
        write_u16_be(p,      f->target.y);         p += 2;
        write_u16_be(p,      f->target.angle_x10); p += 2;
    }

    /* CRC16 over [CMD, LEN, DATA] */
    uint16_t crc = edc_crc16(&out[2], 2 + data_len);
    *p++ = (uint8_t)(crc);
    *p++ = (uint8_t)(crc >> 8);

    return (int)(p - out);  /* 2 + 1 + 1 + data_len + 2 */
}

/* ================================================================
 * Cycle 解析 (逐字节)
 * ================================================================ */

edc_parse_result_t edc_cycle_parse_byte(edc_frame_ctx_t *ctx, uint8_t byte,
                                        edc_cycle_t *cycle)
{
    /* 上一帧解析完成后, 复位准备接收下一帧 */
    if (ctx->state == EDC_PARSE_OK) {
        ctx->idx   = 0;
        ctx->state = EDC_PARSE_WAITING;
    }

    /* ---- 等待 SOF1 ---- */
    if (ctx->idx == 0) {
        if (byte == EDC_PROTO_SOF1) {
            ctx->buf[0] = byte;
            ctx->idx    = 1;
        }
        return EDC_PARSE_WAITING;
    }

    /* ---- 等待 SOF2 ---- */
    if (ctx->idx == 1) {
        if (byte != EDC_PROTO_SOF2) {
            ctx->idx = 0;
            if (byte == EDC_PROTO_SOF1) { ctx->buf[0] = byte; ctx->idx = 1; }
            return EDC_PARSE_WAITING;
        }
        ctx->buf[1] = byte;
        ctx->idx    = 2;
        return EDC_PARSE_WAITING;
    }

    if (ctx->idx < EDC_PROTO_FRAME_MAX) {
        ctx->buf[ctx->idx] = byte;
    }
    ctx->idx++;

    /* 确定帧总长 */
    if (ctx->idx == 4) {
        uint8_t data_len = ctx->buf[3];
        if (data_len < 3 || data_len > EDC_PROTO_FRAME_MAX - 6) {
            ctx->idx = 0; ctx->state = EDC_PARSE_WAITING;
            return EDC_PARSE_LEN_ERR;
        }
        ctx->len = 2 + 1 + 1 + data_len + 2;  /* SOF+CMD+LEN+DATA+CRC */
    }

    /* 收满一帧 (idx>=4 保证 ctx->len 已按 LEN 字段设置) */
    if (ctx->idx >= 4 && ctx->idx >= ctx->len) {
        ctx->idx = ctx->len;

        /* CRC 校验 */
        uint16_t crc_calc = edc_crc16(&ctx->buf[2], 2 + ctx->buf[3]);
        uint16_t crc_recv = ctx->buf[ctx->len - 2] | ((uint16_t)ctx->buf[ctx->len - 1] << 8);
        if (crc_calc != crc_recv) {
            ctx->idx = 0; ctx->state = EDC_PARSE_WAITING;
            return EDC_PARSE_CRC_ERR;
        }

        /* 仅处理 CYCLE_DATA 命令 */
        uint8_t cmd = ctx->buf[2];
        if (cmd != EDC_CMD_CYCLE_DATA) {
            ctx->idx = 0; ctx->state = EDC_PARSE_WAITING;
            return EDC_PARSE_CMD_ERR;
        }

        /* 解析 DATA 区 */
        const uint8_t *d = &ctx->buf[4];
        cycle->cycle_id   = read_u16_be(d);      d += 2;
        cycle->frag_count = *d;                   d += 1;

        if (cycle->frag_count < EDC_FRAG_MIN || cycle->frag_count > EDC_FRAG_MAX) {
            ctx->idx = 0; ctx->state = EDC_PARSE_WAITING;
            return EDC_PARSE_LEN_ERR;
        }

        for (int i = 0; i < cycle->frag_count; i++) {
            edc_fragment_t *f = &cycle->fragments[i];
            f->source.x         = read_u16_be(d); d += 2;
            f->source.y         = read_u16_be(d); d += 2;
            f->source.angle_x10 = read_u16_be(d); d += 2;
            f->target.x         = read_u16_be(d); d += 2;
            f->target.y         = read_u16_be(d); d += 2;
            f->target.angle_x10 = read_u16_be(d); d += 2;
        }
        cycle->timestamp_ms = 0;

        ctx->state = EDC_PARSE_OK;
        return EDC_PARSE_OK;
    }

    return EDC_PARSE_WAITING;
}

/* ================================================================
 * Cycle 解析 (一次性)
 * ================================================================ */

edc_parse_result_t edc_cycle_parse_buf(const uint8_t *data, size_t len,
                                       edc_cycle_t *cycle)
{
    edc_frame_ctx_t ctx = { .state = EDC_PARSE_WAITING };
    edc_parse_result_t res;
    for (size_t i = 0; i < len; i++) {
        res = edc_cycle_parse_byte(&ctx, data[i], cycle);
        if (res != EDC_PARSE_WAITING) return res;
    }
    return EDC_PARSE_WAITING;
}

/* ================================================================
 * 统一收发器
 * ================================================================ */

/* ---- 共享最新 Cycle ---- */
static edc_cycle_t   s_latest_cycle;
static volatile bool s_cycle_updated;
static portMUX_TYPE  s_lock = portMUX_INITIALIZER_UNLOCKED;

/* ---- 统计 ---- */
static edc_proto_stat_t s_stat;

/* ---- 通道使能 ---- */
static bool s_i2c_enabled;
static bool s_uart_enabled;

/* ---- I2C ---- */
static i2c_slave_dev_handle_t s_i2c_handle;
static edc_cycle_t            s_i2c_rx_cycle;
static volatile bool          s_i2c_rx_pending;

/* ---- UART ---- */
static uart_port_t     s_uart_port;
static edc_frame_ctx_t s_uart_ctx;

/* ================================================================
 * 内部: 存入共享 Cycle
 * ================================================================ */

static void store_cycle(const edc_cycle_t *cycle, edc_channel_t ch)
{
    portENTER_CRITICAL(&s_lock);
    memcpy(&s_latest_cycle, cycle, sizeof(edc_cycle_t));
    s_latest_cycle.timestamp_ms = xTaskGetTickCount() * portTICK_PERIOD_MS;
    s_cycle_updated = true;
    portEXIT_CRITICAL(&s_lock);

    if (ch == EDC_CH_I2C)       s_stat.i2c.cycles_ok++;
    else if (ch == EDC_CH_UART) s_stat.uart.cycles_ok++;
    s_stat.last_channel = ch;
    s_stat.last_tick    = xTaskGetTickCount();
}

/* ================================================================
 * I2C 从机 接收
 * ================================================================ */

static bool on_i2c_rx(i2c_slave_dev_handle_t handle,
                      const i2c_slave_rx_done_event_data_t *evt,
                      void *arg)
{
    s_stat.i2c.bytes_total += evt->length;

    edc_cycle_t cycle;
    if (edc_cycle_parse_buf(evt->buffer, evt->length, &cycle) == EDC_PARSE_OK) {
        memcpy(&s_i2c_rx_cycle, &cycle, sizeof(edc_cycle_t));
        s_i2c_rx_cycle.timestamp_ms = xTaskGetTickCountFromISR() * portTICK_PERIOD_MS;
        s_i2c_rx_pending = true;
    }
    return false;
}

static bool i2c_poll_once(void)
{
    if (!s_i2c_rx_pending) return false;

    edc_cycle_t cycle;
    portENTER_CRITICAL(&s_lock);
    memcpy(&cycle, &s_i2c_rx_cycle, sizeof(edc_cycle_t));
    s_i2c_rx_pending = false;
    portEXIT_CRITICAL(&s_lock);

    store_cycle(&cycle, EDC_CH_I2C);
    ESP_LOGD(TAG, "I2C cycle OK (id=%u frags=%u)", cycle.cycle_id, cycle.frag_count);
    return true;
}

/* ================================================================
 * UART 接收
 * ================================================================ */

static bool uart_poll_once(void)
{
    uint8_t byte;
    bool got = false;
    int limit = 64;  /* 每次最多处理 64 字节, 防止饿死 IDLE 任务 */

    while (limit-- > 0 && uart_read_bytes(s_uart_port, &byte, 1, pdMS_TO_TICKS(0)) > 0) {
        s_stat.uart.bytes_total++;

        edc_cycle_t cycle;
        edc_parse_result_t res = edc_cycle_parse_byte(&s_uart_ctx, byte, &cycle);
        if (res == EDC_PARSE_OK) {
            store_cycle(&cycle, EDC_CH_UART);
            got = true;
            ESP_LOGD(TAG, "UART cycle OK (id=%u frags=%u)", cycle.cycle_id, cycle.frag_count);
        } else if (res < 0) {
            s_stat.uart.cycles_err++;
            ESP_LOGW(TAG, "UART parse err: %d", res);
        }
    }
    return got;
}

/* ================================================================
 * 内部: 通过 UART 发送短帧
 * ================================================================ */

static void uart_send_frame(uint8_t cmd, const uint8_t *data, uint8_t data_len)
{
    uint8_t buf[EDC_PROTO_FRAME_MAX];
    buf[0] = EDC_PROTO_SOF1;
    buf[1] = EDC_PROTO_SOF2;
    buf[2] = cmd;
    buf[3] = data_len;
    if (data && data_len) memcpy(&buf[4], data, data_len);

    uint16_t crc = edc_crc16(&buf[2], 2 + data_len);
    buf[4 + data_len]     = (uint8_t)(crc);
    buf[4 + data_len + 1] = (uint8_t)(crc >> 8);

    int total = 4 + data_len + 2;
    uart_write_bytes(s_uart_port, (const char *)buf, total);
}

/* ================================================================
 * 初始化
 * ================================================================ */

void edc_proto_init(const edc_proto_config_t *cfg)
{
    edc_proto_config_t dft = {
        .i2c_enable  = true,
        .i2c_port    = EDC_I2C_PORT_DEFAULT,
        .i2c_addr    = EDC_I2C_SLAVE_ADDR,
        .i2c_sda     = EDC_I2C_SDA_DEFAULT,
        .i2c_scl     = EDC_I2C_SCL_DEFAULT,
        .uart_enable = true,
        .uart_port   = EDC_UART_PORT_DEFAULT,
        .uart_baudrate = EDC_UART_BAUDRATE,
        .uart_rx     = EDC_UART_RX_DEFAULT,
        .uart_tx     = EDC_UART_TX_DEFAULT,
    };
    if (cfg) dft = *cfg;

    s_i2c_enabled  = dft.i2c_enable;
    s_uart_enabled = dft.uart_enable;
    memset(&s_stat, 0, sizeof(s_stat));

    /* I2C */
    if (s_i2c_enabled) {
        i2c_slave_config_t i2c_cfg = {
            .i2c_port          = dft.i2c_port,
            .sda_io_num        = dft.i2c_sda,
            .scl_io_num        = dft.i2c_scl,
            .clk_source        = I2C_CLK_SRC_DEFAULT,
            .send_buf_depth    = 8,    /* 最少需要非零值 */
            .receive_buf_depth = EDC_I2C_BUF_SIZE,
            .slave_addr        = dft.i2c_addr,
            .addr_bit_len      = I2C_ADDR_BIT_LEN_7,
            .intr_priority     = 0,
            .flags.enable_internal_pullup = true,
        };
        ESP_ERROR_CHECK(i2c_new_slave_device(&i2c_cfg, &s_i2c_handle));

        i2c_slave_event_callbacks_t cbs = {
            .on_receive = on_i2c_rx,
            .on_request = NULL,
        };
        ESP_ERROR_CHECK(i2c_slave_register_event_callbacks(s_i2c_handle, &cbs, NULL));
        ESP_LOGI(TAG, "I2C slave OK (0x%02X SDA=%d SCL=%d)",
                 dft.i2c_addr, dft.i2c_sda, dft.i2c_scl);
    }

    /* UART */
    if (s_uart_enabled) {
        s_uart_port = dft.uart_port;
        memset(&s_uart_ctx, 0, sizeof(s_uart_ctx));
        s_uart_ctx.state = EDC_PARSE_WAITING;

        uart_config_t uart_cfg = {
            .baud_rate  = dft.uart_baudrate,
            .data_bits  = UART_DATA_8_BITS,
            .parity     = UART_PARITY_DISABLE,
            .stop_bits  = UART_STOP_BITS_1,
            .flow_ctrl  = UART_HW_FLOWCTRL_DISABLE,
            .source_clk = UART_SCLK_DEFAULT,
        };
        ESP_ERROR_CHECK(uart_driver_install(dft.uart_port,
                                             EDC_UART_BUF_SIZE, EDC_UART_BUF_SIZE,
                                             0, NULL, 0));
        ESP_ERROR_CHECK(uart_param_config(dft.uart_port, &uart_cfg));
        ESP_ERROR_CHECK(uart_set_pin(dft.uart_port,
                                      dft.uart_tx, dft.uart_rx,
                                      UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
        ESP_LOGI(TAG, "UART OK (port=%d baud=%d RX=%d TX=%d)",
                 dft.uart_port, dft.uart_baudrate, dft.uart_rx, dft.uart_tx);
    }
}

/* ================================================================
 * 轮询
 * ================================================================ */

int edc_proto_poll(edc_cycle_t *cycle)
{
    bool got = false;
    if (s_i2c_enabled)  got |= i2c_poll_once();
    if (s_uart_enabled) got |= uart_poll_once();

    if (got) {
        portENTER_CRITICAL(&s_lock);
        memcpy(cycle, &s_latest_cycle, sizeof(edc_cycle_t));
        s_cycle_updated = false;
        portEXIT_CRITICAL(&s_lock);
        return 1;
    }
    return 0;
}

/* ================================================================
 * 发送 DONE / REQUEST
 * ================================================================ */

void edc_proto_send_done(uint16_t cycle_id)
{
    if (!s_uart_enabled) return;
    uint8_t data[2];
    write_u16_be(data, cycle_id);
    uart_send_frame(EDC_CMD_CYCLE_DONE, data, 2);
    s_stat.done_sent++;
    ESP_LOGI(TAG, "DONE sent (cycle_id=%u)", cycle_id);
}

void edc_proto_send_request(void)
{
    if (!s_uart_enabled) return;
    uart_send_frame(EDC_CMD_CYCLE_REQUEST, NULL, 0);
    s_stat.request_sent++;
    ESP_LOGI(TAG, "REQUEST sent");
}

void edc_proto_send_recognize(void)
{
    if (!s_uart_enabled) return;
    uart_send_frame(EDC_CMD_RECOGNIZE, NULL, 0);
    s_stat.request_sent++;
    ESP_LOGI(TAG, "RECOGNIZE sent");
}

void edc_proto_send_task_start(void)
{
    if (!s_uart_enabled) return;
    uart_send_frame(EDC_CMD_TASK_START, NULL, 0);
    s_stat.request_sent++;
    ESP_LOGI(TAG, "TASK_START sent");
}

/* ================================================================
 * 统计
 * ================================================================ */

void edc_proto_get_stats(edc_proto_stat_t *stat)
{
    memcpy(stat, &s_stat, sizeof(edc_proto_stat_t));
}

/* ================================================================
 * 弱回调
 * ================================================================ */

__attribute__((weak))
void edc_proto_on_cycle(const edc_cycle_t *cycle, edc_channel_t ch)
{
    (void)cycle; (void)ch;
}

/* ================================================================
 * FreeRTOS 任务
 * ================================================================ */

void edc_proto_task(void *pvParam)
{
    edc_cycle_t cycle;
    while (1) {
        if (edc_proto_poll(&cycle) > 0) {
            edc_channel_t ch;
            portENTER_CRITICAL(&s_lock);
            ch = s_stat.last_channel;
            portEXIT_CRITICAL(&s_lock);
            edc_proto_on_cycle(&cycle, ch);
        }
        vTaskDelay(pdMS_TO_TICKS(10));
    }
}