/* ----------------------------------------------------------------------------

 * amsi Copyright 2019, Positioning and Navigation Laboratory,
 * Hong Kong Polytechnic University
 * All Rights Reserved
 * Authors: Weisong Wen, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file imuFactorsExample
 * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor navigation code.
 * @author Weisong Wen (weisong.wen@connect.polyu.hk)
 */

/**
 * Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS
 *  - you can test imuFactor (resp. combinedImuFactor) by commenting (resp. uncommenting)
 *  the line #define USE_COMBINED (few lines below)
 *  - we read IMU and GPS data from a CSV file, with the following format:
 *  A row starting with "i" is the first initial position formatted with
 *  N, E, D, qx, qY, qZ, qW, velN, velE, velD
 *  A row starting with "0" is an imu measurement
 *  linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
 *  A row starting with "1" is a gps correction formatted with
 *  N, E, D, qX, qY, qZ, qW
 *  Note that for GPS correction, we're only using the position not the rotation. The
 *  rotation is provided in the file for ground truth comparison.
 */

// GTSAM related includes.
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> // DoglegOptimizer
#include <gtsam/nonlinear/DoglegOptimizer.h> // DoglegOptimizer

#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/geometry/Point2.h>

#include <gtsam/nonlinear/ISAM2.h>

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <fstream>
#include <iostream>

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>

//time
#include <time.h>

// ros and math related head files
#include <vector>
#include <ros/ros.h>

#include <amsi/gnss_tools.hpp>

#include <nmea_msgs/Sentence.h> // message from DGPS of University of California, Berkeley.
#include <sensor_msgs/NavSatFix.h>
#include <Eigen/Dense>
#include <novatel_msgs/BESTPOS.h> // novatel_msgs/INSPVAX

#include <random> // gaussian noise

#include <amsi/gnss_tools.hpp>

#include <nlosExclusion/GNSS_Raw_Array.h>
#include <gtsam/gnssNavigation/nonBiasStates.h>
#include <gtsam/gnssNavigation/PseudorangeFactor_spp.h>
#include <gtsam/robustModels/switchVariableLinear.h>
#include <gtsam/robustModels/PseudorangeSwitchFactor.h>

#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>

// Uncomment line below to use the CombinedIMUFactor as opposed to the standard ImuFactor.
// #define USE_COMBINED

using namespace gtsam;
using namespace std;
using namespace vertigo;

// using namespace Eigen;
#define INF 10000
#define pi 3.1415926
#define D2R 3.1415926/180.0

using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
using symbol_shorthand::C; // Point2 receiver clock bias  (cb_g,cb_b)
using symbol_shorthand::S; // switch factor

bool enable_IMUPreintegration_factor = 0; // enable imu pre-integration factor
bool enable_Pr_factor = 1; //enable pseudorange factor
bool enbale_CVM_factor = 1; // enable constant velocity factor
bool enbale_SCP_factor = 0; // enable switchable constraint pseudorange factor
bool enable_GNSS_loose_factor = 0; // enbale GNSS loose factor (use WLS solution)

double _vx = 0, _vy = 0, _vz = 0; //
clock_t old_clock = clock();
clock_t old_imu_clock = clock();

const string output_filename = "imuFactorExampleResults.csv";

FILE* gps_imu_loose_fg_result = fopen("/home/wenws/amsipolyu/IONGNSS2019_result/gps_imu_loose_fg_result.csv", "w+");

// This will either be PreintegratedImuMeasurements (for ImuFactor) or
// PreintegratedCombinedMeasurements (for CombinedImuFactor).
PreintegrationType *imu_preintegrated_;
sensor_msgs::Imu imu_track;
bool imu_up =0 ;
int imu_co=0;

bool gps_up = 0;
nav_msgs::Odometry odom_track,span_odom, odom_track_ENU, span_odom_ecef;

Eigen::MatrixXd orientation_matrix, acceleration_rotation;
double x,y,z,w;

// gnss_tools
GNSS_Tools gnss_tools_;
Eigen::MatrixXd originllh,originllh_span; // origin llh
Eigen::MatrixXd referencellh; // origin llh
sensor_msgs::NavSatFix ini_navf,ini_navf_span ; // initial sensor msg
// referencellh.resize(3, 1);
// // start point of robot
// referencellh(0) = 116.4986266; //116.4986357 (pre-test) 116.4986266 (semi-final compet ) 116.4986473 (semi-final test )
// referencellh(1) = 39.7917427; // 39.7917481 ( pre-test) 39.7917427 (semi-final  compet ) 39.7917502 (semi-final  test )
// referencellh(2) = 22.1009979248; // 116.4986266 (semi-final )

ros::Subscriber span_BP_sub;
ros::Publisher span_odom_pub;

nlosExclusion::GNSS_Raw_Array _GNSS_data; // save the GNSS data

Point3 nomXYZ(-2418954.90428,5384679.60663,2407391.40139); // niutoujiao data
// Point3 nomXYZ(-2417729.42895,5384849.60804,2408314.07879); // initial position : kowlontoon data
// Point3 nomXYZ(0, 0, 0); // niutoujiao data
double prediction_pre_t=0;

typedef struct
  {
    double wx;
    double wy;
    double wz;
    double ax;
    double ay;
    double az;
  }imu_bias; // imu bias

  typedef struct
  {
    double wx;
    double wy;
    double wz;
    double ax;
    double ay;
    double az;
  }imu_noise; // imu bias

  typedef struct
  {
    imu_bias bias;
    imu_noise noise;
  }imu_para; // imu bias

  typedef struct
  {
    double mean;
    double std;
  }statistical_para; // imu bias || positioning evaluation

imu_para imu_parameter; // online calibrated imu parameters using begin 2000 frames of imu raw measurements
bool use_online_imu_cal = 0;
bool online_cal_success = 0;
bool offline_cal_success = 0;

vector<sensor_msgs::Imu> imu_queue;
int imu_queue_size = 10;

double gps_cov = 0;
double psr_cov = 0;

double nstamp = 0;

  /**
   * @brief statistical parameters inference
   * @param vector<double>
   * @return statistical_para
   @
   */
  statistical_para statis_cal(vector<double> input)
  {
    statistical_para result;
    double sum = std::accumulate(std::begin(input), std::end(input), 0.0);
    double mean =  sum / input.size();

    double accum  = 0.0;
    std::for_each (std::begin(input), std::end(input), [&](const double d) {
      accum  += (d-mean)*(d-mean);
    });

    double stdev = sqrt(accum/(input.size()-1));

    result.mean = mean;
    result.std = stdev;
    return result;
  }

/**
   * @brief imu bias, noise calculation (calibration)
   * @param imu queue
   * @return imu_para
   @
   */
  imu_para imu_calibtation(vector<sensor_msgs::Imu> input)
  {
    imu_para para; // bias + noise
    imu_bias bias; // bias of imu
    imu_noise noise; // noise of imu

    // cout<<"imu bias calculation (calibration)"<<endl;
    vector<double> wx_queue;
    double wx_sum = 0, wx_mean = 0;

    vector<double> wy_queue;
    double wy_sum = 0, wy_mean = 0;

    vector<double> wz_queue;
    double wz_sum = 0, wz_mean = 0;

    vector<double> ax_queue;
    double ax_sum = 0, ax_mean = 0;

    vector<double> ay_queue;
    double ay_sum = 0, ay_mean = 0;

    vector<double> az_queue;
    double az_sum = 0, az_mean = 0;

    for(int i = 0; i < input.size()-1; i ++)
    {
      wx_queue.push_back(input[i].angular_velocity.x);
      wy_queue.push_back(input[i].angular_velocity.y);
      wz_queue.push_back(input[i].angular_velocity.z);

      ax_queue.push_back(input[i].linear_acceleration.x);
      ay_queue.push_back(input[i].linear_acceleration.y);
      az_queue.push_back(input[i].linear_acceleration.z);
    }

    statistical_para result = statis_cal(wx_queue);
    para.bias.wx = result.mean;
    para.noise.wx = result.std;

    result = statis_cal(wy_queue);
    para.bias.wy = result.mean;
    para.noise.wy = result.std;

    result = statis_cal(wz_queue);
    para.bias.wz = result.mean;
    para.noise.wz = result.std;

    result = statis_cal(ax_queue);
    para.bias.ax = result.mean;
    para.noise.ax = result.std;

    result = statis_cal(ay_queue);
    para.bias.ay = result.mean;
    para.noise.ay = result.std;

    result = statis_cal(az_queue);
    para.bias.az = result.mean;
    para.noise.az = result.std;

    return para;
  }

/**
   * @brief imu callback
   * @param imu msg
   * @return void
   * calibrate using the first several epochs, the gravity can be minused
   @
   */
  void imu_callback(const sensor_msgs::Imu::Ptr& input) //
  {
    double g_ = 1;
    // std::cout << " IMU data call back" << input->angular_velocity.x << std::endl;
    imu_track = * input;

    imu_queue.push_back(imu_track);
    // cout<<"imu_queue_size -> "<<imu_queue.size()<<endl;
    if( (imu_queue.size() > imu_queue_size) && (online_cal_success ==0) && (use_online_imu_cal ==1))
    {
      online_cal_success = 1;
      const clock_t begin_time = clock();
      cout<<"-------------------start calibrate imu------------------------ "<<endl;
      imu_parameter = imu_calibtation(imu_queue);
      std::cout << "imu Calibration used  time -> " << double(clock() - begin_time) / CLOCKS_PER_SEC << "\n\n";
      cout<<"bias ax ->" <<imu_parameter.bias.ax<<endl;
      cout<<"bias ay ->" <<imu_parameter.bias.ay<<endl;
      cout<<"bias az ->" <<imu_parameter.bias.az<<endl;

      cout<<"bias wx ->" <<imu_parameter.bias.wx<<endl;
      cout<<"bias wy ->" <<imu_parameter.bias.wy<<endl;
      cout<<"bias wz ->" <<imu_parameter.bias.wz<<endl;
    }
    if(use_online_imu_cal)
    {
        // decrease the bias
      imu_track.angular_velocity.x = imu_track.angular_velocity.x - (imu_parameter.bias.wx);
      imu_track.angular_velocity.y = imu_track.angular_velocity.y - (imu_parameter.bias.wy);
      imu_track.angular_velocity.z = imu_track.angular_velocity.z - (imu_parameter.bias.wz);

      imu_track.linear_acceleration.x = imu_track.linear_acceleration.x - (imu_parameter.bias.ax);
      imu_track.linear_acceleration.y = imu_track.linear_acceleration.y - (imu_parameter.bias.ay);
      imu_track.linear_acceleration.z = imu_track.linear_acceleration.z - (imu_parameter.bias.az);
    }
    else if(!use_online_imu_cal) // if do not online calibrate, use offiline calibration (1h statistical static data)
    {

        // decrease the bias
      imu_track.angular_velocity.x = imu_track.angular_velocity.x - (-0.00231128);
      imu_track.angular_velocity.y = imu_track.angular_velocity.y - (0.0019349);
      imu_track.angular_velocity.z = imu_track.angular_velocity.z - (-0.00109033);

      imu_track.linear_acceleration.x = imu_track.linear_acceleration.x - (-0.0223799);
      imu_track.linear_acceleration.y = imu_track.linear_acceleration.y - (-0.1048587);
      imu_track.linear_acceleration.z = imu_track.linear_acceleration.z - (9.791944); //9.791944
      // imu_track.linear_acceleration.z = imu_track.linear_acceleration.z - (0.01);
      offline_cal_success = 1;
    }

     /****in the tightly coupled GNSS/INS integration, transfer imu from body to local frame***/
    x = imu_track.orientation.x;
    y = imu_track.orientation.y;
    z = imu_track.orientation.z;
    w = imu_track.orientation.w;
    double imu_roll, imu_pitch, imu_yaw;
    tf::Quaternion imu_orientation;
    tf::quaternionMsgToTF(input->orientation, imu_orientation);
    tf::Matrix3x3(imu_orientation).getRPY(imu_roll, imu_pitch, imu_yaw);
    // cout<<"yaw : = " <<imu_yaw*(180/3.14)<<endl;
    // cout<<imu_track.orientation.x<<endl;
    // cout<<"imu_track.linear_acceleration.x : = " <<endl<<imu_track.linear_acceleration.x<<endl;
    Eigen::Quaterniond q(w,x,y,z);
    q.normalized();
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix=q.toRotationMatrix();
    orientation_matrix.resize(3,1);
    orientation_matrix<<imu_track.linear_acceleration.x,
                        imu_track.linear_acceleration.y,
                        imu_track.linear_acceleration.z;
    acceleration_rotation = rotation_matrix.transpose() * orientation_matrix;

    // cout<<"acceleration_rotation : = " <<endl<<acceleration_rotation<<endl;
    imu_track.linear_acceleration.x = acceleration_rotation(0,0);
    imu_track.linear_acceleration.y = acceleration_rotation(1,0);
    imu_track.linear_acceleration.z = acceleration_rotation(2,0);

    /****in the tightly coupled GNSS/INS integration, navigation frame is ECEF***/
    if(originllh_span.rows() > 1)
    {
      double lon = (double)originllh_span(0) * D2R;
      double lat = (double)originllh_span(1) * D2R;

      double e = imu_track.linear_acceleration.x; // acc in enu
      double n = imu_track.linear_acceleration.y;
      double u = imu_track.linear_acceleration.z;

      imu_track.linear_acceleration.x = 0 - sin(lon) * e - cos(lon) * sin(lat) * n + cos(lon) * cos(lat) * u; // transfrom the acc from enu to ecef
      imu_track.linear_acceleration.y = 0 + cos(lon) * e - sin(lon) * sin(lat) * n + cos(lat) * sin(lon) * u;
      imu_track.linear_acceleration.z = 0 + cos(lat) * n + sin(lat) * u;
    }

    imu_up =1;
    imu_co++;
  }

