#include "mainwindow.h"
#include "ui_mainwindow.h"

#include <pcl/io/ply_io.h>
#include <pcl/io/obj_io.h>


static pose gCurPose;

#include <cmath>

struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}


Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
    // Abbreviations for the various angular functions
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);

    Quaternion q;
    q.w = cy * cp * cr + sy * sp * sr;
    q.x = cy * cp * sr - sy * sp * cr;
    q.y = sy * cp * sr + cy * sp * cr;
    q.z = sy * cp * cr - cy * sp * sr;

    return q;
}

double mpos_x = 0;

void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
{
    //设置背景颜色
    viewer.setBackgroundColor(0.0,0.0,0.0);
   viewer.resetCamera();



   viewer.addCube(-0.9,0.9,-2.3,2.3,-1.9,-0.4,0.0,0.0,1.0,"car",0);


}

void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
{
    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" psycho."<<std::endl;

    viewer.removeShape("car");

    Eigen::Vector3f trans;

    Quaternion q = ToQuaternion(gCurPose.yaw,gCurPose.pitch,gCurPose.roll);
    Eigen::Quaternionf quat(q.w,q.x,q.y,q.z);
    trans(0) = gCurPose.x;trans(1) = gCurPose.y;trans(2) = gCurPose.z;

 //   viewer.addCube(mpos_x+20 -0.9,mpos_x + 20+0.9,-2.3,2.3,-1.9,-0.4,0.0,0.0,1.0,"car",0);
    viewer.addCube(trans,quat,6.0,3.0,1.0,"car");

    double yaw_calc = M_PI/2.0 - gCurPose.yaw;

    viewer.setCameraPosition(gCurPose.x,gCurPose.y,100,gCurPose.x,gCurPose.y,gCurPose.z,sin(yaw_calc),cos(yaw_calc),0);

    std::stringstream ss;
 //   viewer.setCameraPosition(mpos_x,0,30,mpos_x + 20,0,0,0,0,0);
 //   ss << "Once per viewer loop: " << count++;

}





MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    mCurPose.x = 0; mCurPose.y = 0; mCurPose.z = 0;
    mCurPose.yaw = 0; mCurPose.pitch = 0; mCurPose.roll = 0;

    gCurPose = mCurPose;
\
    mpthreadpcd = new std::thread(&MainWindow::threadpcdview,this);

}

MainWindow::~MainWindow()
{
    mbRun = false;
    mpthreadpcd->join();
    delete ui;
}


void MainWindow::threadpcdview()
{
     mpviewer = new pcl::visualization::CloudViewer("Cloud View");

     pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                 new pcl::PointCloud<pcl::PointXYZI>());

     pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud2(
                 new pcl::PointCloud<pcl::PointXYZI>());

     pcl::io::loadPCDFile<pcl::PointXYZI>("/home/yuchuli/map/map.pcd",*point_cloud);

 //    pcl::io::loadOBJFile("/home/yuchuli/car.obj",*point_cloud);
//     pcl::io::loadPLYFile("/home/yuchuli/car.ply",*point_cloud);
     mpviewer->showCloud(point_cloud);


     //This will only get called once
     mpviewer->runOnVisualizationThreadOnce (viewerOneOff);



     mpviewer->runOnVisualizationThread (viewerPsycho);


     while((!mpviewer->wasStopped()) && mbRun)
     {
         std::this_thread::sleep_for(std::chrono::milliseconds(10));

     }

     delete mpviewer;

}


void MainWindow::on_pushButton_Test_clicked()
{
    double x = ui->lineEdit_x->text().toDouble();
    double y = ui->lineEdit_y->text().toDouble();
    double z = ui->lineEdit_z->text().toDouble();
    double yaw = ui->lineEdit_yaw->text().toDouble();
    double pitch = ui->lineEdit_pitch->text().toDouble();
    double roll = ui->lineEdit_roll->text().toDouble();
    mCurPose.x = x;
    mCurPose.y = y;
    mCurPose.z = z;
    mCurPose.yaw = yaw;
    mCurPose.pitch = pitch;
    mCurPose.roll = roll;

    gCurPose = mCurPose;
}