//
// Created by zx on 23-6-20.
//
#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 "Edge3Factor.h"
bool SolveXYZ(std::vector<gtsam::Vector3> xyzs)
{

  if(xyzs.size()<1){
    printf("xyzs size must >= 1\n");
    return false;
  }
  using gtsam::symbol_shorthand::X;
  using gtsam::symbol_shorthand::Y;
  using gtsam::symbol_shorthand::Z;
  //定义图
  gtsam::NonlinearFactorGraph graph;
  gtsam::ISAM2Params parameters;
  parameters.relinearizeThreshold = 0.1;
  parameters.relinearizeSkip = 1;
  gtsam::ISAM2* isam=new gtsam::ISAM2(parameters);

  //定义待求因子
  const int NUM=xyzs.size();

  //
  double a=1.0;
  double b=1.0;
  double dx=0.1;
  double dy=0.2;
  double dz= sqrt(a*a*dx*dx+b*b*dy*dy)*2;

  double x=xyzs[0][0];
  double y=xyzs[0][1];
  double z=xyzs[0][2];
  gtsam::Values initials;

  initials.insert(X(0),x);
  initials.insert(Y(0),y);
  initials.insert(Z(0),z);

  for (int i=0;i<NUM;++i){
    double x=xyzs[i][0];
    double y=xyzs[i][1];
    double z=xyzs[i][2];
    printf(" prior xyz :%.5f %.5f %.5f\n",x,y,z);
    //先验分布
    Eigen::Matrix<double,1,1> sigma_x=Eigen::Matrix<double,1,1>::Ones()*dx;
    gtsam::noiseModel::Diagonal::shared_ptr priorModel_x = gtsam::noiseModel::Diagonal::Sigmas(sigma_x);
    graph.add(gtsam::PriorFactor<double>(X(0), x, priorModel_x));

    Eigen::Matrix<double,1,1> sigma_y=Eigen::Matrix<double,1,1>::Ones()*dy;
    gtsam::noiseModel::Diagonal::shared_ptr priorModel_y = gtsam::noiseModel::Diagonal::Sigmas(sigma_y);
    graph.add(gtsam::PriorFactor<double>(Y(0), y, priorModel_y));

    Eigen::Matrix<double,1,1> sigma_z=Eigen::Matrix<double,1,1>::Ones()*dz;
    gtsam::noiseModel::Diagonal::shared_ptr priorModel_z = gtsam::noiseModel::Diagonal::Sigmas(sigma_z);
    graph.add(gtsam::PriorFactor<double>(Z(0), z, priorModel_z));
  }

  for(int i=0;i<NUM;++i){
    Eigen::Matrix<double,2,1> sigma_ab=Eigen::Matrix<double,2,1>::Zero();
    sigma_ab(0,0)=0.0001;
    sigma_ab(1,0)=0.0001;
    gtsam::noiseModel::Diagonal::shared_ptr priorModel_t = gtsam::noiseModel::Diagonal::Sigmas(sigma_ab);
    //增加前后两点z值增量的概率分布，均值为0，方差为sigma_a的分布
    graph.add(Edge3Factor(X(0),Y(0),Z(0), a,b, priorModel_t));
  }
  isam->update(graph,initials);
  isam->update();
  gtsam::Values results=isam->calculateEstimate();


  double ox=results.at<double>(X(0));
  double oy=results.at<double>(Y(0));
  double oz=results.at<double>(Z(0));

  printf(" ox oy oz:%.5f %.5f %.5f  error z:%.5f\n",ox,oy,oz,(a*ox+b*oy)-oz);
  return true;

}

int main(){

  gtsam::Pose3 poseSrc(gtsam::Rot3::RzRyRx(0,0,-3.0),
                       gtsam::Point3(0,0,0));
  gtsam::Pose3 poseTar(gtsam::Rot3::RzRyRx(1,0,0),
                       gtsam::Point3(1,0,0));
  gtsam::Pose3 between = poseSrc.between(poseTar);
  printf("between:(%f %f %f),(%f,%f,%f)\n",between.x(),between.y(),between.z(),
         between.rotation().roll(),between.rotation().pitch(),between.rotation().yaw());
  return 0;
  std::vector<gtsam::Vector3> xyzs;
  gtsam::Vector3 xyz(1,2,4);
  xyzs.push_back(xyz);
  printf(" x y z:%.5f %.5f %.5f\n",xyz[0],xyz[1],xyz[2]);
  SolveXYZ(xyzs);
}