#include "qrosthread.h"
#include "rule.h"

void QRosThread::getRawFrame(const kyber_msgs::Video_encoded_data::ConstPtr &msg)
{
    if(isRunning())
    {
        std::vector<uint8_t> data = msg->raw_data;
        qlonglong t = msg->header.stamp.toNSec();
        QImage imageFrom = cuda[index_of_camera]->decode(data.data(), int(data.size()), qRule->current_camera_width, qRule->current_camera_height);
        if(!imageFrom.isNull())
        {
            QImage imageTo = qRule->correct(imageFrom);
            emit decodedCamera(imageTo, t);
        }
    }
}

size_t QRosThread::findChannelIndex(const sensor_msgs::PointCloud2ConstPtr& cloud, const std::string& channel)
{
    for (size_t i = 0; i < cloud->fields.size(); ++i)
    {
        if (cloud->fields[i].name == channel)
        {
            return size_t(i);
        }
    }
    return size_t(-1);
}

void QRosThread::getLidarFrame(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    qlonglong t = cloud->header.stamp.toNSec();

    size_t x_index = findChannelIndex(cloud, "x");
    size_t y_index = findChannelIndex(cloud, "y");
    size_t z_index = findChannelIndex(cloud, "z");

    size_t intensity_index = findChannelIndex(cloud, "intensity");

    const uint32_t x_off = cloud->fields[x_index].offset;
    const uint32_t y_off = cloud->fields[y_index].offset;
    const uint32_t z_off = cloud->fields[z_index].offset;
    const uint32_t intensity_off = cloud->fields[intensity_index].offset;
    const uint32_t point_step = cloud->point_step;

    const uint8_t* ptr = &cloud->data.front();
    const uint8_t* ptr_end = &cloud->data.back();

    float min_intensity = 999999.0f;
    float max_intensity = -999999.0f;
    for( uint32_t i = 0; i < cloud->width*cloud->height; ++i )
    {
        const uint8_t* data = &cloud->data[(point_step * i) + intensity_off];
        float val = *reinterpret_cast<const uint16_t*>(data);
        min_intensity = std::min(val, min_intensity);
        max_intensity = std::max(val, max_intensity);
    }

    float diff_intensity = max_intensity - min_intensity;

    QList<QVector3D> list2;
    for( uint32_t i = 0; i < cloud->width*cloud->height; ++i )
    {
        const uint8_t* data = &cloud->data[(point_step * i) + intensity_off];
        float val = *reinterpret_cast<const uint16_t*>(data);
        float value = 1.0 - (val - min_intensity)/diff_intensity;
        float h = value * 5.0f + 1.0f;
        int j = floor(h);
        float f = h - j;
        if ( !(j&1) ) f = 1 - f; // if i is even
        int n = int(255 - f*255);
        QVector3D color;
        if(j <= 1)
        {
            color.setX(n); color.setY(0); color.setZ(255);
        }
        else if (j == 2)
        {
            color.setX(0); color.setY(n); color.setZ(255);
        }
        else if (j == 3)
        {
            color.setX(0); color.setY(255); color.setZ(n);
        }
        else if (j == 4)
        {
            color.setX(n); color.setY(255); color.setZ(0);
        }
        else if (j >= 5)
        {
            color.setX(255); color.setY(n); color.setZ(0);
        }
        list2 << color;
    }

    QList<QVector3D> list;
    for (; ptr < ptr_end; ptr += point_step)
    {
        float x = *reinterpret_cast<const float*>(ptr + x_off);
        float y = *reinterpret_cast<const float*>(ptr + y_off);
        float z = *reinterpret_cast<const float*>(ptr + z_off);
        list << QVector3D(x,y,z);
    }

    emit decodedLidar( qRule->decode(list, list2, topic_lidar_name, topic_camera_name), t );
}

QRosThread::QRosThread(QObject *parent) : QThread(parent)
{
    for(int i=0; i<9; i++)
    {
        cuda[i] = new CudaDecode();
    }
}

QRosThread::~QRosThread()
{
    quit();
    wait();
}

void QRosThread::subscribe(QString cameraTopicName, QString lidarTopicName)
{
    ros::NodeHandle n;
    if(cameraTopicName != "")
    {
        topic_camera_name = cameraTopicName;
        index_of_camera = cameraTopicName.split("_").last().toInt();
        qRule->initUndistortMap(index_of_camera);
        subCamera = n.subscribe(topic_camera_name.toStdString(), 1000, &QRosThread::getRawFrame, this);
    }
    if(lidarTopicName != "")
    {
        topic_lidar_name = lidarTopicName;
        subLidar = n.subscribe(topic_lidar_name.toStdString(), 1000, &QRosThread::getLidarFrame, this);
    }
}

void QRosThread::run()
{
    ros::MultiThreadedSpinner spinner;
    spinner.spin();
}