//
// Created by zx on 23-8-30.
//

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)

typedef struct{
    gtsam::Vector3 acc;
    gtsam::Vector3 angular;
    double time;
}ImuData;

int main(){

  gtsam::ISAM2Params isam_param_;
  gtsam::ISAM2* isam_;
  gtsam::NonlinearFactorGraph graph_;
  gtsam::PreintegratedImuMeasurements* imu_preint_;

  isam_param_.factorization = gtsam::ISAM2Params::QR;
  isam_param_.relinearizeThreshold=0.1;
  isam_param_.relinearizeSkip=1;
  isam_=new gtsam::ISAM2(isam_param_);

  boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(9.804);
  p->accelerometerCovariance  = gtsam::Matrix33::Identity(3,3) * pow(3.9939570888238808e-03, 2); // acc white noise in continuous
  p->gyroscopeCovariance      = gtsam::Matrix33::Identity(3,3) * pow(1.5636343949698187e-03, 2); // gyro white noise in continuous
  p->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-5, 2); // error committed in integrating position from velocities

  printf("111111111111\n");
  gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias
  printf("22222222222\n");
  imu_preint_=new gtsam::PreintegratedImuMeasurements(p,prior_imu_bias);

  gtsam::Rot3 rot1=gtsam::Rot3::RzRyRx(gtsam::Vector3(0.013963,0.000259, 0.007608));
  gtsam::Pose3 odom1(rot1,gtsam::Point3(-0.006571,0.098890,-0.001847));

  gtsam::noiseModel::Diagonal::shared_ptr firstPoseNoise = gtsam::noiseModel::Diagonal::Sigmas(
      (gtsam::Vector(6) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m
  gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), odom1, firstPoseNoise);
  graph_.add(priorPose);
  // initial velocity
  gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
  gtsam::PriorFactor<gtsam::Vector3> priorVel1(V(0), gtsam::Vector3(0, 0, 0), priorVelNoise);
  graph_.add(priorVel1);
  // initial bias
  gtsam::imuBias::ConstantBias prevBias1 = gtsam::imuBias::ConstantBias(gtsam::Vector3(0,0,0),gtsam::Vector3(0,0,0));
  gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1);
  gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias1(B(0), prevBias1, priorBiasNoise);
  graph_.add(priorBias1);
  // add values
  gtsam::Values values1;
  values1.insert(X(0), odom1);
  values1.insert(V(0), gtsam::Vector3(0, 0, 0));
  values1.insert(B(0), prevBias1);
  isam_->update(graph_, values1);
  printf("33333333333333333333333\n");
  //----------------------------------
  imu_preint_->integrateMeasurement(gtsam::Vector3(1.030218,-0.420155,9.750587),gtsam::Vector3(-0.011047,0.007924,-0.023697),0.002000);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.897726,-0.491497,9.755317),gtsam::Vector3(-0.010926,0.009534,-0.023994),0.001004);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.827207,-0.540855,9.725967),gtsam::Vector3(-0.008861,0.008472,-0.028502),0.000997);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.922143,-0.452133,9.736796),gtsam::Vector3(-0.002425,0.011689,-0.025543),0.004016);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.962495,-0.422689,9.734901),gtsam::Vector3(-0.007844,0.009144,-0.024148),0.000984);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.827710,-0.500624,9.730005),gtsam::Vector3(-0.008007,0.011182,-0.023402),0.000950);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.860463,-0.433599,9.737790),gtsam::Vector3(-0.007499,0.007286,-0.023085),0.004009);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.964682,-0.399865,9.713343),gtsam::Vector3(-0.004727,0.002974,-0.016032),0.001045);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.833733,-0.482052,9.703702),gtsam::Vector3(-0.007651,0.007997,-0.022628),0.000999);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.784919,-0.498876,9.701908),gtsam::Vector3(-0.006508,0.004443,-0.026508),0.004019);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.919361,-0.416548,9.692280),gtsam::Vector3(-0.005517,0.002733,-0.021557),0.000981);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.935489,-0.360304,9.709715),gtsam::Vector3(-0.007209,0.002510,-0.013403),0.000941);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.777420,-0.476960,9.699254),gtsam::Vector3(-0.008287,-0.000821,-0.013210),0.010026);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.759608,-0.504171,9.676722),gtsam::Vector3(-0.008540,-0.001701,-0.014538),0.001045);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.871459,-0.393245,9.678445),gtsam::Vector3(-0.009490,-0.006492,-0.010146),0.000929);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.779426,-0.450930,9.684661),gtsam::Vector3(-0.007062,-0.008040,-0.012283),0.001067);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.715236,-0.446302,9.701901),gtsam::Vector3(-0.010659,-0.012908,-0.012445),0.000998);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.842707,-0.373925,9.682480),gtsam::Vector3(-0.013231,-0.012922,-0.010434),0.001004);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.854209,-0.415411,9.645583),gtsam::Vector3(-0.013140,-0.017353,-0.003272),0.000951);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.674500,-0.475582,9.687313),gtsam::Vector3(-0.011439,-0.018694,-0.005149),0.000978);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.682276,-0.445558,9.695731),gtsam::Vector3(-0.013350,-0.024521,-0.011790),0.001066);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.876566,-0.322494,9.694254),gtsam::Vector3(-0.015991,-0.027012,-0.005146),0.004012);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.871890,-0.321667,9.709745),gtsam::Vector3(-0.020464,-0.032633,0.001735),0.000934);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.652600,-0.473016,9.703427),gtsam::Vector3(-0.019761,-0.034679,0.001833),0.001030);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.728786,-0.379186,9.736034),gtsam::Vector3(-0.020478,-0.035970,-0.001026),0.004017);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.858471,-0.290078,9.749356),gtsam::Vector3(-0.023504,-0.038637,0.002624),0.000990);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.747279,-0.394640,9.743360),gtsam::Vector3(-0.030160,-0.040991,0.001225),0.001007);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.712789,-0.395489,9.762823),gtsam::Vector3(-0.031004,-0.043510,-0.003281),0.004014);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.848782,-0.306907,9.752598),gtsam::Vector3(-0.033113,-0.046355,0.002112),0.000984);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.804205,-0.341677,9.751830),gtsam::Vector3(-0.035746,-0.051016,0.005141),0.000980);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.700957,-0.404746,9.758069),gtsam::Vector3(-0.040663,-0.051699,-0.002269),0.004012);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.731385,-0.420330,9.742807),gtsam::Vector3(-0.041512,-0.055137,-0.006984),0.000966);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.899754,-0.322278,9.751014),gtsam::Vector3(-0.042045,-0.057319,-0.004849),0.001039);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.872774,-0.397510,9.745684),gtsam::Vector3(-0.043887,-0.058833,-0.000490),0.003997);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.733693,-0.499446,9.732361),gtsam::Vector3(-0.042933,-0.061853,-0.011483),0.001012);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.882428,-0.385174,9.741079),gtsam::Vector3(-0.045303,-0.061822,-0.013289),0.000939);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.919037,-0.400405,9.726874),gtsam::Vector3(-0.046301,-0.060051,-0.015828),0.004067);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.809744,-0.480754,9.730498),gtsam::Vector3(-0.049750,-0.058505,-0.023399),0.000990);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.874894,-0.473431,9.714948),gtsam::Vector3(-0.048886,-0.059212,-0.030573),0.001003);
  imu_preint_->integrateMeasurement(gtsam::Vector3(1.009512,-0.419129,9.695473),gtsam::Vector3(-0.049214,-0.061532,-0.028183),0.004017);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.908168,-0.462400,9.725821),gtsam::Vector3(-0.049176,-0.062181,-0.028629),0.000990);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.837434,-0.529312,9.729707),gtsam::Vector3(-0.045532,-0.057796,-0.039177),0.000997);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.934994,-0.524931,9.703054),gtsam::Vector3(-0.045626,-0.054849,-0.052150),0.004013);
  imu_preint_->integrateMeasurement(gtsam::Vector3(1.071312,-0.480165,9.702712),gtsam::Vector3(-0.042694,-0.054766,-0.047290),0.000925);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.935572,-0.599491,9.685145),gtsam::Vector3(-0.041888,-0.054443,-0.050481),0.001091);
  imu_preint_->integrateMeasurement(gtsam::Vector3(0.886452,-0.601381,9.699040),gtsam::Vector3(-0.040211,-0.051722,-0.063193),0.003927);
  imu_preint_->integrateMeasurement(gtsam::Vector3(1.076890,-0.518920,9.674937),gtsam::Vector3(-0.035591,-0.047722,-0.066651),0.001082);
  imu_preint_->integrateMeasurement(gtsam::Vector3(1.039718,-0.605801,9.631043),gtsam::Vector3(-0.029952,-0.044489,-0.068591),0.001888);

  graph_.resize(0);
  gtsam::NavState prevState(odom1,gtsam::Vector3(0, 0, 0));
  gtsam::NavState propState = imu_preint_->predict(prevState, prevBias1);
  // add pose factor
  gtsam::Rot3 rot2=gtsam::Rot3::RzRyRx(gtsam::Vector3(0.016913 ,0.000888 ,0.011003));
  gtsam::Pose3 odom2(rot2,gtsam::Point3(0.022109, 0.097983 ,0.011235));
  printf("4444444444444444444444\n");
  gtsam::noiseModel::Diagonal::shared_ptr lidarPoseNoise = gtsam::noiseModel::Diagonal::Sigmas(
      (gtsam::Vector(6) << 0.02, 0.02,0.02, 0.5*M_PI/180.0, 0.5*M_PI/180.0, 0.5*M_PI/180.0).finished());
  gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(1), odom2, lidarPoseNoise);
  printf("5555555555555555555555555555\n");
  graph_.add(pose_factor);

  gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise2 = gtsam::noiseModel::Isotropic::Sigma(3, 0.1); // m/s
  gtsam::PriorFactor<gtsam::Vector3> velFactor(V(1),propState.v(),priorVelNoise);
  graph_.add(velFactor);

  double imuAccBias = 6.4356659353532566e-05, imuGyrBias = 3.5640318696367613e-05;
  gtsam::Vector noiseModelBetweenBias = (gtsam::Vector(6)
      << imuAccBias, imuAccBias, imuAccBias, imuGyrBias, imuGyrBias, imuGyrBias).finished();
  printf("666666666666666666666666666666\n");
  graph_.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(0), B(1), gtsam::imuBias::ConstantBias(),
                                                                gtsam::noiseModel::Diagonal::Sigmas(
                                                                    0.1 * noiseModelBetweenBias)));

  const gtsam::PreintegratedImuMeasurements &preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements &>(*imu_preint_);
  gtsam::ImuFactor imu_factor(X(0), V(0), X(1), V(1), B(0), preint_imu);
  graph_.add(imu_factor);
  gtsam::Values values2;
  printf("777777777777777777777\n");
  values2.insert(X(1), propState.pose());
  values2.insert(V(1), propState.v());
  values2.insert(B(1), prevBias1);
  isam_->update(graph_, values2);
  isam_->update();
  printf("8888888888888888888888888888\n");
  gtsam::Values result = isam_->calculateEstimate();
  gtsam::Pose3 pose = result.at<gtsam::Pose3>(X(1));
  gtsam::Vector3 vel= result.at<gtsam::Vector3>(V(1));
  gtsam::imuBias::ConstantBias bias = result.at<gtsam::imuBias::ConstantBias>(B(1));

  printf("  vel:%f %f %f,Bias:%f %f %f,%f %f %f\n",vel.x(),vel.y(),vel.z(),
         bias.vector()[0],bias.vector()[1],bias.vector()[2],
         bias.vector()[3],bias.vector()[4],bias.vector()[5]);

  return 0;

}