void imu_callback1(const sensor_msgs::Imu::Ptr& input) // the pre-integration needs the data in NED, but the imu data is in ENU
{
  // std::cout << " IMU data call back" << input->angular_velocity.x << std::endl;
  imu_track = * input;
  // decrease the bias
  // imu_track.angular_velocity.x = imu_track.angular_velocity.x - (-0.00231128);
  // imu_track.angular_velocity.y = imu_track.angular_velocity.y - (0.0019349);
  // imu_track.angular_velocity.z = imu_track.angular_velocity.z - (-0.000309033);

  // imu_track.linear_acceleration.x = imu_track.linear_acceleration.x - (-0.0000563799);
  // imu_track.linear_acceleration.y = imu_track.linear_acceleration.y - (-0.0004587);
  // imu_track.linear_acceleration.z = imu_track.linear_acceleration.z - (0.0979159);

  // in ENU
  imu_track.angular_velocity.x = imu_track.angular_velocity.x - (-0.00303897);
  imu_track.angular_velocity.y = imu_track.angular_velocity.y - (0.00170488);
  imu_track.angular_velocity.z = imu_track.angular_velocity.z - (0.000641023);

  imu_track.linear_acceleration.x = imu_track.linear_acceleration.x - (-0.00224483);
  imu_track.linear_acceleration.y = imu_track.linear_acceleration.y - (0.000396544);
  // imu_track.linear_acceleration.z = imu_track.linear_acceleration.z - (0.0978841);
  imu_track.linear_acceleration.z = 0;

  // in NED
  // imu_track.angular_velocity.y = imu_track.angular_velocity.x - (-0.00303897);
  // imu_track.angular_velocity.x = imu_track.angular_velocity.y - (0.00170488);
  // imu_track.angular_velocity.z = -1 * (imu_track.angular_velocity.z - (0.000641023));

  // imu_track.linear_acceleration.y = imu_track.linear_acceleration.x - (-0.00224483);
  // imu_track.linear_acceleration.x = imu_track.linear_acceleration.y - (0.000396544);
  // imu_track.linear_acceleration.z = -1 * (imu_track.linear_acceleration.z - (0.0978841));

  imu_up =1;
  imu_co++;

}

void odom_callback(const nav_msgs::Odometry::ConstPtr& input)
{
  //std::cout << __func__ << std::endl;

  // odom_track = *input;
  // gps_up = 1;
}

void ubloxFix_callback(const sensor_msgs::NavSatFixConstPtr& fix_msg)
  {
    // cout<<"jd /fix received "<<endl;
    sensor_msgs::NavSatFix navfix_ ;
    navfix_.header = fix_msg->header;
    navfix_.latitude = fix_msg->latitude;
    navfix_.longitude = fix_msg->longitude;
    navfix_.altitude = fix_msg->altitude;

    if(ini_navf.latitude == NULL)
      {
        ini_navf = navfix_;
        std::cout<<"ini_navf.header  -> "<<ini_navf.header<<std::endl;
        originllh.resize(3, 1);
        originllh(0) = navfix_.longitude;
        originllh(1) = navfix_.latitude;
        originllh(2) = navfix_.altitude;
        std::cout<<"reference longitude: "<<navfix_.longitude<<std::endl;
        std::cout<<"reference latitude: "<<navfix_.latitude<<std::endl;
      }
      Eigen::MatrixXd curLLh; //
      curLLh.resize(3, 1);
      curLLh(0) = navfix_.longitude;
      curLLh(1) = navfix_.latitude;
      curLLh(2) = navfix_.altitude;

      Eigen::MatrixXd ecef; //
      ecef.resize(3, 1);
      ecef = gnss_tools_.llh2ecef(curLLh);
      Eigen::MatrixXd eigenENU;; //
      eigenENU.resize(3, 1);
      eigenENU = gnss_tools_.ecef2enu(originllh_span,ecef);
      /*
      gps(0) = odom_track.pose.pose.position.x;
      gps(1) = odom_track.pose.pose.position.y;
      gps(2) = odom_track.pose.pose.position.z;
      gps(3) = odom_track.pose.pose.orientation.x;
      gps(4) = odom_track.pose.pose.orientation.y;
      gps(5) = odom_track.pose.pose.orientation.z;
      gps(6) = 1;
      */
      odom_track.header.frame_id = "odom";
      // odom_track.pose.pose.position.x = eigenENU(0);
      // odom_track.pose.pose.position.y = eigenENU(1);
      // add Gaussian noise
      std::default_random_engine generator;

      auto dist_E = std::bind(std::normal_distribution<double>{0, 3},
                      std::mt19937(std::random_device{}()));
      double random_E = dist_E();

      auto dist_N = std::bind(std::normal_distribution<double>{0, 4},
                      std::mt19937(std::random_device{}()));
      double random_N = dist_N();

      // odom_track.pose.pose.position.x = eigenENU(0) + random_E;
      // odom_track.pose.pose.position.y = eigenENU(1) + random_N;

      std::cout<<"distribution_E -> "<< random_E<<std::endl;

      odom_track.pose.pose.position.z = 0;
      odom_track.pose.pose.orientation.x = 0.03;
      odom_track.pose.pose.orientation.y = 0.01;
      odom_track.pose.pose.orientation.z = 0.04;
      odom_track.pose.pose.orientation.w = 0.012;
      if(originllh_span.size()) // initial llh from span-cpt is available
        gps_up = 1;

  }

