#include "common.h"
#include "soft_usharpradar.h"
#include <stdbool.h>
#include <stdint.h>

typedef struct
{
    uint16_t obstacle0_dist;
    uint16_t obstacle1_dist;
    uint16_t obstacle2_dist;
    uint16_t obstacle3_dist;
    uint16_t obstacle4_dist;

    char obstacle0_snr;
    char obstacle1_snr;
    char obstacle2_snr;
    char obstacle3_snr;
    char obstacle4_snr;
} uSharp_Radar;

static uSharp_Radar uSharpRadarData;
static comp_status uSharpRadarLinkStatus = COMP_NOEXIST;

uint8_t usharpRadar_analysis(uint8_t *pdata, uint16_t length)
{
    uint16_t index = 0;

    // 如果不足 18 个数, 则不是一帧完整的协议
    if (length < 18)
    {
        return 0;
    }

    // 检查帧头
    if (pdata[index++] == 0xff && pdata[index++] == 0xff)
    {
        // 计算校验
        uint8_t checkSum = 0;
        for (uint8_t i = 0; i < 17; ++i)
        {
            checkSum += pdata[i];
        }
        checkSum = checkSum & 0xff;

        if (checkSum == pdata[17])
        {
            uSharpRadarData.obstacle0_dist = pdata[index++];
            uSharpRadarData.obstacle0_dist += ((uint16_t)pdata[index++] << 8);

            uSharpRadarData.obstacle1_dist = pdata[index++];
            uSharpRadarData.obstacle1_dist += ((uint16_t)pdata[index++] << 8);

            uSharpRadarData.obstacle2_dist = pdata[index++];
            uSharpRadarData.obstacle2_dist += ((uint16_t)pdata[index++] << 8);

            uSharpRadarData.obstacle3_dist = pdata[index++];
            uSharpRadarData.obstacle3_dist += ((uint16_t)pdata[index++] << 8);

            uSharpRadarData.obstacle4_dist = pdata[index++];
            uSharpRadarData.obstacle4_dist += ((uint16_t)pdata[index++] << 8);

            uSharpRadarData.obstacle0_snr = pdata[index++];
            uSharpRadarData.obstacle1_snr = pdata[index++];
            uSharpRadarData.obstacle2_snr = pdata[index++];
            uSharpRadarData.obstacle3_snr = pdata[index++];
            uSharpRadarData.obstacle4_snr = pdata[index++];

            return 1;
        }
        else
        {
            return 0;
        }
    }
    else
    {
        return 0;
    }
}

// @brief 找到最近的障碍距离
// @retval 最近的障碍物距离，若没有障碍物返回 -1, 有障碍则返回最小的障碍值
int usharpRadar_getObstacleDist(void)
{
    int obstacle_dist;
    // 如果 5 个障碍物都没有则返回false

    if (uSharpRadarData.obstacle0_snr > 5)
    {
        obstacle_dist = uSharpRadarData.obstacle0_dist;
    }
    else if (uSharpRadarData.obstacle1_snr > 5)
    {
        obstacle_dist = uSharpRadarData.obstacle1_dist;
    }
    else if (uSharpRadarData.obstacle2_snr > 5)
    {
        obstacle_dist = uSharpRadarData.obstacle2_dist;
    }
    else if (uSharpRadarData.obstacle3_snr > 5)
    {
        obstacle_dist = uSharpRadarData.obstacle3_dist;
    }
    else if (uSharpRadarData.obstacle4_snr > 5)
    {
        obstacle_dist = uSharpRadarData.obstacle4_dist;
    }
    else
    {
        obstacle_dist = -1;
    }

    return obstacle_dist;
}

// @brief 设置 usharp 雷达连接状态
// @param 连接状态
void usharpRadar_setLinkStatus(comp_status status)
{
    uSharpRadarLinkStatus = status;
}

// @brief 获取 usharp 雷达连接状态
// @retval usharp 雷达连接状态
comp_status usharpRadar_getLinkStatus(void) { return uSharpRadarLinkStatus; }