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

#include <ostream>
#include <strstream>
#include <QScrollArea>

#include "gpsimu.pb.h"
#include "gnss_coordinate_convert.h"

extern std::string gstrlidarmsg;
extern std::string gstrimumsg;

extern double gsplit_range ;
extern double gsplit_filter;
extern double gsplit_cur;

extern iv::ndtorigin gndtorigin;

extern bool gbManualSave;

#include <Eigen/Geometry>

#include <tf/tf.h>

// Given two sets of 3D points, find the rotation + translation + scale
// which best maps the first set to the second.
// Source: http://en.wikipedia.org/wiki/Kabsch_algorithm

// The input 3D points are stored as columns.
Eigen::Affine3d Find3DAffineTransform(Eigen::Matrix3Xd in, Eigen::Matrix3Xd out) {

  // Default output
  Eigen::Affine3d A;
  A.linear() = Eigen::Matrix3d::Identity(3, 3);
  A.translation() = Eigen::Vector3d::Zero();

  if (in.cols() != out.cols())
    throw "Find3DAffineTransform(): input data mis-match";

  // First find the scale, by finding the ratio of sums of some distances,
  // then bring the datasets to the same scale.
  double dist_in = 0, dist_out = 0;
  for (int col = 0; col < in.cols()-1; col++) {
    dist_in  += (in.col(col+1) - in.col(col)).norm();
    dist_out += (out.col(col+1) - out.col(col)).norm();
  }
  if (dist_in <= 0 || dist_out <= 0)
    return A;
  double scale = dist_out/dist_in;
  out /= scale;

  // Find the centroids then shift to the origin
  Eigen::Vector3d in_ctr = Eigen::Vector3d::Zero();
  Eigen::Vector3d out_ctr = Eigen::Vector3d::Zero();
  for (int col = 0; col < in.cols(); col++) {
    in_ctr  += in.col(col);
    out_ctr += out.col(col);
  }
  in_ctr /= in.cols();
  out_ctr /= out.cols();
  for (int col = 0; col < in.cols(); col++) {
    in.col(col)  -= in_ctr;
    out.col(col) -= out_ctr;
  }

  // SVD
  Eigen::MatrixXd Cov = in * out.transpose();
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(Cov, Eigen::ComputeThinU | Eigen::ComputeThinV);

  // Find the rotation
  double d = (svd.matrixV() * svd.matrixU().transpose()).determinant();
  if (d > 0)
    d = 1.0;
  else
    d = -1.0;
  Eigen::Matrix3d I = Eigen::Matrix3d::Identity(3, 3);
  I(2, 2) = d;
  Eigen::Matrix3d R = svd.matrixV() * I * svd.matrixU().transpose();

  // The final transform
  A.linear() = scale * R;
  A.translation() = scale*(out_ctr - R*in_ctr);

  return A;
}


iv::ndtorigin PoseToGPS(iv::ndtorigin xori,iv::ndttracepoint xpose)
{
    double x_o,y_o;
    GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
    double lon,lat;
    double hdg_o = (90 - xori.heading)*M_PI/180.0;
    double rel_x = xpose.x * cos(hdg_o) - xpose.y * sin(hdg_o);
    double rel_y = xpose.x * sin(hdg_o) + xpose.y * cos(hdg_o);
    GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
    double hdg_c = hdg_o + xpose.yaw;

    hdg_c = M_PI/2.0 - hdg_c;
    double heading = hdg_c * 180.0/M_PI;
    while(heading < 0)heading = heading + 360;
    while(heading >360)heading = heading - 360;
    iv::ndtorigin xgpspos;
    xgpspos.lon = lon;
    xgpspos.lat = lat;
    xgpspos.height = xpose.z + xori.height;
    xgpspos.heading = heading;
    xgpspos.pitch = xpose.pitch + xori.pitch;
    xgpspos.roll  = xpose.roll + xori.roll;
    return xgpspos;

}


iv::ndttracepoint CalcPose(iv::ndtorigin xori, iv::gps::gpsimu xgpsimu)
{
    double x_o,y_o,hdg_o;
    double x_c,y_c,hdg_c;
    GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
    GaussProjCal(xgpsimu.lon(),xgpsimu.lat(),&x_c,&y_c);
    double dis = sqrt(pow(x_c-x_o,2)+ pow(y_c-y_o,2));
    double rel_x0,rel_y0;
    rel_x0 = x_c -x_o;
    rel_y0 = y_c -y_o;
    double rel_x,rel_y;

    hdg_o = (90 - xori.heading)*M_PI/180.0;
    hdg_c = (90 - xgpsimu.heading())*M_PI/180.0;
    rel_x = rel_x0 * cos(-hdg_o) - rel_y0 * sin(-hdg_o);
    rel_y = rel_x0 * sin(-hdg_o) + rel_y0 * cos(-hdg_o);

//    double diffang = atan(rel_y/rel_x) * 180.0/M_PI;

    double rel_hdg;
    rel_hdg = hdg_c - hdg_o;
    iv::ndttracepoint posex;
    posex.x = rel_x;
    posex.y = rel_y;
    posex.z = xgpsimu.height() - xori.height;
    posex.pitch = xgpsimu.pitch() - xori.pitch;
    posex.roll = xgpsimu.roll() - xori.roll;
    posex.yaw = rel_hdg;

    return posex;

}


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

//    iv::ndtorigin xs;
//    xs.heading = 359.80 -0.72;
////    xs.heading = 350;
//    xs.height = 0;
//    xs.pitch = 0;
//    xs.roll = 0;
//    xs.lat = 39.12051011;
//    xs.lon = 117.02730387;

//    iv::gps::gpsimu xgpsimuobj;
//    xgpsimuobj.set_lat(39.12059277);
//    xgpsimuobj.set_lon(117.02730218);

