#ifndef _WIN32
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include "livox_sdk.h"
#include "open3d/Open3D.h"
#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>

using namespace open3d;

typedef enum {
    kDeviceStateDisconnect = 0,
    kDeviceStateConnect = 1,
    kDeviceStateSampling = 2,
} DeviceState;

typedef struct {
    uint8_t handle;
    DeviceState device_state;
    DeviceInfo info;
} DeviceItem;

DeviceItem devices[kMaxLidarCount];
int lidar_count = 0;

/** 回调函数声明 */
void OnLidarErrorStatusCallback(livox_status status, uint8_t handle, ErrorMessage *message);
void GetLidarData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data);
void MyGetLidarData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data);
void OnSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data);
void OnCommonCommandCallback(livox_status status, uint8_t handle, uint8_t response, void *data);
void OnStopSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data);
void OnDeviceInfoChange(const DeviceInfo *info, DeviceEvent type);
void OnDeviceBroadcast(const BroadcastDeviceInfo *info);

/** 用于Python的回调函数指针 */
void (*python_data_callback)(uint8_t, uint32_t *, uint32_t) = NULL;
void (*python_error_callback)(livox_status, uint8_t, ErrorMessage*) = NULL;
void (*python_device_change_callback)(const DeviceInfo*, DeviceEvent) = NULL;
void (*python_device_broadcast_callback)(const BroadcastDeviceInfo*) = NULL;
void (*python_common_command_callback)(livox_status, uint8_t, uint8_t) = NULL;

