#include "project_marco.h"
#include "rs71_usb_receiver.h"

struct TargetListHeader
{
    uint8_t object_num; // 目标数量
    uint16_t measurement_count; // 循环计数
    uint8_t interface_version; // CAN接口版本号，默认为0
};

struct TargetInfo
{
    uint8_t id; // 目标ID
    float distance_long; // 目标纵向距离 (m)
    float distance_lat; // 目标横向距离 (m)
    float velocity_long; // 目标纵向速度 (m/s)
    uint8_t dyn_prop; // 动态属性
    float velocity_lat; // 目标横向速度 (m/s)
    float rcs; // RCS
};

int main()
{
    RS71UsbReceiver dev(3, 0, 0x1, 0x1c00); // 参数依次为 DevType, DevIdx, ChMask, Baud

    if (!dev.Open())
        return -1;

    if (!dev.Start())
        return -1;

    while (true)
    {
        std::vector<RS71UsbReceiver::CanFrame> data = dev.GetReceivedData(0); // 获取第0路CAN数据
        printf("Frame Count: %zu\n", data.size());
        for (const auto &frame: data)
        {
            // 测试打印查看 -------
            printf("ID: 0x%X TimeStamp: %u Data:", frame.id, frame.timestamp);
            for (auto byte: frame.data)
            {
                printf(" %02X", byte);
            }
            printf("\n");
            // 测试打印查看 -------

            // 解析数据
            if (frame.id == 0x60B && frame.data.size() >= 8)
            {
                TargetInfo target;
                // 目标ID (字节0)
                target.id = frame.data[0];
                // 目标纵向距离
                target.distance_long = (frame.data[1] * 32 + (frame.data[2] >> 3)) * 0.1 - 500;
                // 目标横向距离
                target.distance_lat = ((frame.data[2] & 0x07) * 256 + frame.data[3]) * 0.1 - 102.3;
                // 目标纵向速度
                target.velocity_long = (frame.data[4] * 4 + (frame.data[5] >> 6)) * 0.25 - 128;
                // 目标横向速度
                target.velocity_lat = ((frame.data[5] & 0x3F) * 8 + (frame.data[6] >> 5)) * 0.25 - 64;
                // 动态属性
                target.dyn_prop = (frame.data[6] & 0x07);
                // RCS
                target.rcs = (frame.data[7] * 0.5) - 64;
                // 打印结果
                printf("Target ID: %u\n", target.id);
                printf("Distance Longitudinal: %.2fm\n", target.distance_long);
                printf("Distance Lateral: %.2fm\n", target.distance_lat);
                printf("Velocity Longitudinal: %.2fm/s\n", target.velocity_long);
                printf("Dynamic Property: %u\n", target.dyn_prop);
                printf("Velocity Lateral: %.2fm/s\n", target.velocity_lat);
                printf("RCS: %.2fdB㎡\n", target.rcs);

                // 计算目标径向距离和角度
                float radial_distance = sqrt(
                    target.distance_long * target.distance_long + target.distance_lat * target.distance_lat);
                float theta = atan2(target.distance_lat, target.distance_long) * 180 / M_PI; // 转换为度数
                printf("Radial Distance: %.2fm\n", radial_distance);
                printf("Angle (θ): %.2f°\n", theta);

                // 计算目标速度
                float velocity = target.velocity_long * cos(theta * M_PI / 180) + target.velocity_lat * sin(
                                     theta * M_PI / 180);
                printf("Velocity: %.2fm/s\n", velocity);
            } else if (frame.id == 0x60A)
            {
                std::cout << "-------------" << std::endl;
                TargetListHeader header;

                // 解析目标数量
                header.object_num = frame.data[0];
                printf("Object Number: %d\n", header.object_num);
                // 解析循环计数
                header.measurement_count = (static_cast<uint16_t>(frame.data[3]) << 8) | frame.data[2];
                printf("Measurement Count: %d\n", header.measurement_count);
                // // 解析CAN接口版本号
                // header.interface_version = (frame.data[4] >> 4) & 0x0F;
                // printf("Interface Version: %d\n", header.interface_version);
            }
        }
        std::this_thread::sleep_for(std::chrono::microseconds(20));
    }

    dev.Stop();
    dev.Close();
    return 0;
}