/**
   * @brief satellite set validation
   * @param prn
   * @return ture/false
   @
   */
  bool PRNisGPS(int prn)
  {
    if (prn <= 32 || prn == 84)
      return true;
    else{
      return false;
    }
  }

  /**
   * @brief satellite set validation
   * @param prn
   * @return ture/false
   @
   */
  bool PRNisGLONASS(int prn)
  {
    if (prn > 32 && prn <= 56)
      return true;
    else{
      return false;
    }
  }

  /**
   * @brief satellite set validation
   * @param prn
   * @return ture/false
   @
   */
  bool PRNisBeidou(int prn)
  {
    if ((prn <= 121) && (prn >= 87))
      return true;
    else{
      return false;
    }
  }

  /**
   * @brief covariance estimation
   * @param nlosExclusion::GNSS_Raw_Array GNSS_data
   * @return weight_matrix
   @
   */
  Eigen::MatrixXd cofactorMatrixCal_EKF(nlosExclusion::GNSS_Raw_Array GNSS_data)
  {
    double snr_1 = 50.0; // T = 50
    double snr_A = 30.0; // A = 30
    double snr_a = 30.0;// a = 30
    double snr_0 = 10.0; // F = 10
    Eigen::VectorXd cofactor_;  // cofactor of satellite
    cofactor_.resize(GNSS_data.GNSS_Raws.size());
    for(int i = 0; i < GNSS_data.GNSS_Raws.size(); i++)
    {
      if( (PRNisGPS(GNSS_data.GNSS_Raws[i].prn_satellites_index)) || (PRNisBeidou(GNSS_data.GNSS_Raws[i].prn_satellites_index)) )
      {
        double snr_R = GNSS_data.GNSS_Raws[i].snr;
        double elR = GNSS_data.GNSS_Raws[i].elevation;
        double q_R_1 = 1 / (pow(( sin(elR * pi/180.0 )),2));
        double q_R_2 = pow(10,(-(snr_R - snr_1) / snr_a));
        double q_R_3 = (((snr_A / (pow(10,(-(snr_0 - snr_1) / snr_a))) - 1) / (snr_0 - snr_1)) * (snr_R - snr_1) + 1);
        double q_R = q_R_1* (q_R_2 * q_R_3);
        cofactor_[i]=(float(q_R)); // uncertainty: cofactor_[i] larger, larger uncertainty
      }
    }
    // cout<<"cofactor_ -> "<<cofactor_<<endl;

    Eigen::MatrixXd weight_matrix;
    weight_matrix.resize(GNSS_data.GNSS_Raws.size(),GNSS_data.GNSS_Raws.size());
    weight_matrix.setIdentity();
    for(int k = 0; k < weight_matrix.rows(); k++)
    {
      weight_matrix.row(k) = weight_matrix.row(k) * cofactor_(k);
    }

    return weight_matrix;
  }

  /**
   * @brief covariance estimation
   * @param nlosExclusion::GNSS_Raw_Array GNSS_data
   * @return weight_matrix
   @
   */
  double cofactorMatrixCal_single_satellite(double ele, double snr)
  {
    double cofactor_ = 0;
    double snr_1 = 50.0; // T = 50
    double snr_A = 30.0; // A = 30
    double snr_a = 30.0;// a = 30
    double snr_0 = 10.0; // F = 10

    double snr_R = snr;
    double elR = ele;
    double q_R_1 = 1 / (pow(( sin(elR * pi/180.0 )),2));
    double q_R_2 = pow(10,(-(snr_R - snr_1) / snr_a));
    double q_R_3 = (((snr_A / (pow(10,(-(snr_0 - snr_1) / snr_a))) - 1) / (snr_0 - snr_1)) * (snr_R - snr_1) + 1);
    double q_R = q_R_1* (q_R_2 * q_R_3);
    cofactor_ = (float(q_R)); // uncertainty: cofactor_[i] larger, larger uncertainty

    return cofactor_;
  }

  /**
   * @brief covariance estimation
   * @param nlosExclusion::GNSS_Raw_Array GNSS_data
   * @return weight_matrix
   @
   */
  Eigen::MatrixXd cofactorMatrixCal_WLS(nlosExclusion::GNSS_Raw_Array GNSS_data)
  {
    double snr_1 = 50.0; // T = 50
    double snr_A = 30.0; // A = 30
    double snr_a = 30.0;// a = 30
    double snr_0 = 10.0; // F = 10
    Eigen::VectorXd cofactor_;  // cofactor of satellite
    cofactor_.resize(GNSS_data.GNSS_Raws.size());
    for(int i = 0; i < GNSS_data.GNSS_Raws.size(); i++)
    {
      if( (PRNisGPS(GNSS_data.GNSS_Raws[i].prn_satellites_index)) || (PRNisBeidou(GNSS_data.GNSS_Raws[i].prn_satellites_index)) )
      {
        double snr_R = GNSS_data.GNSS_Raws[i].snr;
        double elR = GNSS_data.GNSS_Raws[i].elevation;
        double q_R_1 = 1 / (pow(( sin(elR * pi/180.0 )),2));
        double q_R_2 = pow(10,(-(snr_R - snr_1) / snr_a));
        double q_R_3 = (((snr_A / (pow(10,(-(snr_0 - snr_1) / snr_a))) - 1) / (snr_0 - snr_1)) * (snr_R - snr_1) + 1);
        double q_R = q_R_1* (q_R_2 * q_R_3);
        cofactor_[i]=(1.0/float(q_R)); // uncertainty: cofactor_[i] larger, larger uncertainty
      }
    }
    // cout<<"cofactor_ -> "<<cofactor_<<endl;

    Eigen::MatrixXd weight_matrix;
    weight_matrix.resize(GNSS_data.GNSS_Raws.size(),GNSS_data.GNSS_Raws.size());
    weight_matrix.setIdentity();
    for(int k = 0; k < weight_matrix.rows(); k++)
    {
      weight_matrix.row(k) = weight_matrix.row(k) * cofactor_(k);
    }

    return weight_matrix;
  }

  /**
   * @brief  least square for signle point positioning
   * @param eAllSVPositions ((n,4) prn, sx, sy, sz, )     eAllSVPositions ((n,3) PRN CNO Pseudorange)
   * @return eWLSSolution 5 unknowns with two clock bias variables
   @
  */
  Eigen::MatrixXd LeastSquare(Eigen::MatrixXd eAllSVPositions, Eigen::MatrixXd eAllMeasurement){

    Eigen::MatrixXd eWLSSolution;
    eWLSSolution.resize(5, 1);

    /**after read the obs file, one measure is not right**/
    int validNumMeasure=0;
    std::vector<int> validMeasure;
    for (int idx = 0; idx < eAllMeasurement.rows(); idx++){
      for (int jdx = 0; jdx < eAllSVPositions.rows(); jdx++){
        if (int(eAllMeasurement(idx, 0)) == int(eAllSVPositions(jdx, 0))){
          validNumMeasure++;
          validMeasure.push_back(int(eAllMeasurement(idx, 0)));
        }
      }
    }

    Eigen::MatrixXd validMeasurement; // for WLS
    validMeasurement.resize(validNumMeasure,eAllMeasurement.cols());
    for (int idx = 0; idx < eAllMeasurement.rows(); idx++){
      for (int jdx = 0; jdx < eAllSVPositions.rows(); jdx++){
        if (int(eAllMeasurement(idx, 0)) == int(eAllSVPositions(jdx, 0))){
          for (int kdx = 0; kdx < eAllMeasurement.cols(); kdx++){
            // std::cout<<"satellite prn -> "<<eAllMeasurement(idx, 0)<<"\n"<<std::endl;
            validMeasurement(idx, kdx) = eAllMeasurement(idx, kdx);

          }
        }
      }
    }

    int iNumSV = validMeasurement.rows();

    /*Find the received SV and Sort based on the order of Measurement matrix*/
    Eigen::MatrixXd eExistingSVPositions; // for WLS
    eExistingSVPositions.resize(iNumSV, eAllSVPositions.cols());

    for (int idx = 0; idx < validMeasurement.rows(); idx++){
      for (int jdx = 0; jdx < eAllSVPositions.rows(); jdx++){
        if (int(validMeasurement(idx, 0)) == int(eAllSVPositions(jdx, 0))){
          for (int kdx = 0; kdx < eAllSVPositions.cols(); kdx++){
            // std::cout<<"satellite prn -> "<<eAllMeasurement(idx, 0)<<"\n"<<std::endl;
            eExistingSVPositions(idx, kdx) = eAllSVPositions(jdx, kdx);

          }
        }
      }
    }
    //for (int idx = 0; idx < eExistingSVPositions.rows(); idx++){
    //  printf("%2d-[%3d] - (%10.2f,%10.2f,%10.2f) %f\n", idx, int(eExistingSVPositions(idx, 0)), eExistingSVPositions(idx, 1), eExistingSVPositions(idx, 2), eExistingSVPositions(idx, 3), eExistingSVPositions(idx, 4)*CLIGHT);
    //}

    //Intialize the result by guessing.
    for (int idx = 0; idx < eWLSSolution.rows(); idx++){
      eWLSSolution(idx, 0) = 0;
    }

    // for the case of insufficient satellite
    if (iNumSV < 5){
      return eWLSSolution;
    }

    bool bWLSConverge = false;

    int count = 0;
    while (!bWLSConverge)
    {
      Eigen::MatrixXd eH_Matrix;
      eH_Matrix.resize(iNumSV, eWLSSolution.rows());

      Eigen::MatrixXd eDeltaPr;
      eDeltaPr.resize(iNumSV, 1);

      Eigen::MatrixXd eDeltaPos;
      eDeltaPos.resize(eWLSSolution.rows(), 1);

      for (int idx = 0; idx < iNumSV; idx++){

        int prn = int(validMeasurement(idx, 0));
        double pr = validMeasurement(idx, 2);

        // Calculating Geometric Distance
        double rs[3], rr[3], e[3];
        double dGeoDistance;

        rs[0] = eExistingSVPositions(idx, 1);
        rs[1] = eExistingSVPositions(idx, 2);
        rs[2] = eExistingSVPositions(idx, 3);

        rr[0] = eWLSSolution(0);
        rr[1] = eWLSSolution(1);
        rr[2] = eWLSSolution(2);

        // dGeoDistance = geodist(rs, rr, e);
        dGeoDistance = sqrt(pow((rs[0] - rr[0]),2) + pow((rs[1] - rr[1]),2) +pow((rs[2] - rr[2]),2));

        // Making H matrix
        eH_Matrix(idx, 0) = -(rs[0] - rr[0]) / dGeoDistance;
        eH_Matrix(idx, 1) = -(rs[1] - rr[1]) / dGeoDistance;
        eH_Matrix(idx, 2) = -(rs[2] - rr[2]) / dGeoDistance;

        if (PRNisGPS(prn)){
          eH_Matrix(idx, 3) = 1;
          eH_Matrix(idx, 4) = 0;
        }
        else if (PRNisBeidou(prn))
        {
          eH_Matrix(idx, 3) = 1;
          eH_Matrix(idx, 4) = 1;
        }

        // Making delta pseudorange
        double rcv_clk_bias;
        if (PRNisGPS(prn)){
          rcv_clk_bias = eWLSSolution(3);
        }
        else if (PRNisBeidou(prn))
        {
          rcv_clk_bias = eWLSSolution(4);
        }
        // double sv_clk_bias = eExistingSVPositions(idx, 4) * CLIGHT;
        eDeltaPr(idx, 0) = pr - dGeoDistance + rcv_clk_bias;
        //printf("%2d - %f %f %f %f \n", prn, pr, dGeoDistance, eDeltaPr(idx, 0), rcv_clk_bias);
      }

      // Least Square Estimation
      eDeltaPos = (eH_Matrix.transpose() * eH_Matrix).ldlt().solve(eH_Matrix.transpose() *  eDeltaPr);
      //eDeltaPos = (eH_Matrix.transpose() * eH_Matrix).inverse() * eH_Matrix.transpose() *  eDeltaPr;
      //eDeltaPos = eH_Matrix.householderQr().solve(eDeltaPr);

      //for (int idx = 0; idx < eDeltaPos.rows(); idx++)
      //  printf("%f ", eDeltaPos(idx));
      //printf("\n");

      eWLSSolution(0) += eDeltaPos(0);
      eWLSSolution(1) += eDeltaPos(1);
      eWLSSolution(2) += eDeltaPos(2);
      eWLSSolution(3) += eDeltaPos(3);
      eWLSSolution(4) += eDeltaPos(4);

      for (int i = 0; i < 3; ++i){
        //printf("%f\n", fabs(eDeltaPos(i)));
        if (fabs(eDeltaPos(i)) >1e-4)
        {
          bWLSConverge = false;
        }
        else {
          bWLSConverge = true;
        };

      }
      count += 1;
      if (count > 6)
        bWLSConverge = true;
    }
    // printf("WLS -> (%11.2f,%11.2f,%11.2f)\n\n", eWLSSolution(0), eWLSSolution(1), eWLSSolution(2));
    std::cout << std::setprecision(12);
    // cout<< "---------------WLS (ECEF) x, y, z, bias_gps, bias_beidou-----------------  \n"<<eWLSSolution<<endl;

    return eWLSSolution;
  }

  /**
   * @brief  weighted least square for signle point positioning
   * @param eAllSVPositions ((n,4) prn, sx, sy, sz, )     eAllSVPositions ((n,3) PRN CNO Pseudorange)
   * @return eWLSSolution 5 unknowns with two clock bias variables
   @
  */
  Eigen::MatrixXd WeightedLeastSquare(Eigen::MatrixXd eAllSVPositions, Eigen::MatrixXd eAllMeasurement, nlosExclusion::GNSS_Raw_Array GNSS_data){

    Eigen::MatrixXd eWLSSolution;
    eWLSSolution.resize(5, 1);

    Eigen::MatrixXd weight_matrix = cofactorMatrixCal_WLS(GNSS_data);

    /**after read the obs file, one measure is not right**/
    int validNumMeasure=0;
    std::vector<int> validMeasure;
    for (int idx = 0; idx < eAllMeasurement.rows(); idx++){
      for (int jdx = 0; jdx < eAllSVPositions.rows(); jdx++){
        if (int(eAllMeasurement(idx, 0)) == int(eAllSVPositions(jdx, 0))){
          validNumMeasure++;
          validMeasure.push_back(int(eAllMeasurement(idx, 0)));
        }
      }
    }

    Eigen::MatrixXd validMeasurement; // for WLS
    validMeasurement.resize(validNumMeasure,eAllMeasurement.cols());
    for (int idx = 0; idx < eAllMeasurement.rows(); idx++){
      for (int jdx = 0; jdx < eAllSVPositions.rows(); jdx++){
        if (int(eAllMeasurement(idx, 0)) == int(eAllSVPositions(jdx, 0))){
          for (int kdx = 0; kdx < eAllMeasurement.cols(); kdx++){
            // std::cout<<"satellite prn -> "<<eAllMeasurement(idx, 0)<<"\n"<<std::endl;
            validMeasurement(idx, kdx) = eAllMeasurement(idx, kdx);

          }
        }
      }
    }

    int iNumSV = validMeasurement.rows();

    /*Find the received SV and Sort based on the order of Measurement matrix*/
    Eigen::MatrixXd eExistingSVPositions; // for WLS
    eExistingSVPositions.resize(iNumSV, eAllSVPositions.cols());

    for (int idx = 0; idx < validMeasurement.rows(); idx++){
      for (int jdx = 0; jdx < eAllSVPositions.rows(); jdx++){
        if (int(validMeasurement(idx, 0)) == int(eAllSVPositions(jdx, 0))){
          for (int kdx = 0; kdx < eAllSVPositions.cols(); kdx++){
            // std::cout<<"satellite prn -> "<<eAllMeasurement(idx, 0)<<"\n"<<std::endl;
            eExistingSVPositions(idx, kdx) = eAllSVPositions(jdx, kdx);

          }
        }
      }
    }
    //for (int idx = 0; idx < eExistingSVPositions.rows(); idx++){
    //  printf("%2d-[%3d] - (%10.2f,%10.2f,%10.2f) %f\n", idx, int(eExistingSVPositions(idx, 0)), eExistingSVPositions(idx, 1), eExistingSVPositions(idx, 2), eExistingSVPositions(idx, 3), eExistingSVPositions(idx, 4)*CLIGHT);
    //}

    //Intialize the result by guessing.
    for (int idx = 0; idx < eWLSSolution.rows(); idx++){
      eWLSSolution(idx, 0) = 0;
    }

    // for the case of insufficient satellite
    if (iNumSV < 5){
      return eWLSSolution;
    }

    bool bWLSConverge = false;

    int count = 0;
    while (!bWLSConverge)
    {
      Eigen::MatrixXd eH_Matrix;
      eH_Matrix.resize(iNumSV, eWLSSolution.rows());

      Eigen::MatrixXd eDeltaPr;
      eDeltaPr.resize(iNumSV, 1);

      Eigen::MatrixXd eDeltaPos;
      eDeltaPos.resize(eWLSSolution.rows(), 1);

      for (int idx = 0; idx < iNumSV; idx++){

        int prn = int(validMeasurement(idx, 0));
        double pr = validMeasurement(idx, 2);

        // Calculating Geometric Distance
        double rs[3], rr[3], e[3];
        double dGeoDistance;

        rs[0] = eExistingSVPositions(idx, 1);
        rs[1] = eExistingSVPositions(idx, 2);
        rs[2] = eExistingSVPositions(idx, 3);

        rr[0] = eWLSSolution(0);
        rr[1] = eWLSSolution(1);
        rr[2] = eWLSSolution(2);

        // dGeoDistance = geodist(rs, rr, e);
        dGeoDistance = sqrt(pow((rs[0] - rr[0]),2) + pow((rs[1] - rr[1]),2) +pow((rs[2] - rr[2]),2));

        // Making H matrix
        eH_Matrix(idx, 0) = -(rs[0] - rr[0]) / dGeoDistance;
        eH_Matrix(idx, 1) = -(rs[1] - rr[1]) / dGeoDistance;
        eH_Matrix(idx, 2) = -(rs[2] - rr[2]) / dGeoDistance;

        if (PRNisGPS(prn)){
          eH_Matrix(idx, 3) = 1;
          eH_Matrix(idx, 4) = 0;
        }
        else if (PRNisBeidou(prn))
        {
          eH_Matrix(idx, 3) = 1;
          eH_Matrix(idx, 4) = 1;
        }

        // Making delta pseudorange
        double rcv_clk_bias;
        if (PRNisGPS(prn)){
          rcv_clk_bias = eWLSSolution(3);
        }
        else if (PRNisBeidou(prn))
        {
          rcv_clk_bias = eWLSSolution(4);
        }
        // double sv_clk_bias = eExistingSVPositions(idx, 4) * CLIGHT;
        eDeltaPr(idx, 0) = pr - dGeoDistance + rcv_clk_bias;
        //printf("%2d - %f %f %f %f \n", prn, pr, dGeoDistance, eDeltaPr(idx, 0), rcv_clk_bias);
      }

      // Least Square Estimation
      // eDeltaPos = (eH_Matrix.transpose() * weight_matrix * eH_Matrix).ldlt().solve(eH_Matrix.transpose() * weight_matrix *  eDeltaPr);
      eDeltaPos = (eH_Matrix.transpose() * weight_matrix * eH_Matrix).inverse() * eH_Matrix.transpose() * weight_matrix * eDeltaPr;
      //eDeltaPos = eH_Matrix.householderQr().solve(eDeltaPr);

      //for (int idx = 0; idx < eDeltaPos.rows(); idx++)
      //  printf("%f ", eDeltaPos(idx));
      //printf("\n");

      eWLSSolution(0) += eDeltaPos(0);
      eWLSSolution(1) += eDeltaPos(1);
      eWLSSolution(2) += eDeltaPos(2);
      eWLSSolution(3) += eDeltaPos(3);
      eWLSSolution(4) += eDeltaPos(4);

      for (int i = 0; i < 3; ++i){
        //printf("%f\n", fabs(eDeltaPos(i)));
        if (fabs(eDeltaPos(i)) >1e-4)
        {
          bWLSConverge = false;
        }
        else {
          bWLSConverge = true;
        };

      }
      count += 1;
      if (count > 6)
        bWLSConverge = true;
    }
    // printf("WLS -> (%11.2f,%11.2f,%11.2f)\n\n", eWLSSolution(0), eWLSSolution(1), eWLSSolution(2));
    std::cout << std::setprecision(12);
    // cout<< "---------------WLS (ECEF) x, y, z, bias_gps, bias_beidou-----------------  \n"<<eWLSSolution<<endl;

    return eWLSSolution;
  }

  /**
   * @brief delay function
   * @param seconds for delay
   * @return void
   @
   */
  void wait(int seconds) // delay function
  {
    clock_t endwait,start;
    start = clock();
    endwait = clock() + seconds * CLOCKS_PER_SEC;
    while (clock() < endwait) {
      if(clock() - start > CLOCKS_PER_SEC)
      {
        start = clock();
        std::cout<<".......1 s"<<std::endl;
      }
    }
  }

 /**
   * @brief GNSS raw callback
   * @param GNSS msg
   * @return void
    std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
    nlosExclusion/GNSS_Raw[] GNSS_Raws
    float64 GNSS_time
    float64 total_sv
    float64 prn_satellites_index
    float64 pseudorange
    float64 snr
    float64 elevation
    float64 azimuth
    float64 err_tropo
    float64 err_iono
    float64 sat_clk_err
    float64 sat_pos_x
    float64 sat_pos_y
    float64 sat_pos_z
    int64 visable
    string sat_system
    BeiDou: 88->88+37
    GPS: 1->32
   @
   */
  void GNSS_raw_callback(const nlosExclusion::GNSS_Raw_Array::Ptr& input)
  {
    nstamp ++;
    double ps_std = 10;
    // cout<<"GNSS raw data"<<endl;
    nlosExclusion::GNSS_Raw_Array GNSS_data = *input;
    _GNSS_data = GNSS_data;
    /**
   * @brief weighted least square for signle point positioning
   * @param eAllSVPositions ((n,4) prn, sx, sy, sz, )     eAllSVPositions ((n,3) PRN CNO Pseudorange)
   * @return eWLSSolution 5 unknowns with two clock bias variables
   @
    */
    Eigen::MatrixXd eAllSVPositions; // satellite positions
    Eigen::MatrixXd eAllMeasurement; // pseudorange measurements
    eAllSVPositions.resize(GNSS_data.GNSS_Raws.size(), 4);
    eAllMeasurement.resize(GNSS_data.GNSS_Raws.size(), 3);
    // double stamp_ = (ros::Time::now()).toSec() - 1551496923;
    for(int i =0; i < GNSS_data.GNSS_Raws.size(); i++) // for weighted least square
    {
      // add Gaussian noise
      std::default_random_engine generator;

      auto dist_E = std::bind(std::normal_distribution<double>{0, ps_std},
                      std::mt19937(std::random_device{}()));
      double random_psr = dist_E();

      eAllSVPositions(i,0) = GNSS_data.GNSS_Raws[i].prn_satellites_index;
      eAllSVPositions(i,1) = GNSS_data.GNSS_Raws[i].sat_pos_x;
      eAllSVPositions(i,2) = GNSS_data.GNSS_Raws[i].sat_pos_y;
      eAllSVPositions(i,3) = GNSS_data.GNSS_Raws[i].sat_pos_z;

      eAllMeasurement(i,0) = GNSS_data.GNSS_Raws[i].prn_satellites_index;
      eAllMeasurement(i,1) = GNSS_data.GNSS_Raws[i].snr;
      // eAllMeasurement(i,2) = GNSS_data.GNSS_Raws[i].pseudorange + random_psr;
      eAllMeasurement(i,2) = GNSS_data.GNSS_Raws[i].pseudorange ;

      // std::cout<<"span_ecef -> "<<span_ecef<<std::endl;
    }
    Eigen::MatrixXd  eWLSSolutionECEF; // 5 unknowns with two clock bias variables
    // eWLSSolutionECEF = LeastSquare(eAllSVPositions, eAllMeasurement);  //
    eWLSSolutionECEF = WeightedLeastSquare(eAllSVPositions, eAllMeasurement, GNSS_data);  //WeightedLeastSquare

    Eigen::MatrixXd ecef;
    Eigen::MatrixXd enu_wls;
    enu_wls.resize(3, 1);
    ecef.resize(3, 1);
    ecef(0) = eWLSSolutionECEF(0);
    ecef(1) = eWLSSolutionECEF(1);
    ecef(2) = eWLSSolutionECEF(2);
    enu_wls = gnss_tools_.ecef2enu(originllh_span, ecef);

    odom_track.header.frame_id = "odom";

    odom_track_ENU.header.frame_id = "odom";

    // if(((int)nstamp%100) == 0)
    // {
    //   random_N = 35;
    //   random_E = 35;
    // }

    Eigen::MatrixXd weight_matrix = cofactorMatrixCal_WLS(GNSS_data);
    psr_cov = 0;
    for(int m = 0;  m< weight_matrix.cols(); m++)
    {
      psr_cov = psr_cov + 1.0 / weight_matrix(m,m);
    }
    psr_cov = psr_cov / weight_matrix.cols();
    psr_cov = psr_cov/ 100.0;

    odom_track.pose.pose.position.x = ecef(0);
    odom_track.pose.pose.position.y = ecef(1);
    odom_track.pose.pose.position.z = ecef(2);

    odom_track_ENU.pose.pose.position.x = enu_wls(0);
    odom_track_ENU.pose.pose.position.y = enu_wls(1);
    odom_track_ENU.pose.pose.position.z = enu_wls(2);

    // odom_track.pose.pose.position.x = enu_wls(0) + random_E;
    // odom_track.pose.pose.position.y = enu_wls(1) + random_N;

    // std::cout<< "enu_wls(0) -> "<< enu_wls(0) <<"enu_wls(1)-> "<< enu_wls(1) <<"psr_cov -> " <<psr_cov <<  std::endl;

    if(originllh_span.size()) // initial llh from span-cpt is available
        gps_up = 1;
  }

  void span_bp_callback(const novatel_msgs::BESTPOSConstPtr& fix_msg)
  {
    // cout<<"jd /fix received "<<endl;
    sensor_msgs::NavSatFix navfix_ ;
    navfix_.latitude = fix_msg->latitude;
    navfix_.longitude = fix_msg->longitude;
    navfix_.altitude = fix_msg->altitude;

    if(ini_navf_span.latitude == NULL)
      {
        ini_navf_span = navfix_;
        std::cout<<"ini_navf_span.header  -> "<<ini_navf_span.header<<std::endl;
        originllh_span.resize(3, 1);
        originllh_span(0) = navfix_.longitude;
        originllh_span(1) = navfix_.latitude;
        originllh_span(2) = navfix_.altitude;
        std::cout<<"reference longitude: "<<navfix_.longitude<<std::endl;
        std::cout<<"reference latitude: "<<navfix_.latitude<<std::endl;
      }
      Eigen::MatrixXd curLLh; //
      curLLh.resize(3, 1);
      curLLh(0) = navfix_.longitude;
      curLLh(1) = navfix_.latitude;
      curLLh(2) = navfix_.altitude;

      Eigen::MatrixXd ecef; //
      ecef.resize(3, 1);
      ecef = gnss_tools_.llh2ecef(curLLh);
      Eigen::MatrixXd eigenENU;; //
      eigenENU.resize(3, 1);
      eigenENU = gnss_tools_.ecef2enu(originllh_span,ecef);

      span_odom.header.frame_id = "odom";
      span_odom.pose.pose.position.x = eigenENU(0);
      span_odom.pose.pose.position.y = eigenENU(1);
      span_odom.pose.pose.position.z = 0;
      span_odom.pose.pose.orientation.x = 0.03;
      span_odom.pose.pose.orientation.y = 0.01;
      span_odom.pose.pose.orientation.z = 0.04;
      span_odom.pose.pose.orientation.w = 0.012;
      span_odom_pub.publish(span_odom);

      span_odom_ecef = span_odom;
      span_odom_ecef.pose.pose.position.x = ecef(0);
      span_odom_ecef.pose.pose.position.y = ecef(1);
      span_odom_ecef.pose.pose.position.z = ecef(2);
      span_odom_ecef.pose.pose.orientation.x = 0.03;
      span_odom_ecef.pose.pose.orientation.y = 0.01;
      span_odom_ecef.pose.pose.orientation.z = 0.04;
      span_odom_ecef.pose.pose.orientation.w = 0.012;

  }

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "GNSSINS2NonLinear");
  std::cout<<"GNSSINS2NonLinear node......"<<std::endl;

  ros::NodeHandle nh;

  ros::Subscriber param_sub = nh.subscribe("/imu/data", 50, imu_callback); // imu data from Xsens Mti 10
  ros::Subscriber odom_sub = nh.subscribe("/Odom_0", 50, odom_callback); //  odom information from Zhixingzhe
  ros::Publisher factorGraphPose_pub = nh.advertise<nav_msgs::Odometry>("/factorGraphPose", 10); // fused result
  ros::Publisher GNSS_odom_pub = nh.advertise<nav_msgs::Odometry>("/gnss_odom", 10); // GNSS in ENU from WLS
  ros::Subscriber jdFix_sub =nh.subscribe("/ublox_gps_node/fix2", 50, ubloxFix_callback); // subscribe data from ublox solution
  span_BP_sub =nh.subscribe("/novatel_data/bestpos", 50, span_bp_callback); // ground truth position from span-cpt
  span_odom_pub = nh.advertise<nav_msgs::Odometry>("/span_odom", 10); // publish the span-cpt solution in ENU.

  ros::Subscriber gps_raw_sub = nh.subscribe("/GNSS_", 50, GNSS_raw_callback); // subscribe the raw GNSS data

  ros::Rate rate(20);
  // while(ros::ok()){
  if(1){
        // ros::spin();
        // rate.sleep();
      string data_filename;
  if (argc < 2) {
    // printf("using default CSV file\n");
    data_filename = findExampleDataFile("imuAndGPSdata.csv");
  } else {
    data_filename = argv[1];
  }

  // Set up output file for plotting errors
  FILE* fp_out = fopen(output_filename.c_str(), "w+");
  fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,gt_qy,gt_qz,gt_qw\n");

  // Begin parsing the CSV file.  Input the first line for initialization.
  // From there, we'll iterate through the file and we'll preintegrate the IMU
  // or add in the GPS given the input.
  ifstream file(data_filename.c_str());
  string value;

  /************Step 1: Get initial state***************/
  // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
  Eigen::Matrix<double,10,1> initial_state = Eigen::Matrix<double,10,1>::Zero();
  getline(file, value, ','); // i
  for (int i=0; i<9; i++) {
    getline(file, value, ',');
    initial_state(i) = atof(value.c_str());
  }
  getline(file, value, '\n');
  initial_state(9) = atof(value.c_str());
  // cout << "initial state:\n" << initial_state.transpose() << "\n\n";

  // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
  Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
                                         initial_state(4), initial_state(5));
  // Point3 prior_point(initial_state.head<3>());
  Point3 prior_point = nomXYZ;
  Pose3 prior_pose(prior_rotation, prior_point);
  Vector3 prior_velocity(initial_state.tail<3>());
  imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
  Point2 prior_cb(0,0); // assume zero initial clock bias/
  SwitchVariableLinear sw_prior(1.0);
  // Point2 prior_cb(56098484.0491,-98362089.033); // assume zero initial clock bias

  Values initial_values;
  int correction_count = 0;
  initial_values.insert(X(correction_count), prior_pose);
  initial_values.insert(V(correction_count), prior_velocity);
  initial_values.insert(B(correction_count), prior_imu_bias);
  initial_values.insert(C(correction_count), prior_cb);
  if(enbale_SCP_factor)
  {
    initial_values.insert(S(correction_count), sw_prior);
  }

  // Assemble prior noise model and add it the graph.
  noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.3, 0.3, 0.3, 5, 5, 5).finished()); // rad,rad,rad,m, m, m
  noiseModel::Diagonal::shared_ptr velocity_noise_model = noiseModel::Isotropic::Sigma(3,1); // 0.1 m/s
  noiseModel::Diagonal::shared_ptr bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3);
  noiseModel::Diagonal::shared_ptr cb_noise_model = noiseModel::Diagonal::Sigmas((Vector(2) << 10, 10).finished()); //  m, m noise model for clock noise
  noiseModel::Diagonal::shared_ptr sw_noise_model = noiseModel::Diagonal::Sigmas((Vector(1) << 0.1).finished()); //  m, m noise model for clock noise

  // Add all prior factors (pose, velocity, bias) to the graph.
  NonlinearFactorGraph *graph = new NonlinearFactorGraph();

  // Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization
  // and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter
  // structure is available that allows the user to set various properties, such as the relinearization threshold
  // and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result
  // will approach the batch result.
  ISAM2Params parameters;
  parameters.relinearizeThreshold = 0.01;
  parameters.relinearizeSkip = 1;
  ISAM2 isam(parameters);

  graph->add(PriorFactor<Pose3>(X(correction_count), prior_pose, pose_noise_model));
  graph->add(PriorFactor<Vector3>(V(correction_count), prior_velocity,velocity_noise_model));
  graph->add(PriorFactor<imuBias::ConstantBias>(B(correction_count), prior_imu_bias,bias_noise_model));
  graph->add(PriorFactor<Point2>(C(correction_count), prior_cb,cb_noise_model)); // add prior for clock bias factor
   if(enbale_SCP_factor)
  {
    graph->add(PriorFactor<SwitchVariableLinear>(S(correction_count), sw_prior,sw_noise_model)); // add prior for switchable factor
  }

  // We use the sensor specs to build the noise model for the IMU factor.

  // gievn in GTSAM example
  // double accel_noise_sigma = 0.0003924;
  // double gyro_noise_sigma = 0.000205689024915;
  // double accel_bias_rw_sigma = 0.004905;
  // double gyro_bias_rw_sigma = 0.000001454441043;

  // Xsens 100 Ti
  // double accel_noise_sigma = 0.03924;
  // double gyro_noise_sigma = 0.0205689024915;

  double accel_noise_sigma = 0.1624;
  double gyro_noise_sigma = 0.205689024915;

  double accel_bias_rw_sigma = 0.1905;
  double gyro_bias_rw_sigma = 0.1454441043;

  Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
  Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
  Matrix33 integration_error_cov = Matrix33::Identity(3,3)*5; // error committed in integrating position from velocities

  Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2);
  Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2);
  Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*2; // error in the bias used for preintegration

  boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
  // PreintegrationBase params:
  p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
  p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
  // should be using 2nd order integration
  // PreintegratedRotation params:
  p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
  // PreintegrationCombinedMeasurements params:
  p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
  p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
  p->biasAccOmegaInt = bias_acc_omega_int;

