#include <iostream>
#include <unistd.h>
#include <fstream>
#include <thread>
// #include "device_factory.hpp"
#include "tool/point_tool.h"
#include "conversion.hpp"
#include "pahoc/mqtt_async.hpp"

// int main(int argc, char** argv) {
//     std::fstream txtFile;
//     // LidarDevice* lidar = DeviceFactory::createLidar(DeviceFactory::LIDAR_RSLIDAR);
//     pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> cloud = ReadTxtCloud("../cloud.txt");
//     printf("---Debug Get cloud.txt\n");
//     std::string data;
//     Conversion::PackStringPointCloud(cloud, data);
//     // cloud->clear();
//     // Conversion.UnPackStringPointCloud(data, cloud);
//     unsigned int length = cloud->size() * sizeof(float) * 3 + 4;
//     // unsigned char result[length] = {};
//     unsigned char hex_data[length - 4] = {};
//     // Conversion.PackHexPointCloud(cloud, result, sizeof(result));
//     Conversion::String2Hex(data, hex_data, sizeof(hex_data));
//     printf("---Debug length = %d", length);
//     printf("---Debug hex_data[0] = %#X ", hex_data[0]);
//     printf("---Debug hex_data[1] = %#X ", hex_data[1]);
//     printf("---Debug hex_data[2] = %#X\n", hex_data[2]);
//     cloud->clear();
//     if (!Conversion::UnPackHexPointCloud(hex_data, cloud)) {
//         txtFile.open("txtFile", std::ios_base::out | std::ios::app);
//         txtFile.write((char*)hex_data, sizeof(hex_data));
//         txtFile.close();
//     }
//     // Conversion.PointCloud(cloud, result, length);
//     printf("---Debug End cloud.txt\n");
//     return 0;
// }

const std::string address = "tcp://localhost:1883";
// const std::string address = "mqtt://192.178.2.132";
const std::string send_id = "send";
const std::string receive_id = "receive";
const int port = 1883;
const std::string topic = "data/time";

int call_back(void* context, char* topicName, int topicLen, MQTTAsync_message* message) {
    std::fstream txtFile;
    txtFile.open("txtFile", std::ios_base::out | std::ios::app);
    txtFile.write((const char*)message->payload, message->payloadlen);
    txtFile.close();
    return 1;
}

void send_func() {
    MqttAsyncClient client;

    client.onConnect(address, send_id, port);
    while (true)
    {
        sleep(1);
        pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> cloud = ReadTxtCloud("../cloud.txt");
        unsigned int length = cloud->size() * sizeof(float) * 3 + 4;
        unsigned char result[length] = {};
        Conversion::PackHexPointCloud(cloud, result, sizeof(result));
        client.onSendMessage(topic.c_str(), result, sizeof(result));
    }
    
}

void receive_func() {
    MqttAsyncClient client;
    client.onConnect(address, receive_id, port);
    while(client.onSubscribe(topic.c_str()) == false) {
        sleep(1);
    }
    client.setCallback(call_back);
    while (true)
    {
        sleep(1);
    }
}

int main(int argc, char** argv) {
    printf("Begain.\n");
    std::thread send(send_func);
    std::thread receive(receive_func);
    while (true)
    {
        sleep(1);
    }
    
    
}