#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/slam/PoseRotationPrior.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <thread>
using namespace std;
using namespace gtsam;
#include <opencv2/opencv.hpp>


int height=500;
int width=500;
cv::Mat image=cv::Mat::zeros(width,height,CV_8UC3)+cv::Scalar(255,255,255);

void init_image(gtsam::Values& poses,cv::Scalar color)
{
    int size=poses.size();
    //poses.print("123");
    for(int i=0;i<size;++i)
    {
        gtsam::Pose2 pose=poses.at<Pose2>(i);
        int x=int(pose.x()*20+width/2);
        int y=int(pose.y()*20+height/2);

        cv::circle(image,cv::Point(x,y),3,color);
    }
}
void show_pose(gtsam::Values& poses,cv::Scalar color)
{
    int size=poses.size();
    std::cout<<" result size"<<size<<std::endl;
    cv::Mat rgb=image.clone();
    for(int i=0;i<size;++i)
    {
        gtsam::Pose2 pose=poses.at<Pose2>(i);
        int x=int(pose.x()*20+width/2);
        int y=int(pose.y()*20+height/2);

        cv::circle(rgb,cv::Point(x,y),3,color);
    }
    cv::imshow("win",rgb);
    //cv::imwrite("./img.jpg",image);
    cv::waitKey(0);
}