//    iv::ndttracepoint xt = CalcPose(xs,xgpsimuobj);

    mfCalPitch = 0.0;
    mfCalRoll = 0.0;
    mfCalYaw = 0.0;
    //创建参数组合框
    CreateView();

    m_nP= 0;
    //启动定时器
    connect(&mTimer,SIGNAL(timeout()),this,SLOT(onTimer()));
    //启动mndtthread线程
    mndtthread.start();
    //设置窗口标题
    setWindowTitle("ADC NDT Mapping ------ Lidar Localization");

    mCalibMatrix<<1.0,0,0,
            0,1.0,0,
            0,0,1.0;

    Eigen::Matrix3d t = mCalibMatrix;
    tf::Matrix3x3 mat_l;  // localizer
    mat_l.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)),
                   static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)),
                   static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2)));

    double roll,pitch,yaw;
    mat_l.getRPY(roll, pitch, yaw, 1);

    std::ostrstream ostr;

    ostr<<"Rotation:"<<std::endl;
    ostr<<t<<std::endl;

    std::cout<<"roll:"<<roll<<" pitch:"<<pitch<<" yaw:"<<yaw<<std::endl;

    ostr<<"roll:"<<roll<<" pitch:"<<pitch<<" yaw:"<<yaw<<std::endl;

    std::string strres = ostr.str();
    ui->plainTextEdit_Matrix->setPlainText(strres.substr(0,ostr.pcount()).data());



//    ui->Dial->setStyleSheet("background-color:Black");
//    ui->Dial->setStyleSheet("color:rgb(0,0,0)");
}

MainWindow::~MainWindow()
{

    mndtthread.requestInterruption();
    while(!mndtthread.isFinished())
    {

    }
    delete ui;
}

