#include "hwt101.h"

#include <stdbool.h>
#include "cmsis_os2.h"
#include "FreeRTOS.h"
#include "stm32h7xx_hal_uart.h"
#include "task.h"

/* 寄存器地址 */
#define REG_SAVE         0x00
#define REG_RRATE        0x03
#define REG_BAUD         0x04
#define REG_KEY          0x69
#define REG_CALIYAW      0x76

/* D2 域特区分配 */
__attribute__((section(".dma_buffer"))) __attribute__((aligned(32)))
static uint8_t s_dma_buf[HWT101_DMA_BUF_SIZE];

/* 内部静态变量 */
static UART_HandleTypeDef *s_huart = NULL;
static HWT101_Data_t s_data = {0};
static uint16_t s_read_ptr = 0;
static uint8_t s_frame[11];
static uint8_t s_frame_idx = 0;

/* yaw unwrap 状态：将 [-180, +180) 的原始 yaw 转为连续角度 */
static float s_yaw_prev       = 0.0f;   // 上一次原始 yaw (deg)
static float s_yaw_continuous  = 0.0f;   // 累计连续角度 (deg)
static bool  s_yaw_initialized = false;  // 首次标记

void HWT101_Init(UART_HandleTypeDef *huart) {
    if (huart == NULL) return;
    s_huart = huart;
    s_read_ptr = 0;

    // 【终极修复 1】：强制使用 32 位指针，将这块 (NOLOAD) 内存全部写 0。
    // 必须是 32 位写入！这会彻底激活 RAM_D2 的硬件 ECC，消灭上电时的随机乱码，防止 DMA 单字节写入引发总线错误。
    uint32_t *p32 = (uint32_t *)s_dma_buf;
    for (uint32_t i = 0; i < (HWT101_DMA_BUF_SIZE / 4); i++) {
        p32[i] = 0;
    }

    // ⚠️ 【终极修复 2】：因为 .dma_buffer 所在的 0x30000000 已被 MPU 配置为 Device/Non-Cacheable 内存，
    // 绝对不能调用 SCB_InvalidateDCache_by_Addr，否则 Cortex-M7 会判定越权操作直接进入 HardFault 死机！
    // (已删除原有的 SCB_InvalidateDCache_by_Addr 代码)

    HAL_UART_Receive_DMA(s_huart, s_dma_buf, HWT101_DMA_BUF_SIZE);
}

void HWT101_Process(void) {
    if (s_huart == NULL) return;

    // 获取当前 DMA 写指针
    uint16_t write_ptr = HWT101_DMA_BUF_SIZE - (uint16_t)__HAL_DMA_GET_COUNTER(s_huart->hdmarx);

    // ⚠️ 【终极修复 3】：删除原有的刷 Cache 代码。
    // 依靠硬件 MPU 免 Cache 机制，CPU 读到的必定是 DMA 搬运过来的最新数据，不会有数据一致性问题。

    while (s_read_ptr != write_ptr) {
        uint8_t byte = s_dma_buf[s_read_ptr];
        s_read_ptr = (s_read_ptr + 1) % HWT101_DMA_BUF_SIZE;

        // 状态机加速：如果不是帧头，直接跳过，不用进入后续逻辑
        if (s_frame_idx == 0 && byte != 0x55) continue;

        s_frame[s_frame_idx++] = byte;
        if (s_frame_idx >= 11) {
            uint8_t sum = 0;
            for (int i = 0; i < 10; i++) sum += s_frame[i];

            if (sum == s_frame[10]) {
                // 更新全局变量时使用临界区保护，防止数据撕裂
                taskENTER_CRITICAL();
                if (s_frame[1] == 0x52) {
                    int16_t wz_raw = (int16_t)((s_frame[7] << 8) | s_frame[6]);
                    s_data.wz = (float)wz_raw / 32768.0f * 2000.0f;
                } else if (s_frame[1] == 0x53) {
                    int16_t yaw_raw = (int16_t)((s_frame[7] << 8) | s_frame[6]);
                    float yaw_deg = (float)yaw_raw / 32768.0f * 180.0f;

                    /* 保存原始 yaw ([-180, +180) 度) */
                    s_data.yaw = yaw_deg;

                    /* ---- yaw unwrap: 消除 ±180° 跳变，生成连续角度 ---- */
                    if (!s_yaw_initialized) {
                        s_yaw_continuous  = yaw_deg;
                        s_yaw_prev        = yaw_deg;
                        s_yaw_initialized = true;
                    } else {
                        float diff = yaw_deg - s_yaw_prev;
                        if (diff >  180.0f) diff -= 360.0f;
                        if (diff < -180.0f) diff += 360.0f;
                        s_yaw_continuous += diff;
                        s_yaw_prev = yaw_deg;
                    }
                    s_data.yaw_continuous = s_yaw_continuous;

                    s_data.last_update = HAL_GetTick();
                    s_data.online = 1;
                }
                taskEXIT_CRITICAL();
            }
            s_frame_idx = 0;
        }
    }

    // 超时检测
    if (HAL_GetTick() - s_data.last_update > 200) {
        s_data.online = 0;
    }
}