extern "C" {

/** 初始化Livox SDK */
bool init_sdk() {
    printf("Livox SDK initializing.\n");
    if (!Init()) {
        return false;
    }
    printf("Livox SDK has been initialized.\n");
    LivoxSdkVersion _sdkversion;
    GetLivoxSdkVersion(&_sdkversion);
    printf("Livox SDK version %d.%d.%d .\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);

    memset(devices, 0, sizeof(devices));
    return true;
}

/** 启动设备扫描 */
bool start_discovery() {
    SetBroadcastCallback(OnDeviceBroadcast);
    SetDeviceStateUpdateCallback(OnDeviceInfoChange);

    if (!Start()) {
        printf("Failed to start device discovery.\n");
        Uninit();
        return false;
    }
    printf("Started device discovery.\n");
    return true;
}

/** 停止SDK并清理 */
void stop_sdk() {
    printf("Stopping Livox SDK.\n");
    for (int i = 0; i < kMaxLidarCount; ++i) {
        if (devices[i].device_state == kDeviceStateSampling) {
            LidarStopSampling(devices[i].handle, OnStopSampleCallback, NULL);
        }
    }
    Uninit();
}

int connect(const char *broadcast_code) {
    uint8_t handle = 0;
    livox_status result = AddLidarToConnect(broadcast_code, &handle);
    if (result == kStatusSuccess) {
        /** Set the point cloud data for a specific Livox LiDAR. */
        // SetDataCallback(handle, GetLidarData, NULL);
        SetDataCallback(handle, MyGetLidarData, NULL);
        
    }
    return result;
}

int start_sampling(uint8_t handle) {
    livox_status result = LidarStartSampling(handle, OnSampleCallback, NULL);
    devices[handle].device_state = kDeviceStateSampling;
    return result;
}

int stop_sampling(uint8_t handle) {
    livox_status result = LidarStopSampling(handle, OnSampleCallback, NULL);
    devices[handle].device_state = kDeviceStateSampling;
    return result;
}

int set_mode(LidarMode mode) {
    return LidarSetMode(devices[0].handle, mode, OnCommonCommandCallback, NULL);
}

/** 注册数据回调函数给Python调用 */
void register_data_callback(void (*data_callback)(uint8_t handle, uint32_t *data, uint32_t data_num)) {
    python_data_callback = data_callback;
}

/** 注册错误回调函数给Python调用 */
void register_error_callback(void (*error_callback)(livox_status status, uint8_t handle, ErrorMessage *message)) {
    python_error_callback = error_callback;
}

/** 注册设备状态变化回调给Python调用 */
void register_device_change_callback(void (*device_change_callback)(const DeviceInfo* info, DeviceEvent type)) {
    python_device_change_callback = device_change_callback;
}

/** 注册设备广播回调给Python调用 */
void register_device_broadcast_callback(void (*device_broadcast_callback)(const BroadcastDeviceInfo *info)) {
    python_device_broadcast_callback = device_broadcast_callback;
}

/** 注册设备广播回调给Python调用 */
void register_common_command_callback(void (*common_command_callback)(livox_status status, uint8_t handle, uint8_t response)) {
    python_common_command_callback = common_command_callback;
}

}

/** 回调函数的定义 */
void OnLidarErrorStatusCallback(livox_status status, uint8_t handle, ErrorMessage *message) {
    if (python_error_callback) {
        python_error_callback(status, handle, message);
    }
}

void OnCommonCommandCallback(livox_status status, uint8_t handle, uint8_t response, void *data) {
    if (python_common_command_callback) {
        python_common_command_callback(status, handle, response);
    }
}

void GetLidarData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data) {
    if (data) {
        /** Parsing the timestamp and the point cloud data. */
        uint64_t cur_timestamp = *((uint64_t *)(data->timestamp));
        if(data ->data_type == kCartesian) {
            LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
        }else if ( data ->data_type == kSpherical) {
            LivoxSpherPoint *p_point_data = (LivoxSpherPoint *)data->data;
        }else if ( data ->data_type == kExtendCartesian) {
            LivoxExtendRawPoint *p_point_data = (LivoxExtendRawPoint *)data->data;
            LivoxExtendRawPoint point_array[data_num];
            uint32_t *converted_data = (uint32_t *)malloc(data_num * 4 * sizeof(uint32_t));
            for (uint32_t i = 0; i < data_num; ++i) {
                LivoxExtendRawPoint *point = &p_point_data[i];
                // 将结构体的 x, y, z 放入 uint32_t 数组中
                converted_data[i * 4] = point->x;
                converted_data[i * 4 + 1] = point->y;
                converted_data[i * 4 + 2] = point->z;
                // 将 reflectivity 和 tag 合并成一个 uint32_t（高16位和低16位）
                converted_data[i * 4 + 3] = (uint32_t)(point->reflectivity << 8 | point->tag);
            }
            // 调用 Python 回调函数
            if (python_data_callback) {
                python_data_callback(handle, converted_data, data_num);
            }
        }else if ( data ->data_type == kExtendSpherical) {
            LivoxExtendSpherPoint *p_point_data = (LivoxExtendSpherPoint *)data->data;
        }else if ( data ->data_type == kDualExtendCartesian) {
            LivoxDualExtendRawPoint *p_point_data = (LivoxDualExtendRawPoint *)data->data;
        }else if ( data ->data_type == kDualExtendSpherical) {
            LivoxDualExtendSpherPoint *p_point_data = (LivoxDualExtendSpherPoint *)data->data;
        }else if ( data ->data_type == kImu) {
            LivoxImuPoint *p_point_data = (LivoxImuPoint *)data->data;
        }else if ( data ->data_type == kTripleExtendCartesian) {
            LivoxTripleExtendRawPoint *p_point_data = (LivoxTripleExtendRawPoint *)data->data;
        }else if ( data ->data_type == kTripleExtendSpherical) {
            LivoxTripleExtendSpherPoint *p_point_data = (LivoxTripleExtendSpherPoint *)data->data;
        }
        // printf("data_type %d\n", data->data_type);
    }
}

void OnSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data) {
    if (status == kStatusSuccess && response != 0) {
        devices[handle].device_state = kDeviceStateConnect;
    } else if (status == kStatusTimeout) {
        devices[handle].device_state = kDeviceStateConnect;
    }
}

void OnStopSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data) {
    // 停止采样时的回调处理
}

/** Query the firmware version of Livox LiDAR. */
void OnDeviceInformation(livox_status status, uint8_t handle, DeviceInformationResponse *ack, void *data) {
    if (status != kStatusSuccess) {
        printf("Device Query Informations Failed %d\n", status);
    }
    if (ack) {
        printf("firm ver: %d.%d.%d.%d\n",
            ack->firmware_version[0],
            ack->firmware_version[1],
            ack->firmware_version[2],
            ack->firmware_version[3]);
    }
}

void LidarConnect(const DeviceInfo *info) {
    uint8_t handle = info->handle;
    QueryDeviceInformation(handle, OnDeviceInformation, NULL);
    if (devices[handle].device_state == kDeviceStateDisconnect) {
        devices[handle].device_state = kDeviceStateConnect;
        devices[handle].info = *info;
    }
}