Eigen::Quaterniond rpy_quat(const Eigen::Vector3d& rpy)
{
    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(rpy[0],Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(rpy[1],Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(rpy[2],Eigen::Vector3d::UnitZ()));
    Eigen::Quaterniond quaternion;
    quaternion=yawAngle*pitchAngle*rollAngle;
    return quaternion;
}

Eigen::Vector3d quat_rpy(Eigen::Quaterniond quaternion)
{
    return quaternion.matrix().eulerAngles(2,1,0);
}


bool string2point(std::string str,pcl::PointXYZ& point)
{
    std::istringstream iss;
    iss.str(str);
    float value;
    float data[3]={0};
    for(int i=0;i<3;++i)
    {
        if(iss>>value)
        {
            data[i]=value;
        }
        else
        {
            return false;
        }
    }
    point.x=data[0]/1000.0;
    point.y=data[1]/1000.0;
    point.z=data[2]/1000.;
    return true;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file)
{
    std::ifstream fin(file.c_str());
    const int line_length=64;
    char str[line_length]={0};
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    while(fin.getline(str,line_length))
    {
        pcl::PointXYZ point;
        if(string2point(str,point))
        {
            cloud->push_back(point);
        }
    }
    return cloud;
}


const int THREAD_NUM=1;
bool exits[THREAD_NUM]={false};
std::vector<pcl::PointXYZ>  line;
void test_zzjj(float mean)
{
    using gtsam::symbol_shorthand::Y; //


    NonlinearFactorGraph graph;

    gtsam::ISAM2Params parameters;
    parameters.relinearizeThreshold = 0.1;
    parameters.relinearizeSkip = 1;
    gtsam::ISAM2* isam=new gtsam::ISAM2(parameters);

    Values initials;

    const int NUM=line.size();
    
/*
 * 定义两个sigma及高斯模型
 * sigma代表测量数据概率分布（可信度）
 * sigma_a代表前后两个点z值增量概率分布（可信度）
 */

    Eigen::Matrix<double,1,1> sigma=Eigen::Matrix<double,1,1>::Ones()*0.15;
    noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Sigmas(sigma);
    //sigma_a=0.01 时允许优化出来的线条前后值差在 1cm左右（调节该值即调整优化曲线的平滑度）
    Eigen::Matrix<double,1,1> sigma_a=Eigen::Matrix<double,1,1>::Ones()*0.001;
    noiseModel::Diagonal::shared_ptr priorModel_a = noiseModel::Diagonal::Sigmas(sigma_a);

    //添加优化变量节点，赋初值为测量值
    for(int i=0;i<NUM;++i)
    {
        initials.insert(Y(i),line[i].z);
    }
    graph.add(PriorFactor<float>(Y(0), line[0].z, priorModel));
    for(int i=0;i<NUM-1;++i)
    {
        //增加测量值先验概率分布， 均值为测量值，方差为sigma_t的分布
        float dy=line[i+1].z-mean;
        //动态调整测量值的先验概率分布， 低点赋予高可信度
        float r=0.2/(1.0+exp(-10000.*(dy-0.001)))+0.003;
        Eigen::Matrix<double,1,1> sigma_t=Eigen::Matrix<double,1,1>::Ones()*r;
        noiseModel::Diagonal::shared_ptr priorModel_t = noiseModel::Diagonal::Sigmas(sigma_t);
        graph.add(PriorFactor<float>(Y(i+1), line[i+1].z, priorModel_t));
        //增加前后两点z值增量的概率分布，均值为0，方差为sigma_a的分布
        graph.add(BetweenFactor<float>(Y(i), Y(i+1), 0., priorModel_a));
    }


    isam->update(graph,initials);
    isam->update();
    gtsam::Values results=isam->calculateEstimate();


    //保存结果
    FILE* p=fopen("./line.txt","w");

    for(int i=0;i<line.size();++i)
    {
        pcl::PointXYZ point=line[i];

        fprintf(p,"%f %f %f 0 255 0\n",point.x,point.y,results.at<float>(Y(i)));
        if(point.z-results.at<float>(Y(i))>0.003)
        {
            fprintf(p,"%f %f %f 255 0 0\n",point.x,point.y,point.z);
        }
        else
        {
            fprintf(p,"%f %f %f 255 255 255\n",point.x,point.y,point.z);
        }
    }

    /*std::cout<<std::endl<<"-------------y-----------"<<std::endl;

    for(int i=0;i<20;++i)
    {
        printf("%.5f   ",float(i)/10.0);
    }

    printf("\n");
    for(int i=0;i<20;++i)
    {
        printf("%.5f   ",y[i]);
    }

    printf("\n");


    for(int i=0;i<20;++i)
    {
        double s=results.at<Point2>(Y(i)).x();

        if(y[i]-s>0.001)
        {
            printf("%.4f*   ",s);
        }
        else
        {
            printf("%.5f   ", s);
        }
    }
    printf("\n");
*/
    exits[0]=true;

}
//升序排列
bool cmp(const pcl::PointXYZ& value1,const pcl::PointXYZ& value2)
{
    return value1.x>value2.x;
}

int main(int argc, char** argv) 
{
    //读点云， /1000，调整单位为m
    pcl::PointCloud<pcl::PointXYZ>::Ptr  cloud=ReadTxtCloud("/home/zx/zzw/TESTCODE/zhjj/in_4.txt");

    //抽取一帧数据
    float sum=0;
    for(int i=0;i<cloud->size();++i)
    {
        pcl::PointXYZ point=cloud->points[i];
        if(fabs(point.y-1.100)<0.00001)
        {
            line.push_back(point);
            sum+=point.z;
        }
    }
    float mean=sum/float(line.size());
    printf(" line size : %d  mean:%f\n",line.size(),mean);
    //排序
    std::sort(line.begin(),line.end(),cmp);

    //计时，等待线程完成
    auto t0 = std::chrono::steady_clock::now();
    //在线程中优化
    std::thread* th1=new std::thread(test_zzjj,mean);

    while(1)
    {
        bool quite=true;
        for(int i=0;i<THREAD_NUM;++i)
        {
            if(exits[i]==false)
            {
                quite = false;
                break;
            }
        }
        if(quite==true)
            break;
        usleep(1);
    }
    auto t1 = std::chrono::steady_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0);
    double time=double(duration.count()) * std::chrono::microseconds::period::num /
            std::chrono::microseconds::period::den;
    std::cout << "time : "<<time<<std::endl;
    printf("  completed!!!");

    return 0;
}