#ifdef USE_COMBINED
  imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias);
#else
  imu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias);
#endif

  // Store previous state for the imu integration and the latest predicted outcome.
  NavState prev_state(prior_pose, prior_velocity);
  NavState prop_state = prev_state;
  imuBias::ConstantBias prev_bias = prior_imu_bias;
  Point2 prev_cb = prior_cb;

  // Keep track of the total error over the entire run for a simple performance metric.
  double current_position_error = 0.0, current_orientation_error = 0.0;

  double output_time = 0.0;

  double dt = 0.01;  // The real system has noise, but here, results are nearly  100 hz for our imu
                      // exactly the same, so keeping this for simplicity.

  // All priors have been set up, now iterate through the data file.
  ros::Rate rate(1000); // usually ok when rate(10)
  while (ros::ok()) { //ros::ok()
    ros::spinOnce();
    rate.sleep();
    GNSS_odom_pub.publish(odom_track_ENU);
    if ((imu_up == 1) && ( (online_cal_success ==1) || (offline_cal_success ==1))) { // IMU measurement obtained and do pre-integration
      // cout<<"imu_up-> "<<imu_up<<endl;
      Eigen::Matrix<double,6,1> imu = Eigen::Matrix<double,6,1>::Zero();

      imu(0) = imu_track.linear_acceleration.x ;
      imu(1) = imu_track.linear_acceleration.y ;
      imu(2) = imu_track.linear_acceleration.z ;

      imu(3) = imu_track.angular_velocity.x ;
      imu(4) = imu_track.angular_velocity.y ;
      imu(5) = imu_track.angular_velocity.z ;

      // Adding the IMU preintegration.
      double delta_clock_imu = float(clock() - old_imu_clock) / CLOCKS_PER_SEC;
      old_imu_clock = clock();
      // Adding the IMU preintegration.
        if(prediction_pre_t == 0)
      {
        prediction_pre_t = imu_track.header.stamp.toSec();
      }

      double delta_t = imu_track.header.stamp.toSec() -  prediction_pre_t;
      // cout<<"delta_t ->" <<delta_t<<endl;
      imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
      prediction_pre_t = imu_track.header.stamp.toSec();

      // cout<<"imu_track.linear_acceleration.z ->" <<imu_track.linear_acceleration.z <<  "  delta_clock_imu-> " << delta_clock_imu<<endl;
      // imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), delta_t);
      imu_up = 0;

    }
    // GPS measurement obtained and add gps factor and do optimization
     if ((gps_up == 1) && ( (online_cal_success ==1) || (offline_cal_success ==1))) {
     // if (imu_co > 20) { // imu  measurement
      // cout<<"gps_up-> "<<gps_up<<endl;
      Eigen::Matrix<double,7,1> gps = Eigen::Matrix<double,7,1>::Zero();

      gps(0) = odom_track.pose.pose.position.x;
      gps(1) = odom_track.pose.pose.position.y;
      gps(2) = odom_track.pose.pose.position.z;
      gps(3) = odom_track.pose.pose.orientation.x;
      gps(4) = odom_track.pose.pose.orientation.y;
      gps(5) = odom_track.pose.pose.orientation.z;
      gps(6) = 1;

      // gps(0) = span_odom_ecef.pose.pose.position.x;
      // gps(1) = span_odom_ecef.pose.pose.position.y;
      // gps(2) = span_odom_ecef.pose.pose.position.z;
      // gps(3) = span_odom_ecef.pose.pose.orientation.x;
      // gps(4) = span_odom_ecef.pose.pose.orientation.y;
      // gps(5) = span_odom_ecef.pose.pose.orientation.z;
      // gps(6) = 1;

      correction_count++;
      gps_up = 0;
      imu_co = 0;

      // Adding IMU factor and GPS factor and optimizing.
#ifdef USE_COMBINED
      PreintegratedCombinedMeasurements *preint_imu_combined = dynamic_cast<PreintegratedCombinedMeasurements*>(imu_preintegrated_);
      CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
                                   X(correction_count  ), V(correction_count  ),
                                   B(correction_count-1), B(correction_count  ),
                                   *preint_imu_combined);
      if(enable_IMUPreintegration_factor)
      {
        graph->add(imu_factor);
      }
#else
      PreintegratedImuMeasurements *preint_imu = dynamic_cast<PreintegratedImuMeasurements*>(imu_preintegrated_);
      ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
                           X(correction_count  ), V(correction_count  ),
                           B(correction_count-1),
                           *preint_imu);
      imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
      if(enable_IMUPreintegration_factor)
      {
        graph->add(imu_factor);
        graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
                                                        B(correction_count  ),
                                                        zero_bias, bias_noise_model));
      }

