#include "rslidar_mqtt_async.h"

void RslidarMqttAsyncClient::init() {
    m_message_arrived_callback_ = CloudDataArrived;

    MqttAsyncClient::init_from_proto("../etc/rslidar_mqtt_async.json");
}

bool RslidarMqttAsyncClient::setCloudData(const std::string &topic, pcl::PointCloud<pcl::PointALL>::Ptr &cloud) {
    if (cloud->empty()) {
        printf("---Debug cloud size %ld\n", cloud->size());
        return false;
    }

//    auto iter_cloud = m_cloud_.find(topic);
    auto iter_full_cloud = m_full_cloud_.find(topic);
//    auto iter_time = m_topic_time_.find(topic);
    if (iter_full_cloud == m_full_cloud_.end()) {
//        m_cloud_.insert(std::pair<std::string, pcl::PointCloud<pcl::PointALL>::Ptr>(topic,new pcl::PointCloud<pcl::PointALL>));
        m_full_cloud_.insert(std::pair<std::string, pcl::PointCloud<pcl::PointALL>::Ptr>(topic,new pcl::PointCloud<pcl::PointALL>));
//        m_topic_time_.insert(std::pair<std::string, std::chrono::steady_clock::time_point>(topic, std::chrono::steady_clock::now()));
//        iter_cloud = m_cloud_.find(topic);
        iter_full_cloud = m_full_cloud_.find(topic);
//        iter_time = m_topic_time_.find(topic);
    }

//    std::chrono::duration<double> diff = std::chrono::steady_clock::now() - iter_time->second;
//    if (diff.count() * 1000 > 100) {
//        iter_time->second = std::chrono::steady_clock::now();
//        iter_full_cloud->second->swap(*iter_cloud->second);
//        iter_cloud->second->clear();
//    }
//
//    iter_cloud->second->insert(iter_cloud->second->end(), cloud->begin(), cloud->end());
//    LOG(INFO) << topic << "   " << iter_full_cloud->first;
//    iter_full_cloud->second->points.insert(iter_full_cloud->second->end(), cloud->points.begin(), cloud->points.end());
    {
        std::lock_guard<std::mutex> lck(m_cloud_mutex);
        iter_full_cloud->second->swap(*cloud);
    }
//    iter_cloud->second->clear();

    return true;
}

bool RslidarMqttAsyncClient::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
    std::lock_guard<std::mutex> lck(m_cloud_mutex);
    auto iter = m_full_cloud_.find(topic);
    if (iter == m_full_cloud_.end()) {
        LOG(WARNING) << topic << " can't find data.";
        return false;
    }

    cloud->clear();
    pcl::PointXYZI cloud_p;
    for (auto &point:*iter->second) {
        cloud_p.x = point.x;
        cloud_p.y = point.y;
        cloud_p.x = point.z;
        cloud_p.intensity = point.intensity;
        cloud->points.emplace_back(cloud_p);
    }
    return true;
}

bool RslidarMqttAsyncClient::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
    std::lock_guard<std::mutex> lck(m_cloud_mutex);
    auto iter = m_full_cloud_.find(topic);
    if (iter == m_full_cloud_.end()) {
        return false;
    }
    cloud->clear();
    for (auto &point: *iter->second) {
        cloud->points.emplace_back(point.x, point.y, point.z);
    }
    return true;
}

bool RslidarMqttAsyncClient::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud, double *color) {
    std::lock_guard<std::mutex> lck(m_cloud_mutex);
    auto iter = m_full_cloud_.find(topic);
    if (iter == m_full_cloud_.end()) {
        return false;
    }
    cloud->clear();
    pcl::PointXYZRGB rgb;
    for (auto &point: *iter->second) {
        rgb.x = point.x;
        rgb.y = point.y;
        rgb.z = point.z;
        rgb.r = color[0];
        rgb.g = color[1];
        rgb.b = color[2];
        cloud->points.emplace_back(rgb);
    }
    return true;
}

int RslidarMqttAsyncClient::CloudDataArrived(MqttAsyncClient *client, char *topicName, int topicLen,
                                             MQTTAsync_message *message) {
    pcl::PointCloud<pcl::PointALL>::Ptr cloud(new pcl::PointCloud<pcl::PointALL>);
    CloudPointUnpacket unpacket;
    unpacket.unpacket((unsigned char *) message->payload, message->payloadlen, cloud);
    RslidarMqttAsyncClient *t_client = (RslidarMqttAsyncClient *) client;
    t_client->setCloudData(topicName, cloud);
    return 1;
}