void MainWindow::on_pushButton_Load_clicked()
{

    gsplit_range = mpLE_SplitRange->text().toDouble();
    gsplit_filter = mpLE_FilterResolution->text().toDouble();

    gndtorigin.lon = mpLE_OriginLon->text().toDouble();
    gndtorigin.lat = mpLE_OriginLat->text().toDouble();
    gndtorigin.height = mpLE_OriginHeight->text().toDouble();
    gndtorigin.heading = mpLE_OriginHeading->text().toDouble();
    gndtorigin.pitch = 0;
    gndtorigin.roll = 0;


    mnPac = 0;

    mbCalcCalib = false;
    mbCalcCalib_Roll = false;

    int ncalindex = mpCB_Mode->currentIndex();
    if(ncalindex == 0)
    {
        mndtthread.setusecpu();
    }
    else
    {
        if(ncalindex == 1)
        {
            mndtthread.setuseomp();
        }
        else
            mndtthread.setusegpu();
    }
    //获取文件路径
    QString str = QFileDialog::getOpenFileName(this,tr("Open file"),"",tr("Record File(*.ivd)"));
    if(str.isEmpty())return;

    mvectorcalib.clear();
    mvectorgps.clear();
    mvectorrawgps.clear();
    mvectortrace.clear();
    mvectortracecalib.clear();
    mvectorglobalgps.clear();

    mbIVDSetOrigin = false;
    mndtthread.reset();
//    ui->pushButton_Start->setEnabled(false);
    mbReplay = false;
    mMutex.lock();



    if(mFile.isOpen())mFile.close();
    mFile.setFileName(str);

    //以只读打开文件
    if(mFile.open(QFile::ReadOnly))
    {

        if(mbSaveDiff)
        {
            QFileInfo fileinfo = QFileInfo(str);
            std::string strpath = getenv("HOME");
            strpath = strpath + "/map/";
            strpath = strpath + fileinfo.fileName().toLatin1().data();
            strpath = strpath +".txt";
            if(mFileDiff.isOpen())mFileDiff.close();
            mFileDiff.setFileName(strpath.data());
            if(mFileDiff.open(QIODevice::ReadWrite))
            {
                mbFileDiffOpen = true;
                char strout[1000];
                snprintf(strout,1000,"Pos\tDiff\tndt_x\tndt_y\tndt_z\tndt_yaw\trtk_x\trtk_y\trtk_z\trtk_yaw\t\n");
                mFileDiff.write(strout,strnlen(strout,1000));
                mFileDiff.flush();
            }
            else
            {
                mbFileDiffOpen = false;
            }

        }
        //设置当前文件打开状态
        mbOpen = true;
        //文件大小
        mnFileSize = mFile.size();
        //读取到文件的位置
        mnPos = 0;
        if(mnskip > 0)
        {
            int nReadSize = 0;
            int nDataSize;
            char * strData;
            char * strName;
            //        读取记录
            bool bx = ReadARecord(nReadSize,&strName,&nDataSize,&strData);
            mnPac++;
            int nr= 1;
            while((nr < mnskip)&&bx)
            {
                delete strName;
                delete strData;
                bx = ReadARecord(nReadSize,&strName,&nDataSize,&strData);
                mnPac++;
                nr++;
            }
            delete strName;
            delete strData;
        }
    }
//    if(mbOpen)
//    {
//        ui->pushButton_Start->setEnabled(true);
//    }
//    else
//    {
//        ui->pushButton_Start->setEnabled(false);
//    }

    mbIVDSetOrigin = false;
    mbIVDSetGlobalOrigin = false;
    mMutex.unlock();
    //更新地图参数
    UpdateMapParam();
    //启动定时器
    mTimer.start(10);
}
//定时器执行程序
void MainWindow::onTimer()
{
    int nP;
    std::string str;
    iv::ndttracepoint xndtpoint;
    qint64 xlidartime;
    //获取状态信息，获取全局变量g_np和gstr
    GetState(nP,str,xndtpoint,xlidartime);
    static double fOldDis = gsplit_cur;
    static double bFirst = true;
    //每执行一次point_ndtmapping操作，全局变量g_np会增加1，如果np有变化则更新文本框内的数据
    if(nP != m_nP)
    {
        ui->plainTextEdit->setPlainText(str.c_str());
        ui->lineEdit_Dis->setText(QString::number(gsplit_cur,'f',3));
        mvectortrace.push_back(xndtpoint);
        mptraceview->SetNDTTrace(mvectortrace);
        mvectortracecalib.push_back(xndtpoint);
        calibcalc(&mvectortracecalib[mvectortracecalib.size()-1]);
        mptraceview->SetNDTCalibTrace(mvectortracecalib);
        bool bHaveRTKPoint = false;
        iv::ndttracepoint xtrrtk;
        while(mvectorrawgps.size()>0)
        {
 //           std::cout<<" time diff: "<<(mvectorrawgps[0].recvtime - xlidartime)<<std::endl;
 //           std::cout<<" ldiar time: "<<xlidartime<<std::endl;
            if((mvectorrawgps[0].recvtime<xlidartime) &&(mvectorrawgps[0].recvtime>=(xlidartime-30)))
            {
                iv::ndttracepoint xtr;
                xtr = mvectorrawgps[0].xtrace;
                mvectorgps.push_back(xtr);
                bHaveRTKPoint = true;
                xtrrtk = xtr;
                break;
            }
            mvectorrawgps.erase(mvectorrawgps.begin());
        }

        if(mbFileDiffOpen)
        {
            if(((fabs(gsplit_cur - fOldDis )>=1.0) || bFirst)&&(bHaveRTKPoint))
            {
                bFirst = false;
                fOldDis = gsplit_cur;
                char strout[1000];
                double fdis = sqrt(pow(xndtpoint.x - xtrrtk.x,2)+pow(xndtpoint.y - xtrrtk.y,2));
                snprintf(strout,1000,"%6.2f\t%6.2f\t%6.2f\t%6.2f\t%6.2f\t%6.2f\t%6.2f\t%6.2f\t%6.2f\t%6.2f\t\n",
                         gsplit_cur,fdis,xndtpoint.x,xndtpoint.y,xndtpoint.z,xndtpoint.yaw,
                         xtrrtk.x,xtrrtk.y,xtrrtk.z,xtrrtk.yaw);
                mFileDiff.write(strout,strnlen(strout,1000));
                mFileDiff.flush();
            }
        }
        mptraceview->SetGPSTrace(mvectorgps);
        mptraceview->SetGlobalGPSTrace(mvectorglobalgps);
        if(mvectorgps.size()>0)
        {
            iv::ndttracepoint xtr = mvectorgps[mvectorgps.size()-1];
            char strtemp[1000];
            snprintf(strtemp,1000,"%6.3f  %6.3f  %6.3f  %6.3f",
                     xtr.x,xtr.y,xtr.z,xtr.yaw);
            ui->lineEdit_GPS->setText(strtemp);
        }
        if(mvectortrace.size()>0)
        {
            iv::ndttracepoint xtr = mvectortrace[mvectortrace.size()-1];
            char strtemp[1000];
            snprintf(strtemp,1000,"%6.3f  %6.3f  %6.3f  %6.3f",
                     xtr.x,xtr.y,xtr.z,xtr.yaw);
            ui->lineEdit_NDT->setText(strtemp);
        }
        if(mvectortracecalib.size()>0)
        {
            iv::ndttracepoint xtr = mvectortracecalib[mvectortracecalib.size()-1];
            char strtemp[1000];
            snprintf(strtemp,1000,"%6.3f  %6.3f  %6.3f  %6.3f",
                     xtr.x,xtr.y,xtr.z,xtr.yaw);
            ui->lineEdit_Calib->setText(strtemp);
        }
        if((gsplit_cur>50.0)&&(mbCalcCalib == false))
        {
            CalcCalib();
            mbCalcCalib = true;
        }
        if((gsplit_cur>100.0) &&(mbCalcCalib_Roll == false))
        {
            if(mvectortrace.size()>0)
            {
                if(fabs(mvectortrace[mvectortrace.size()-1].y) > 30.0)
                {
                    CalcCalibRoll();
                    mbCalcCalib_Roll = true;
                }
            }
        }
        update();
    }
    //记录执行了多少次
    m_nP = nP;

    if(mbPause)return;

    bool bLidarGood = true;
    if(mndtthread.AbleNext())//如果可执行下一次
    {

        int nReadSize = 0;
        int nDataSize;
        char * strData;
        char * strName;
//        读取记录
        bool bx = ReadARecord(nReadSize,&strName,&nDataSize,&strData);
        mnPac++;
        //循环读取数据，直到读取到"lidar_pc"
        while((bx == true)&&(strcmp(strName,gstrlidarmsg.data()) != 0))
        {
            if((strcmp(strName,gstrimumsg.data()) == 0))
            {
                                iv::gps::gpsimu xgpsimu;
                                bool bParse;
                                bParse = xgpsimu.ParseFromArray(strData,nDataSize);
                                if(bParse)
                                {
                //                    if(fabs(xgpsimu.vd())>0.5)
                //                    {
                //                        qDebug("vd is %f",xgpsimu.vd());
                //                    }
                 //
                                    if(mbIVDSetOrigin == false)
                                    {
                                        if(xgpsimu.rtk_state() == 6)
                                        {
                                            double fheading = xgpsimu.heading();
                                            fheading = fheading - 0;
                                            if(fheading < 0)fheading = fheading + 360;

                                            double flat,flon;

                                            mpLE_OriginLat->setText(QString::number(xgpsimu.lat(),'f',7));
                                            mpLE_OriginLon->setText(QString::number(xgpsimu.lon(),'f',7));
                                            mpLE_OriginHeight->setText(QString::number(xgpsimu.height(),'f',2));
                                            mpLE_OriginHeading->setText(QString::number(fheading,'f',2));
                                            gndtorigin.lon = mpLE_OriginLon->text().toDouble();
                                            gndtorigin.lat = mpLE_OriginLat->text().toDouble();
                                            gndtorigin.height = mpLE_OriginHeight->text().toDouble();
                                            gndtorigin.heading = fheading;

                                            iv::ndttracepoint trarm;
                                            trarm.x = arm_x;
                                            trarm.y = arm_y;
                                            trarm.z = 0;
                                            trarm.yaw = 0;
                                            trarm.pitch = 0;
                                            trarm.roll = 0;
                                            gndtorigin = PoseToGPS(gndtorigin,trarm);

                                            mpLE_OriginLat->setText(QString::number(gndtorigin.lat,'f',7));
                                            mpLE_OriginLon->setText(QString::number(gndtorigin.lon,'f',7));

                                            mbIVDSetOrigin = true;
                                            if(mbIVDSetGlobalOrigin == false)
                                            {
                                                mbIVDSetGlobalOrigin = true;
                                                mglobalndtorigin = gndtorigin;
                                            }
                                        }
                                    }
                                    if(mbIVDSetOrigin)
                                    {
                                        iv::ndttracepoint  xtr = CalcPose(gndtorigin,xgpsimu);
                                        double x,y;
                                        x = arm_x * cos(xtr.yaw) - arm_y * sin(xtr.yaw);
                                        y = arm_x * sin(xtr.yaw) + arm_y * cos(xtr.yaw);
                                        xtr.x = xtr.x + x - arm_x;
                                        xtr.y = xtr.y + y - arm_y;
                                        iv::gpstrace xg;
                                        if(xgpsimu.has_msgtime())
                                            xg.recvtime = xgpsimu.msgtime();
                                        else
                                            xg.recvtime = xgpsimu.time();
                               //         xg.recvtime = xgpsimu.msgtime();
                                        xg.xtrace = xtr;
                                        mvectorrawgps.push_back(xg);
                                    }
                                    if(mbIVDSetGlobalOrigin)
                                    {
                                        iv::ndttracepoint  xtr = CalcPose(mglobalndtorigin,xgpsimu);
                                        double x,y;
                                        x = arm_x * cos(xtr.yaw) - arm_y * sin(xtr.yaw);
                                        y = arm_x * sin(xtr.yaw) + arm_y * cos(xtr.yaw);
                                        xtr.x = xtr.x + x - arm_x;
                                        xtr.y = xtr.y + y - arm_y;
                                        iv::gpstrace xg;
                                        mvectorglobalgps.push_back(xtr);
                                    }
                                    if(xgpsimu.has_acce_x())
                                    {
//                                        setuseimu(true);
//                                        imu_update(xgpsimu.time(),
//                                                   xgpsimu.roll(),xgpsimu.pitch(),
//                                                   xgpsimu.heading(),xgpsimu.acce_x(),
//                                                   xgpsimu.acce_y(),xgpsimu.acce_z());
                                    }
//                                    if(xgpsimu.has_acce_z())
//                                    {
//                                        if(fabs(xgpsimu.acce_z() + 1.0)>0.5)
//                                        {
//                                            bLidarGood = true;
//                                            qDebug("acc z is %f vd is %f",xgpsimu.acce_z(),xgpsimu.vd());
//                                        }
//                                    }
                                }
            }
//            if(strcmp(strName,"ins550d_gpsimu") == 0)
//            {
//                iv::gps::gpsimu xgpsimu;
//                bool bParse;
//                bParse = xgpsimu.ParseFromArray(strData,nDataSize);
//                if(bParse)
//                {
////                    if(fabs(xgpsimu.vd())>0.5)
////                    {
////                        qDebug("vd is %f",xgpsimu.vd());
////                    }
// //
//                    if(xgpsimu.has_acce_z())
//                    {
//                        if(fabs(xgpsimu.acce_z() + 1.0)>0.5)
//                        {
//                            bLidarGood = true;
//                            qDebug("acc z is %f vd is %f",xgpsimu.acce_z(),xgpsimu.vd());
//                        }
//                    }
//                }
//            }
            delete strData;
            delete strName;
//            读取一条记录并将读取的结果赋值给变量：strName,nDataSize,strData
            bx = ReadARecord(nReadSize,&strName,&nDataSize,&strData);
            mnPac++;
        }

        ui->lineEdit_Pac->setText(QString::number(mnPac));
        //更新进度条
        ui->horizontalSlider->setValue(mFile.pos()*100/mnFileSize);
//        文件读取完毕，进度条设为100，停止计时器
        if(bx == false)
        {
            ui->horizontalSlider->setValue(100);
            mTimer.stop();
        }
        else//读取到lidar数据
        {
            qDebug("find a lidar_pc");
            //预处理数据
            if(bLidarGood == true)procapcd(strData,nDataSize);
            else
            {
                bLidarGood = true;
                qDebug("skip 1 lidarpc");
            }
            delete strData;
            delete strName;
        }


    }
}

