#include "ivpcdview.h"

#include <QTime>


IVPCDView::IVPCDView()
{
    image = new QImage(2000, 2000, QImage::Format_RGB32);//画布的初始化大小设为300*300，使用32位颜色
    mimagepaint = new QImage(2000, 2000, QImage::Format_RGB32);
    painter = new QPainter(image);
    painter->end();
}

void IVPCDView::run()
{
    while(!QThread::isInterruptionRequested())
    {
        if(mnReadIndex != mnWriteIndex)
        {
            paint();
            mnReadIndex = mnWriteIndex;
            mbImageUpdate = true;
        }
        else
        {
            msleep(1);
        }
    }
}

void IVPCDView::SetPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr pc)
{
    mMutex.lock();
    pc.swap(mpoint_cloud);
    mnWriteIndex++;
    mMutex.unlock();
}

void IVPCDView::paint()
{
    QTime xTime;
    xTime.start();
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());


    int nz = 0;

    mMutex.lock();
    point_cloud.swap(mpoint_cloud);
    mMutex.unlock();


    if(point_cloud->width <1)return;


    painter->begin(image);

    image->fill(QColor(255, 255, 255));//对画布进行填充
//    std::vector<iv::GPSData> navigation_data = brain->navigation_data;
    painter->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式，好看一点

    painter->translate(1000,1000);

    painter->setPen(Qt::black);

    painter->drawLine(-1000,0,1000,0);
    painter->drawLine(0,-1000,0,1000);

    int i;
    for(i=-9;i<=9;i++)
    {
        painter->drawLine(i*100,0,i*100,-20);
//        painter->drawText(i*100-20,-50,40,20,Qt::AlignHCenter | Qt::AlignTop,QString::number(i*10));
    }

//     std::cout<<"enter paint."<<std::endl;

    painter->save();
    painter->rotate(90);
    for(i=-9;i<=9;i++)
    {
        painter->drawLine(i*100,0,i*100,-20);
//        painter->drawText(i*100-20,-50,40,20,Qt::AlignHCenter | Qt::AlignTop,QString::number(i*(-10)));
    }
    painter->restore();


    if(point_cloud != 0)
    {
    for(i=0;i<point_cloud->width;i++)
    {
        pcl::PointXYZI pxyzi = point_cloud->at(i);
        int x,y;

 //       std::cout<<" x is "<<pxyzi.x<<" y is "<<pxyzi.y<<std::endl;
        x =(int) (pxyzi.x*10.0);
        y = (int)(pxyzi.y*(-10.0));
        if((fabs(x)<5) &&(fabs(y)<5))
        {
            nz++;
        }
//        std::cout<<" x is "<<x<<" y is "<<y<<std::endl;
        int inten = pxyzi.intensity;
        inten = inten/2;
        QColor color;
        color.setHsl(inten,255,120);
        painter->setPen(color);
        painter->drawPoint(x,y);
    }
    }



 //   qDebug("nz = %d",nz);

//    QColor x;
//    x.setHsl(200,255,120);
//    painter->setPen(x);
//    painter->drawText(-300,-300,40,40,Qt::AlignHCenter | Qt::AlignTop,"hello");
    painter->end();

    mMutexPaint.lock();
//    delete mimagepaint;
//    mimagepaint = new QImage(*image);
    *mimagepaint = image->copy();
    mMutexPaint.unlock();

    qDebug("paint time is %d ",xTime.elapsed());
}

QImage IVPCDView::GetImage()
{
    mMutexPaint.lock();
 //   QImage imagertn(*mimagepaint);
    QImage imagertn = mimagepaint->copy();
    mMutexPaint.unlock();
    mbImageUpdate = false;
    return imagertn;
}

bool IVPCDView::IsHaveNew()
{
    return mbImageUpdate;
}

int IVPCDView::GetType()
{
    return 1;
}