#include "laser_manager.h"
#include <string.h>
#include "cmsis_os2.h"
#include "FreeRTOS.h"
#include "task.h"

/* --- 配置区 --- */
#define STP_TIMEOUT_MS      200U
#define ATK_TIMEOUT_MS      500U

#define FILTER_WIN_SZ       3U   // 中值滤波窗口大小（推荐 3 或 5）

#define FAULT_NONE          0x00
#define FAULT_STP_CHECKSUM  0x41
#define FAULT_STP_FRAME     0x42
#define FAULT_TIMEOUT       0x80

/* MPU 无 Cache 段宏定义 */
#if defined(__GNUC__)
#define LASER_DMA_RAM __attribute__((section(".dma_buffer"))) __attribute__((aligned(32)))
#else
#define LASER_DMA_RAM
#endif

/* 缓冲区大小 */
#define STP_DMA_SZ 512U
#define ATK_DMA_SZ 256U

/* 分配在无 Cache 区域的 DMA 内存 */
static LASER_DMA_RAM uint8_t g_stp_front_dma[STP_DMA_SZ];
static LASER_DMA_RAM uint8_t g_atk_front_dma[ATK_DMA_SZ];
static LASER_DMA_RAM uint8_t g_stp_rear_dma[STP_DMA_SZ];
static LASER_DMA_RAM uint8_t g_atk_rear_dma[ATK_DMA_SZ];

/* 数据节点与快照 */
typedef struct {
    UART_HandleTypeDef *huart;
    uint8_t *dma_buf;
    uint16_t dma_sz;
    uint16_t read_ptr;
    uint32_t last_tick;

    // 滤波队列缓存
    uint16_t dist_history[FILTER_WIN_SZ];
    uint8_t  hist_idx;
    uint8_t  hist_cnt;

    laser_simple_data_t data;
} laser_node_t;

static laser_node_t g_nodes[LASER_CH_MAX] = {
    [LASER_CH_FRONT_STP] = { &huart2, g_stp_front_dma, STP_DMA_SZ, 0, 0, {0}, 0, 0, {0} },
    [LASER_CH_FRONT_ATK] = { &huart3, g_atk_front_dma, ATK_DMA_SZ, 0, 0, {0}, 0, 0, {0} },
    [LASER_CH_REAR_STP]  = { &huart4, g_stp_rear_dma,  STP_DMA_SZ, 0, 0, {0}, 0, 0, {0} },
    [LASER_CH_REAR_ATK]  = { &huart6, g_atk_rear_dma,  ATK_DMA_SZ, 0, 0, {0}, 0, 0, {0} }
};

static laser_simple_snapshot_t g_snapshot;
static osThreadId_t laserTaskHandle;

/* --- 辅助宏与函数 --- */
#define READ_RING(buf, sz, pos, offset) (buf[((pos) + (offset)) % (sz)])

static uint16_t le16_read(const uint8_t *p) { return (uint16_t)(p[0] | (p[1] << 8)); }

static uint16_t apply_median_filter(laser_node_t *n, uint16_t new_val) {
    n->dist_history[n->hist_idx] = new_val;
    n->hist_idx = (n->hist_idx + 1) % FILTER_WIN_SZ;
    if (n->hist_cnt < FILTER_WIN_SZ) n->hist_cnt++;

    uint16_t temp[FILTER_WIN_SZ];
    for (int i = 0; i < n->hist_cnt; i++) temp[i] = n->dist_history[i];

    for (int i = 0; i < n->hist_cnt - 1; i++) {
        for (int j = i + 1; j < n->hist_cnt; j++) {
            if (temp[i] > temp[j]) {
                uint16_t t = temp[i]; temp[i] = temp[j]; temp[j] = t;
            }
        }
    }
    return temp[n->hist_cnt / 2];
}