void LidarDisConnect(const DeviceInfo *info) {
    uint8_t handle = info->handle;
    devices[handle].device_state = kDeviceStateDisconnect;
}

void LidarStateChange(const DeviceInfo *info) {
    uint8_t handle = info->handle;
    devices[handle].info = *info;
}

void OnDeviceInfoChange(const DeviceInfo *info, DeviceEvent type) {
    if (info == NULL) {
        return;
    }

    uint8_t handle = info->handle;
    if (handle >= kMaxLidarCount) {
        return;
    }
    if (type == kEventConnect) {
        LidarConnect(info);
        printf("[WARNING] Lidar sn: [%s] Connect!!!\n", info->broadcast_code);
    } else if (type == kEventDisconnect) {
        LidarDisConnect(info);
        printf("[WARNING] Lidar sn: [%s] Disconnect!!!\n", info->broadcast_code);
    } else if (type == kEventStateChange) {
        LidarStateChange(info);
        printf("[WARNING] Lidar sn: [%s] StateChange!!!\n", info->broadcast_code);
    }
    printf("Device Working State %d\n", devices[handle].info.state);
    if (devices[handle].device_state == kDeviceStateConnect) {
        if (devices[handle].info.state == kLidarStateInit) {
            printf("Device State Change Progress %u\n", devices[handle].info.status.progress);
        } else {
            printf("Device State Error Code 0X%08x\n", devices[handle].info.status.status_code.error_code);
        }
        printf("Device feature %d\n", devices[handle].info.feature);
        SetErrorMessageCallback(handle, OnLidarErrorStatusCallback);
        if (devices[handle].info.state == kLidarStateNormal) {
            LidarStartSampling(handle, OnSampleCallback, NULL);
            devices[handle].device_state = kDeviceStateSampling;
        }
    }
    if (python_device_change_callback) {
        python_device_change_callback(info, type);
    }
}

void OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
    // if (python_device_broadcast_callback) {
    //     python_device_broadcast_callback(info);
    // }
    printf("connect to device %d \n", info->broadcast_code);
    connect(info->broadcast_code);
}

static void delay_second(double second)
{
	clock_t start_time;
	start_time = clock();
	for (; (clock() - start_time) < second * CLOCKS_PER_SEC;);
}