void MainWindow::paintEvent(QPaintEvent *)
{
    if(!mptraceview->IsHaveNew())
    {
        return;
    }

    QImage image = mptraceview->GetImage();

    mscene->clear();
    mscene->addPixmap(QPixmap::fromImage(image));
    mmyview->setScene(mscene);
    mmyview->show();
}
//获取rh时间
inline QDateTime MainWindow::GetDateTimeFromRH(iv::RecordHead rh)
{
    QDateTime dt;
    QDate datex;
    QTime timex;
    datex.setDate(rh.mYear,rh.mMon,rh.mDay);
    timex.setHMS(rh.mHour,rh.mMin,rh.mSec,rh.mMSec);
    dt.setDate(datex);
    dt.setTime(timex);

    return dt;
}

//处理数据
void MainWindow::procapcd(char *strdata, int nSize)
{

    if(nSize <=16)return;
    //头文件大小
    unsigned int * pHeadSize = (unsigned int *)strdata;
    if(*pHeadSize > nSize)
    {
//        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
    }
    //构建点云对象
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());

//    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(
//                new pcl::PointCloud<pcl::PointXYZ>());
//    获取名字
    int nNameSize;
    nNameSize = *pHeadSize - 4-4-8;
    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
    point_cloud->header.frame_id = strName;
