#ifndef __TEST_H__
#define __TEST_H__

#include "rtthread.h"
#include "application.h"
#include <stdbool.h>
#include <string.h>
#include "KelmanFilter.h"

#define FRONT_OBSTACLE_OUT          (0xA01310)           //4D前避障雷达点云信息 Can ID
#define FRONT_OBSTACLE_CAN_ID       (0xA01302)           //4D前避障雷达障碍信息 Can ID
#define FRONT_TERRAIN_CAN_ID        (0xA01305)           //4D前避障雷达彷地信息 Can ID

#define REAR_OBSTACLE_OUT           (0xB01310)           //4D后避障雷达点云信息 Can ID
#define REAR_OBSTACLE_CAN_ID        (0xB01302)           //4D后避障雷达障碍信息 Can ID
#define REAR_TERRAIN_CAN_ID         (0xB01305)           //4D后避障雷达彷地信息 Can ID


#define LEFT_OBSTACLE_CAN_ID        (0xC01302)           //4D左避障雷达障碍信息 Can ID

#define RIGHT_OBSTACLE_CAN_ID       (0xD01302)           //4D后避障雷达障碍信息 Can ID


typedef struct
{
    float x;
    float y;
    float z;
    uint32_t pow;
} radar_point;

typedef struct
{
    float q[4];                         // 由飞控传递 当前姿态
    float p[3];                         // 当前坐标
    uint32_t vaild_point_num;           // 有效点个数
    uint32_t ValidFlag;                 // 有效标志位
    radar_point vaild_point_arr[100];   // 有效点云
} Point_Cloud_Frame;


typedef struct
{
    radar_point all_point[1500];
    uint32_t vaild_len;
} FinalPointCloutStruct;


typedef struct
{
    float x, y, z;
    int valid;
} RegionResult;

//发送雷达点云原始数据至PMU
struct Can2PMU_Data
{
    uint8_t head[7];    //帧头
    uint8_t data[1280]; //点云数据
};

#pragma pack(push, 1)  // 确保1字节紧凑对齐
typedef struct
{
    uint8_t eof  : 1;  // bit0: 结束标志
    uint8_t sof  : 1;  // bit1: 起始标志
    uint8_t seq  : 6;  // bit2-7: 帧序号（可表示1-63）
} FrameFlag;
#pragma pack(pop)

typedef union
{
    FrameFlag flags;
    uint8_t   u8flag;
} FlagUnion;

void ClusterTask( void *parameter );
void NodeStatusTask( void *parameter );

void parse_radar_response( const uint8_t *usart_data, uint32_t len ) ;
void PointCloudFilter( void ) ;
void PointCloudConvert( void );
void TerrainPredict ( void );
void quat_rotate_point( float q_new[4], float q_old[4], float p_enu_new[3], float p_enu_old[3], float x, float y, float z, float * xyz );
void quat_rotate_point_filter( float q_new[4], float q_old[4], float x, float y, float z, float * xyz );
bool PowerFilter( float fRangeValue, uint32_t uPowerValue );
void DetectObstacle_ByDynamicY( void );
uint8_t calculate_flag( int index, int pack, FlagUnion* _f );
void TerrainPointsSegmentation (void);
void TerrianPointsFilter (void );
//void verify_radar_crc(void);

#endif