#ifndef _WIN32
static bool bCouldIsRecv = false;
static bool bCouldIsGet = false;
static uint32_t need_points_size = 120 * 10000;
static float points_data[240 * 10000 * 4];
static uint8_t points_param_data[240 * 10000 * 4];
static std::atomic<uint32_t> cur_point_num(0);
void MyGetLidarData(uint8_t handle, LivoxEthPacket* data, uint32_t data_num, void* client_data)
{
	if (data) {
		/** Parsing the timestamp and the point cloud data. */		uint64_t cur_timestamp = *((uint64_t*)(data->timestamp));
		if (data->data_type == kCartesian) {
			LivoxRawPoint* p_point_data = (LivoxRawPoint*)data->data;
		}
		else if (data->data_type == kSpherical) {
			LivoxSpherPoint* p_point_data = (LivoxSpherPoint*)data->data;
		}
		else if (data->data_type == kExtendCartesian) {
			LivoxExtendRawPoint* p_point_data = (LivoxExtendRawPoint*)data->data;
#ifdef _WIN32
			LivoxExtendRawPoint point_array[10 * 10000];
#else
			LivoxExtendRawPoint point_array[data_num];
#endif
			//uint32_t* converted_data = (uint32_t*)malloc(data_num * 4 * sizeof(uint32_t));
			if (!bCouldIsRecv)
			{
				// memory_order_seq_cst
                // memory_order_relaxed
                uint32_t _cur_point_num = cur_point_num.fetch_add(1, std::memory_order_seq_cst);
				_cur_point_num = _cur_point_num * data_num;
				// need_points_size = G(cloud_need_points_size);
				uint32_t _offset = _cur_point_num * 4;

				for (uint32_t i = 0; i < data_num; ++i) 
				{
					_offset = _offset + i * 4;
					points_data[_offset] = p_point_data[i].x / 1000.0;
					points_data[_offset + 1] = p_point_data[i].y / 1000.0;
					points_data[_offset + 2] = p_point_data[i].z / 1000.0;
					// points_data[_offset + 3] = (uint32_t)(p_point_data[i].reflectivity << 8 | p_point_data[i].tag);
                    points_param_data[_offset] = p_point_data[i].reflectivity;
                    points_param_data[_offset + 1] = p_point_data[i].tag;
                }

				uint32_t _cur_count = _cur_point_num + data_num;
				if ((_cur_count % 100000) <= 100)
					std::cout << _cur_count << "  need data size : " << need_points_size  << std::endl;
				if (_cur_count < need_points_size)
					return;
				else
					bCouldIsRecv = true; 
			}
			else
				return;
			
			// 等待点云拷贝结束
			//__p_point_data = p_point_data;
			//__data_num = data_num;
			while (true)
			{
				if (!bCouldIsGet)
				{
					delay_second(0.01);
					continue;
				}
				cur_point_num = 0;
				uint32_t* p = (uint32_t*)points_data;
				memset(p , 0, need_points_size * 4);
				bCouldIsGet = false;
				break;
			}
		}
		else if (data->data_type == kExtendSpherical) {
			LivoxExtendSpherPoint* p_point_data = (LivoxExtendSpherPoint*)data->data;
		}
		else if (data->data_type == kDualExtendCartesian) {
			LivoxDualExtendRawPoint* p_point_data = (LivoxDualExtendRawPoint*)data->data;
		}
		else if (data->data_type == kDualExtendSpherical) {
			LivoxDualExtendSpherPoint* p_point_data = (LivoxDualExtendSpherPoint*)data->data;
		}
		else if (data->data_type == kImu) {
			LivoxImuPoint* p_point_data = (LivoxImuPoint*)data->data;
		}
		else if (data->data_type == kTripleExtendCartesian) {
			LivoxTripleExtendRawPoint* p_point_data = (LivoxTripleExtendRawPoint*)data->data;
		}
		else if (data->data_type == kTripleExtendSpherical) {
			LivoxTripleExtendSpherPoint* p_point_data = (LivoxTripleExtendSpherPoint*)data->data;
		}
		// printf("data_type %d\n", data->data_type);
	}
}
#else
/** Extend cartesian coordinate format. */
typedef struct {
	int32_t x;            /**< X axis, Unit:mm */
	int32_t y;            /**< Y axis, Unit:mm */
	int32_t z;            /**< Z axis, Unit:mm */
	uint8_t reflectivity; /**< Reflectivity */
	uint8_t tag;          /**< Tag */
} LivoxExtendRawPoint;
typedef struct {
	uint8_t version;              /**< Packet protocol version. */
	uint8_t slot;                 /**< Slot number used for connecting LiDAR. */
	uint8_t id;                   /**< LiDAR id. */
	uint8_t rsvd;                 /**< Reserved. */
	uint32_t err_code;      /**< Device error status indicator information. */
	uint8_t timestamp_type;       /**< Timestamp type. */
	/** Point cloud coordinate format, refer to \ref PointDataType . */
	uint8_t data_type;
	uint8_t timestamp[8];         /**< Nanosecond or UTC format timestamp. */
	uint8_t data[1];              /**< Point cloud data. */
} LivoxEthPacket;
static bool bCouldIsRecv = false;
static bool bCouldIsGet = false;
static uint32_t need_points_size = 200 * 10000;
static float points_data[240 * 10000 * 4];
//static uint32_t cur_data_num = 0;
static std::atomic<uint32_t> cur_point_num(0);
typedef void (*FuncGetLidarData)(uint8_t handle, LivoxEthPacket* data, uint32_t data_num, void* client_data);
FuncGetLidarData GetLidarData;
#endif // !_WIN32

// 定义一个结构体来保存每个点的信息，包括位置和自定义属性
struct CustomPoint {
    Eigen::Vector3d position;
    uint32_t reflectivity;
};

static void WriteCustomPLY(const std::string& filename, const std::vector<CustomPoint>& points) {
    std::ofstream file(filename);
    if (!file.is_open()) {
        utility::LogError("Failed to open file: {}", filename);
        return;
    }

    // 写入 PLY 文件头
    file << "ply\n";
    file << "format ascii 1.0\n";
    file << "element vertex " << points.size() << "\n";
    file << "property float x\n";
    file << "property float y\n";
    file << "property float z\n";
    file << "property int reflectivity\n"; // 自定义属性
    // file << "property float custom_float_attribute\n"; // 声明为 float 类型
    file << "end_header\n";

    // 设置输出精度为6个小数位（适合 float 类型）
    // file << std::fixed << std::setprecision(4);

    // 写入点的数据
    for (const auto& point : points) {
        file << point.position[0] << " "
             << point.position[1] << " "
             << point.position[2] << " "
             << point.reflectivity << "\n";
    }

    file.close();
}