//    序列号
    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
//    时间戳
    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
//    点数量
    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
    int i;

    double yaw,pitch,roll;
    yaw = (mfCalYaw)*M_PI/180.0;
    pitch = (mfCalPitch)*M_PI/180.0;
    roll = (mfCalRoll)*M_PI/180.0;
    double x1,y1,z1;
//    把点的数据赋值给点云对象中的变量
    pcl::PointXYZI * p;
    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
    for(i=0;i<nPCount;i++)
    {
        pcl::PointXYZI xp;
        xp.x = p->y;
        xp.y = p->x * (-1.0);
        xp.z = p->z;
        x1 = p->y;
        y1 = p->x * (-1.0);
        z1 = p->z;
        xp.x = x1*cos(yaw) -y1*sin(yaw);
        xp.y = x1*sin(yaw) + y1*cos(yaw);
        x1 = xp.x;
        z1 = xp.z;

        xp.z = z1*cos(pitch) -x1*sin(pitch);
        xp.x = z1*sin(pitch) + x1*cos(pitch);

        y1 = xp.y;
        z1 = xp.z;
        xp.y = y1*cos(roll) -z1*sin(roll);
        xp.z = y1*sin(roll) + z1*cos(roll);
        xp.intensity = p->intensity;
        point_cloud->push_back(xp);
        p++;
    }
//    把点云数据传给处理点云数据的线程对象
    mndtthread.procapcd(point_cloud);

}

inline bool MainWindow::ReadARecord(int & nRecSize,char ** pstrName, int * pnDataSize, char ** pstrData)
{
    char strmark[10];
    //消息总大小、消息头大小、消息名字大小、消息数据大小
    int nTotalSize,nHeadSize,nNameSize,nDataSize;
    int nRead = mFile.read(strmark,1);
    if(nRead == 0)return false;
    //获取消息总大小、消息头大小、消息名字大小、消息数据大小
    nRead = mFile.read((char *)&nTotalSize,sizeof(int));
    if(nRead < sizeof(int))return false;
    nRead = mFile.read((char *)&nHeadSize,sizeof(int));
    if(nRead < sizeof(int))return false;
    nRead = mFile.read((char *)&nNameSize,sizeof(int));
    if(nRead<sizeof(int))return false;
    nRead = mFile.read((char *)&nDataSize,sizeof(int));
    if(nRead<sizeof(int))return false;
    //校验数据关系
    if(nTotalSize !=(nHeadSize + nNameSize + nDataSize + 4*sizeof(int) ))
    {
        return false;
    }

    iv::RecordHead rh;
    char * strName = new char[1000];
    char * strData = new char[nDataSize];
    //获取时间
    nRead = mFile.read((char *)&rh,sizeof(iv::RecordHead));
    if(nRead < sizeof(iv::RecordHead))
    {
        delete strData;
        delete strName;
        return false;
    }

    mdtcurpos = GetDateTimeFromRH(rh);
    //获取名字
    nRead = mFile.read(strName,nNameSize);
    if(nRead < nNameSize)
    {
        delete strData;
        delete strName;
        return false;
    }
//    使字符数组变为字符串
    strName[nNameSize] = 0;
//    qDebug(strName);
//    qDebug("file pos is %d ms is %d",mFile.pos(),rh.mMSec);
    //获取雷达数据
    nRead = mFile.read(strData,nDataSize);
    if(nRead < nDataSize)
    {
        delete strData;
        delete strName;
        return false;
    }

    //Share Data
    //指针赋值
    *pnDataSize = nDataSize;
    *pstrName = strName;
    *pstrData = strData;


//    delete strData;
    nRecSize = nTotalSize + 1;
    return true;

}
//点击save按钮
void MainWindow::on_pushButton_Save_clicked()
{
//    获取存储的路径
    QString str = QFileDialog::getSaveFileName(this,tr("Open file"),"",tr("PCD File(*.pcd)"));
    if(str.isEmpty())return;

    if(!str.contains(".pcd"))str = str + ".pcd";
    //点云过滤网格尺寸
    double filter_res = mpLE_FilterResolution->text().toDouble();
    //存储地图
    savemap(str.toLatin1().data(),filter_res);

    QString strtrace = str;
    strtrace = str.left(strtrace.size()-4);
    strtrace = strtrace + ".txt";
    savetrace(strtrace.toLatin1().data());
    QString strorigin = str;
    strorigin = str.left(strorigin.size()-4);
    strorigin = strorigin + ".ori";
    saveorigin(strorigin.toLatin1().data());
}
//创建左侧标题为"Param"的组合框
void MainWindow::CreateView()
{
//    QDesktopWidget* desktopWidget = QApplication::desktop();
//    QRect screenRect = desktopWidget->screenGeometry();
 //   g_nActScreenW = screenRect.width();
 //   g_nActScreenH = screenRect.height();

 //   qDebug("width = %d, height = %d",g_nActScreenW,g_nActScreenH);




    QGroupBox * gp1 = new QGroupBox(ui->centralWidget);
    gp1->setTitle(QStringLiteral("Param"));
    gp1->setGeometry(QRect(10,60,430,650));
    QGridLayout * gll1 = new QGridLayout(ui->centralWidget);

    gp1->setLayout(gll1);

    CreateParamView(gll1);

    QGroupBox * gp2 = new QGroupBox(ui->centralWidget);
    gp2->setTitle(QStringLiteral("Calib"));
    gp2->setGeometry(QRect(10,730,430,150));
    QGridLayout * gll2 = new QGridLayout(ui->centralWidget);

    gp2->setLayout(gll2);

    CreateCalibView(gll2);



    mptraceview = new ivtraceview();
    mptraceview->start();

    QLabel * pLabel = new QLabel(ui->centralWidget);
    pLabel->setGeometry(QRect(1400,60,80,30));
    pLabel->setText("Scale View");

    mpSlider = new QSlider(ui->centralWidget);
    mpSlider->setGeometry(QRect(1500,60,300,30));
    mpSlider->setOrientation(Qt::Horizontal);
    mpSlider->setValue(1);
    mpSlider->setRange(1,10);
    connect(mpSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeViewFac(int)));

    pLabel = new QLabel(ui->centralWidget);
    pLabel->setGeometry(QRect(1400,150,90,30));
    pLabel->setText("Cor Yaw:");

    QLineEdit * pLE = new QLineEdit(ui->centralWidget);
    pLE->setGeometry(QRect(1500,150,90,30));
    pLE->setText(QString::number(mfCalYaw));
    mpLE_CalYaw = pLE;

    pLabel = new QLabel(ui->centralWidget);
    pLabel->setGeometry(QRect(1600,150,90,30));
    pLabel->setText("Cor Pitch:");

    pLE = new QLineEdit(ui->centralWidget);
    pLE->setGeometry(QRect(1700,150,90,30));
    pLE->setText(QString::number(mfCalPitch));
    mpLE_CalPitch = pLE;

    pLabel = new QLabel(ui->centralWidget);
    pLabel->setGeometry(QRect(1400,200,90,30));
    pLabel->setText("Cor Roll:");

    pLE = new QLineEdit(ui->centralWidget);
    pLE->setGeometry(QRect(1500,200,90,30));
    pLE->setText(QString::number(mfCalRoll));
    mpLE_CalRoll = pLE;

    QPushButton * pPB = new QPushButton(ui->centralWidget);
    pPB->setGeometry(QRect(1700,200,90,30));
    pPB->setText("Set Cor");
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onSetCor()));


    pLabel = new QLabel(ui->centralWidget);
    pLabel->setGeometry(QRect(1400,300,90,30));
    pLabel->setText("Arm x:");

    pLE = new QLineEdit(ui->centralWidget);
    pLE->setGeometry(QRect(1500,300,90,30));
    pLE->setText(QString::number(mfCalYaw));
    mpLE_Armx = pLE;

    pLabel = new QLabel(ui->centralWidget);
    pLabel->setGeometry(QRect(1600,300,90,30));
    pLabel->setText("Arm y:");

    pLE = new QLineEdit(ui->centralWidget);
    pLE->setGeometry(QRect(1700,300,90,30));
    pLE->setText(QString::number(mfCalPitch));
    mpLE_Army = pLE;

    pLabel = new QLabel(ui->centralWidget);
    pLabel->setGeometry(QRect(1400,350,260,30));
    pLabel->setText("Arm is LIDAR TO GPS:");

    pPB = new QPushButton(ui->centralWidget);
    pPB->setGeometry(QRect(1700,350,90,30));
    pPB->setText("Set Arm");
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onSetArm()));

    mmyview = new MyView(ui->centralWidget);
    mmyview->setObjectName(QStringLiteral("graphicsView"));
    mmyview->setGeometry(QRect(460, 430, 600, 600));

    mmyview->setCacheMode(mmyview->CacheBackground);

    mscene = new QGraphicsScene;

    ui->pushButton_Reset->setEnabled(false);
}

