/*
    N100 陀螺仪数据
*/

/* Includes ----------------------------------------------------------------- */
#include "n100.h"

#include <string.h>
#include "device/device.h"

#include "bsp/uart.h"
#include "component/user_math.h"
#include "component/crc8.h"
#include "component/crc16.h"
/* Private define ----------------------------------------------------------- */
#define RX_BUFF_SIZE 56
#define N100_BAG_HEAD 0xFC
#define N100_BAG_END 0xFD
// #define RX_BUFF_SIZE (sizeof(N100_AHRS_t))
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static volatile uint32_t drop_message = 0;

static uint8_t rxbuf[RX_BUFF_SIZE] __attribute__ ((section(".AXI_SRAM")));

// static N100_AHRS_t rxbuf __attribute__((section(".AXI_SRAM")));

static bool inited = false;

static osThreadId_t thread_alert;

/* Private function  -------------------------------------------------------- */

static void N100_RxCpltCallback(void) {
  osThreadFlagsSet(thread_alert, SIGNAL_N100_NEW_DATA);
}

static void N100_ErrorCallback(void) {
  N100_Restart();
}

static void N100_Decode(N100_t *n100, const uint8_t *rxbuf) {
    if (n100 == NULL || rxbuf == NULL) return;

    size_t offset = 0;

    // header
    n100->rx_raw.header.head   = rxbuf[offset++];
    n100->rx_raw.header.cmd    = rxbuf[offset++];
    n100->rx_raw.header.len    = rxbuf[offset++];
    n100->rx_raw.header.count  = rxbuf[offset++];
    n100->rx_raw.header.crc8   = rxbuf[offset++];

    // data.crc16
    n100->rx_raw.data.crc16 = (uint16_t)(rxbuf[offset] | (rxbuf[offset+1] << 8));
    offset += 2;

    //都是float32
    memcpy(&n100->rx_raw.data.gyro.x, rxbuf + offset, 4);
    offset += 4;
    memcpy(&n100->rx_raw.data.gyro.y, rxbuf + offset, 4);
    offset += 4;
    memcpy(&n100->rx_raw.data.gyro.z, rxbuf + offset, 4);
    offset += 4;

    memcpy(&n100->rx_raw.data.eulr.rol, rxbuf + offset, 4);
    offset += 4;
    memcpy(&n100->rx_raw.data.eulr.pit, rxbuf + offset, 4);
    offset += 4;
    memcpy(&n100->rx_raw.data.eulr.yaw, rxbuf + offset, 4);
    offset += 4;
    // data.quat
    memcpy(&n100->rx_raw.data.quat, rxbuf + offset, sizeof(AHRS_Quaternion_t));
    offset += sizeof(AHRS_Quaternion_t);

    // data.time
    memcpy(&n100->rx_raw.data.time, rxbuf + offset, sizeof(int64_t));
    offset += sizeof(int64_t);

    // end
    n100->rx_raw.end = rxbuf[offset];
}

// 直接对原始数据 rxbuf 校验
static bool N100_Verify(const uint8_t *rxbuf) {
    if (rxbuf == NULL) return false;
    // 检查包头包尾
    if (rxbuf[0] != N100_BAG_HEAD || rxbuf[RX_BUFF_SIZE - 1] != N100_BAG_END) {
        drop_message++;
        return false;
    }
    // // CRC8 校验（包头4字节）
    // if (rxbuf[4] != crc8_calc(rxbuf, 4)) {
    //     drop_message++;
    //     return false;
    // }
    // // CRC16 校验（数据段，从第7字节开始，长度为RX_BUFF_SIZE-8）
    // uint16_t crc16 = (uint16_t)(rxbuf[5] | (rxbuf[6] << 8));
    // if (crc16 != crc16_calc(rxbuf + 7, RX_BUFF_SIZE - 8)) {
    //     drop_message++;
    //     return false;
    // }
    return true;
}

/* Exported functions ------------------------------------------------------- */
int8_t N100_Init(N100_t *n100){
    ASSERT(n100);
    if (inited) return DEVICE_ERR_INITED;
    if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
    
    BSP_UART_RegisterCallback(BSP_UART_IMU_N100, BSP_UART_RX_CPLT_CB, N100_RxCpltCallback);
    BSP_UART_RegisterCallback(BSP_UART_IMU_N100, BSP_UART_ERROR_CB, N100_ErrorCallback);
    inited = true;
    return DEVICE_OK;
}

int8_t N100_Restart(void) {
  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_IMU_N100));
  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_IMU_N100));
  return DEVICE_OK;
}

int8_t N100_StartReceiving(N100_t *n100) {
  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_IMU_N100), rxbuf, RX_BUFF_SIZE) != HAL_OK)
    return DEVICE_ERR;
  return DEVICE_OK;
}

bool N100_WaitDmaCplt(void) {
  return (osThreadFlagsWait(SIGNAL_N100_NEW_DATA, osFlagsWaitAll, 200) ==
          SIGNAL_N100_NEW_DATA);
}

int8_t N100_ParseData (N100_t *n100, N100_Cali_t *cali) {
    if (n100 == NULL) return DEVICE_ERR_NULL;
    // 先校验原始数据
    if (!N100_Verify(rxbuf)) {
        return DEVICE_ERR;
    }
    // 校验通过后再解码
    N100_Decode(n100, rxbuf);
    n100->eulr = n100->rx_raw.data.eulr;
    n100->gyro = n100->rx_raw.data.gyro;
    n100->quat = n100->rx_raw.data.quat;
    n100->eulr.rol += cali->offset_x; /* 偏航角校准 */
    n100->eulr.pit += cali->offset_y; /* 俯仰角校准 */
    n100->eulr.yaw += cali->offset_z; /* 翻滚角校准 */
    return DEVICE_OK;
}

int8_t N100_HandleOffline(N100_t *n100) {
  if (n100 == NULL) return DEVICE_ERR_NULL;
    /*清空n100结构体*/
    memset(n100, 0, sizeof(*n100));
  return DEVICE_OK;
}