int main(int argc, char *argv[])
{
    if (argc == 0)
	{
		printf("argc == 0 [%d]\n", -1);
	}
    int cloud_points_count = atoi(argv[1]);
	if (cloud_points_count < 30 * 10000)
	{
		printf("cloud_points_count is too small == [%d]\n", cloud_points_count);
		return -1;
	}
    if (cloud_points_count > 200 * 10000)
	{
		printf("cloud_points_count is too big == [%d]\n", cloud_points_count);
		return -1;
	}
	printf("cloud_points_count ==  [%d]\n", cloud_points_count);
    need_points_size = cloud_points_count;

    int ret = 0;
    bool succ = init_sdk();
    if (!succ)
    {
        printf("start_discovery init_sdk is %d \n", -1);
        return -1;
    }
        
    succ = start_discovery();
    if (!succ)
    {
        printf("start_discovery error is %d \n", -1);
        return -1;
    }

    delay_second(2);
        
    ret = start_sampling(0);
    if (ret != 0)
    {
        printf("start_sampling error is %d \n", ret);
        return -1;
    }


    // 写入点云对象
    geometry::PointCloud* pCloud = new geometry::PointCloud();
    geometry::PointCloud* pCloud_data = new geometry::PointCloud();

    while (true)
    {
        if (!bCouldIsRecv)
        {
            delay_second(0.05);
            continue;
        }
        delay_second(0.01);
        break;
    }

    // pCloud->points_.resize(need_points_size);
    // pCloud_data->points_.resize(need_points_size);
    // uint32_t effect_cnt = 0;
    // for (uint32_t i = 0; i < need_points_size; ++i) {
    //     char tag = points_param_data[i * 4 + 1];
    //     if (tag & 0x03 != 0 || 
    //         tag >> 2 & 0x03 != 0 ||
    //         tag >> 4 & 0x03 == 0 || 
    //         tag >> 6 & 0x03 != 0)
    //         continue;

    //     pCloud->points_[i][0] = points_data[i * 4];
    //     pCloud->points_[i][1] = points_data[i * 4 + 1];
    //     pCloud->points_[i][2] = points_data[i * 4 + 2];
    //     //pCloud->points_.push_back({ (float)point[0], (float)point[1], (float)point[2] });
    //     memcpy(&pCloud_data->points_[i][0], (char*)&points_param_data[i * 4], 1);
    //     memcpy(&pCloud_data->points_[i][1], (char*)&points_param_data[i * 4 + 1], 1);
    //     pCloud_data->points_[i][2] = 0.0;

    //     effect_cnt++;
    // }

    // io::WritePointCloud("output.ply", *pCloud);
    // io::WritePointCloud("output_data.ply", *pCloud_data);
    // printf("effect points is %d \n", effect_cnt);
        
    std::vector<CustomPoint> points;
    CustomPoint curPt;
    uint32_t effect_cnt = 0;
    for (uint32_t i = 0; i < need_points_size; ++i) {
        char tag = points_param_data[i * 4 + 1];
        if (tag & 0x03 != 0 || 
            tag >> 2 & 0x03 != 0 ||
            tag >> 4 & 0x03 == 0 || 
            tag >> 6 & 0x03 != 0 ||
            (points_data[i * 4] == 0.0 && 
            points_data[i * 4 + 1] == 0.0 &&
            points_data[i * 4 + 2] == 0.0 ))
            continue;

        curPt.position[0] = (float)points_data[i * 4];
        curPt.position[1] = (float)points_data[i * 4 + 1];
        curPt.position[2] = (float)points_data[i * 4 + 2];
        curPt.reflectivity = points_param_data[i * 4];
        // curPt.tag = points_param_data[i * 4 + 1];
        points.push_back(curPt);
        effect_cnt++;
    }
    printf("effect points is %d \n", effect_cnt);
    WriteCustomPLY("output.ply", points);

    bCouldIsGet = true;
    delay_second(0.01);
    bCouldIsRecv = false; //开始接收数据
    
    ret = stop_sampling(0);
    if (ret != 0)
    {
        printf("stop_sampling error is %d \n", ret);
        return ret;
    }

	return 0;
}
#endif