void MainWindow::resizeEvent(QResizeEvent *event)
{
    qDebug("resize");
    QSize sizemain = ui->centralWidget->size();
    qDebug("size x = %d y=%d",sizemain.width(),sizemain.height());

    if((sizemain.width()>500) &&(sizemain.height()>450))
    {
        mmyview->setGeometry(460,430,sizemain.width()-500,sizemain.height()-450);
 //       ui->plainTextEdit->setGeometry(460,60,sizemain.width()-500,350);
    }


}

void MainWindow::CreateCalibView(QGridLayout * gl)
{
    gl->setSpacing(10);
    int i = 0;

    QLabel * pl = new QLabel(this);
    pl->setText("Yaw");
    pl->setFixedWidth(200);
    QLineEdit * ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText(" ");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    mpLE_CalcCalib_Yaw = ple;
    i++;

    pl = new QLabel(this);
    pl->setText("Pitch");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText(" ");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    mpLE_CalcCalib_Pitch = ple;
    i++;

    pl = new QLabel(this);
    pl->setText("Roll");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText(" ");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    mpLE_CalcCalib_Roll = ple;
    i++;
}

//创建组合框内的Label和LineEdit，并设置初始化值
void MainWindow::CreateParamView(QGridLayout *gl)
{
    gl->setSpacing(10);
    int i = 0;

    QLabel * pl = new QLabel(this);
    pl->setText("Resolution");
    pl->setFixedWidth(200);
    QLineEdit * ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("1");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_Resolution = ple;

    pl = new QLabel(this);
    pl->setText("Step Size");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("0.1");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_StepSize = ple;

    pl = new QLabel(this);
    pl->setText("Transformation Epsilon");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("0.01");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_TransFormationEpsilon = ple;

    pl = new QLabel(this);
    pl->setText("Maximum Iterations");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("30");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_MaximumIterations = ple;

    pl = new QLabel(this);
    pl->setText("Leaf Size");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("2");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_LeafSize = ple;

    pl = new QLabel(this);
    pl->setText("Minimum Scan Range");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("5");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_MinimunScanRange = ple;

    pl = new QLabel(this);
    pl->setText("Maximum Scan Range");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("200");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_MaximumScanRange = ple;

    pl = new QLabel(this);
    pl->setText("Minimum Add Scan Shift");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("1");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_MinimumAddScanShfit = ple;

    pl = new QLabel(this);
    pl->setText("Mode");
    pl->setFixedWidth(200);
    QComboBox * pcb = new QComboBox(this);
    pcb->addItem("CPU");
    pcb->addItem("OpenMP");

#ifdef CUDA_FOUND
    pcb->addItem("GPU");
#endif
    pcb->setFixedWidth(100);
    gl->addWidget(pl,i,0);
    gl->addWidget(pcb,i,1);
    i++;
    mpCB_Mode = pcb;

    pl = new QLabel(this);
    pl->setText("Filter Resolution");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("0.2");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_FilterResolution = ple;

    pl = new QLabel(this);
    pl->setText("Split Range");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("2000");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_SplitRange = ple;

    pl = new QLabel(this);
    pl->setText("Origin Longitude");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("117");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_OriginLon = ple;

    pl = new QLabel(this);
    pl->setText("Origin Latitude");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("39");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_OriginLat =ple;

    pl = new QLabel(this);
    pl->setText("Origin Height");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("0");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_OriginHeight = ple;

    pl = new QLabel(this);
    pl->setText("Origin Heading");
    pl->setFixedWidth(200);
    ple = new QLineEdit(this);
    ple->setFixedWidth(100);
    ple->setText("90");
    gl->addWidget(pl,i,0);
    gl->addWidget(ple,i,1);
    i++;
    mpLE_OriginHeading = ple;

    QPushButton * pPB =  new QPushButton(this);
    pPB->setText("Change Param");
    pPB->setFixedWidth(200);
    gl->addWidget(pPB,i,0);
    mpPB_ChangeParam = pPB;
    pPB =  new QPushButton(this);
    pPB->setText("Split");
    pPB->setFixedWidth(100);
    gl->addWidget(pPB,i,1);
    mpPB_Split = pPB;
    i++;

    connect(mpPB_ChangeParam,SIGNAL(clicked(bool)),this,SLOT(onChangeParam()));
    connect(mpPB_Split,SIGNAL(clicked(bool)),this,SLOT(onSplit()));

    QSpacerItem * xpsi2 = new QSpacerItem(200,1000,QSizePolicy::Maximum);

    gl->addItem(xpsi2,i,0);
}