// 安全的数据获取接口
void HWT101_GetData(HWT101_Data_t *out_data) {
    if (out_data == NULL) return;
    // 关中断拷贝，绝对防止浮点数撕裂
    taskENTER_CRITICAL();
    *out_data = s_data;
    taskEXIT_CRITICAL();
}

/* ==========================================
 * 传感器配置函数 (与原版保持一致)
 * ========================================== */
static HAL_StatusTypeDef HWT101_Unlock(void) {
    uint8_t cmd[5] = {0xFF, 0xAA, REG_KEY, 0x88, 0xB5};
    return HAL_UART_Transmit(s_huart, cmd, 5, 20);
}

static HAL_StatusTypeDef HWT101_Save(void) {
    uint8_t cmd[5] = {0xFF, 0xAA, REG_SAVE, 0x00, 0x00};
    HAL_StatusTypeDef res = HAL_UART_Transmit(s_huart, cmd, 5, 20);
    osDelay(200);
    return res;
}

static HAL_StatusTypeDef HWT101_WriteReg(uint8_t reg, int16_t value) {
    if (HWT101_Unlock() != HAL_OK) return HAL_ERROR;
    osDelay(20);
    uint8_t cmd[5] = {0xFF, 0xAA, reg, (uint8_t)(value & 0xFF), (uint8_t)(value >> 8)};
    return HAL_UART_Transmit(s_huart, cmd, 5, 20);
}

HAL_StatusTypeDef HWT101_Config(HWT101_Rate_t rate, HWT101_Baud_t baud) {
    if (HWT101_WriteReg(REG_RRATE, (int16_t)rate) != HAL_OK) return HAL_ERROR;
    osDelay(100);
    if (HWT101_WriteReg(REG_BAUD, (int16_t)baud) != HAL_OK) return HAL_ERROR;
    osDelay(100);
    return HWT101_Save();
}

HAL_StatusTypeDef HWT101_ZeroYaw(void) {
    if (HWT101_WriteReg(REG_CALIYAW, 0x0000) != HAL_OK) return HAL_ERROR;
    osDelay(500);

    /* 重置 unwrap 状态，下次收到帧时重新初始化连续角度 */
    taskENTER_CRITICAL();
    s_yaw_initialized = false;
    s_yaw_continuous  = 0.0f;
    s_yaw_prev        = 0.0f;
    s_data.yaw_continuous = 0.0f;
    taskEXIT_CRITICAL();

    return HWT101_Save();
}

void HWT101_ErrorRecovery(UART_HandleTypeDef *huart) {
    if (s_huart != NULL && huart == s_huart) {
        HAL_UART_DMAStop(s_huart);

        // 暴力清除 ORE/NE/FE/PE 错误标志，防止无限死循环中断风暴
        __HAL_UART_CLEAR_FLAG(s_huart, UART_CLEAR_OREF | UART_CLEAR_NEF | UART_CLEAR_PEF | UART_CLEAR_FEF);
        // 读取一次 RDR 寄存器，把里面卡住的垃圾字节倒掉
        volatile uint32_t dump = s_huart->Instance->RDR;
        (void)dump;

        // 干净地重新启动 DMA
        HAL_UART_Receive_DMA(s_huart, s_dma_buf, HWT101_DMA_BUF_SIZE);
        s_read_ptr = 0;
        s_frame_idx = 0;
    }
}