#include "soft_terrain.h"
#include "stdint.h"
#include "stdbool.h"
#include "tim.h"
#include "can.h"
#include "soft_can.h"
#include "common.h"
#include "soft_seed_device.h"
#include "soft_crc.h"
#include "string.h"
#include "soft_obstacle.h"
#include "soft_seed_device.h"
#include "soft_version.h"
#include "soft_p_2_c.h"

UAVH30 uavh30_dist;

uavr_terrain uavr56_info = {.get_radar_sensi = 50};
uavr_terrain mimo_ter_info;

_dev_par DM_4DRADARMAG;
// muniu muniu_ter_info;

bool terrain_is_link = false;
uint16_t terrain_height = 0;

/**
 * @file    can_recv_mocib_terrain
 * @brief   莫之比防地解析
 * @param   none
 * @details
 * @author  Zhang Sir
 **/
void can_recv_mocib_terrain(uint8_t *data)
{
    uavh30_dist.powerful = (data[0] << 8) + data[1];
    uavh30_dist.near = (data[2] << 8) + data[3];
    uavh30_dist.far = (data[4] << 8) + data[5];

    uavr56_info.height = uavh30_dist.powerful;
    uavr56_info.Link.recv_time = HAL_GetTick();
    uavr56_info.Link.connect_status = COMP_NORMAL;
}

/**
 * @file    can_recv_enzhao_terrain
 * @brief   恩曌防地解析
 * @param   none
 * @details
 * @author  Zhang Sir
 **/
mimo_part_radar T_radar[3];
void can_recv_enzhao_terrain(uint32_t CanID, uint8_t data[], uint8_t len)
{
    switch (CanID)
    {
    case CAN_MIMO_T_ID: // 单点雷达
        mimo_ter_info.crc = data[3] + data[4] + data[5] + data[6];
        if (data[7] == mimo_ter_info.crc)
        {
            mimo_ter_info.height = (256 * data[4]) + (data[3]); // cm
            //  printf("%d          %d\n", mimo_ter_info.height, m.muniu_hight);   //输出到串口助手上 需要pringtf重定向
        }

        mimo_ter_info.Link.recv_time = HAL_GetTick();
        mimo_ter_info.Link.connect_status = COMP_NORMAL;
        Dev.Radar.facid_T = FAC_MIMO_RT;
        break;
    case CAN_MIMO_T_ID1:
        memcpy(&T_radar[0], data, 8);
        Dev.Part_Tradar_Link.recv_time = HAL_GetTick();
        Dev.Part_Tradar_Link.connect_status = COMP_NORMAL;
        Dev.Part_radarT.facid = FAC_MIMO_RT;
        Dev.Radar.facid_T = FAC_MIMO_RT;
        break;
    case CAN_MIMO_T_ID2:
        memcpy(&T_radar[1], data, 8);
        break;
    case CAN_MIMO_T_ID3:
        memcpy(&T_radar[2], data, 8);
        break;
    default:
        break;
    }
}

// 木牛仿地雷达
//  void can_recv_muniu_terrain(uint8_t *data)
//  {
//      muniu_ter_info.muniu_hight = (data[0] << 8) + data[1];
//      muniu_ter_info.muniu_SNR = (data[2] << 8) + data[3];

//     muniu_ter_info.Link.connect_status = COMP_NORMAL;
//     muniu_ter_info.Link.recv_time = HAL_GetTick();
// }