#endif

      //noiseModel::Isotropic::Sigma(3,0.01)
      // noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3, 0.1); // 3,0.01   gps(2) good and the performance is improved
      // noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3, 0.1); // 3,0.01   gps(2)
      // std::cout<<"cov-> " << gps_cov/10 <<std::endl;
      noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3, psr_cov ); // 3,0.01   gps(2)
      GPSFactor gps_factor(X(correction_count),
                           Point3(gps(0),  // N,
                                  gps(1),  // E,
                                  gps(2)), // D,
                           correction_noise);
      if(enable_GNSS_loose_factor)
      {
        graph->add(gps_factor);
      }

      /************add CVM factor************/
      if(enbale_CVM_factor) // use constant velocity model
      {
        // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
        Rot3 between_rotation = Rot3::Quaternion(0.01, 0.01,
                                               0.01, 0.01);
        double delta_clock_CVM = float(clock() - old_clock) / CLOCKS_PER_SEC;
        // Point3 between_point(0.5, 0.5, 0.5);
        Point3 between_point(_vx * delta_clock_CVM, _vy * delta_clock_CVM, _vz * delta_clock_CVM);
        Pose3 between_pose(between_rotation, between_point);
        Vector3 between_velocity(0.1, 0.1, 0.1);
        noiseModel::Diagonal::shared_ptr between_pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 5, 5, 5).finished()); // rad,rad,rad,m, m, m
        noiseModel::Diagonal::shared_ptr between_velocity_noise_model = noiseModel::Isotropic::Sigma(3,1); // 0.1 m/s
        noiseModel::Diagonal::shared_ptr between_bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3);
        noiseModel::Diagonal::shared_ptr between_cb_noise_model = noiseModel::Diagonal::Sigmas((Vector(2) << 10, 10).finished()); //  m, m noise model for clock noise
        graph->add(BetweenFactor<Pose3>(X(correction_count-1),
                                                        X(correction_count  ),
                                                        between_pose, between_pose_noise_model));

        graph->add(BetweenFactor<Vector3>(V(correction_count-1),
                                                        V(correction_count  ),
                                                        between_velocity, between_velocity_noise_model));
        graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
                                                      B(correction_count  ),
                                                      zero_bias, bias_noise_model));
      }

    /***************add pseudorange factor****************/
      for(int i =0; i < _GNSS_data.GNSS_Raws.size(); i++) //
    {
      // add Gaussian noise
      std::default_random_engine generator;

      auto dist_E = std::bind(std::normal_distribution<double>{0, 10},
                      std::mt19937(std::random_device{}()));
      double random_psr = dist_E();

      // std::cout<<"span_ecef -> "<<span_ecef<<std::endl;

      // Add constraint for pseudorange observable
      double sv_prn = _GNSS_data.GNSS_Raws[i].prn_satellites_index;
      Point3 satXYZ(_GNSS_data.GNSS_Raws[i].sat_pos_x, _GNSS_data.GNSS_Raws[i].sat_pos_y, _GNSS_data.GNSS_Raws[i].sat_pos_z);
      double range = _GNSS_data.GNSS_Raws[i].pseudorange;
      double SV_weight = cofactorMatrixCal_single_satellite(_GNSS_data.GNSS_Raws[i].elevation, _GNSS_data.GNSS_Raws[i].snr);
      // SV_weight = SV_weight * 0.01; // 10  error is about 10~50 meters
      // SV_weight = 3; // 10

      // Add clock bias prior factor for pseudorange observable
      // graph->add(PriorFactor<Point2>(C(correction_count), prev_cb,cb_noise_model)); // add prior for clock bias factor

      // boost::shared_ptr<PriorFactor <Point2> > cbPrior(
      //                   new PriorFactor<Point2>(C(correction_count), prev_cb, cb_noise_model));

      if(enable_Pr_factor)
      {
        PseudorangeFactor_spp pseudorange_Factor_spp(X(correction_count), C(correction_count),range, satXYZ, sv_prn, nomXYZ, noiseModel::Diagonal::Sigmas( (gtsam::Vector(1) << SV_weight).finished()));
        graph->add(pseudorange_Factor_spp);
        std::cout<< "pseudorange weighting-> "<< SV_weight << std::endl;
      }

       if(enbale_SCP_factor)
      {
         /***********switchable constraint****************/
        PseudorangeSwitchFactor PseudorangeSwitch_Factor(X(correction_count), C(correction_count), S(correction_count),range, satXYZ, sv_prn, nomXYZ, noiseModel::Diagonal::Sigmas( (gtsam::Vector(1) << SV_weight).finished()));
        graph->add(PseudorangeSwitch_Factor);
      }

    }

      /***********add clock bias factor************/
      Point2 zero_cb(0,0); // assume zero initial clock bias
      // graph->add(BetweenFactor<Point2>(C(correction_count-1),
      //                                                 C(correction_count  ),
      //                                                 zero_cb, cb_noise_model));

      // Now optimize and compare results.
      prop_state = imu_preintegrated_->predict(prev_state, prev_bias);
      initial_values.insert(X(correction_count), prop_state.pose());
      initial_values.insert(V(correction_count), prop_state.v());
      initial_values.insert(B(correction_count), prev_bias);
      initial_values.insert(C(correction_count), prev_cb); // clock bias
      initial_values.insert(S(correction_count),SwitchVariableLinear(sw_prior)); // switchable factor
      if(enbale_SCP_factor)
      {
        graph->add(PriorFactor<SwitchVariableLinear>(S(correction_count), sw_prior,sw_noise_model)); // add prior for switchable factor
        // initial_values.insert(S(correction_count),SwitchVariableLinear(sw_prior)); // switchable factor
      }

      // GaussNewtonOptimizer optimizer(*graph, initial_values);
      // const clock_t begin_time = clock();
      // Values result = optimizer.optimize();
      // std::cout << "this optimizer used  time -> " << float(clock() - begin_time) / CLOCKS_PER_SEC << "\n\n";

      // LevenbergMarquardtOptimizer optimizer(*graph, initial_values); // LevenbergMarquardtOptimizer
      // const clock_t begin_time = clock();
      // // DoglegOptimizer optimizer(*graph, initial_values); // DoglegOptimizer
      // Values result = optimizer.optimize();
      // cout << "****************************************************" << endl;
      // std::cout << "this optimizer used  time -> " << float(clock() - begin_time) / CLOCKS_PER_SEC << "\n\n";

      const clock_t begin_time = clock();
      isam.update(*graph,initial_values);
      isam.update();
      Values result = isam.calculateEstimate();

      // result.print("Current estimate: ");
      graph->resize(0);
      initial_values.clear();
      std::cout << "this optimizer used  time -> " << float(clock() - begin_time) / CLOCKS_PER_SEC << "\n\n";

      // Overwrite the beginning of the preintegration for the next step.
      prev_state = NavState(result.at<Pose3>(X(correction_count)),
                            result.at<Vector3>(V(correction_count)));
      prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));

      prev_cb = result.at<Point2>(C(correction_count));
      double SC = (result.at<SwitchVariableLinear>(S(correction_count))).value();
      std::cout<< "prev_cb-> " << prev_cb << "  S(correction_count)-> "<< SC << std::endl;

      // Reset the preintegration object.
      imu_preintegrated_->resetIntegrationAndSetBias(prev_bias);

      // Print out the position and orientation error for comparison.
      Vector3 gtsam_position = prev_state.pose().translation();

      /** *********estimate velocity **************/
      double delta_clock = float(clock() - old_clock) / CLOCKS_PER_SEC;
      _vx = ( gtsam_position.x() - result.at<Pose3>(X(correction_count - 1)).translation().x() ) / delta_clock;
      _vy = ( gtsam_position.y() - result.at<Pose3>(X(correction_count - 1)).translation().y() ) / delta_clock;
      _vx = ( gtsam_position.z() - result.at<Pose3>(X(correction_count - 1)).translation().z() ) / delta_clock;
      old_clock = clock();

      Eigen::MatrixXd ecef_fg;
      Eigen::MatrixXd enu_fg;
      enu_fg.resize(3, 1);
      ecef_fg.resize(3, 1);
      ecef_fg(0) = gtsam_position(0);
      ecef_fg(1) = gtsam_position(1);
      ecef_fg(2) = gtsam_position(2);
      enu_fg = gnss_tools_.ecef2enu(originllh_span, ecef_fg);
      gtsam_position = enu_fg;

      // cout<<"current state ->" <<gtsam_position<<endl;
      nav_msgs::Odometry facgrapose = odom_track;
      facgrapose.header.frame_id = "odom";
      facgrapose.pose.pose.position.x = gtsam_position(0);
      facgrapose.pose.pose.position.y = gtsam_position(1);
      facgrapose.pose.pose.position.z = gtsam_position(2);
      factorGraphPose_pub.publish(facgrapose);
      Vector3 position_error = gtsam_position - gps.head<3>();
      current_position_error = position_error.norm();

      Quaternion gtsam_quat = prev_state.pose().rotation().toQuaternion();
      Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
      Quaternion quat_error = gtsam_quat * gps_quat.inverse();
      quat_error.normalize();
      Vector3 euler_angle_error(quat_error.x()*2,
                                 quat_error.y()*2,
                                 quat_error.z()*2);
      current_orientation_error = euler_angle_error.norm();

      // display statistics
      // cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n";
      cout<<"size of the graph ->"<<graph->size()<<endl;
      fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
              output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2),
              gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(),
              gps(0), gps(1), gps(2),
              gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
      // print the result
      double del_x = span_odom.pose.pose.position.x - gtsam_position(0);
      double del_y = span_odom.pose.pose.position.y - gtsam_position(1);
      double del_z = span_odom.pose.pose.position.z - gtsam_position(2);
      double error_2D_fg = sqrt(pow(del_x, 2) + pow(del_y, 2));
      std::cout<<"error_2D_fg -> "<< error_2D_fg << std::endl;

      double del_x_fix = span_odom.pose.pose.position.x - odom_track_ENU.pose.pose.position.x;
      double del_y_fix = span_odom.pose.pose.position.y - odom_track_ENU.pose.pose.position.y;
      double del_z_fix = span_odom.pose.pose.position.z - odom_track_ENU.pose.pose.position.z;
      double error_2D_fix = sqrt(pow(del_x_fix, 2) + pow(del_y_fix, 2));
      std::cout<<"error_2D_fix -> "<< error_2D_fix << std::endl;

      fprintf(gps_imu_loose_fg_result, "%3.2f,%3.5f,%3.5f,%3.5f,%3.5f,%3.5f,%3.5f,%3.5f,%3.5f\n", output_time,
            error_2D_fix,error_2D_fg, span_odom.pose.pose.position.x, span_odom.pose.pose.position.y, odom_track_ENU.pose.pose.position.x,odom_track_ENU.pose.pose.position.y,gtsam_position(0), gtsam_position(1)); // generateData_gt3    not always span_ecef available at the first epoch

      output_time += 1.0;

      cout << "****************************************************" << endl << endl << endl;

    }
    // else {
    //   cerr << "waiting for gps and imu data\n";
    //   // return 1;
    // }
  }
  fclose(fp_out);
  // cout << "Complete, results written to " << output_filename << "\n\n";;
  }

  return 0;
}