void MainWindow::UpdateMapParam()
{
   double resolution,stepsize,epsilon,maxiter,leafsize,minscan,maxscan,minscanshift;
   bool bUseGPU = false;
   resolution = mpLE_Resolution->text().toDouble();
   stepsize = mpLE_StepSize->text().toDouble();
   epsilon = mpLE_TransFormationEpsilon->text().toDouble();
   maxiter = mpLE_MaximumIterations->text().toDouble();
   leafsize = mpLE_LeafSize->text().toDouble();
   minscan = mpLE_MinimunScanRange->text().toDouble();
   maxscan = mpLE_MaximumScanRange->text().toDouble();
   minscanshift = mpLE_MinimumAddScanShfit->text().toDouble();
   SetParam(resolution,stepsize,epsilon,maxiter,leafsize,minscan,maxscan,minscanshift,bUseGPU);
}

void MainWindow::onChangeParam()
{
    UpdateMapParam();
}

void MainWindow::onSplit()
{
    gbManualSave = true;
}

void MainWindow::on_pushButton_UsePoint_clicked()
{
    if((mvectorgps.size() <1)||(mvectortrace.size()<1))
    {
        return;
    }
    double x1,y1,z1,x2,y2,z2;
    iv::ndttracepoint xtrndt,xtrgps;
    xtrndt = mvectortrace[mvectortrace.size()-1];
    xtrgps = mvectorgps[mvectorgps.size()-1];
    iv::calib xcalib;
    xcalib.mndt = xtrndt;
    xcalib.mgps = xtrgps;
    mvectorcalib.push_back(xcalib);

    int i;
    char strout[10000];
    snprintf(strout,10000,"");
    for(i=0;i<mvectorcalib.size();i++)
    {
        char strtem[1000];
        snprintf(strtem,1000,"%6.2f %6.2f %6.2f   %6.2f %6.2f %6.2f\n",
                 mvectorcalib[i].mndt.x,mvectorcalib[i].mndt.y,mvectorcalib[i].mndt.z,
                 mvectorcalib[i].mgps.x,mvectorcalib[i].mgps.y,mvectorcalib[i].mgps.z);
        strncat(strout,strtem,10000);
    }
    ui->plainTextEdit_NDTGPS->setPlainText(strout);

}

void MainWindow::on_pushButton_Calc_clicked()
{
    int nsize = mvectorcalib.size();
    Eigen::Matrix3Xd in(3, nsize), out(3, nsize);

    int a = in.cols();
    a = a+1;
    for (int row = 0; row < in.rows(); row++) {
 //       for (int col = 0; col < in.cols(); col++) {
          in(row, 0) = mvectorcalib[row].mndt.x;
          in(row, 1) = mvectorcalib[row].mndt.y;
          in(row, 2) = mvectorcalib[row].mndt.z;
          out(row,0) = mvectorcalib[row].mgps.x;
          out(row,1) = mvectorcalib[row].mgps.y;
          out(row,2) = mvectorcalib[row].mgps.z;
 //       }
      }

    std::ostrstream ostr;
    Eigen::Affine3d A = Find3DAffineTransform(in, out);
    std::cout<<A.linear()<<std::endl;
    std::cout<<A.translation()<<std::endl;

    ostr<<"Rotation:"<<std::endl;
    ostr<<A.linear()<<std::endl;
    ostr<<"TransLation:"<<std::endl;
    ostr<<A.translation()<<std::endl;

    Eigen::Matrix3d t = A.linear();
    tf::Matrix3x3 mat_l;  // localizer
    mat_l.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)),
                   static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)),
                   static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2)));

    double roll,pitch,yaw;
    mat_l.getRPY(roll, pitch, yaw, 1);   

    std::cout<<"roll:"<<roll<<" pitch:"<<pitch<<" yaw:"<<yaw<<std::endl;

    ostr<<"roll:"<<roll<<" pitch:"<<pitch<<" yaw:"<<yaw<<std::endl;

    std::string strres = ostr.str();
    ui->plainTextEdit_Matrix->setPlainText(strres.substr(0,ostr.pcount()).data());

    mCalibMatrix = t;

    int i;
    for(i=0;i<mvectortracecalib.size();i++)
    {

        iv::ndttracepoint xtr = calibcalc(&mvectortracecalib[i]);
        mvectortracecalib[i].x = xtr.x;
        mvectortracecalib[i].y = xtr.y;
        mvectortracecalib[i].z = xtr.z;
    }

    mptraceview->SetNDTCalibTrace(mvectortracecalib);
}

