#include "mqtt_communication.h"

void Tof3DMqttAsyncClient::init(const std::string &file) {
    m_message_arrived_callback_ = CloudDataArrived;

    MqttAsyncClient::init_from_proto(file);

    condit = new Thread_condition();

    t = new std::thread(&Tof3DMqttAsyncClient::run, this);

    condit->notify_all(true);
}

// 识别结果数据包含识别结果、时间序号、具体识别信息
int Tof3DMqttAsyncClient::CloudDataArrived(MqttAsyncClient *client, char *topicName, int topicLen,
                                             MQTTAsync_message *message) {
    // 1、查看识别结果

    // 2、队列取数据
//    cv::Mat merge_mat = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
//    memcpy(merge_mat.data, message->payload, message->payloadlen);
//    cv::imshow("receive", merge_mat);
    LOG(INFO) << topicName;

    // 3、分离出四份点云

    // 4、发送对应队列

    return 1;
}

void Tof3DMqttAsyncClient::run() {

    std::this_thread::sleep_for(std::chrono::milliseconds(3000));
    auto t_start_time = std::chrono::steady_clock::now();
    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;

    while (condit->is_alive()) {
        condit->wait();
        cost = std::chrono::steady_clock::now() - t_start_time;
        std::this_thread::sleep_for(std::chrono::milliseconds(100 - std::min<int>(99, cost.count() * 1000)));
        t_start_time = std::chrono::steady_clock::now();

        if (condit->is_alive()) {
//            // 1、获取图片并合并
//            cv::Mat merge_mat = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
//            DeviceTof3D::DeviceTof3DSaveInfo devices_info[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
//            for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
//                devices_info[device_index] = DeviceTof3D::iter()->getDeviceSaveInfo((DeviceAzimuth) device_index);
//                devices_info[device_index].irMat.copyTo(merge_mat(cv::Rect((device_index & 0x1) * 640,
//                                                                           ((device_index & 0x2) >> 1) * 480, 640, 480)));
//            }
//            cv::imshow("merge_mat", merge_mat);
//            cv::waitKey(1);

//            // 2、信息放入队列
//            m_cloud_mutex.lock();
//            for (auto iter = m_lf_info.begin(); iter != m_lf_info.end();) {
//                long save_time = std::chrono::steady_clock::now().time_since_epoch().count() - iter->first;
//                if (save_time > 1000000000) {
//                    m_rf_info.erase(iter->first);
//                    m_lr_info.erase(iter->first);
//                    m_rr_info.erase(iter->first);
//                    iter = m_lf_info.erase(m_lf_info.find(iter->first));
//                } else {
//                    iter++;
//                }
//            }
//            long time_point = std::chrono::steady_clock::now().time_since_epoch().count();
//            m_lf_info.insert(std::pair<long, DeviceTof3D::DeviceTof3DSaveInfo>(
//                    time_point, devices_info[DeviceAzimuth::DEVICE_AZIMUTH_LF]));
//            m_rf_info.insert(std::pair<long, DeviceTof3D::DeviceTof3DSaveInfo>(
//                    time_point, devices_info[DeviceAzimuth::DEVICE_AZIMUTH_RF]));
//            m_lr_info.insert(std::pair<long, DeviceTof3D::DeviceTof3DSaveInfo>(
//                    time_point, devices_info[DeviceAzimuth::DEVICE_AZIMUTH_LR]));
//            m_rr_info.insert(std::pair<long, DeviceTof3D::DeviceTof3DSaveInfo>(
//                    time_point, devices_info[DeviceAzimuth::DEVICE_AZIMUTH_RR]));
//            m_cloud_mutex.unlock();

            // 3、将ir图打包发送mqtt
//            std::vector<unsigned char> message;
////            message.push_back(time_point);
//
////            for (int rows = 0; rows < merge_mat.rows - 0; rows++) {
////                for (int cols = 0; cols < merge_mat.cols - 0; cols++) {
////                    message.push_back(merge_mat.at<uchar>(rows, cols));
////                }
////            }
//            for (auto iter = merge_mat.datastart; iter != merge_mat.dataend; iter++) {
//                message.push_back(*iter);
//            }
//            cv::Mat merge_mat_2 = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
//            memcpy(merge_mat_2.data, message.data(), message.size());
            std::vector<unsigned char > test;
            for (int i = 0; i < 640*480*2*1; i++) {
                test.push_back('a');
            }
            LOG(INFO) << "message size " << test.size();
            SendMessage("tof/ir", test.data(), test.size());
        }
    }
}