#include "mqtt_communication.h"
#include "tool/time.hpp"

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(void *client, char *topicName, int topicLen,
                                             MQTTAsync_message *message) {
    std::string topic = topicName;
    if (topic == "tof/seg") {
        JetStream::LabelYolo seg_results;
        seg_results.ParseFromArray(message->payload, message->payloadlen);
        LabelYoloDataBuffer::iter()->lock_exe(&LabelYoloDataBuffer::set, seg_results);
        int lable = TimeTool::timeDropoutLL(TimeTool::TemporalPrecision::TemporalPrecisionMs, TimeTool::TemporalPrecision::TemporalPrecisionHour);
        // LOG(INFO) << "after " << (lable - seg_results.label()) << "ms";
    }
    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()) {
            int lable = TimeTool::timeDropoutLL(TimeTool::TemporalPrecision::TemporalPrecisionMs, TimeTool::TemporalPrecision::TemporalPrecisionHour);
            // 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);
                MatDataBuffer::iter()->lock_exe(&MatDataBuffer::set, device_index, lable, devices_info[device_index].pointMat);
                devices_info[device_index].irMat.copyTo(merge_mat(cv::Rect((device_index & 0x1) * 640,
                                                                           ((device_index & 0x2) >> 1) * 480, 640, 480)));
            }
            MatDataBuffer::iter()->lock_exe(&MatDataBuffer::setIr, lable, merge_mat);
            // cv::imwrite(ETC_PATH PROJECT_NAME "/failed/merge_mat_" + std::to_string(lable) + ".jpg", merge_mat);
            JetStream::LabelImage testimage;
            NetMessageTrans::lableMat2Proto(lable, merge_mat, testimage, true);
            int size_1 = testimage.ByteSizeLong();
            unsigned char data[size_1];
            testimage.SerializeToArray((void *)data, size_1);
            SendMessage("tof/ir", data, size_1);

            // int test_lable = 0;
            // cv::Mat pic;
            // NetMessageTrans::Proto2lableMat(testimage, test_lable, pic);
            // cv::imshow("pic", pic);
            // cv::waitKey(1);
        }
    }
}
