#include "livox_mid70_sdk.h"
#include "livox_mid70_def.h"
#include <iostream>
#include <thread>
#include <chrono>
#include <cstring>

// 设备状态枚举
typedef enum {
    kDeviceStateDisconnect = 0,
    kDeviceStateConnect = 1,
    kDeviceStateSampling = 2,
} Livox_mid70_DeviceState;

// 设备信息结构
typedef struct {
    uint8_t handle;
    Livox_mid70_DeviceState device_state;
    DeviceInfo info;
} Livox_mid70_DeviceItem;

// 全局变量
Livox_mid70_DeviceItem mid70_devices[kMaxLidarCount];
const char* livox_mid70_broadcast_code = "6565N8V00200631"; // 请替换为实际雷达广播码

// 设备广播信息回调
void OnMid70DeviceBroadcast(const BroadcastDeviceInfo* info) {
    if (info == nullptr) return;
    printf("发现设备广播: %s\n", info->broadcast_code);
}

// 设备状态变化回调
void OnMid70DeviceInfoChange(const DeviceInfo* info, DeviceEvent type) {
    if (info == nullptr) return;

    uint8_t handle = info->handle;
    if (handle >= kMaxLidarCount) return;

    if (type == kEventConnect) {
        mid70_devices[handle].device_state = kDeviceStateConnect;
        mid70_devices[handle].info = *info;
        printf("设备连接成功，句柄: %d\n", handle);
    }
    else if (type == kEventDisconnect) {
        mid70_devices[handle].device_state = kDeviceStateDisconnect;
        printf("设备断开连接，句柄: %d\n", handle);
    }
}

// 点云数据回调函数
void GetMid70LidarData(uint8_t handle, LivoxEthPacket* data, uint32_t data_num, void* client_data) {
    if (data == nullptr) return;

    switch (data->data_type) {
    case kCartesian: {
        LivoxRawPoint* points = (LivoxRawPoint*)data->data;
        for (uint32_t i = 0; i < data_num; i++) {
            float x = points[i].x / 1000.0f;
            float y = points[i].y / 1000.0f;
            float z = points[i].z / 1000.0f;
            float reflectivity = points[i].reflectivity;

            printf("点云坐标: (%.3f, %.3f, %.3f), 反射率: %d\n", x, y, z, (int)reflectivity);
        }
        break;
    }

    case kSpherical: {
        LivoxSpherPoint* spherical_points = (LivoxSpherPoint*)data->data;
        for (uint32_t i = 0; i < data_num; i++) {
            float depth = spherical_points[i].depth / 1000.0f;
            float theta = spherical_points[i].theta / 100.0f;
            float phi = spherical_points[i].phi / 100.0f;
            float reflectivity = spherical_points[i].reflectivity;

            printf("球坐标: 距离=%.3fm, 方位角=%.2f°, 俯仰角=%.2f°, 反射率: %d\n",
                depth, theta, phi, (int)reflectivity);
        }
        break;
    }

    case kExtendCartesian: {
        // 扩展笛卡尔坐标系数据
        LivoxExtendRawPoint* extend_points = (LivoxExtendRawPoint*)data->data;
        for (uint32_t i = 0; i < data_num; i++) {
            float x = extend_points[i].x / 1000.0f;
            float y = extend_points[i].y / 1000.0f;
            float z = extend_points[i].z / 1000.0f;
            float reflectivity = extend_points[i].reflectivity;
            if (reflectivity > 0)
            {
                std::cout << "扩展笛卡尔坐标: (" << x << ", " << y << ", " << z << "), 反射: " << reflectivity << "   , 标签: " << (int)extend_points[i].tag << std::endl;

            }
            //
        }
        break;
    }

    default:
        printf("未知数据类型: %d\n", (int)data->data_type);
        break;
    }
}

int main() {
    printf("=== Livox Mid-70雷达点云采集程序 ===\n");
    printf("开始初始化Livox Mid-70雷达...\n");

    // 1. 初始化SDK
    if (!Init()) {
        printf("SDK初始化失败!\n");
        return -1;
    }
    printf("Livox SDK初始化完成\n");

    // 显示SDK版本信息
    LivoxSdkVersion livox_mid70_sdk_version;
    GetLivoxSdkVersion(&livox_mid70_sdk_version);
    printf("Livox SDK版本 %d.%d.%d\n", livox_mid70_sdk_version.major, livox_mid70_sdk_version.minor, livox_mid70_sdk_version.patch);

    // 初始化设备数组
    memset(mid70_devices, 0, sizeof(mid70_devices));

    // 2. 设置回调函数
    SetBroadcastCallback(OnMid70DeviceBroadcast);
    SetDeviceStateUpdateCallback(OnMid70DeviceInfoChange);

    // 3. 开始设备发现
    if (!Start()) {
        printf("启动设备发现失败!\n");
        Uninit();
        return -1;
    }
    printf("开始设备发现...\n");

    // 4. 等待设备准备就绪
    printf("等待设备初始化...\n");
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // 5. 添加设备连接
    uint8_t livox_mid70_handle = 0;
    livox_status result = AddLidarToConnect(livox_mid70_broadcast_code, &livox_mid70_handle);
    if (result == kStatusSuccess) {
        mid70_devices[livox_mid70_handle].handle = livox_mid70_handle;
        mid70_devices[livox_mid70_handle].device_state = kDeviceStateDisconnect;
        printf("添加设备成功，句柄: %d\n", livox_mid70_handle);
    }
    else {
        printf("添加设备失败! 请检查:\n");
        printf("1. 雷达是否上电\n");
        printf("2. 网络连接是否正常\n");
        printf("3. 广播码是否正确\n");
        Uninit();
        return -1;
    }

    // 6. 等待设备连接
    printf("等待设备连接...\n");
    int wait_count = 0;
    while (mid70_devices[livox_mid70_handle].device_state != kDeviceStateConnect && wait_count < 10) {
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        wait_count++;
    }

    if (mid70_devices[livox_mid70_handle].device_state != kDeviceStateConnect) {
        printf("设备连接超时!\n");
        Uninit();
        return -1;
    }

    // 7. 设置点云数据回调
    SetDataCallback(livox_mid70_handle, GetMid70LidarData, nullptr);
    printf("设置数据回调完成\n");

    // 8. 启动采样
    livox_status status = LidarStartSampling(livox_mid70_handle, nullptr, nullptr);
    if (status == kStatusSuccess) {
        printf("开始点云数据采集...\n");
    }
    else {
        printf("启动采样失败! 错误码: %d\n", status);
        Uninit();
        return -1;
    }

    // 9. 保持程序运行
    printf("程序运行中，采集10秒后自动停止...\n");
    printf("======================================\n");

    for (int i = 0; i < 10; i++) {
        printf("运行时间: %d秒\n", i + 1);
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }

    // 10. 停止采样并清理资源
    printf("停止采样...\n");
    LidarStopSampling(livox_mid70_handle, nullptr, nullptr);
    Uninit();

    printf("程序正常结束\n");
    return 0;
}