/* --- STP 环形无 memmove 解析 (保持不变) --- */
static void process_stp(laser_channel_t ch) {
    laser_node_t *n = &g_nodes[ch];
    uint16_t write_ptr = n->dma_sz - __HAL_DMA_GET_COUNTER(n->huart->hdmarx);
    uint16_t available = (write_ptr - n->read_ptr + n->dma_sz) % n->dma_sz;

    while (available >= 11U) {
        uint16_t head = 0xFFFF;
        for (uint16_t i = 0; i <= available - 4; i++) {
            if (READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i) == 0xAA &&
                READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i+1) == 0xAA &&
                READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i+2) == 0xAA &&
                READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i+3) == 0xAA) {
                head = i; break;
            }
        }

        if (head == 0xFFFF) {
            n->read_ptr = (n->read_ptr + available - 3) % n->dma_sz;
            break;
        }

        n->read_ptr = (n->read_ptr + head) % n->dma_sz;
        available -= head;
        if (available < 11) break;

        uint8_t data_len = READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, 8);
        uint16_t frame_len = 10 + data_len + 1;

        if (data_len > 200) { n->read_ptr = (n->read_ptr + 1) % n->dma_sz; available--; continue; }
        if (available < frame_len) break;

        uint8_t frame[256];
        uint32_t sum = 0;
        for (uint16_t i = 0; i < frame_len; i++) {
            frame[i] = READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i);
            if (i >= 4 && i < frame_len - 1) sum += frame[i];
        }

        if ((sum & 0xFF) == frame[frame_len - 1] && frame[5] == 0x02 && data_len == 0xB8) {
            uint32_t p = 10;
            uint16_t best = 0xFFFF;

            for (int i = 0; i < 12; i++) {
                int16_t dist = le16_read(&frame[p]); p += 8;
                uint8_t conf = frame[p]; p += 7;
                if (dist >= 70 && dist <= 7500 && conf >= 1 && dist < best) best = dist;
            }

            taskENTER_CRITICAL();
            if (best != 0xFFFF) {
                n->data.raw_distance_mm = best;
                n->data.distance_mm = apply_median_filter(n, best);
                n->data.valid = 1;
                n->data.fault_code = 0;
            } else {
                n->data.valid = 0;
            }
            n->last_tick = HAL_GetTick();
            taskEXIT_CRITICAL();
        }

        n->read_ptr = (n->read_ptr + frame_len) % n->dma_sz;
        available -= frame_len;
    }
}

/* --- 【全新】ATK 环形滑动窗口解析 (无视断帧与粘包) --- */
static void process_atk(laser_channel_t ch) {
    laser_node_t *n = &g_nodes[ch];
    uint16_t write_ptr = n->dma_sz - __HAL_DMA_GET_COUNTER(n->huart->hdmarx);
    uint16_t available = (write_ptr - n->read_ptr + n->dma_sz) % n->dma_sz;

    // ATK 哪怕报故障，最小长度也有类似 "State:1\r\n" 约 9 字节
    while (available >= 5) {
        char c0 = (char)READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, 0);
        char c1 = (char)READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, 1);

        // 匹配帧头 "d:" 或 "St" (State的开头)
        if ((c0 == 'd' && c1 == ':') || (c0 == 'S' && c1 == 't')) {
            // 寻找帧尾 '\n'
            uint16_t lf_pos = 0xFFFF;
            for (uint16_t i = 0; i < available; i++) {
                if (READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i) == '\n') {
                    lf_pos = i;
                    break;
                }
            }

            // 如果没找到换行符，说明这帧 DMA 还没搬运完，立刻退出等下次任务周期
            if (lf_pos == 0xFFFF) {
                break;
            }

            uint16_t frame_len = lf_pos + 1;
            char line[64] = {0};
            for (uint16_t i = 0; i < frame_len && i < sizeof(line) - 1; i++) {
                line[i] = READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i);
            }

            taskENTER_CRITICAL();
            if (line[0] == 'S' && line[1] == 't') {
                int state_val = line[6] - '0';
                if (state_val >= 0 && state_val <= 9) {
                    n->data.fault_code = state_val;
                    if (state_val != 0) n->data.valid = 0;
                    n->last_tick = HAL_GetTick();
                }
            }
            else if (line[0] == 'd' && line[1] == ':') {
                int dist_val = 0;
                const char *p = &line[2];
                while (*p == ' ') p++;
                while (*p >= '0' && *p <= '9') {
                    dist_val = dist_val * 10 + (*p - '0');
                    p++;
                }

                if (dist_val > 0 && n->data.fault_code == 0) {
                    n->data.raw_distance_mm = dist_val;
                    n->data.distance_mm = apply_median_filter(n, dist_val);
                    n->data.valid = 1;
                } else {
                    n->data.valid = 0;
                }
                n->last_tick = HAL_GetTick();
            }
            taskEXIT_CRITICAL();

            // 成功解析一帧，滑动读取指针
            n->read_ptr = (n->read_ptr + frame_len) % n->dma_sz;
            available -= frame_len;
        } else {
            // 没找到帧头，说明有垃圾数据，把指针往后挪 1 字节继续找
            n->read_ptr = (n->read_ptr + 1) % n->dma_sz;
            available--;
        }
    }
}