// MD5.CC - source code for the C++/object oriented translation and
//          modification of MD5.

// Translation and modification (c) 1995 by Mordechai T. Abzug

// This translation/ modification is provided "as is," without express or
// implied warranty of any kind.

// The translator/ modifier does not claim (1) that MD5 will do what you think
// it does; (2) that this translation/ modification is accurate; or (3) that
// this software is "merchantible."  (Language for this disclaimer partially
// copied from the disclaimer below).

/* based on:

   MD5.H - header file for MD5C.C
   MDDRIVER.C - test driver for MD2, MD4 and MD5

   Copyright (C) 1991-2, RSA Data Security, Inc. Created 1991. All
rights reserved.

License to copy and use this software is granted provided that it
is identified as the "RSA Data Security, Inc. MD5 Message-Digest
Algorithm" in all material mentioning or referencing this software
or this function.

License is also granted to make and use derivative works provided
that such works are identified as "derived from the RSA Data
Security, Inc. MD5 Message-Digest Algorithm" in all material
mentioning or referencing the derived work.

RSA Data Security, Inc. makes no representations concerning either
the merchantability of this software or the suitability of this
software for any particular purpose. It is provided "as is"
without express or implied warranty of any kind.

These notices must be retained in any copies of any part of this
documentation and/or software.

*/

#include <boost/cstdint.hpp>