void MainWindow::on_pushButton_Clear_clicked()
{
    mvectorcalib.erase(mvectorcalib.begin(),mvectorcalib.end());
    ui->plainTextEdit_NDTGPS->setPlainText("");

}

iv::ndttracepoint MainWindow::calibcalc(iv::ndttracepoint *p)
{
    iv::ndttracepoint x;
    Eigen::Vector3d xin;
    xin(0) = p->x;
    xin(1) = p->y;
    xin(2) = p->z;
    Eigen::Vector3d xout;
    xout = mCalibMatrix * xin;
    p->x = xout(0);
    p->y = xout(1);
    p->z = xout(2);
    x = *p;
    x.x = p->x;
    x.y = p->y;
    x.z = p->z;
    return x;

}

void MainWindow::on_pushButton_Pause_clicked()
{
    if(ui->pushButton_Pause->text() == "Pause")
    {
        ui->pushButton_Pause->setText("Conitinue");
        ui->pushButton_Reset->setEnabled(true);
        mbPause = true;
    }
    else
    {
        ui->pushButton_Pause->setText("Pause");
        ui->pushButton_Reset->setEnabled(false);
        mbPause = false;
    }
}

void MainWindow::on_pushButton_SetSkip_clicked()
{
    mnskip = ui->lineEdit_skip->text().toInt();
}

void MainWindow::on_pushButton_Reset_clicked()
{
    mvectorcalib.clear();
    mvectorgps.clear();
    mvectorrawgps.clear();
    mvectortrace.clear();
    mvectortracecalib.clear();

    mbIVDSetOrigin = false;
    mndtthread.reset();
}

void MainWindow::onChangeViewFac(int nFac)
{
    mptraceview->SetFac(nFac);
}

void MainWindow::onSetCor()
{
    mfCalYaw = mpLE_CalYaw->text().toDouble();
    mfCalPitch = mpLE_CalPitch->text().toDouble();
    mfCalRoll = mpLE_CalRoll->text().toDouble();
}

void MainWindow::onSetArm()
{
    arm_x = mpLE_Armx->text().toDouble();
    arm_y = mpLE_Army->text().toDouble();
}

void MainWindow::CalcCalib()
{
    if((mvectorgps.size()>0) &&(mvectortrace.size()>0))
    {
        double gps_x = mvectorgps[mvectorgps.size()-1].x;
        double gps_y = mvectorgps[mvectorgps.size()-1].y;
        double gps_z = mvectorgps[mvectorgps.size()-1].z;

        double ndt_x = mvectortrace[mvectortrace.size()-1].x;
        double ndt_y = mvectortrace[mvectortrace.size()-1].y;
        double ndt_z = mvectortrace[mvectortrace.size()-1].z;

        double gps_dis = sqrt(pow(gps_x,2)+pow(gps_y,2));
        double ndt_dis = sqrt(pow(ndt_x,2)+pow(ndt_y,2));

        if((gps_dis>0)&&(ndt_dis>0))
        {
            double gps_angle = asin(gps_y/gps_dis);
            if(gps_x < 0)
            {
                gps_angle = M_PI - gps_angle;
            }

            double ndt_angle = asin(ndt_y/ndt_dis);
            if(ndt_x < 0)
            {
                ndt_angle = M_PI - ndt_angle;
            }

            double calib_yaw = gps_angle - ndt_angle;
            while(calib_yaw>2.0*M_PI) calib_yaw = calib_yaw - 2.0*M_PI;
            while(calib_yaw < 0)calib_yaw = calib_yaw + 2.0*M_PI;

            mpLE_CalcCalib_Yaw->setText(QString::number(calib_yaw*180.0/M_PI));


            double calib_pitch = (-1.0) * asin((gps_z - ndt_z)/gps_dis);
            while(calib_pitch>2.0*M_PI) calib_pitch = calib_pitch - 2.0*M_PI;
            while(calib_pitch < 0)calib_pitch = calib_pitch + 2.0*M_PI;

            mpLE_CalcCalib_Pitch->setText(QString::number(calib_pitch * 180.0/M_PI));


        }

    }

}

void MainWindow::CalcCalibRoll()
{
    if((mvectorgps.size()>0) &&(mvectortrace.size()>0))
    {
        double gps_x = mvectorgps[mvectorgps.size()-1].x;
        double gps_y = mvectorgps[mvectorgps.size()-1].y;
        double gps_z = mvectorgps[mvectorgps.size()-1].z;

        double ndt_x = mvectortrace[mvectortrace.size()-1].x;
        double ndt_y = mvectortrace[mvectortrace.size()-1].y;
        double ndt_z = mvectortrace[mvectortrace.size()-1].z;

        double gps_dis = sqrt(pow(gps_x,2)+pow(gps_y,2));
        double ndt_dis = sqrt(pow(ndt_x,2)+pow(ndt_y,2));

        if((gps_dis>0)&&(ndt_dis>0) &&(fabs(gps_y)>10))
        {



            double calib_roll = (1.0) * asin((gps_z - ndt_z)/gps_y);
            while(calib_roll>2.0*M_PI) calib_roll = calib_roll - 2.0*M_PI;
            while(calib_roll < 0)calib_roll = calib_roll + 2.0*M_PI;

            mpLE_CalcCalib_Roll->setText(QString::number(calib_roll * 180.0/M_PI));


        }

    }

}