/* --- 核心 RTOS 任务 (全轮询模式) --- */
static void LaserTask(void *arg) {
    (void)arg;
    for(;;) {
        // 【架构升级】：不再死等中断标志，而是以 10ms 的心跳主动去 DMA 缓冲区提货。
        // 50Hz 等于 20ms 来一帧，10ms 巡视一次性能富裕且绝不漏包。
        osDelay(10);
        uint32_t now = HAL_GetTick();

        // 纯 DMA 内存操作，极快
        process_stp(LASER_CH_FRONT_STP);
        process_stp(LASER_CH_REAR_STP);
        process_atk(LASER_CH_FRONT_ATK);
        process_atk(LASER_CH_REAR_ATK);

        // 统一心跳/超时/快照检查
        for (int i = 0; i < LASER_CH_MAX; i++) {
            uint32_t timeout = (i == LASER_CH_FRONT_STP || i == LASER_CH_REAR_STP) ? STP_TIMEOUT_MS : ATK_TIMEOUT_MS;

            taskENTER_CRITICAL();
            if (now - g_nodes[i].last_tick > timeout) {
                g_nodes[i].data.online = 0;
                g_nodes[i].data.valid = 0;
                g_nodes[i].data.fault_code = FAULT_TIMEOUT;
                g_nodes[i].hist_cnt = 0; // 掉线时清空历史滤波数组
            } else {
                g_nodes[i].data.online = 1;
                g_nodes[i].data.update_tick_ms = g_nodes[i].last_tick;
            }
            g_snapshot.ch[i] = g_nodes[i].data;
            taskEXIT_CRITICAL();
        }
    }
}

/* --- API 接口 --- */
void LASER_SIMPLE_Init(void) {
    memset(&g_snapshot, 0, sizeof(g_snapshot));

    // 【重要变更】：全员使用普通循环 DMA，彻底抛弃 IDLE 接收
    HAL_UART_Receive_DMA(g_nodes[LASER_CH_FRONT_STP].huart, g_nodes[LASER_CH_FRONT_STP].dma_buf, STP_DMA_SZ);
    HAL_UART_Receive_DMA(g_nodes[LASER_CH_REAR_STP].huart, g_nodes[LASER_CH_REAR_STP].dma_buf, STP_DMA_SZ);
    HAL_UART_Receive_DMA(g_nodes[LASER_CH_FRONT_ATK].huart, g_nodes[LASER_CH_FRONT_ATK].dma_buf, ATK_DMA_SZ);
    HAL_UART_Receive_DMA(g_nodes[LASER_CH_REAR_ATK].huart, g_nodes[LASER_CH_REAR_ATK].dma_buf, ATK_DMA_SZ);

    osThreadAttr_t attr = { .name = "LaserTsk", .stack_size = 1024 * 4, .priority = osPriorityAboveNormal };
    laserTaskHandle = osThreadNew(LaserTask, NULL, &attr);
}

void LASER_SIMPLE_Poll(uint32_t tick_ms) {
    (void)tick_ms;
}

const laser_simple_snapshot_t *LASER_SIMPLE_GetSnapshot(void) {
    return &g_snapshot;
}

static uint16_t nearest_of_two(const laser_simple_data_t *a, const laser_simple_data_t *b) {
    if (a->online && a->valid && b->online && b->valid) {
        return (a->distance_mm < b->distance_mm) ? a->distance_mm : b->distance_mm;
    }
    if (a->online && a->valid) return a->distance_mm;
    if (b->online && b->valid) return b->distance_mm;
    return 0U;
}

uint16_t LASER_SIMPLE_GetFrontNearest(void) {
    return nearest_of_two(&g_snapshot.ch[LASER_CH_FRONT_STP], &g_snapshot.ch[LASER_CH_FRONT_ATK]);
}

uint16_t LASER_SIMPLE_GetRearNearest(void) {
    return nearest_of_two(&g_snapshot.ch[LASER_CH_REAR_STP], &g_snapshot.ch[LASER_CH_REAR_ATK]);
}

/* --- 中断桥接 (已大幅瘦身，仅保留错误恢复) --- */

// 因为改为轮询模式，不再需要依赖这几个正常接收的中断了。保留空函数防止外部调用报错。
void LASER_UART_RxCpltCallback(UART_HandleTypeDef *huart) { (void)huart; }
void LASER_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) { (void)huart; }
void LASER_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size) { (void)huart; (void)Size; }

// 错误自动恢复保留，这是底层硬件 ORE 报错时重启 DMA 必需的
void LASER_UART_ErrorRecovery(UART_HandleTypeDef *huart) {
    for (int i=0; i<LASER_CH_MAX; i++) {
        if (g_nodes[i].huart == huart) {
            HAL_UART_DMAStop(huart);
            // 统一恢复为普通 DMA 接收
            if (i == LASER_CH_FRONT_STP || i == LASER_CH_REAR_STP)
                HAL_UART_Receive_DMA(huart, g_nodes[i].dma_buf, STP_DMA_SZ);
            else
                HAL_UART_Receive_DMA(huart, g_nodes[i].dma_buf, ATK_DMA_SZ);
        }
    }
}