class MD5 {

public:
// methods for controlled operation:
  MD5              ();  // simple initializer
  void  update     (boost::uint8_t *input, boost::uint32_t input_length);
  void  finalize   ();

// methods to acquire finalized result
  boost::uint8_t    *raw_digest ();  // digest as a 16-byte binary array

private:

// first, some types:
  typedef boost::uint32_t uint4;
  typedef boost::uint16_t uint2;
  typedef boost::uint8_t  uint1;

// next, the private data:
  uint4 state[4];
  uint4 count[2];     // number of *bits*, mod 2^64
  uint1 buffer[64];   // input buffer
  uint1 digest[16];
  uint1 finalized;

// last, the private methods, mostly static:
  void init             ();                 // called by all constructors
  void transform        (uint1 buffer[64]); // does the real update work.  Note
                                            // that length is implied to be 64.

  static void encode    (uint1 *dest, uint4 *src, uint4 length);
  static void decode    (uint4 *dest, uint1 *src, uint4 length);

  static inline uint4  rotate_left (uint4 x, uint4 n);
  static inline uint4  F           (uint4 x, uint4 y, uint4 z);
  static inline uint4  G           (uint4 x, uint4 y, uint4 z);
  static inline uint4  H           (uint4 x, uint4 y, uint4 z);
  static inline uint4  I           (uint4 x, uint4 y, uint4 z);
  static inline void   FF  (uint4& a, uint4 b, uint4 c, uint4 d, uint4 x,
			    uint4 s, uint4 ac);
  static inline void   GG  (uint4& a, uint4 b, uint4 c, uint4 d, uint4 x,
			    uint4 s, uint4 ac);
  static inline void   HH  (uint4& a, uint4 b, uint4 c, uint4 d, uint4 x,
			    uint4 s, uint4 ac);
  static inline void   II  (uint4& a, uint4 b, uint4 c, uint4 d, uint4 x,
			    uint4 s, uint4 ac);

};


/*
 * array_utils.cc
 * Copyright 2014-2015 John Lawson
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "array_utils.h"
#include <algorithm>

#include <boost/functional/hash.hpp>

namespace cluster {
	namespace arrays {
		int *sort(int *arr, const int size) {
			std::sort(arr, arr + size);
			return arr;
		}

		std::size_t hash(const int *arr, const int size) {
			return boost::hash_range(arr, arr + size);
		}

		std::size_t hash(const std::vector<std::pair<int, int>>& vec) {
			size_t hash { 47 };
			for(uint i = 0; i < vec.size(); ++i) {
				hash *= 31;
				hash += vec[i].first;
			}
			for(uint i = 0; i < vec.size(); ++i) {
				hash *= 31;
				hash += vec[i].second;
			}
			return hash;
		}

		int number_in(const std::vector<int>& arr, const int val) {
			int count = 0;
			for (uint i = 0; i < arr.size(); i++) {
				if (arr[i] == val) {
					count++;
				}
			}
			return count;
		}
		bool equal(const std::vector<int>& a, const std::vector<int>& b) {
			if (a.size() != b.size()) {
				return false;
			}
			for (uint i = 0; i < a.size(); i++) {
				if (a[i] != b[i]) {
					return false;
				}
			}
			return true;
		}
	}
}


#include <tuple>
#include <Eigen/Core>
#include <npe.h>

npe_function(bool_array)
npe_arg(a, dense_bool)
npe_arg(b, npe_matches(a))
npe_doc(R"(Add two boolean matrices)")
npe_begin_code()

    npe_Matrix_a ret1 = a + b;

    return npe::move(ret1);

npe_end_code()

npe_function(bool_array_sparse)
npe_arg(a, sparse_bool)
npe_arg(b, npe_matches(a))
npe_doc(R"(Add two boolean matrices)")
npe_begin_code()

    npe_Matrix_a ret1 = a + b;

    return npe::move(ret1);

npe_end_code()


#ifndef SM_RANDOM_HPP
#define SM_RANDOM_HPP

#include <boost/cstdint.hpp>

namespace sm {
namespace random {

/// \brief Return a sample from a normally distributed random distribution with zero mean and standard deviation 1.
double normal();
/// \brief Return a sample from a normally distributed random distribution with zero mean and standard deviation 1.
double randn();

/// \brief Return a sample from a uniform distribution between 0.0 and 1.0
double uniform();

/// \brief Return a sample from a uniform distribution between 0.0 and 1.0
double rand();

/// \brief Return a sample from a uniform distribution in the half-open range [lowerBound, upperBound)
double randLU(double lowerBoundInclusive, double upperBoundExclusive);

/// \brief Return a sample from a uniform distribution in the half-open range [lowerBound, upperBound)
int randLUi(int lowerBoundInclusive, int upperBoundExclusive);

/// \brief Seed the random number generator.
void seed(boost::uint64_t s);

}  // namespace random
}  // namespace sm

#endif /* SM_RANDOM_HPP */


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2019 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include "open3d/geometry/PointCloud.h"
#include "open3d/geometry/Qhull.h"
#include "open3d/geometry/TetraMesh.h"
#include "open3d/geometry/TriangleMesh.h"
#include "open3d/utility/Console.h"

#include <Eigen/Dense>

#include <iostream>
#include <list>

namespace open3d {
namespace geometry {

std::shared_ptr<TriangleMesh> TriangleMesh::CreateFromPointCloudAlphaShape(
        const PointCloud& pcd,
        double alpha,
        std::shared_ptr<TetraMesh> tetra_mesh,
        std::vector<size_t>* pt_map) {
    std::vector<size_t> pt_map_computed;
    if (tetra_mesh == nullptr) {
        utility::LogDebug(
                "[CreateFromPointCloudAlphaShape] "
                "ComputeDelaunayTetrahedralization");
        std::tie(tetra_mesh, pt_map_computed) =
                Qhull::ComputeDelaunayTetrahedralization(pcd.points_);
        pt_map = &pt_map_computed;
        utility::LogDebug(
                "[CreateFromPointCloudAlphaShape] done "
                "ComputeDelaunayTetrahedralization");
    }

    utility::LogDebug("[CreateFromPointCloudAlphaShape] init triangle mesh");
    auto mesh = std::make_shared<TriangleMesh>();
    mesh->vertices_ = tetra_mesh->vertices_;
    if (pcd.HasNormals()) {
        mesh->vertex_normals_.resize(mesh->vertices_.size());
        for (size_t idx = 0; idx < (*pt_map).size(); ++idx) {
            mesh->vertex_normals_[idx] = pcd.normals_[(*pt_map)[idx]];
        }
    }
    if (pcd.HasColors()) {
        mesh->vertex_colors_.resize(mesh->vertices_.size());
        for (size_t idx = 0; idx < (*pt_map).size(); ++idx) {
            mesh->vertex_colors_[idx] = pcd.colors_[(*pt_map)[idx]];
        }
    }
    utility::LogDebug(
            "[CreateFromPointCloudAlphaShape] done init triangle mesh");

    std::vector<double> vsqn(tetra_mesh->vertices_.size());
    for (size_t vidx = 0; vidx < vsqn.size(); ++vidx) {
        vsqn[vidx] = tetra_mesh->vertices_[vidx].squaredNorm();
    }

    utility::LogDebug(
            "[CreateFromPointCloudAlphaShape] add triangles from tetras that "
            "satisfy constraint");
    const auto& verts = tetra_mesh->vertices_;
    for (size_t tidx = 0; tidx < tetra_mesh->tetras_.size(); ++tidx) {
        const auto& tetra = tetra_mesh->tetras_[tidx];
        // clang-format off
        Eigen::Matrix4d tmp;
        tmp << verts[tetra(0)](0), verts[tetra(0)](1), verts[tetra(0)](2), 1,
                verts[tetra(1)](0), verts[tetra(1)](1), verts[tetra(1)](2), 1,
                verts[tetra(2)](0), verts[tetra(2)](1), verts[tetra(2)](2), 1,
                verts[tetra(3)](0), verts[tetra(3)](1), verts[tetra(3)](2), 1;
        double a = tmp.determinant();
        tmp << vsqn[tetra(0)], verts[tetra(0)](0), verts[tetra(0)](1), verts[tetra(0)](2),
                vsqn[tetra(1)], verts[tetra(1)](0), verts[tetra(1)](1), verts[tetra(1)](2),
                vsqn[tetra(2)], verts[tetra(2)](0), verts[tetra(2)](1), verts[tetra(2)](2),
                vsqn[tetra(3)], verts[tetra(3)](0), verts[tetra(3)](1), verts[tetra(3)](2);
        double c = tmp.determinant();
        tmp << vsqn[tetra(0)], verts[tetra(0)](1), verts[tetra(0)](2), 1,
                vsqn[tetra(1)], verts[tetra(1)](1), verts[tetra(1)](2), 1,
                vsqn[tetra(2)], verts[tetra(2)](1), verts[tetra(2)](2), 1,
                vsqn[tetra(3)], verts[tetra(3)](1), verts[tetra(3)](2), 1;
        double dx = tmp.determinant();
        tmp << vsqn[tetra(0)], verts[tetra(0)](0), verts[tetra(0)](2), 1,
                vsqn[tetra(1)], verts[tetra(1)](0), verts[tetra(1)](2), 1,
                vsqn[tetra(2)], verts[tetra(2)](0), verts[tetra(2)](2), 1,
                vsqn[tetra(3)], verts[tetra(3)](0), verts[tetra(3)](2), 1;
        double dy = tmp.determinant();
        tmp << vsqn[tetra(0)], verts[tetra(0)](0), verts[tetra(0)](1), 1,
                vsqn[tetra(1)], verts[tetra(1)](0), verts[tetra(1)](1), 1,
                vsqn[tetra(2)], verts[tetra(2)](0), verts[tetra(2)](1), 1,
                vsqn[tetra(3)], verts[tetra(3)](0), verts[tetra(3)](1), 1;
        double dz = tmp.determinant();
        // clang-format on
        if (a == 0) {
            utility::LogError(
                    "[CreateFromPointCloudAlphaShape] invalid tetra in "
                    "TetraMesh");
        }
        double r = std::sqrt(dx * dx + dy * dy + dz * dz - 4 * a * c) /
                   (2 * std::abs(a));

        if (r <= alpha) {
            mesh->triangles_.push_back(TriangleMesh::GetOrderedTriangle(
                    tetra(0), tetra(1), tetra(2)));
            mesh->triangles_.push_back(TriangleMesh::GetOrderedTriangle(
                    tetra(0), tetra(1), tetra(3)));
            mesh->triangles_.push_back(TriangleMesh::GetOrderedTriangle(
                    tetra(0), tetra(2), tetra(3)));
            mesh->triangles_.push_back(TriangleMesh::GetOrderedTriangle(
                    tetra(1), tetra(2), tetra(3)));
        }
    }
    utility::LogDebug(
            "[CreateFromPointCloudAlphaShape] done add triangles from tetras "
            "that satisfy constraint");

    utility::LogDebug(
            "[CreateFromPointCloudAlphaShape] remove triangles within "
            "the mesh");
    std::unordered_map<Eigen::Vector3i, int,
                       utility::hash_eigen<Eigen::Vector3i>>
            triangle_count;
    for (size_t tidx = 0; tidx < mesh->triangles_.size(); ++tidx) {
        Eigen::Vector3i triangle = mesh->triangles_[tidx];
        if (triangle_count.count(triangle) == 0) {
            triangle_count[triangle] = 1;
        } else {
            triangle_count[triangle] += 1;
        }
    }

    size_t to_idx = 0;
    for (size_t tidx = 0; tidx < mesh->triangles_.size(); ++tidx) {
        Eigen::Vector3i triangle = mesh->triangles_[tidx];
        if (triangle_count[triangle] == 1) {
            mesh->triangles_[to_idx] = triangle;
            to_idx++;
        }
    }
    mesh->triangles_.resize(to_idx);
    utility::LogDebug(
            "[CreateFromPointCloudAlphaShape] done remove triangles within "
            "the mesh");

    utility::LogDebug(
            "[CreateFromPointCloudAlphaShape] remove duplicate triangles and "
            "unreferenced vertices");
    mesh->RemoveDuplicatedTriangles();
    mesh->RemoveUnreferencedVertices();
    utility::LogDebug(
            "[CreateFromPointCloudAlphaShape] done remove duplicate triangles "
            "and unreferenced vertices");

    return mesh;
}

}  // namespace geometry
}  // namespace open3d


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE LibmeshNodalShapeFunctionTester
#include <boost/test/included/unit_test.hpp>