//电目雷达
DM_RADAR DM_T_info,FMU_DM_info; //原始数据
uint8_t dm_i = 0;
uint8_t DM_recv_flag = 0;
uint8_t DM4d_recv_flag = 0;
Connect_check DM_status;
Connect_check DM_4dstatus;
uavr_terrain DM_ter_info;
void DM_terrain_recieved_hookfuction(uint32_t cellCanID, uint8_t data[], uint8_t len)
{
    if (cellCanID == 0x901300) // 多点协议
    {
        DM_T_info.byte7.frame_flag = data[7];

        DM_status.connect_status = COMP_NORMAL;
        DM_status.recv_time = HAL_GetTick();

        if (DM_T_info.byte7.flag.head != 0) // 头
        {
            memcpy(&DM_T_info.target_num, &data[0], 7);
            if( DM_T_info.target_num > 30 )
                    DM_T_info.target_num = 30;
            dm_i = 0;
            if (DM_T_info.target_num == 0 && DM_recv_flag == 0)
            {
                FMU_DM_info.target_num = 0;
            }
        }
        else if (DM_T_info.byte7.flag.tail != 0) // 尾
        {
            if (DM_T_info.target_num != 1)
            {
                memcpy(&DM_T_info.buf[dm_i], &data[0], DM_T_info.target_num * 4 % 7);
            }
            else
            {
                memcpy(&DM_T_info.buf[dm_i], &data[0], 4);
            }

            if (DM_T_info.crc == Get_Crc16(&DM_T_info.buf[0], DM_T_info.target_num * 4) && DM_recv_flag == 0)
            {
                memcpy(&FMU_DM_info.target_num, &DM_T_info.target_num, DM_T_info.target_num * 4 + 8);
                // memcpy(&FMU_DM_info.buf[0], &DM_T_info.buf[0], DM_T_info.target_num * 4);
            }
        }
        else
        {
            memcpy(&DM_T_info.buf[dm_i], &data[0], 7);
            dm_i += 7;
        }
        if (dm_i >= 255 - 7)
        {
            dm_i = 0;
        }
    }
    else if (cellCanID == 0x901301) // 单点协议
    {
        DM_ter_info.Link.connect_status = COMP_NORMAL;
        DM_ter_info.Link.recv_time = HAL_GetTick();
        Dev.Radar.facid_T = FAC_DM_RT;

        DM_ter_info.height = data[3] + data[4] * 256;
    }

    // 版本信息
    if (cellCanID == 0x981301 && data[0] == 0x1)
    {
        uint32_t version_temp = 0;
        DM_T_info.byte7.frame_flag = data[7];

        if (DM_T_info.byte7.flag.head != 0) // 头
        {
            memcpy(&version_temp, &data[1], 4);
            Int2String(version_temp, DM_ter_info.sn, 9);
            // 通过SN序号判断新旧boot
            if ((version_temp % 10000000) < 2502999)
                DM_ter_info.version[3] = 'O';
            else
                DM_ter_info.version[3] = 'N';

            regist_dev_info(&dev_ter, DEVICE_TERRA, false, DM_ter_info.sn, 9, NULL, 0, NULL, 0, "dmter", 6);
        }
        else if (DM_T_info.byte7.flag.tail != 0) // 尾
        {
            memcpy(&version_temp, &data[1], 4);
            Int2String(version_temp, &DM_ter_info.version[4], 6);
            DM_ter_info.version[0] = 'D';
            DM_ter_info.version[1] = 'S';
            DM_ter_info.version[2] = '1';

            regist_dev_info(&dev_ter, DEVICE_TERRA, false, NULL, 0, DM_ter_info.version, 10, NULL, 0, "dmter", 6);

            DM_ter_info.get_radar_ver_flag = true;
            pmu_send = PMU_SEND_VERSION; // 旧版APP
        }
    }
    else if (cellCanID == 0x981301 && (data[0] == 0x8 || data[0] == 0x5))
    {
        if (data[0] == 0x8)
            DM_ter_info.get_radar_blind_flag = true;
        pmu_set_ack(_MSGID_SET, MSGID_SET_TR_BLIND, 0x56, data[1] + data[2] * 256);
    }
    else if (cellCanID == 0x981301 && (data[0] == 0x9 || data[0] == 0x7))
    {
        if (data[0] == 0x9)
            DM_ter_info.get_radar_power_flag = true;
        pmu_set_ack(_MSGID_SET, MSGID_SET_BR_POWER, 0x56, data[1] + data[2] * 256);
    }
    else if (cellCanID == 0x981301 && (data[0] == 0xA || data[0] == 0xB))
    {
        if (data[0] == 0xB)
            DM_ter_info.get_radar_rawSwitch_flag = true;
        pmu_set_ack(_MSGID_SET, MSGID_SET_RAW_SWITCH, 0x56, data[1] + data[2] * 256);
    }
    else if (cellCanID == 0x981301 && data[0] == 0x4)
    {
        pmu_set_ack(_MSGID_SET, MSGID_SET_R_FUNC, 0, data[1] + data[2] * 256);
    }
   
}