#include <cmath>
#include <iostream>
#include <sstream>
#include <vector>

#include "LibmeshAdjacencies.hpp"
#include "LibmeshEntity.hpp"
#include "LibmeshNodalShapeFunction.hpp"

#include "Teuchos_Array.hpp"
#include "Teuchos_ArrayRCP.hpp"
#include "Teuchos_DefaultComm.hpp"
#include "Teuchos_Ptr.hpp"
#include "Teuchos_RCP.hpp"
#include <Teuchos_DefaultMpiComm.hpp>

#include <libmesh/cell_hex8.h>
#include <libmesh/equation_systems.h>
#include <libmesh/libmesh.h>
#include <libmesh/linear_implicit_system.h>
#include <libmesh/mesh.h>
#include <libmesh/node.h>
#include <libmesh/parallel.h>
#include <libmesh/point.h>
#include <libmesh/system.h>

//---------------------------------------------------------------------------//
// Hex-8 test.
BOOST_AUTO_TEST_CASE( checkLibmeshNodalShapeFunction )
{
	const std::string argv_string = "unit_test --keep-cout";
	const char *argv_char = argv_string.c_str();
	Teuchos::GlobalMPISession mpiSession(
			&boost::unit_test::framework::master_test_suite().argc,
			&boost::unit_test::framework::master_test_suite().argv);
	auto comm = Teuchos::DefaultComm<int>::getComm();
	auto mpi_comm = Teuchos::rcp_dynamic_cast<const Teuchos::MpiComm<int>>(
			comm);
	auto opaque_comm = mpi_comm->getRawMpiComm();
	auto raw_comm = (*opaque_comm)();

    // Create the mesh.
    int space_dim = 3;
    libMesh::LibMeshInit libmesh_init( 1, &argv_char, raw_comm );
    BOOST_ASSERT( libMesh::initialized() );
    BOOST_ASSERT( (int)libmesh_init.comm().rank() == comm->getRank() );
    Teuchos::RCP<libMesh::Mesh> mesh =
        Teuchos::rcp( new libMesh::Mesh( libmesh_init.comm(), space_dim ) );

    // Create the nodes.
    int rank = comm->getRank();
    Teuchos::Array<libMesh::Node *> nodes( 8 );
    double node_coords[3];
    node_coords[0] = 0.0;
    node_coords[1] = 0.0;
    node_coords[2] = 0.0;
    nodes[0] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 0,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 0.0;
    node_coords[2] = 0.0;
    nodes[1] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 1,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 1.0;
    node_coords[2] = 0.0;
    nodes[2] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 2,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 1.0;
    node_coords[2] = 0.0;
    nodes[3] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 3,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 0.0;
    node_coords[2] = 1.0;
    nodes[4] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 4,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 0.0;
    node_coords[2] = 1.0;
    nodes[5] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 5,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 1.0;
    node_coords[2] = 1.0;
    nodes[6] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 6,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 1.0;
    node_coords[2] = 1.0;
    nodes[7] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 7,
        rank );

    // Make a hex-8.
    int num_nodes = 8;
    libMesh::Elem *hex_elem = mesh->add_elem( new libMesh::Hex8 );
    hex_elem->processor_id() = rank;
    hex_elem->set_id() = 2 * rank;
    for ( int i = 0; i < 8; ++i )
        hex_elem->set_node( i ) = nodes[i];

    // Check libmesh validity.
    mesh->libmesh_assert_valid_parallel_ids();

    // Make an adjacency data structure.
    LibmeshAdapter::LibmeshAdjacencies adjacencies( mesh );

    // Create a  entity for the hex.
   auto dtk_entity =
        LibmeshAdapter::LibmeshEntity<libMesh::Elem>(
            Teuchos::ptr( hex_elem ), mesh.ptr(),
            Teuchos::ptrFromRef( adjacencies ) );

    // Make a libmesh system. We will put a first order linear basis on the
    // elements.
    libMesh::EquationSystems equation_systems( *mesh );
    libMesh::LinearImplicitSystem &system =
        equation_systems.add_system<libMesh::LinearImplicitSystem>( "Test" );
    system.add_variable( "test_var", libMesh::FIRST );

    // Create a shape function.
    auto shape_function =
        Teuchos::rcp( new LibmeshAdapter::LibmeshNodalShapeFunction(
            mesh, Teuchos::rcpFromRef( system ) ) );

    // Test the shape function dof ids for the hex.
    Teuchos::Array<unsigned long int> dof_ids;
    shape_function->entitySupportIds( dtk_entity, dof_ids );
    BOOST_ASSERT( num_nodes == dof_ids.size() );
    for ( int n = 0; n < num_nodes; ++n )
    {
    	BOOST_ASSERT( dof_ids[n] == nodes[n]->id() );
    }

    // Test the value evaluation for the hex.
    Teuchos::Array<double> ref_point( space_dim, 0.0 );
    Teuchos::Array<double> values;
    shape_function->evaluateValue( dtk_entity, ref_point(), values );
    BOOST_ASSERT( values.size() == num_nodes );
    for ( int n = 0; n < num_nodes; ++n )
    {
    	BOOST_ASSERT( values[n] == 1.0 / num_nodes );
    }
    ref_point[0] = -1.0;
    ref_point[1] = -1.0;
    ref_point[2] = -1.0;
    shape_function->evaluateValue( dtk_entity, ref_point(), values );
    BOOST_ASSERT( values.size() == num_nodes );
    BOOST_ASSERT( values[0] == 1.0 );
    for ( int n = 1; n < num_nodes; ++n )
    {
    	BOOST_ASSERT( values[n] == 0.0 );
    }

    // Test the shape function dof ids for the nodes.
    for ( int n = 0; n < num_nodes; ++n )
    {
        dof_ids.clear();
       auto dtk_node =
            LibmeshAdapter::LibmeshEntity<libMesh::Node>(
                Teuchos::ptr( nodes[n] ), mesh.ptr(),
                Teuchos::ptrFromRef( adjacencies ) );
        shape_function->entitySupportIds( dtk_node, dof_ids );
        BOOST_ASSERT( dof_ids.size() == 1 );
        BOOST_ASSERT( dof_ids[0] == nodes[n]->id() );
    }
}

//---------------------------------------------------------------------------//
// end of tstLibmeshNodalShapeFunction.cpp
//---------------------------------------------------------------------------//


// Copyright (C) 2018 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@mail.wvu.edu)

#include "theia/sfm/estimators/estimate_rigid_transformation_2d_3d.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <memory>
#include <vector>
#include <glog/logging.h>

#include "theia/sfm/estimators/camera_and_feature_correspondence_2d_3d.h"
#include "theia/sfm/estimators/feature_correspondence_2d_3d.h"
#include "theia/sfm/pose/upnp.h"
#include "theia/sfm/rigid_transformation.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

// An estimator for computing the absolute pose from 2 feature
// correspondences. The feature correspondences should be normalized by the
// focal length with the principal point at (0, 0) and rotated into the same
// coordinate system as the points.
class NonCentralCameraPoseEstimator
    : public Estimator<CameraAndFeatureCorrespondence2D3D,
                       RigidTransformation> {
 public:
  NonCentralCameraPoseEstimator() {
    estimator_.reset(new class Upnp);
  }
  ~NonCentralCameraPoseEstimator() = default;

  // Get the minimum number of samples needed to generate a model.
  virtual double SampleSize() const override { return 4; }

  // Given a set of data points, estimate the model. Users should implement this
  // function appropriately for the task being solved. Returns true for
  // successful model estimation (and outputs model), false for failed
  // estimation. Typically, this is a minimal set, but it is not required to be.
  bool EstimateModel(
      const std::vector<CameraAndFeatureCorrespondence2D3D>& data,
      std::vector<RigidTransformation>* models) const override {
    CHECK_GE(data.size(), static_cast<size_t>(SampleSize()));
    CHECK_NOTNULL(models)->clear();
    // Input datum for Upnp.
    std::vector<Eigen::Vector3d> ray_directions;
    std::vector<Eigen::Vector3d> ray_origins;
    std::vector<Eigen::Vector3d> world_points;

    // Prepare data.
    ray_directions.reserve(data.size());
    ray_origins.reserve(data.size());
    world_points.reserve(data.size());
    for (const CameraAndFeatureCorrespondence2D3D& correspondence : data) {
      ray_directions.emplace_back(correspondence.camera.PixelToUnitDepthRay(
          correspondence.observation).normalized());
      ray_origins.emplace_back(correspondence.camera.GetPosition());
      world_points.emplace_back(correspondence.point3d.hnormalized());
    }

    // Estimate pose.
    std::vector<Eigen::Quaterniond> soln_rotations;
    std::vector<Eigen::Vector3d> soln_translations;
    if (!estimator_->EstimatePose(ray_origins,
                                  ray_directions,
                                  world_points,
                                  &soln_rotations,
                                  &soln_translations)) {
      return false;
    }

    // Generate models.
    models->resize(soln_rotations.size());
    for (int i = 0; i < soln_rotations.size(); ++i) {
      models->at(i).rotation = soln_rotations[i].toRotationMatrix();
      models->at(i).translation = soln_translations[i];
    }
    return true;
  }

  // Given a model and a data point, calculate the error. Users should implement
  // this function appropriately for the task being solved.
  double Error(const CameraAndFeatureCorrespondence2D3D& data,
               const RigidTransformation& model) const override {
    Eigen::Vector2d reprojection;
    // If the point is reprojected behind the camera, return the maximum
    // possible error.
    const Eigen::Vector4d transformed_point =
        (model.rotation * data.point3d.hnormalized() +
         model.translation).homogeneous();
    if (data.camera.ProjectPoint(transformed_point, &reprojection) < 0) {
      return std::numeric_limits<double>::max();
    }

    // Return the squared reprojection error.
    return (data.observation - reprojection).squaredNorm();
  }

 private:
  // Upnp estimator.
  std::unique_ptr<class Upnp> estimator_;

  DISALLOW_COPY_AND_ASSIGN(NonCentralCameraPoseEstimator);
};

}  // namespace

bool EstimateRigidTransformation2D3D(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<CameraAndFeatureCorrespondence2D3D>& correspondences,
    RigidTransformation* estimated_pose,
    RansacSummary* ransac_summary) {
  NonCentralCameraPoseEstimator pose_estimator;
  std::unique_ptr<SampleConsensusEstimator<NonCentralCameraPoseEstimator>>
      ransac = CreateAndInitializeRansacVariant(ransac_type,
                                                ransac_params,
                                                pose_estimator);
  // Estimate the pose.
  return ransac->Estimate(correspondences,
                          estimated_pose,
                          ransac_summary);
}

bool EstimateRigidTransformation2D3D(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence2D3D>& normalized_correspondences,
    RigidTransformation* estimated_pose,
    RansacSummary* ransac_summary) {
  const int num_correspondences = normalized_correspondences.size();
  std::vector<CameraAndFeatureCorrespondence2D3D> correspondences(
      num_correspondences);
  for (int i = 0; i < num_correspondences; ++i) {
    correspondences[i].point3d =
        normalized_correspondences[i].world_point.homogeneous();
    correspondences[i].observation =
        normalized_correspondences[i].feature;
    correspondences[i].camera.SetPosition(Eigen::Vector3d::Zero());
    correspondences[i].camera.SetOrientationFromRotationMatrix(
        Eigen::Matrix3d::Identity());
    correspondences[i].camera.SetFocalLength(1.0);
    correspondences[i].camera.SetPrincipalPoint(0.0, 0.0);
  }
  return EstimateRigidTransformation2D3D(ransac_params,
                                         ransac_type,
                                         correspondences,
                                         estimated_pose,
                                         ransac_summary);
}

}  // namespace theia