int16_t F_4DRadar[3][3] = {0}; // X Y Z
DM_4dFRADAR DM_F4d;
int dm_4df_i = 0;
DM_4DRADAR FMU_4D_info;
bool F4d_send_flag = false;
bool DM4Dmsg_send_fmu=false;
void DM_Fobs_recieved_hookfuction(uint32_t cellCanID, uint8_t data[], uint8_t len)
{
    if (cellCanID == 0XA01300) // 多点协议
    {
        DM_T_info.byte7.frame_flag = data[7];

        DM_status.connect_status = COMP_NORMAL;
        DM_status.recv_time = HAL_GetTick();

        if (DM_T_info.byte7.flag.head != 0) // 头
        {
            memcpy(&DM_T_info.target_num, &data[0], 7);
            if( DM_T_info.target_num > 30 )
                    DM_T_info.target_num = 30;
            dm_i = 0;
            if (DM_T_info.target_num == 0 && DM_recv_flag == 0)
            {
                FMU_DM_info.target_num = 0;
            }
        }
        else if (DM_T_info.byte7.flag.tail != 0) // 尾
        {
            if (DM_T_info.target_num != 1)
            {
                memcpy(&DM_T_info.buf[dm_i], &data[0], DM_T_info.target_num * 4 % 7);
            }
            else
            {
                memcpy(&DM_T_info.buf[dm_i], &data[0], 4);
            }

            if (DM_T_info.crc == Get_Crc16(&DM_T_info.buf[0], DM_T_info.target_num * 4) && DM_recv_flag == 0)
            {
                memcpy(&FMU_DM_info.target_num, &DM_T_info.target_num, DM_T_info.target_num * 4 + 8);
                // memcpy(&FMU_DM_info.buf[0], &DM_T_info.buf[0], DM_T_info.target_num * 4);
            }
        }
        else
        {
            memcpy(&DM_T_info.buf[dm_i], &data[0], 7);
            dm_i += 7;
        }
        if (dm_i >= 255 - 7)
        {
            dm_i = 0;
        }
    }
    else if (cellCanID == 0XA01301) // 单点协议
    {
        DM_f_info.Link.connect_status = COMP_NORMAL;
        DM_f_info.Link.recv_time = HAL_GetTick();
        Dev.Radar.facid_F = FAC_DM_RF;

        DM_f_info.distance_y = data[3] + data[4] * 256;
    }

    // 4D前避障雷达协议 点云
    if (cellCanID == 0XA01310)
    {
        DM_4dstatus.connect_status = COMP_NORMAL;
        DM_4dstatus.recv_time = HAL_GetTick();
        Dev.Part_radarF.facid = FAC_DM_RF_4D;

        DM_F4d.byte7.frame_flag = data[7];

        if (DM_F4d.byte7.flag.head != 0) // 头
        {
            memcpy(&DM_F4d.target_num, &data[0], 7);
            if(DM_F4d.target_num > 130)
                DM_F4d.target_num = 130;
            dm_4df_i = 0;
        }
        else if (DM_F4d.byte7.flag.tail != 0) // 尾
        {
            if (DM_F4d.target_num != 1)
            {
                if ((DM_F4d.target_num * 5) % 7 != 0)
                {
                    memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], (DM_F4d.target_num * 5) % 7);
                    dm_4df_i += (DM_F4d.target_num * 5) % 7;
                }
                else
                {
                    memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], 7);
                    dm_4df_i += 7;
                }
            }
            else
            {
                memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], 5);
                dm_4df_i += 5;
            }
            if (DM_F4d.crc == Get_Crc16(&DM_F4d.RawData[0], DM_F4d.target_num * 5) && DM4d_recv_flag == 0)
            {
                FMU_4D_info.target_num = DM_F4d.target_num;
                memcpy(&FMU_4D_info.buf, &DM_F4d.RawData, DM_F4d.target_num * 5);
                F4d_send_flag = true;
            }
        }
        else
        {
            memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], 7);
            dm_4df_i += 7;
        }
        if (dm_4df_i >= 254 * 5)
        {
            dm_4df_i = 0;
        }
    }
    // 4D前避障雷达协议 单点
    if (cellCanID == 0XA01302)
    {
        F_4DRadar[0][0] = data[1] + data[2] * 256;
        F_4DRadar[0][1] = data[3] + data[4] * 256;
        F_4DRadar[0][2] = data[5] + data[6] * 256;

        Dev.Part_Fradar_Link.recv_time = HAL_GetTick();
        Dev.Part_Fradar_Link.connect_status = COMP_NORMAL;
        Dev.Part_radarF.facid = FAC_DM_RF_4D;
    }
    else if (cellCanID == 0XA01303)
    {
        F_4DRadar[1][0] = data[1] + data[2] * 256;
        F_4DRadar[1][1] = data[3] + data[4] * 256;
        F_4DRadar[1][2] = data[5] + data[6] * 256;
    }
    else if (cellCanID == 0XA01304)
    {
        F_4DRadar[2][0] = data[1] + data[2] * 256;
        F_4DRadar[2][1] = data[3] + data[4] * 256;
        F_4DRadar[2][2] = data[5] + data[6] * 256;
    }

    // 版本信息
    if (cellCanID == 0XA81301 && data[0] == 0x1)
    {
        uint32_t version_temp = 0;
        DM_T_info.byte7.frame_flag = data[7];

        if (DM_T_info.byte7.flag.head != 0) // 头
        {
            memcpy(&version_temp, &data[1], 4);
            Int2String(version_temp, DM_f_info.sn, 9);
            // 通过SN序号判断新旧boot
            if ((version_temp % 10000000) < 2502999)
                DM_f_info.version[3] = 'O';
            else
                DM_f_info.version[3] = 'N';

            regist_dev_info(&dev_obsf, DEVICE_OBSF, false, DM_f_info.sn, 9, NULL, 0, NULL, 0, "dmter", 6);
        }
        else if (DM_T_info.byte7.flag.tail != 0) // 尾
        {
            memcpy(&version_temp, &data[1], 4);
            Int2String(version_temp, &DM_f_info.version[4], 6);
            DM_f_info.version[0] = 'D';
            DM_f_info.version[1] = 'W';
            DM_f_info.version[2] = '1';

            regist_dev_info(&dev_obsf, DEVICE_OBSF, false, NULL, 0, DM_f_info.version, 10, NULL, 0, "dmter", 6);

            DM_f_info.get_radar_ver_flag = true;
            pmu_send = PMU_SEND_VERSION; // 旧版APP
        }
    }
    else if (cellCanID == 0xA81301 && (data[0] == 0x8 || data[0] == 0x5))
    {
        if (data[0] == 0x8)
            DM_f_info.get_radar_blind_flag = true;
        pmu_set_ack(_MSGID_SET, MSGID_SET_TR_BLIND, 0x11, data[1] + data[2] * 256);
    }
    else if (cellCanID == 0xA81301 && (data[0] == 0x9 || data[0] == 0x7))
    {
        if (data[0] == 0x9)
            DM_f_info.get_radar_power_flag = true;
        pmu_set_ack(_MSGID_SET, MSGID_SET_BR_POWER, 0x11, data[1] + data[2] * 256);
    }
    else if (cellCanID == 0xA81301 && (data[0] == 0xA || data[0] == 0xB))
    {
        if (data[0] == 0xB)
            DM_f_info.get_radar_rawSwitch_flag = true;
        pmu_set_ack(_MSGID_SET, MSGID_SET_RAW_SWITCH, 0x11, data[1] + data[2] * 256);
    }
    else if (cellCanID == 0xA81301 && data[0] == 0x4)
    {
        pmu_set_ack(_MSGID_SET, MSGID_SET_R_FUNC, 0, data[1] + data[2] * 256);
    }
    else if (cellCanID == 0xA81302 && (data[0] == 0xD || data[0] == 0xC))
    {
        if (data[0] == 0xD)
            DM_4DRADARMAG.get_angel_4DF = true;
        DM_4DRADARMAG.angel_4DF = data[1] + data[2] * 256;
        DM4Dmsg_send_fmu = true;
    }
    else if (cellCanID == 0xA81302 && (data[0] == 0xF || data[0] == 0xE))
    {
        if (data[0] == 0xF)
            DM_4DRADARMAG.get_ground_filter_4DF = true;
        DM_4DRADARMAG.ground_filter_4DF = data[1] + data[2] * 256;
        DM4Dmsg_send_fmu = true;
    }
    else if (cellCanID == 0xA81302 && (data[0] == 0xA || data[0] == 0xB))
    {
        if (data[0] == 0xB)
            DM_4DRADARMAG.get_dotcloud_switch_4DF = true;
        DM_4DRADARMAG.dotcloud_switch_4DF = data[1] + data[2] * 256;
        DM4Dmsg_send_fmu = true;
    }
    else if(cellCanID == 0xA81302 && (data[0] == 0x1))
    {

    }
}
int16_t B_4DRadar[3][3] = {0}; // X Y Z
DM_4dFRADAR DM_B4d;
int dm_4dB_i = 0;
// DM_4DRADAR FMU_4DB_info;
uint8_t DM4dB_recv_flag = 0;
void DM_Bobs_recieved_hookfuction(uint32_t cellCanID, uint8_t data[], uint8_t len)
{
    // 4D后避障雷达协议 点云
    if (cellCanID == 0XB01310)
    {
        // DM_4dstatus.connect_status = COMP_NORMAL;
        // DM_4dstatus.recv_time = HAL_GetTick();
        Dev.Part_radarB.facid = FAC_DM_RB_4D;

        DM_B4d.byte7.frame_flag = data[7];

        if (DM_B4d.byte7.flag.head != 0) // 头
        {
            memcpy(&DM_B4d.target_num, &data[0], 7);
            dm_4dB_i = 0;
        }
        else if (DM_B4d.byte7.flag.tail != 0) // 尾
        {
            if (DM_B4d.target_num != 1)
            {
                if ((DM_B4d.target_num * 5) % 7 != 0)
                {
                    memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], (DM_B4d.target_num * 5) % 7);
                    dm_4dB_i += (DM_B4d.target_num * 5) % 7;
                }
                else
                {
                    memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], 7);
                    dm_4dB_i += 7;
                }
            }
            else
            {
                memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], 5);
                dm_4dB_i += 5;
            }
            if (DM_4dstatus.connect_status != COMP_NORMAL)
            {
                if (DM_B4d.crc == Get_Crc16(&DM_B4d.RawData[0], DM_B4d.target_num * 5) && DM4dB_recv_flag == 0)
                {
                    FMU_4D_info.target_num = DM_B4d.target_num;
                    memcpy(&FMU_4D_info.buf, &DM_B4d.RawData, DM_B4d.target_num * 5);
                    F4d_send_flag = true;
                }
            }
        }
        else
        {
            memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], 7);
            dm_4dB_i += 7;
        }
        if (dm_4dB_i >= 254 * 5)
        {
            dm_4dB_i = 0;
        }
    }
    // 4D后避障雷达协议 单点
    if (cellCanID == 0XB01302)
    {
        B_4DRadar[0][0] = data[1] + data[2] * 256;
        B_4DRadar[0][1] = data[3] + data[4] * 256;
        B_4DRadar[0][2] = data[5] + data[6] * 256;

        Dev.Part_Bradar_Link.recv_time = HAL_GetTick();
        Dev.Part_Bradar_Link.connect_status = COMP_NORMAL;
        Dev.Part_radarB.facid = FAC_DM_RB_4D;
    }
    else if (cellCanID == 0XB01303)
    {
        B_4DRadar[1][0] = data[1] + data[2] * 256;
        B_4DRadar[1][1] = data[3] + data[4] * 256;
        B_4DRadar[1][2] = data[5] + data[6] * 256;
    }
    else if (cellCanID == 0XB01304)
    {
        B_4DRadar[2][0] = data[1] + data[2] * 256;
        B_4DRadar[2][1] = data[3] + data[4] * 256;
        B_4DRadar[2][2] = data[5] + data[6] * 256;
    }
    else if (cellCanID == 0xB81302 && (data[0] == 0xA || data[0] == 0xB))
    {
        if (data[0] == 0xB)
            DM_4DRADARMAG.get_dotcloud_switch_4DB = true;
        DM_4DRADARMAG.dotcloud_switch_4DB = data[1] + data[2] * 256;
        DM4Dmsg_send_fmu = true;
    }
    else if (cellCanID == 0xB81302 && (data[0] == 0xD || data[0] == 0xC))
    {
        if (data[0] == 0xD)
            DM_4DRADARMAG.get_angel_4DB = true;
        DM_4DRADARMAG.angel_4DB = data[1] + data[2] * 256;
        DM4Dmsg_send_fmu = true;
    }
    else if (cellCanID == 0xB81302 && (data[0] == 0xF || data[0] == 0xE))
    {
        if (data[0] == 0xF)
            DM_4DRADARMAG.get_ground_filter_4DB = true;
        DM_4DRADARMAG.ground_filter_4DB = data[1] + data[2] * 256;
        DM4Dmsg_send_fmu = true;
    }
}