#include <mutex>
#include <memory>
#include <iostream>
#include <boost/circular_buffer.hpp>

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>

#include <pcl/filters/voxel_grid.h>

#include <pclomp/ndt_omp.h>
#include <pclomp/gicp_omp.h>

#include <hdl_localization/pose_estimator.hpp>

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>

#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
using namespace gtsam;

namespace hdl_localization {

class HdlLocalizationNodelet : public nodelet::Nodelet {
public:
  using PointT = pcl::PointXYZI;

  HdlLocalizationNodelet() {
  }
  virtual ~HdlLocalizationNodelet() {
  }

  void onInit() override {
    nh = getNodeHandle();
    mt_nh = getMTNodeHandle();
    private_nh = getPrivateNodeHandle();

    processing_time.resize(16);
    imuAccNoise = private_nh.param<double>("imuAccNoise", 0.01);
    imuGyrNoise = private_nh.param<double>("imuGyrNoise", 0.001);
    odomLinearNoise = private_nh.param<double>("odomLinearNoise", 0.01);
    odomAngularNoise = private_nh.param<double>("odomAngularNoise", 0.01);
    gravity= private_nh.param<double>("gravity", 10.04);
    opt_frames = private_nh.param<int>("opt_frames", 20);
    odom_child_frame_id = private_nh.param<std::string>("odom_child_frame_id", "base_link");
    use_imu = private_nh.param<bool>("use_imu", true);
    invert_imu = private_nh.param<bool>("invert_imu", false);
    key_interval = private_nh.param<float>("key_interval", 0.1);
    imu_sub = mt_nh.subscribe("/imu/data_raw", 256, &HdlLocalizationNodelet::imu_callback, this);
    points_sub = mt_nh.subscribe("/velodyne_points", 5, &HdlLocalizationNodelet::points_callback, this);
    globalmap_sub = nh.subscribe("/globalmap", 1, &HdlLocalizationNodelet::globalmap_callback, this);
    initialpose_sub = nh.subscribe("/initialpose", 8, &HdlLocalizationNodelet::initialpose_callback, this);
    pose_incre_pub = nh.advertise<nav_msgs::Odometry>("/ndt/incre_odom", 30, false);
    pose_pub = nh.advertise<nav_msgs::Odometry>("/ndt/global_odom", 20, false);
    initialize_params();

  }

private:
  void initialize_params() {
    // intialize scan matching method
    double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);
    std::string ndt_neighbor_search_method = private_nh.param<std::string>("ndt_neighbor_search_method", "DIRECT7");

    double ndt_resolution = private_nh.param<double>("ndt_resolution", 1.0);
    boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
    voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
    downsample_filter = voxelgrid;

    pclomp::NormalDistributionsTransform<PointT, PointT>::Ptr ndt(new pclomp::NormalDistributionsTransform<PointT, PointT>());
    pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>::Ptr gicp(new pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>());

    ndt->setTransformationEpsilon(0.01);
    ndt->setResolution(ndt_resolution);
    if(ndt_neighbor_search_method == "DIRECT1") {
      NODELET_INFO("search_method DIRECT1 is selected");
      ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1);
      registration = ndt;
//        ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
//      localregistration=ndt;
    } else if(ndt_neighbor_search_method == "DIRECT7") {
      NODELET_INFO("search_method DIRECT7 is selected");
      ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
      registration = ndt;
//      localregistration=ndt;

    } else if(ndt_neighbor_search_method == "GICP_OMP"){
      NODELET_INFO("search_method GICP_OMP is selected");
      registration = gicp;
    }
     else {
      if(ndt_neighbor_search_method == "KDTREE") {
        NODELET_INFO("search_method KDTREE is selected");
      } else {
        NODELET_WARN("invalid search method was given");
        NODELET_WARN("default method is selected (KDTREE)");
      }
      ndt->setNeighborhoodSearchMethod(pclomp::KDTREE);
      registration = ndt;

    }

    // initialize pose estimator
    if(private_nh.param<bool>("specify_init_pose", true))
    {
      NODELET_INFO("initialize pose estimator with specified parameters!!");
      pose_estimator.reset(new hdl_localization::PoseEstimator(registration,
        ros::Time::now(),
        Eigen::Vector3d(private_nh.param<double>("init_pos_x", 0.0), private_nh.param<double>("init_pos_y", 0.0), private_nh.param<double>("init_pos_z", 0.0)),
        Eigen::Quaterniond(private_nh.param<double>("init_ori_w", 1.0), private_nh.param<double>("init_ori_x", 0.0), private_nh.param<double>("init_ori_y", 0.0), private_nh.param<double>("init_ori_z", 0.0)),
        private_nh.param<double>("cool_time_duration", 0.5)
      ));
    }
//    poseFrom = gtsam::Pose3(gtsam::Rot3::Quaternion(1.0,0.0,0.0,0.0),gtsam::Point3(0.0,0.0,0.0));
      //*****************factor graph********************//
      boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(gravity);
      p->accelerometerCovariance  = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
      p->gyroscopeCovariance      = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
      p->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
      gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished()); // assume zero initial bias
      priorPoseNoise  = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
      priorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e2); // m/s
      priorBiasNoise  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
      correctionNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1); // meter
      noiseModelBetweenBias = (gtsam::Vector(6) << 3.99395e-03, 3.99395e-03, 3.99395e-03, 1.56363e-03, 1.56363e-03, 1.56363e-03).finished();
      imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias);

      key_count=0;
      systemInitialized= false;

//**************************************************************//
  }

private:
  /**
   * @brief callback for imu data
   * @param imu_msg
   */
  void imu_callback(const sensor_msgs::ImuConstPtr& imu_msg) {
      std::lock_guard<std::mutex> lock(imu_data_mutex);
      imu_data.push_back(imu_msg);
//      ROS_INFO("hahahahahahah");
  }
  /**
   * @brief callback for point cloud data
   * @param points_msg
   */
  void points_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {

      points_curr_time=points_msg->header.stamp;

      if(firstkey)
      {
          points_pre_time=points_msg->header.stamp;
          firstkey= false;
      }

      auto time_interval= points_curr_time-points_pre_time;

      if(time_interval.toSec()<key_interval) return;// select key frames according to time interval

      std::lock_guard<std::mutex> estimator_lock(pose_estimator_mutex);

      if(!pose_estimator) {
          NODELET_ERROR("waiting for initial pose input!!");
          return;
      }

      if(!globalmap) {
          NODELET_ERROR("globalmap has not been received!!");
          return;
      }

      const auto& stamp = points_msg->header.stamp;
      pcl::PointCloud<PointT>::Ptr pcl_cloud(new pcl::PointCloud<PointT>());
      pcl::fromROSMsg(*points_msg, *pcl_cloud);

      if(pcl_cloud->empty()) {
          NODELET_ERROR("cloud is empty!!");
          return;
      }

      // transform pointcloud into odom_child_frame_id
      pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
      if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_listener)) {
          NODELET_ERROR("point cloud cannot be transformed into target frame!!");
          return;
      }

      auto filtered = downsample(cloud);

      auto t1 = ros::WallTime::now();

//      cur_cloud=filtered;
//      localregistration->setInputTarget(pre_cloud);
//      localregistration->setInputSource(cur_cloud);
//      Eigen::Matrix4f localOdom_temp;
//      Eigen::Matrix4d localOdom;
//      localOdom_temp = localregistration->getFinalTransformation();
//      localOdom=localOdom_temp.cast<double>();

      if(!systemInitialized){

          resetOptimization();

          newgraph.add(PriorFactor<Pose3>(X(0), prevPose_, priorPoseNoise));
          std::cout<<"initial value:"<<prevPose_.x()<<" "<<prevPose_.y()<<" "<<prevPose_.z()<<" "<<prevPose_.rotation().roll()<<" "
          <<prevPose_.rotation().pitch()<<" "<<prevPose_.rotation().yaw()<<std::endl;

          //initial velocity
          prevVel_ = gtsam::Vector3(0, 0, 0);
          gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
          newgraph.add(priorVel);

          //initial bias
          prevBias_ = gtsam::imuBias::ConstantBias();
          gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
          newgraph.add(priorBias);

          //add values
          initialEstimate.insert(X(0), prevPose_);
          initialEstimate.insert(V(0), prevVel_);
          initialEstimate.insert(B(0), prevBias_);

          optimizer->update(newgraph, initialEstimate);

          newgraph.resize(0);
          initialEstimate.clear();
          imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
          systemInitialized=true;
          key_count=1;
          return;
      }
      else{

          if(key_count==opt_frames)
          {
              gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer->marginalCovariance(X(key_count-1)));
              gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise  = gtsam::noiseModel::Gaussian::Covariance(optimizer->marginalCovariance(V(key_count-1)));
              gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer->marginalCovariance(B(key_count-1)));
              // reset graph
              resetOptimization();
              // add pose
              gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
              newgraph.add(priorPose);
              // add velocity
              gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
              newgraph.add(priorVel);
              // add bias
              gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
              newgraph.add(priorBias);
              // add values
              initialEstimate.insert(X(0), prevPose_);
              initialEstimate.insert(V(0), prevVel_);
              initialEstimate.insert(B(0), prevBias_);
              // optimize once
              optimizer->update(newgraph, initialEstimate);
              newgraph.resize(0);
              initialEstimate.clear();

              key_count=1;
          }
          gtsam::Pose3 poseFrom = gtsam::Pose3(gtsam::Rot3::Quaternion(pose_estimator->quat().w(),pose_estimator->quat().x(),pose_estimator->quat().y(),pose_estimator->quat().z()),
                                               gtsam::Point3(pose_estimator->pos()(0),pose_estimator->pos()(1),pose_estimator->pos()(2)));
          std::cout<<"prior value:"<<poseFrom.x()<<" "<<poseFrom.y()<<" "<<poseFrom.z()<<" "<<poseFrom.rotation().roll()<<" "
                   <<poseFrom.rotation().pitch()<<" "<<poseFrom.rotation().yaw()<<std::endl;

          auto aligned = pose_estimator->correct(filtered);

          gtsam::Pose3 poseTo = gtsam::Pose3(gtsam::Rot3::Quaternion(pose_estimator->quat().w(),pose_estimator->quat().x(),pose_estimator->quat().y(),pose_estimator->quat().z()),
                                             gtsam::Point3(pose_estimator->pos()(0),pose_estimator->pos()(1),pose_estimator->pos()(2)));
          std::cout<<"update value:"<<poseTo.x()<<" "<<poseTo.y()<<" "<<poseTo.z()<<" "<<poseTo.rotation().roll()<<" "
                   <<poseTo.rotation().pitch()<<" "<<poseTo.rotation().yaw()<<std::endl;

           std::lock_guard<std::mutex> lock(imu_data_mutex);
           auto imu_iter = imu_data.begin();
           for(imu_iter; imu_iter != imu_data.end(); imu_iter++) {
                  if(stamp < (*imu_iter)->header.stamp) {
                      break;
                  }
                  const auto& acc = (*imu_iter)->linear_acceleration;
                  const auto& gyro = (*imu_iter)->angular_velocity;
                  imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(acc.x, acc.y, acc.z),gtsam::Vector3(gyro.x, gyro.y, gyro.z), 0.01);
              }
           imu_data.erase(imu_data.begin(),imu_iter);

          gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key_count), poseTo, correctionNoise);
          newgraph.add(pose_factor);

          const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorImu_);
          gtsam::ImuFactor imu_factor(X(key_count - 1), V(key_count - 1), X(key_count), V(key_count), B(key_count - 1), preint_imu);

          newgraph.add(imu_factor);
          newgraph.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key_count - 1), B(key_count), gtsam::imuBias::ConstantBias(),
                                                                              gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorImu_->deltaTij()) * noiseModelBetweenBias)));

          gtsam::NavState propState_ = imuIntegratorImu_->predict(prevState_, prevBias_);

          std::cout<<" imu integre propState:"<<propState_.pose().x()<<" "<<propState_.pose().y()<<" "<<propState_.pose().z()<<" "<<
                   propState_.pose().rotation().roll()<<" "<<propState_.pose().rotation().pitch()<<" "<<propState_.pose().rotation().yaw()<<std::endl;

          if(propState_.pose().z()<-0.1)
          {
              gtsam::Quaternion quat(propState_.quaternion());
              quat.normalized();
              poseTo = gtsam::Pose3(gtsam::Rot3::Quaternion(quat.w(),quat.x(),quat.y(),quat.z()),gtsam::Point3(propState_.pose().x(),propState_.pose().y(),-0.1));
          }
          else if (propState_.pose().z()>0.1)
          {

              gtsam::Quaternion quat(propState_.quaternion());
              quat.normalized();
              poseTo = gtsam::Pose3(gtsam::Rot3::Quaternion(quat.w(),quat.x(),quat.y(),quat.z()),gtsam::Point3(propState_.pose().x(),propState_.pose().y(),0.10));
          }

          std::cout<<"propState_.pose().z():"<<propState_.pose().z()<<std::endl;

          noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << odomLinearNoise, odomLinearNoise, odomLinearNoise, odomAngularNoise, odomAngularNoise, odomAngularNoise).finished());

          newgraph.add(BetweenFactor<Pose3>(X(key_count-1), X(key_count), poseFrom.between(poseTo), odometryNoise));

          initialEstimate.insert(X(key_count), poseTo);
          initialEstimate.insert(V(key_count), propState_.v());
          initialEstimate.insert(B(key_count), prevBias_);
      }

      optimizer->update(newgraph, initialEstimate);
      newgraph.resize(0);
      initialEstimate.clear();
      gtsam::Pose3 latestEstimate;
      isamCurrentEstimate = optimizer->calculateEstimate();

      prevPose_ = isamCurrentEstimate.at<Pose3>(X(key_count));

      prevVel_ =isamCurrentEstimate.at<Vector3>(V(key_count));
      prevState_ = gtsam::NavState(prevPose_, prevVel_);
      prevBias_=isamCurrentEstimate.at<imuBias::ConstantBias>(B(key_count));
      imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);

      pose_estimator->update(prevState_,prevBias_);

      Eigen::Quaterniond quat(prevPose_.rotation().toQuaternion().w(),prevPose_.rotation().toQuaternion().x(),prevPose_.rotation().toQuaternion().y(),prevPose_.rotation().toQuaternion().z());
      quat.normalize();
      geometry_msgs::Quaternion odom_quat;
      odom_quat.w = quat.w();
      odom_quat.x = quat.x();
      odom_quat.y = quat.y();
      odom_quat.z = quat.z();

      geometry_msgs::TransformStamped odom_trans;
      odom_trans.header.stamp = stamp;
      odom_trans.header.frame_id = "map";
      odom_trans.child_frame_id = odom_child_frame_id;

      odom_trans.transform.translation.x = prevPose_.x();
      odom_trans.transform.translation.y = prevPose_.y();
      odom_trans.transform.translation.z = prevPose_.z();
      odom_trans.transform.rotation = odom_quat;

      pose_broadcaster.sendTransform(odom_trans);

      nav_msgs::Odometry laserOdometryROS;
      laserOdometryROS.header.stamp = points_msg->header.stamp;
      laserOdometryROS.header.frame_id = "map";
      laserOdometryROS.child_frame_id = odom_child_frame_id;
      laserOdometryROS.pose.pose.position.x = prevPose_.x();
      laserOdometryROS.pose.pose.position.y = prevPose_.y();
      laserOdometryROS.pose.pose.position.z = prevPose_.z();
      laserOdometryROS.pose.pose.orientation = odom_quat;
      pose_pub.publish(laserOdometryROS);

      points_pre_time = points_curr_time;

//      pre_cloud=filtered;
      ++key_count;

      auto t2 = ros::WallTime::now();
      NODELET_INFO_STREAM("processing_time: " <<(t2 - t1).toSec()* 1000.0 << "[msec]");

  }

  /**
   * @brief callback for globalmap input
   * @param points_msg
   */
  void globalmap_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {
    NODELET_INFO("globalmap received!");
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*points_msg, *cloud);
    globalmap = cloud;
    registration->setInputTarget(globalmap);
  }

  /**
   * @brief callback for initial pose input ("2D Pose Estimate" on rviz)
   * @param pose_msg
   */
  void initialpose_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg) {
    NODELET_INFO("initial pose received!!");
    std::lock_guard<std::mutex> lock(pose_estimator_mutex);
    const auto& p = pose_msg->pose.pose.position;
    const auto& q = pose_msg->pose.pose.orientation;
    pose_estimator.reset(
          new hdl_localization::PoseEstimator(
            registration,
            ros::Time::now(),
            Eigen::Vector3d(p.x, p.y, p.z),
            Eigen::Quaterniond(q.w, q.x, q.y, q.z),
            private_nh.param<double>("cool_time_duration", 0.5))
    );
  }

  /**
   * @brief downsampling
   * @param cloud   input cloud
   * @return downsampled cloud
   */
  pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
    if(!downsample_filter) {
      return cloud;
    }
    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    downsample_filter->setInputCloud(cloud);
    downsample_filter->filter(*filtered);
    filtered->header = cloud->header;
    return filtered;
  }

  /**
   * @brief publish odometry
   * @param stamp  timestamp
   * @param pose   odometry pose to be published
   */

  void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4d& pose) {
    // broadcast the transform over tf
    geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "map", odom_child_frame_id);
    pose_broadcaster.sendTransform(odom_trans);

    // publish the transform
    nav_msgs::Odometry odom;
    odom.header.stamp = stamp;
    odom.header.frame_id = "map";

    odom.pose.pose.position.x = pose(0, 3);
    odom.pose.pose.position.y = pose(1, 3);
    odom.pose.pose.position.z = pose(2, 3);
    odom.pose.pose.orientation = odom_trans.transform.rotation;

    odom.child_frame_id = odom_child_frame_id;
    odom.twist.twist.linear.x = 0.0;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.angular.z = 0.0;

    pose_pub.publish(odom);
  }

    void publish_incre_odometry(const ros::Time& stamp, const Eigen::Matrix4d& pose) {
        // broadcast the transform over tf
        geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_child_frame_id, "incre_baselink");
        pose_incre_broadcaster.sendTransform(odom_trans);

        // publish the transform
        nav_msgs::Odometry odom;
        odom.header.stamp = stamp;
        odom.header.frame_id = odom_child_frame_id;

        odom.pose.pose.position.x = pose(0, 3);
        odom.pose.pose.position.y = pose(1, 3);
        odom.pose.pose.position.z = pose(2, 3);
        odom.pose.pose.orientation = odom_trans.transform.rotation;

        odom.child_frame_id = "incre_baselink";
        odom.twist.twist.linear.x = 0.0;
        odom.twist.twist.linear.y = 0.0;
        odom.twist.twist.angular.z = 0.0;

        pose_incre_pub.publish(odom);
    }

  /**
   * @brief convert a Eigen::Matrix to TransformedStamped
   * @param stamp           timestamp
   * @param pose            pose matrix
   * @param frame_id        frame_id
   * @param child_frame_id  child_frame_id
   * @return transform
   */
  geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4d& pose, const std::string& frame_id, const std::string& child_frame_id) {
    Eigen::Quaterniond quat(pose.block<3, 3>(0, 0));
    quat.normalize();
    geometry_msgs::Quaternion odom_quat;
    odom_quat.w = quat.w();
    odom_quat.x = quat.x();
    odom_quat.y = quat.y();
    odom_quat.z = quat.z();

    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = stamp;
    odom_trans.header.frame_id = frame_id;
    odom_trans.child_frame_id = child_frame_id;

    odom_trans.transform.translation.x = pose(0, 3);
    odom_trans.transform.translation.y = pose(1, 3);
    odom_trans.transform.translation.z = pose(2, 3);
    odom_trans.transform.rotation = odom_quat;

    return odom_trans;
  }

    void resetOptimization()
    {

        std::cout<<"resetOptimization"<<std::endl;
        gtsam::ISAM2Params optParameters;
        optParameters.relinearizeThreshold = 0.1;
        optParameters.relinearizeSkip = 1;
        optimizer = new gtsam::ISAM2(optParameters);
        gtsam::NonlinearFactorGraph newGraphFactors;
        newgraph = newGraphFactors;
        gtsam::Values NewGraphValues;
        initialEstimate = NewGraphValues;

//        gtsam::LevenbergMarquardtParams optParameters;
//        optParameters.setVerbosity("ERROR");
//        optParameters.setOrderingType("METIS");
//        optParameters.setLinearSolverType("MULTIFRONTAL_CHOLESKY");
//        gtsam::NonlinearFactorGraph newGraphFactors;
//        newgraph = newGraphFactors;
//        gtsam::Values NewGraphValues;
//        initialEstimate = NewGraphValues;
//        optimizer= new gtsam::LevenbergMarquardtOptimizer optimizer(newgraph,initialEstimate,optParameters);

    }

private:
  // ROS
  ros::NodeHandle nh;
  ros::NodeHandle mt_nh;
  ros::NodeHandle private_nh;
  ros::Publisher pose_incre_pub;
  ros::Publisher pose_pub;

  std::string odom_child_frame_id;

  bool use_imu;
  bool invert_imu;

  float key_interval;
  int opt_frames;
  ros::Subscriber imu_sub;
  ros::Subscriber points_sub;
  ros::Subscriber globalmap_sub;
  ros::Subscriber initialpose_sub;
  ros::Subscriber updateGlobalPose_sub;

  ros::Time points_pre_time,points_curr_time;

  ros::Publisher aligned_pub;
  tf::TransformBroadcaster pose_broadcaster;
  tf::TransformBroadcaster pose_incre_broadcaster;
  tf::TransformListener tf_listener;

  // imu input buffer
  std::mutex imu_data_mutex;
  std::vector<sensor_msgs::ImuConstPtr> imu_data;

  // globalmap and registration method
  pcl::PointCloud<PointT>::Ptr globalmap;
  pcl::Filter<PointT>::Ptr downsample_filter;
  pcl::Registration<PointT, PointT>::Ptr registration;
  pcl::Registration<PointT, PointT>::Ptr localregistration;

  // pose estimator
  std::mutex pose_estimator_mutex;
  std::unique_ptr<hdl_localization::PoseEstimator> pose_estimator;

  // processing time buffer
  boost::circular_buffer<double> processing_time;

  // ********************the followings are for graph optimization*********************
public:
  bool firstkey=true;
  NonlinearFactorGraph newgraph;
  gtsam::ISAM2 *optimizer;
//  gtsam::LevenbergMarquardtOptimizer *optimizer;
  gtsam::Values initialEstimate;
  gtsam::Values isamCurrentEstimate;
  gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
  bool systemInitialized;
  int key_count;
  double imuAccNoise,imuGyrNoise,gravity;
  double odomLinearNoise,odomAngularNoise;

  gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
  gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
  gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
  gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
  gtsam::Vector noiseModelBetweenBias;

//  pcl::PointCloud<PointT>::Ptr pre_cloud;  //registration for local odometry
//  pcl::PointCloud<PointT>::Ptr cur_cloud;  //registration for local odometry
  gtsam::Vector3 prevVel_;
  gtsam::imuBias::ConstantBias prevBias_;
  gtsam::NavState prevState_;
  gtsam::Pose3 prevPose_;
//  gtsam::Pose3 poseFrom;

};

}

PLUGINLIB_EXPORT_CLASS(hdl_localization::HdlLocalizationNodelet, nodelet::Nodelet)


#pragma once

#include "graphics/opengl/opengl.hpp"
#include "graphics/color.hpp"
#include <boost/gil/gil_all.hpp>
#include <boost/mpl/vector_c.hpp>

namespace gil = boost::gil;
namespace mpl = boost::mpl;

namespace graphics
{
  template <unsigned Denom>
  struct DownsampleImpl
  {
    template <typename Ch1, typename Ch2>
    void operator()(Ch1 const & ch1, Ch2 & ch2) const
    {
      ch2 = ch1 / Denom;
    }
  };

  template <unsigned FromBig, unsigned ToSmall>
  struct Downsample
  {
    static const int Denom = 1 << (FromBig - ToSmall);

    template <typename SrcP, typename DstP>
    void operator()(SrcP const & src, DstP & dst) const
    {
      static_for_each(src, dst, DownsampleImpl<Denom>());
    }
  };

  struct RGBA8Traits
  {
    typedef gil::rgba8_pixel_t pixel_t;
    typedef gil::rgba8c_pixel_t const_pixel_t;
    typedef gil::rgba8_view_t view_t;
    typedef gil::rgba8c_view_t const_view_t;
    typedef gil::rgba8_image_t image_t;

    static const int maxChannelVal = 255;
    static const int channelScaleFactor = 1;

    static const int gl_pixel_data_type = GL_UNSIGNED_BYTE;
    static const int gl_pixel_format_type = GL_RGBA;

    typedef Downsample<8, 8> color_converter;

    static pixel_t const createPixel(graphics::Color const & c)
    {
      return pixel_t((unsigned char)(c.r / 255.0f) * maxChannelVal,
                     (unsigned char)(c.g / 255.0f) * maxChannelVal,
                     (unsigned char)(c.b / 255.0f) * maxChannelVal,
                     (unsigned char)(c.a / 255.0f) * maxChannelVal);
    }
  };

  struct RGB565Traits
  {
    typedef gil::packed_pixel_type<
        unsigned short,
        mpl::vector3_c<unsigned, 5, 6, 5>,
        gil::bgr_layout_t
    >::type pixel_t;

    typedef gil::memory_based_step_iterator<pixel_t*> iterator_t;
    typedef gil::memory_based_2d_locator<iterator_t> locator_t;
    typedef gil::image_view<locator_t> view_t;

    typedef pixel_t const const_pixel_t;

    typedef gil::memory_based_step_iterator<pixel_t const *> const_iterator_t;
    typedef gil::memory_based_2d_locator<const_iterator_t> const_locator_t;
    typedef gil::image_view<const_locator_t> const_view_t;

    typedef gil::image<pixel_t, false> image_t;

    static const int maxChannelVal = 32;
    static const int channelScaleFactor = 8;

    static const int gl_pixel_data_type = GL_UNSIGNED_SHORT_5_6_5;
    static const int gl_pixel_format_type = GL_RGB;

    typedef Downsample<8, 5> color_converter;

    static pixel_t const createPixel(graphics::Color const & c)
    {
      return pixel_t((int)(c.r / 255.0f) * maxChannelVal,
                     (int)(c.g / 255.0f) * maxChannelVal, //< fix this channel
                     (int)(c.b / 255.0f) * maxChannelVal);
    }
  };

  struct RGBA4Traits
  {
    typedef gil::packed_pixel_type<
        unsigned short,
        mpl::vector4_c<unsigned, 4, 4, 4, 4>,
        gil::abgr_layout_t
    >::type pixel_t;

    typedef gil::memory_based_step_iterator<pixel_t*> iterator_t;
    typedef gil::memory_based_2d_locator<iterator_t> locator_t;
    typedef gil::image_view<locator_t> view_t;

    typedef pixel_t const const_pixel_t;

    typedef gil::memory_based_step_iterator<pixel_t const *> const_iterator_t;
    typedef gil::memory_based_2d_locator<const_iterator_t> const_locator_t;
    typedef gil::image_view<const_locator_t> const_view_t;

    typedef gil::image<pixel_t, false> image_t;

    static const int maxChannelVal = 15;
    static const int channelScaleFactor = 16;

    static const int gl_pixel_data_type = GL_UNSIGNED_SHORT_4_4_4_4;
    static const int gl_pixel_format_type = GL_RGBA;

    typedef Downsample<8, 4> color_converter;

    static pixel_t const createPixel(graphics::Color const & c)
    {
      return pixel_t((int)(c.r / 255.0f) * maxChannelVal,
                     (int)(c.g / 255.0f) * maxChannelVal,
                     (int)(c.b / 255.0f) * maxChannelVal,
                     (int)(c.a / 255.0f) * maxChannelVal);
    }
  };
}

#ifdef OMIM_GL_ES
  #define DATA_TRAITS graphics::RGBA4Traits
  #define RT_TRAITS graphics::RGBA4Traits
#else
  #define DATA_TRAITS graphics::RGBA8Traits
  #define RT_TRAITS graphics::RGBA8Traits
#endif


#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "hoNDArray_fileio.h"
#include "cuNDArray.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"
#include "cuConebeamProjectionOperator.h"
#include "hoCuConebeamProjectionOperator.h"
#include "cuConvolutionOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"
#include "hoCuNDArray_blas.h"
#include "cgSolver.h"
#include "CBCT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuGPBBSolver.h"
#include "cuTvOperator.h"
#include "cuTvPicsOperator.h"
#include "cuNCGSolver.h"
#include "hoCuCgDescentSolver.h"
#include "hoNDArray_utils.h"
#include "hoCuPartialDerivativeOperator.h"
#include "cuDCTOperator.h"
#include "cuNDArray_fileio.h"
#include "cuATrousOperator.h"
#include "hdf5_utils.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>
#include <cuTv1dOperator.h>
#include "dicomWriter.h"
//#include "cuDWTOperator.h"
#include "cuATvOperator.h"

using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;

boost::shared_ptr<cuNDArray<float> > calculate_prior(boost::shared_ptr<CBCT_binning>  binning,boost::shared_ptr<CBCT_acquisition> ps, hoCuNDArray<float>& projections, std::vector<size_t> is_dims, floatd3 imageDimensions){
	std::cout << "Calculating FDK prior" << std::endl;
	boost::shared_ptr<CBCT_binning> binning_pics=binning->get_3d_binning();
	std::vector<size_t> is_dims3d = is_dims;
	is_dims3d.pop_back();
	boost::shared_ptr< hoCuConebeamProjectionOperator >
	Ep( new hoCuConebeamProjectionOperator() );
	Ep->setup(ps,binning_pics,imageDimensions);
	Ep->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());
	Ep->set_domain_dimensions(&is_dims3d);
	Ep->set_use_filtered_backprojection(true);
	boost::shared_ptr<hoCuNDArray<float> > prior3d(new hoCuNDArray<float>(&is_dims3d));
	Ep->mult_MH(&projections,prior3d.get());

	hoCuNDArray<float> tmp_proj(*ps->get_projections());
	Ep->mult_M(prior3d.get(),&tmp_proj);
	float s = dot(ps->get_projections().get(),&tmp_proj)/dot(&tmp_proj,&tmp_proj);
	*prior3d *= s;
	boost::shared_ptr<cuNDArray<float> > prior(new cuNDArray<float>(*expand( prior3d.get(), is_dims.back() )));
	std::cout << "Prior complete" << std::endl;
	return prior;
}

int main(int argc, char** argv)
{
	string acquisition_filename;
	string outputFile;
	uintd3 imageSize;
	floatd3 voxelSize;
	int device;
	unsigned int downsamples;
	unsigned int iterations;
	float rho,dct_weight;
	po::options_description desc("Allowed options");

	desc.add_options()
    		("help", "produce help message")
    		("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
    		("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    		("output,f", po::value<string>(&outputFile)->default_value("reconstruction.hdf5"), "Output filename")
    		("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
    		("binning,b",po::value<string>(),"Binning file for 4d reconstruction")
    		("SAG","Use exact SAG correction if present")
    		("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
    		("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
    		("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    		("TV,T",po::value<float>(),"TV Weight ")
					("ATV",po::value<float>(),"TV Weight ")
					("TV4D",po::value<float>(),"Total variation weight in temporal dimensions")
    		("PICS",po::value<float>(),"TV Weight of the prior image (Prior image compressed sensing)")
    		("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
    		("downsample,D",po::value<unsigned int>(&downsamples)->default_value(0),"Downsample projections this factor")
    		("rho",po::value<float>(&rho)->default_value(0.5f),"Rho-value for line search. Must be between 0 and 1. Smaller value means faster runtime, but less stable algorithm.")
    		("DCT",po::value<float>(&dct_weight)->default_value(0),"DCT regularization")
    		("use_prior","Use an FDK prior")
    		("Wavelet,W",po::value<float>(),"Wavelet weight")
    		("3D","Only use binning for selecting valid projections")
    		;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	std::stringstream command_line_string;
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		command_line_string << it->first << ": ";
		if (a.type() == typeid(std::string)) command_line_string << it->second.as<std::string>();
		else if (a.type() == typeid(int)) command_line_string << it->second.as<int>();
		else if (a.type() == typeid(unsigned int)) command_line_string << it->second.as<unsigned int>();
		else if (a.type() == typeid(float)) command_line_string << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) command_line_string << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) command_line_string << it->second.as<vector_td<int,3> >();
		else if (a.type() == typeid(vector_td<unsigned int,3>)) command_line_string << it->second.as<vector_td<unsigned int,3> >();
		else command_line_string << "Unknown type" << std::endl;
		command_line_string << std::endl;
	}

	std::cout << command_line_string.str();

	cudaSetDevice(device);
	cudaDeviceReset();

	//Really weird stuff. Needed to initialize the device?? Should find real bug.
	cudaDeviceManager::Instance()->lockHandle();
	cudaDeviceManager::Instance()->unlockHandle();

	boost::shared_ptr<CBCT_acquisition> ps(new CBCT_acquisition());
	ps->load(acquisition_filename);
	ps->get_geometry()->print(std::cout);
	ps->downsample(downsamples);

	boost::shared_ptr<CBCT_binning> binning(new CBCT_binning());
	if (vm.count("binning")){
		std::cout << "Loading binning data" << std::endl;
		binning->load(vm["binning"].as<string>());
		if (vm.count("3D"))
			binning = binning->get_3d_binning();
	} else binning->set_as_default_3d_bin(ps->get_projections()->get_size(2));
	binning->print(std::cout);

	floatd3 imageDimensions;
	if (vm.count("dimensions")){
		imageDimensions = vm["dimensions"].as<floatd3>();
		voxelSize = imageDimensions/imageSize;
	}
	else imageDimensions = voxelSize*imageSize;

	std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);

	std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
	std::cout << "Image size " << imageDimensions << std::endl;

	is_dims.push_back(binning->get_number_of_bins());

	cuNCGSolver<float> solver;
	//gpBbSolver<cuNDArray<float>> solver;

	hoCuNDArray<float>* projections = ps->get_projections().get();
	std::cout << "Projection nrm" << nrm2(projections) << std::endl;
	boost::shared_ptr<cuNDArray<float>> prior;
	if (vm.count("use_prior")){
		prior = calculate_prior(binning,ps,*projections,is_dims,imageDimensions);
		solver.set_x0(prior);
	}

	// auto projections2 = ps->get_projections();
	//auto result = calculate_prior(binning,ps,*projections2,is_dims,imageDimensions);
	// Define encoding matrix
	boost::shared_ptr< cuConebeamProjectionOperator >
	E( new cuConebeamProjectionOperator() );

	E->setup(ps,binning,imageDimensions);
	E->set_domain_dimensions(&is_dims);
	E->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());

	//hoCuGPBBSolver<float> solver;
	//hoCuCgDescentSolver<float> solver;

	solver.set_encoding_operator(E);
	solver.set_domain_dimensions(&is_dims);
	solver.set_max_iterations(iterations);
	solver.set_output_mode(hoCuGPBBSolver<float>::OUTPUT_VERBOSE);
	solver.set_non_negativity_constraint(true);
	solver.set_rho(rho);

	cuNDArray<float> cuproj(*projections);
	if (E->get_use_offset_correction())
		E->offset_correct(&cuproj);

	if (vm.count("TV")){
		std::cout << "Total variation regularization in use" << std::endl;
		boost::shared_ptr<cuTvOperator<float,3> > tv(new cuTvOperator<float,3>);
		tv->set_weight(vm["TV"].as<float>());
		solver.add_nonlinear_operator(tv);
		/*
    boost::shared_ptr<hoCuTvOperator<float,4> > tv2(new hoCuTvOperator<float,4>);
    tv2->set_step(2);
    tv2->set_weight(vm["TV"].as<float>());
    solver.add_nonlinear_operator(tv2);
    boost::shared_ptr<hoCuTvOperator<float,4> > tv3(new hoCuTvOperator<float,4>);
    tv3->set_step(3);
    tv3->set_weight(vm["TV"].as<float>());
    solver.add_nonlinear_operator(tv3);
		 */

	}

	if (vm.count("ATV")){
		std::cout << "Advanced Total variation regularization in use" << std::endl;
		boost::shared_ptr<cuATvOperator<float,3> > tv(new cuATvOperator<float,3>);
		tv->set_weight(vm["ATV"].as<float>());
		solver.add_nonlinear_operator(tv);
		/*
    boost::shared_ptr<hoCuTvOperator<float,4> > tv2(new hoCuTvOperator<float,4>);
    tv2->set_step(2);
    tv2->set_weight(vm["TV"].as<float>());
    solver.add_nonlinear_operator(tv2);
    boost::shared_ptr<hoCuTvOperator<float,4> > tv3(new hoCuTvOperator<float,4>);
    tv3->set_step(3);
    tv3->set_weight(vm["TV"].as<float>());
    solver.add_nonlinear_operator(tv3);
		 */

	}

	if (vm.count("TV4D")) {
		std::cout << "Total variation 4d regularization in use" << std::endl;
		boost::shared_ptr<cuTv1DOperator<float, 4> > tv4d(new cuTv1DOperator<float, 4>);
		tv4d->set_weight(vm["TV4D"].as<float>());
		solver.add_nonlinear_operator(tv4d);
	}

	if (vm.count("Wavelet")){
		//auto wave=  boost::make_shared<cuATrousOperator<float>>();
//		wave->set_domain_dimensions(&is_dims);
		//wave->set_levels({2,2,2,2});
		auto wave=  boost::make_shared<cuDWTOperator<float,3>>();
		wave->set_domain_dimensions(&is_dims);
		wave->set_codomain_dimensions(&is_dims);
		wave->set_levels(3);
		wave->set_weight(vm["Wavelet"].as<float>());
		solver.add_regularization_operator(wave,1);
	}

	if (dct_weight > 0){
		auto dctOp = boost::make_shared<cuDCTOperator<float>>();
		dctOp->set_domain_dimensions(&is_dims);
		dctOp->set_weight(dct_weight);
		solver.add_regularization_operator(dctOp);
	}

  if (vm.count("PICS")){
    std::cout << "PICS in use" << std::endl;
    if (!prior) prior = calculate_prior(binning,ps,*projections,is_dims,imageDimensions);
    boost::shared_ptr<cuTvPicsOperator<float,3> > pics (new cuTvPicsOperator<float,3>);
    pics->set_prior(prior);
    pics->set_weight(vm["PICS"].as<float>());
    solver.add_nonlinear_operator(pics);
    solver.set_x0(prior);
  }

	auto result = solver.solve(&cuproj);

	write_dicom(result.get(),command_line_string.str(),imageDimensions);
	//write_nd_array( result.get(), outputFile.c_str());
	saveNDArray2HDF5(result.get(),outputFile,imageDimensions,vector_td<float,3>(0),command_line_string.str(),iterations);

}


//#include <sba/sba.h>
#include <Eigen/SVD>
#include <Eigen/LU>
#include <iostream>
#include <limits>
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"

namespace ndt_feature_reg
{

template <typename PointSource, typename PointTarget>
PoseEstimator<PointSource,PointTarget>::PoseEstimator(int NRansac,
        double maxidx, double maxidd)
{
    numRansac = NRansac;
    maxInlierXDist2 = maxidx*maxidx;
    maxInlierDDist2 = maxidd*maxidd;
    rot.setIdentity();
    trans.setZero();

    // matcher
    matcher = new cv::BFMatcher(cv::NORM_L2);
    wx = 92;
    wy = 48;
    windowed = false;
    projectMatches = true;

    maxDist = std::numeric_limits<double>::max();
    minDist = -1.;
}

template <typename PointSource, typename PointTarget>
void
PoseEstimator<PointSource,PointTarget>::matchFrames(const NDTFrame<PointSource>& f0, const NDTFrame<PointTarget>& f1, std::vector<cv::DMatch>& fwd_matches)
{
    cv::Mat mask;
    if (windowed)
        mask = cv::windowedMatchingMask(f0.kpts, f1.kpts, wx, wy);

    //require at least 3 kpts each
    if(f0.kpts.size() > 3 && f1.kpts.size() > 3)
    {
	matcher->match(f0.dtors, f1.dtors, fwd_matches, mask);
    }
}

template <typename PointSource, typename PointTarget>
size_t PoseEstimator<PointSource,PointTarget>::estimate(const NDTFrame<PointSource> &f0, const NDTFrame<PointTarget> &f1)
{
    // set up match lists
    matches.clear();
    inliers.clear();

//     std::cout<<"N KPTS: "<<f0.kpts.size()<<" "<<f1.kpts.size()<<std::endl;
//     std::cout<<"descriptors: "<<f0.dtors.size().width<<"x"<<f0.dtors.size().height<<" "
//			       <<f1.dtors.size().width<<"x"<<f1.dtors.size().height<<std::endl;
    // do forward and reverse matches
    std::vector<cv::DMatch> fwd_matches, rev_matches;
    matchFrames(f0, f1, fwd_matches);
    matchFrames(f1, f0, rev_matches);
//     printf("**** Forward matches: %d, reverse matches: %d ****\n", (int)fwd_matches.size(), (int)rev_matches.size());

    // combine unique matches into one list
    for (int i = 0; i < (int)fwd_matches.size(); ++i)
    {
        if (fwd_matches[i].trainIdx >= 0)
            matches.push_back( cv::DMatch(i, fwd_matches[i].trainIdx, fwd_matches[i].distance) );
        //matches.push_back( cv::DMatch(fwd_matches[i].queryIdx, fwd_matches[i].trainIdx, fwd_matches[i].distance) );
    }
    for (int i = 0; i < (int)rev_matches.size(); ++i)
    {
        if (rev_matches[i].trainIdx >= 0 && i != fwd_matches[rev_matches[i].trainIdx].trainIdx)
            matches.push_back( cv::DMatch(rev_matches[i].trainIdx, i, rev_matches[i].distance) );
        //matches.push_back( cv::DMatch(rev_matches[i].trainIdx, rev_matches[i].queryIdx, rev_matches[i].distance) );
    }
//     printf("**** Total unique matches: %d ****\n", (int)matches.size());

    // do it
    return estimate(f0, f1, matches);
}

template <typename PointSource, typename PointTarget>
size_t PoseEstimator<PointSource,PointTarget>::estimate(const NDTFrame<PointSource>& f0, const NDTFrame<PointTarget>& f1,
        const std::vector<cv::DMatch> &matches)
{
    // convert keypoints in match to 3d points
    std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > p0; // homogeneous coordinates
    std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > p1;

    int nmatch = matches.size();
    //srand(getDoubleTime());

    // set up data structures for fast processing
    // indices to good matches
    std::vector<int> m0, m1;
    for (int i=0; i<nmatch; i++)
    {
        m0.push_back(matches[i].queryIdx);
        m1.push_back(matches[i].trainIdx);
        //std::cout<<m0[i]<<" "<<m1[i]<<std::endl;
    }

    nmatch = m0.size();

    if (nmatch < 3) return 0;   // can't do it...

    int bestinl = 0;

    // RANSAC loop
//#pragma omp parallel for shared( bestinl )
    for (int i=0; i<numRansac; i++)
    {
        //std::cout << "ransac loop : " << i << std::endl;
        // find a candidate
        int a=rand()%nmatch;
        int b = a;
        while (a==b)
            b=rand()%nmatch;
        int c = a;
        while (a==c || b==c)
            c=rand()%nmatch;

        int i0a = m0[a];
        int i0b = m0[b];
        int i0c = m0[c];
        int i1a = m1[a];
        int i1b = m1[b];
        int i1c = m1[c];

        if (i0a == i0b || i0a == i0c || i0b == i0c ||
                i1a == i1b || i1a == i1c || i1b == i1c)
            continue;

        //std::cout<<a<<" "<<b<<" "<<c<<std::endl;
        //std::cout<<i0a<<" "<<i0b<<" "<<i0c<<std::endl;
        //std::cout<<i1a<<" "<<i1b<<" "<<i1c<<std::endl;

        // get centroids
        Eigen::Vector3d p0a = f0.pts[i0a].head(3);
        Eigen::Vector3d p0b = f0.pts[i0b].head(3);
        Eigen::Vector3d p0c = f0.pts[i0c].head(3);
        Eigen::Vector3d p1a = f1.pts[i1a].head(3);
        Eigen::Vector3d p1b = f1.pts[i1b].head(3);
        Eigen::Vector3d p1c = f1.pts[i1c].head(3);

        Eigen::Vector3d c0 = (p0a+p0b+p0c)*(1.0/3.0);
        Eigen::Vector3d c1 = (p1a+p1b+p1c)*(1.0/3.0);

        //std::cout<<c0.transpose()<<std::endl;
        //std::cout<<c1.transpose()<<std::endl;
        // subtract out
        p0a -= c0;
        p0b -= c0;
        p0c -= c0;
        p1a -= c1;
        p1b -= c1;
        p1c -= c1;

        Eigen::Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
                            p1c*p0c.transpose();

        // do the SVD thang
        Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::Matrix3d V = svd.matrixV();
        Eigen::Matrix3d R = V * svd.matrixU().transpose();
        double det = R.determinant();
        //ntot++;
        if (det < 0.0)
        {
            //nneg++;
            V.col(2) = V.col(2)*-1.0;
            R = V * svd.matrixU().transpose();
        }
        Eigen::Vector3d tr = c0-R*c1;    // translation

        // transformation matrix, 3x4
        Eigen::Matrix<double,3,4> tfm;
        //        tfm.block<3,3>(0,0) = R.transpose();
        //        tfm.col(3) = -R.transpose()*tr;
        tfm.block<3,3>(0,0) = R;
        tfm.col(3) = tr;

#if 0
        // find inliers, based on image reprojection
        int inl = 0;
        for (int i=0; i<nmatch; i++)
        {
            Vector3d pt = tfm*f1.pts[m1[i]];
            Vector3d ipt = f0.cam2pix(pt);
            const cv::KeyPoint &kp = f0.kpts[m0[i]];
            double dx = kp.pt.x - ipt.x();
            double dy = kp.pt.y - ipt.y();
            double dd = f0.disps[m0[i]] - ipt.z();
            if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
                    dd*dd < maxInlierDDist2)
            {
                inl+=(int)sqrt(ipt.z()); // clever way to weight closer points
//		 inl+=(int)sqrt(ipt.z()/matches[i].distance);
//		 cout << "matches[i].distance : " << matches[i].distance << endl;
//		 inl++;
            }
        }
#endif
        int inl = 0;
        for (int i=0; i<nmatch; i++)
        {
            Eigen::Vector3d pt1 = tfm*f1.pts[m1[i]];
            Eigen::Vector3d pt0 = f0.pts[m0[i]].head(3);

//	       double z = fabs(pt1.z() - pt0.z())*0.5;
            double z = pt1.z();
            double dx = pt1.x() - pt0.x();
            double dy = pt1.y() - pt0.y();
            double dd = pt1.z() - pt0.z();

            if (projectMatches)
            {
                // The central idea here is to divide by the distance (this is essentially what cam2pix does).
                dx = dx / z;
                dy = dy / z;
            }
            if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
                    dd*dd < maxInlierDDist2)
            {
//----		    inl+=(int)sqrt(pt0.z()); // clever way to weight closer points
//		 inl+=(int)sqrt(ipt.z()/matches[i].distance);
//		 cout << "matches[i].distance : " << matches[i].distance << endl;
                inl++;
            }
        }

//#pragma omp critical
        if (inl > bestinl)
        {
            bestinl = inl;
            rot = R;
            trans = tr;
//	       std::cout << "bestinl : " << bestinl << std::endl;
        }
    }

    //printf("Best inliers: %d\n", bestinl);
    //printf("Total ransac: %d  Neg det: %d\n", ntot, nneg);

    // reduce matches to inliers
    std::vector<cv::DMatch> inls;    // temporary for current inliers
    inliers.clear();
    Eigen::Matrix<double,3,4> tfm;
    tfm.block<3,3>(0,0) = rot;
    tfm.col(3) = trans;

    //std::cout<<"f0: "<<f0.pts.size()<<" "<<f0.kpts.size()<<" "<<f0.pc_kpts.size()<<std::endl;
    //std::cout<<"f1: "<<f1.pts.size()<<" "<<f1.kpts.size()<<" "<<f1.pc_kpts.size()<<std::endl;

    nmatch = matches.size();
    for (int i=0; i<nmatch; i++)
    {
        Eigen::Vector3d pt1 = tfm*f1.pts[matches[i].trainIdx];
        //Eigen::Vector3d pt1_unchanged = f1.pts[matches[i].trainIdx].head(3);
        //Vector3d pt1 = pt1_unchanged;
#if 0
        Vector3d ipt = f0.cam2pix(pt);
        const cv::KeyPoint &kp = f0.kpts[matches[i].queryIdx];
        double dx = kp.pt.x - ipt.x();
        double dy = kp.pt.y - ipt.y();
        double dd = f0.disps[matches[i].queryIdx] - ipt.z();
#endif
        Eigen::Vector3d pt0 = f0.pts[matches[i].queryIdx].head(3);

        //double z = fabs(pt1.z() - pt0.z())*0.5;
        double z = pt1.z();
        double dx = pt1.x() - pt0.x();
        double dy = pt1.y() - pt0.y();
        double dd = pt1.z() - pt0.z();

        if (projectMatches)
        {
            // The central idea here is to divide by the distance (this is essentially what cam2pix does).
            dx = dx / z;
            dy = dy / z;
        }

        if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
                dd*dd < maxInlierDDist2)
        {
            if (z < maxDist && z > minDist)

//	       if (fabs(f0.kpts[matches[i].queryIdx].pt.y - f1.kpts[matches[i].trainIdx].pt.y) > 300)
            {
//		   std::cout << " ---------- " << dx << "," << dy << "," << dd << ",\npt0 " << pt0.transpose() << "\npt1 " << pt1.transpose() << f0.kpts[matches[i].queryIdx].pt << "," <<
//			 f1.kpts[matches[i].trainIdx].pt << "\n unchanged pt1 " << pt1_unchanged.transpose() << std::endl;
                inliers.push_back(matches[i]);
            }
        }
    }

#if 0
    // Test with the SBA...
    {
        // system
        SysSBA sba;
        sba.verbose = 0;

#if 0
        // set up nodes
        // should have a frame => node function
        Vector4d v0 = Vector4d(0,0,0,1);
        Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
        sba.addNode(v0, q0, f0.cam, true);

        Quaterniond qr1(rot);   // from rotation matrix
        Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);

        //        sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
        qr1.normalize();
        sba.addNode(temptrans, qr1, f1.cam, false);

        int in = 3;
        if (in > (int)inls.size())
            in = inls.size();

        // set up projections
        for (int i=0; i<(int)inls.size(); i++)
        {
            // add point
            int i0 = inls[i].queryIdx;
            int i1 = inls[i].trainIdx;
            Vector4d pt = f0.pts[i0];
            sba.addPoint(pt);

            // projected point, ul,vl,ur
            Vector3d ipt;
            ipt(0) = f0.kpts[i0].pt.x;
            ipt(1) = f0.kpts[i0].pt.y;
            ipt(2) = ipt(0)-f0.disps[i0];
            sba.addStereoProj(0, i, ipt);

            // projected point, ul,vl,ur
            ipt(0) = f1.kpts[i1].pt.x;
            ipt(1) = f1.kpts[i1].pt.y;
            ipt(2) = ipt(0)-f1.disps[i1];
            sba.addStereoProj(1, i, ipt);
        }

        sba.huber = 2.0;
        sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
        int nbad = sba.removeBad(2.0); // 2.0
        cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
        sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);

//        cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;

        // get the updated transform
        trans = sba.nodes[1].trans.head(3);
        Quaterniond q1;
        q1 = sba.nodes[1].qrot;
        quat = q1;
        rot = q1.toRotationMatrix();

        // set up inliers
        inliers.clear();
        for (int i=0; i<(int)inls.size(); i++)
        {
            ProjMap &prjs = sba.tracks[i].projections;
            if (prjs[0].isValid && prjs[1].isValid) // valid track
                inliers.push_back(inls[i]);
        }

        printf("Inliers: %d   After polish: %d\n", (int)inls.size(), (int)inliers.size());
#endif
    }
#endif

//     std::cout << std::endl << trans.transpose().head(3) << std::endl << std::endl;
//     std::cout << rot << std::endl;

    return inliers.size();
}
}


#include "translate.h"

#include "dynamic_value/boolean_value.h"
#include "dynamic_value/float_value.h"
#include "dynamic_value/get_value_as.h"
#include "geometry_component.h"
#include "geometry_system.h"
#include "hierarchical_component.h"
#include "hierarchical_system.h"
#include "math_lib/matrix_base.h"
#include "procedural_object_system.h"

#include "geometry_operations/matrix_transform.h"

#include <boost/qvm/map_mat_vec.hpp>
#include <boost/qvm/map_vec_mat.hpp>

namespace pagoda
{
const std::string Translate::s_inputGeometry("in");
const std::string Translate::s_outputGeometry("out");

Translate::Translate(ProceduralObjectSystemPtr objectSystem) : ProceduralOperation(objectSystem)
{
	CreateInputInterface(s_inputGeometry);
	CreateOutputInterface(s_outputGeometry);

	RegisterValues({{"x", std::make_shared<FloatValue>(0.0f)},
	                {"y", std::make_shared<FloatValue>(0.0f)},
	                {"z", std::make_shared<FloatValue>(0.0f)},
	                {"world", std::make_shared<Boolean>(false)}});
}
Translate::~Translate() {}

void Translate::DoWork()
{
	START_PROFILE;

	auto geometrySystem = m_proceduralObjectSystem->GetComponentSystem<GeometrySystem>();
	auto hierarchicalSystem = m_proceduralObjectSystem->GetComponentSystem<HierarchicalSystem>();

	while (HasInput(s_inputGeometry))
	{
		ProceduralObjectPtr inObject = GetInputProceduralObject(s_inputGeometry);
		ProceduralObjectPtr outObject = CreateOutputProceduralObject(s_outputGeometry);

		auto inGeometryComponent = geometrySystem->GetComponentAs<GeometryComponent>(inObject);
		GeometryPtr inGeometry = inGeometryComponent->GetGeometry();
		auto outGeometryComponent = geometrySystem->CreateComponentAs<GeometryComponent>(outObject);
		auto outGeometry = std::make_shared<Geometry>();
		outGeometryComponent->SetGeometry(outGeometry);

		auto inScope = inGeometryComponent->GetScope();
		UpdateValue("x");
		UpdateValue("y");
		UpdateValue("z");
		UpdateValue("world");

		auto x = get_value_as<float>(*GetValue("x"));
		auto y = get_value_as<float>(*GetValue("y"));
		auto z = get_value_as<float>(*GetValue("z"));
		auto inWorldCoordinates = get_value_as<std::string>(*GetValue("world")) == "true";
		Mat4x4F matrix;
		if (inWorldCoordinates)
		{
			matrix = boost::qvm::translation_mat(Vec3F{x, y, z});
		}
		else
		{
			matrix = boost::qvm::translation_mat(inScope.GetLocalVector(Vec3F{x, y, z}));
		}
		MatrixTransform<Geometry> transform(matrix);

		transform.Execute(inGeometry, outGeometry);

		outGeometryComponent->SetScope(Scope::FromGeometryAndConstrainedRotation(outGeometry, inScope.GetRotation()));

		auto inHierarchicalComponent = hierarchicalSystem->GetComponentAs<HierarchicalComponent>(inObject);
		auto outHierarchicalComponent = hierarchicalSystem->CreateComponentAs<HierarchicalComponent>(outObject);
		hierarchicalSystem->SetParent(outHierarchicalComponent, inHierarchicalComponent);
	}
}
}  // namespace pagoda


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include <Eigen/Core>

#include <tudat/astro/basic_astro/physicalConstants.h>
#include <tudat/basics/testMacros.h>
#include <tudat/math/basic/mathematicalConstants.h>
#include "tudat/astro/basic_astro/unitConversions.h"
#include <tudat/astro/basic_astro/orbitalElementConversions.h>

#include <tudat/io/basicInputOutput.h>
#include <tudat/io/applicationOutput.h>

#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/astro/trajectory_design/trajectory.h"
#include "tudat/astro/trajectory_design/exportTrajectory.h"
#include "tudat/astro/trajectory_design/planetTrajectory.h"

int main( )
{
    using namespace tudat;
    using namespace tudat::input_output;
    using namespace tudat::input_output::parsed_data_vector_utilities;
    using namespace tudat::transfer_trajectories;

    //////////////////////////////////////////////////////////////////////////
    ////////////////////////// CASSSINI //////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////

    // Specify required parameters
    // Specify the number of legs and type of legs.
    int numberOfLegs = 6;
    std::vector< TransferLegType > legTypeVector;
    legTypeVector.resize( numberOfLegs );
    legTypeVector[ 0 ] = mga_Departure;
    legTypeVector[ 1 ] = mga_Swingby;
    legTypeVector[ 2 ] = mga_Swingby;
    legTypeVector[ 3 ] = mga_Swingby;
    legTypeVector[ 4 ] = mga_Swingby;
    legTypeVector[ 5 ] = capture;

    // Create the ephemeris vector.
    std::vector< ephemerides::EphemerisPointer >
            ephemerisVector( numberOfLegs );
    ephemerisVector[ 0 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter );
    ephemerisVector[ 1 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus );
    ephemerisVector[ 2 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus );
    ephemerisVector[ 3 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter );
    ephemerisVector[ 4 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::jupiter );
    ephemerisVector[ 5 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::saturn );

    // Create gravitational parameter vector
    Eigen::VectorXd gravitationalParameterVector( numberOfLegs );
    gravitationalParameterVector << 3.9860119e14, 3.24860e14, 3.24860e14, 3.9860119e14, 1.267e17, 3.79e16;

    // Create variable vector.
    Eigen::VectorXd variableVector( numberOfLegs + 1 );
    variableVector << -789.8117, 158.302027105278, 449.385873819743, 54.7489684339665,
            1024.36205846918, 4552.30796805542, 1/*dummy*/;
    variableVector *= physical_constants::JULIAN_DAY;

    // Create departure and capture variables.
    Eigen::VectorXd semiMajorAxes( 2 ), eccentricities( 2 );
    semiMajorAxes << std::numeric_limits< double >::infinity( ), 1.0895e8 / 0.02;
    eccentricities << 0.0, 0.98;

    // Sun gravitational parameter
    const double sunGravitationalParameter = 1.32712428e20;

    // Create minimum pericenter radii vector
    Eigen::VectorXd minimumPericenterRadii( numberOfLegs );
    minimumPericenterRadii << 6778000.0, 6351800.0, 6351800.0, 6778000.0, 600000000.0, 600000000.0;

    // Create the trajectory problem.
    Trajectory Cassini1( numberOfLegs, legTypeVector, ephemerisVector,
                         gravitationalParameterVector, variableVector, sunGravitationalParameter,
                         minimumPericenterRadii, semiMajorAxes, eccentricities );

    // Vectors for the specific maneuvers and the total delta v
    std::vector< Eigen::Vector3d > positionVector;
    std::vector< double > timeVector;
    std::vector< double > deltaVVector;
    double resultingDeltaV;

    // Calculate the orbits
    Cassini1.calculateTrajectory( resultingDeltaV );
    Cassini1.maneuvers( positionVector, timeVector, deltaVVector );

    std::cout << " Cassini Mission: " << std::endl;
    std::cout << " Total Delta V needed: " << resultingDeltaV <<std::endl;
    std::cout << " Time of Earth departure: " << timeVector[ 0 ]
              << ". Delta V needed at Earth: " << deltaVVector[ 0 ] << std::endl;
    std::cout << " Time of Venus visit: " << timeVector[ 1 ]
              << ". Delta V needed at Venus: " << deltaVVector[ 1 ] << std::endl;
    std::cout << " Time of second Venus visit: " << timeVector[ 2 ]
              << ". Delta V needed at Venus: " << deltaVVector[ 2 ] << std::endl;
    std::cout << " Time of Earth visit: " << timeVector[ 3 ]
              << ". Delta V needed at Earth: " << deltaVVector[ 3 ] << std::endl;
    std::cout << " Time of Jupiter visit: " << timeVector[ 4 ]
              << ". Delta V needed at Jupiter: " << deltaVVector[ 4 ] << std::endl;
    std::cout << " Time of Saturn capture: " << timeVector[ 5 ]
              << ". Delta V needed at Saturn: " << deltaVVector[ 5 ] << std::endl;

    std::cout << std::endl << std::endl;

    //////////////////////////////////////////////////////////////////////////
    ////////////////////////// MESSENGER /////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////

    // Specify required parameters
    // Specify the number of legs and type of legs.
    numberOfLegs = 5;
    legTypeVector.resize( numberOfLegs );
    legTypeVector[ 0 ] = mga1DsmVelocity_Departure;
    legTypeVector[ 1 ] = mga1DsmVelocity_Swingby;
    legTypeVector[ 2 ] = mga1DsmVelocity_Swingby;
    legTypeVector[ 3 ] = mga1DsmVelocity_Swingby;
    legTypeVector[ 4 ] = capture;

    // Create the ephemeris vector.
    ephemerisVector.resize( numberOfLegs );
    ephemerisVector[ 0 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter );
    ephemerisVector[ 1 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter );
    ephemerisVector[ 2 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus );
    ephemerisVector[ 3 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus );
    ephemerisVector[ 4 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::mercury );

    // Create gravitational parameter vector
    gravitationalParameterVector.resize( numberOfLegs );
    gravitationalParameterVector << 3.9860119e14, 3.9860119e14, 3.24860e14, 3.24860e14, 2.2321e13;

    // Create variable vector.
    variableVector.resize( numberOfLegs /*time of flight*/ + 1 /*start epoch*/ +
                           4 * ( numberOfLegs - 1 ) /*additional variables for model, except the final capture leg*/ );

    // Add the time of flight and start epoch, which are in JD.
    variableVector << 1171.64503236 * physical_constants::JULIAN_DAY,
            399.999999715 * physical_constants::JULIAN_DAY,
            178.372255301 * physical_constants::JULIAN_DAY,
            299.223139512 * physical_constants::JULIAN_DAY,
            180.510754824 * physical_constants::JULIAN_DAY,
            1, // The capture time is irrelevant for the final leg.
            // Add the additional variables.
            0.234594654679, 1408.99421278, 0.37992647165 * 2 * 3.14159265358979,
            std::acos(  2 * 0.498004040298 - 1. ) - 3.14159265358979 / 2, // 1st leg.
            0.0964769387134, 1.35077257078, 1.80629232251 * 6.378e6, 0.0, // 2nd leg.
            0.829948744508, 1.09554368115, 3.04129845698 * 6.052e6, 0.0, // 3rd leg.
            0.317174785637, 1.34317576594, 1.10000000891 * 6.052e6, 0.0; // 4th leg.

    // Create minimum pericenter radii vector
    minimumPericenterRadii.resize( numberOfLegs );
    minimumPericenterRadii << TUDAT_NAN, TUDAT_NAN, TUDAT_NAN, TUDAT_NAN, TUDAT_NAN;

    // Create departure and capture variables.
    semiMajorAxes << std::numeric_limits< double >::infinity( ),
            std::numeric_limits< double >::infinity( );
    eccentricities << 0.0, 0.0;

    // Create the trajectory problem.
    Trajectory Messenger( numberOfLegs, legTypeVector, ephemerisVector,
                          gravitationalParameterVector, variableVector, sunGravitationalParameter,
                          minimumPericenterRadii, semiMajorAxes, eccentricities );

    // Vectors for the specific maneuvers and the total delta v
    std::vector< Eigen::Vector3d > positionVectorMessenger;
    std::vector< double > timeVectorMessenger;
    std::vector< double > deltaVVectorMessenger;
    double resultingDeltaVMessenger;

    // Calculate the orbits
    Messenger.calculateTrajectory( resultingDeltaVMessenger );
    Messenger.maneuvers( positionVectorMessenger, timeVectorMessenger, deltaVVectorMessenger );

    std::cout << " Messenger Mission: " << std::endl;
    std::cout << " Total Delta V: " << resultingDeltaVMessenger <<std::endl;
    std::cout << " Time of Earth departure: " << timeVectorMessenger[ 0 ]
              << ". Delta V needed at Earth: " << deltaVVectorMessenger[ 0 ] << std::endl;
    std::cout << " Time of 1st DSM: " << timeVectorMessenger[ 1 ]
              << ". Delta V needed for 1st DSM: " << deltaVVectorMessenger[ 1 ] << std::endl;
    std::cout << " Time of second Earth visit: " << timeVectorMessenger[ 2 ]
              << ". Delta V needed at Earth: " << deltaVVectorMessenger[ 2 ] << std::endl;
    std::cout << " Time of 2nd DSM: " << timeVectorMessenger[ 3 ]
              << ". Delta V needed for 2nd DSM: " << deltaVVectorMessenger[ 3 ] << std::endl;
    std::cout << " Time of Venus visit: " << timeVectorMessenger[ 4 ]
              << ". Delta V needed at Venus: " << deltaVVectorMessenger[ 4 ] << std::endl;
    std::cout << " Time of 3d DSM: " << timeVectorMessenger[ 5 ]
              << ". Delta V needed for 3d DSM: " << deltaVVectorMessenger[ 5 ] << std::endl;
    std::cout << " Time of second Venus visit: " << timeVectorMessenger[ 6 ]
              << ". Delta V needed at Venus: " << deltaVVectorMessenger[ 6 ] << std::endl;
    std::cout << " Time of 4th DSM: " << timeVectorMessenger[ 7 ]
              << ". Delta V needed for 4th DSM: " << deltaVVectorMessenger[ 7 ] << std::endl;
    std::cout << " Time of Mercury capture: " << timeVectorMessenger[ 8 ]
              << ". Delta V needed at Mercury: " << deltaVVectorMessenger[ 8 ] << std::endl;

    // Define vectors to calculate intermediate points
    std::vector< Eigen::Vector3d > interPositionVectorMessenger;
    std::vector< double > interTimeVectorMessenger;

    // Calculate intermediate points and write to file
    std::string outputFileTraj = tudat_applications::getOutputPath( ) + "/messengerTrajectory.dat";
    Messenger.intermediatePoints( 1000.0 , interPositionVectorMessenger, interTimeVectorMessenger );
    writeTrajectoryToFile( interPositionVectorMessenger, interTimeVectorMessenger, outputFileTraj );

    // Define vectors to calculate intermediate points
    std::vector< Eigen::Vector3d > manPositionVectorMessenger;
    std::vector< double > manTimeVectorMessenger;
    std::vector< double > manDeltaVVectorMessenger;

    // Calculate maneuvers and write to file
    std::string outputFileMan = tudat_applications::getOutputPath( ) + "/messengerManeuvers.dat";
    Messenger.maneuvers( manPositionVectorMessenger, manTimeVectorMessenger, manDeltaVVectorMessenger );
    writeTrajectoryToFile( manPositionVectorMessenger, manTimeVectorMessenger, outputFileMan );

    // Calculate trajectories of the planets and output to file
    std::vector< Eigen::Vector3d > positionVectorEarth;
    std::vector< double > timeVectorEarth;
    std::vector< Eigen::Vector3d > positionVectorVenus;
    std::vector< double > timeVectorVenus;
    std::vector< Eigen::Vector3d > positionVectorMercury;
    std::vector< double > timeVectorMercury;

    // Earth
    returnSingleRevolutionPlanetTrajectory(
                std::make_shared< ephemerides::ApproximatePlanetPositions >(
                    ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter ),
                sunGravitationalParameter,
                1171.64503236,
                1000.0,
                positionVectorEarth,
                timeVectorEarth );

    // Venus
    returnSingleRevolutionPlanetTrajectory(
                std::make_shared< ephemerides::ApproximatePlanetPositions >(
                    ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus ),
                sunGravitationalParameter,
                1171.64503236,
                1000.0,
                positionVectorVenus,
                timeVectorVenus );

    // Mercury
    returnSingleRevolutionPlanetTrajectory(
                std::make_shared< ephemerides::ApproximatePlanetPositions >(
                    ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::mercury ),
                sunGravitationalParameter,
                1171.64503236,
                1000.0,
                positionVectorMercury,
                timeVectorMercury );

    std::string outputFilePlanetE = tudat_applications::getOutputPath(  ) + "earthTrajectory.dat";
    writeTrajectoryToFile( positionVectorEarth, timeVectorEarth, outputFilePlanetE );

    std::string outputFilePlanetV = tudat_applications::getOutputPath(  ) + "venusTrajectory.dat";
    writeTrajectoryToFile( positionVectorVenus, timeVectorVenus, outputFilePlanetV );

    std::string outputFilePlanetM = tudat_applications::getOutputPath(  ) + "mercuryTrajectory.dat";
    writeTrajectoryToFile( positionVectorMercury, timeVectorMercury, outputFilePlanetM );

    // Final statement.
    // The exit code EXIT_SUCCESS indicates that the program was successfully executed.
    return EXIT_SUCCESS;
}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_FIRSTBITSET_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_FIRSTBITSET_HPP_INCLUDED

#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/scalar/bitwise_cast.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( firstbitset_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0, unsigned>;
    BOOST_FORCEINLINE result_t operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return a0 & (~a0+One<result_t>());
    }
  };

  BOOST_DISPATCH_OVERLOAD ( firstbitset_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0, unsigned>;
    result_t operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return firstbitset(bitwise_cast<result_t>(a0));
    }
  };
} } }

#endif


#include <iostream>
#include <string>

#include <boost/algorithm/string.hpp>

#include <fc/io/json.hpp>
#include <fc/reflect/reflect.hpp>
#include <fc/variant.hpp>

#include <graphene/utilities/key_conversion.hpp>

#include <btcm/chain/protocol/types.hpp>
using namespace btcm::chain;

struct signing_request
{
   fc::sha256                         dig;
   std::string                        wif;
};

struct signing_result
{
   fc::sha256                         dig;
   btcm::chain::public_key_type    key;
   btcm::chain::signature_type     sig;
};

FC_REFLECT( signing_request, (dig)(wif) )
FC_REFLECT( signing_result, (dig)(key)(sig) )

int main()
{
   // hash key pairs on stdin
   while( std::cin )
   {
      std::string line;
      std::getline( std::cin, line );
      boost::trim(line);
      if( line == "" )
         continue;

      fc::variant v = fc::json::from_string( line );
      signing_request sreq;
      fc::from_variant( v, sreq, 10 );
      signing_result sres;
      sres.dig = sreq.dig;
      fc::ecc::private_key priv_key = *graphene::utilities::wif_to_key( sreq.wif );
      sres.sig = priv_key.sign_compact( sreq.dig );
      sres.key = btcm::chain::public_key_type( priv_key.get_public_key() );
      std::cout << fc::json::to_string( sres ) << std::endl;
   }
   return 0;
}


/* Copyright (c) 2016, the Cap authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#define BOOST_TEST_MODULE ExactTransientSolution
#define BOOST_TEST_MAIN
#include <cap/energy_storage_device.h>
#include <cap/mp_values.h>
#include <deal.II/base/types.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/dofs/dof_handler.h>
#include <boost/test/unit_test.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/info_parser.hpp>
#include <cmath>
#include <iostream>
#include <fstream>
#include <numeric>

namespace cap
{

void compute_parameters(
    std::shared_ptr<boost::property_tree::ptree const> input_database,
    std::shared_ptr<boost::property_tree::ptree> output_database)
{
  double const cm2_to_m2 = 0.0001;
  double const cm_to_m = 0.01;
  double const cross_sectional_area =
      cm2_to_m2 * input_database->get<double>("geometry.geometric_area");
  double const electrode_width =
      cm_to_m *
      input_database->get<double>("geometry.anode_electrode_thickness");
  double const separator_width =
      cm_to_m * input_database->get<double>("geometry.separator_thickness");

  // getting the material parameters values
  std::shared_ptr<boost::property_tree::ptree> material_properties_database =
      std::make_shared<boost::property_tree::ptree>(
          input_database->get_child("material_properties"));
  cap::MPValuesParameters<2> mp_values_params(material_properties_database);
  std::shared_ptr<boost::property_tree::ptree> geometry_database =
      std::make_shared<boost::property_tree::ptree>(
          input_database->get_child("geometry"));
  mp_values_params.geometry =
      std::make_shared<cap::DummyGeometry<2>>(geometry_database);
  std::shared_ptr<cap::MPValues<2>> mp_values =
      std::shared_ptr<cap::MPValues<2>>(new cap::MPValues<2>(mp_values_params));
  // build dummy cell itertor and set its material id
  dealii::Triangulation<2> triangulation;
  dealii::GridGenerator::hyper_cube(triangulation);
  dealii::DoFHandler<2> dof_handler(triangulation);
  dealii::DoFHandler<2>::active_cell_iterator cell = dof_handler.begin_active();
  // electrode
  cell->set_material_id(input_database->get<dealii::types::material_id>(
      "geometry.anode_electrode_material_id"));
  std::vector<double> electrode_solid_electrical_conductivity_values(1);
  std::vector<double> electrode_liquid_electrical_conductivity_values(1);
  std::vector<double> electrode_specific_capacitance_values(1);
  std::vector<double> electrode_exchange_current_density_values(1);
  std::vector<double> electrode_electron_thermal_voltage_values(1);
  mp_values->get_values("solid_electrical_conductivity", cell,
                        electrode_solid_electrical_conductivity_values);
  mp_values->get_values("liquid_electrical_conductivity", cell,
                        electrode_liquid_electrical_conductivity_values);
  mp_values->get_values("specific_capacitance", cell,
                        electrode_specific_capacitance_values);
  mp_values->get_values("faradaic_reaction_coefficient", cell,
                        electrode_exchange_current_density_values);
  mp_values->get_values("electron_thermal_voltage", cell,
                        electrode_electron_thermal_voltage_values);
  if (electrode_exchange_current_density_values[0] == 0.0)
    throw std::runtime_error("test assumes faradaic processes are present, "
                             "exchange_current_density has to be non zero");
  double const total_current = -1.0; // normalized
  double const dimensionless_exchange_current_density =
      electrode_exchange_current_density_values[0] *
      std::pow(electrode_width, 2) *
      (1.0 / electrode_solid_electrical_conductivity_values[0] +
       1.0 / electrode_liquid_electrical_conductivity_values[0]);
  double const dimensionless_current_density =
      total_current * electrode_width /
      electrode_liquid_electrical_conductivity_values[0] /
      electrode_electron_thermal_voltage_values[0];
  double const ratio_of_solution_phase_to_matrix_phase_conductivities =
      electrode_liquid_electrical_conductivity_values[0] /
      electrode_solid_electrical_conductivity_values[0];

  output_database->put("dimensionless_current_density",
                       dimensionless_current_density);
  output_database->put("dimensionless_exchange_current_density",
                       dimensionless_exchange_current_density);
  output_database->put("ratio_of_solution_phase_to_matrix_phase_conductivities",
                       ratio_of_solution_phase_to_matrix_phase_conductivities);

  output_database->put("position_normalization_factor", electrode_width);
  output_database->put(
      "time_normalization_factor",
      electrode_specific_capacitance_values[0] *
          (1.0 / electrode_solid_electrical_conductivity_values[0] +
           1.0 / electrode_liquid_electrical_conductivity_values[0]) *
          std::pow(electrode_width, 2));

  // separator
  cell->set_material_id(input_database->get<dealii::types::material_id>(
      "geometry.separator_material_id"));
  std::vector<double> separator_liquid_electrical_conductivity_values(1);
  mp_values->get_values("liquid_electrical_conductivity", cell,
                        separator_liquid_electrical_conductivity_values);

  double const potential_drop_across_the_separator =
      -total_current * separator_width /
      separator_liquid_electrical_conductivity_values[0];
  double const voltage_normalization_factor =
      electrode_electron_thermal_voltage_values[0];
  output_database->put("potential_drop_across_the_separator",
                       potential_drop_across_the_separator);
  output_database->put("voltage_normalization_factor",
                       voltage_normalization_factor);
  output_database->put("cross_sectional_area", cross_sectional_area);
}

void verification_problem(
    std::shared_ptr<cap::EnergyStorageDevice> dev,
    std::shared_ptr<boost::property_tree::ptree const> database,
    std::ostream &os = std::cout)
{
  double dimensionless_current_density =
      database->get<double>("dimensionless_current_density");
  double dimensionless_exchange_current_density =
      database->get<double>("dimensionless_exchange_current_density");
  double ratio_of_solution_phase_to_matrix_phase_conductivities =
      database->get<double>(
          "ratio_of_solution_phase_to_matrix_phase_conductivities");

  int const infty =
      database->get<int>("terms_in_truncation_of_infinite_series");
  double const pi = std::acos(-1.0);

  auto compute_dimensionless_overpotential =
      [infty, pi, &ratio_of_solution_phase_to_matrix_phase_conductivities,
       &dimensionless_exchange_current_density, &dimensionless_current_density](
          double const dimensionless_time, double const dimensionless_position)
  {
    std::vector<double> coefficients(infty);
    for (int n = 0; n < infty; ++n)
    {
      coefficients[n] =
          (ratio_of_solution_phase_to_matrix_phase_conductivities *
               std::cos(n * pi) +
           1.0) /
          (dimensionless_exchange_current_density +
           std::pow(n, 2) * std::pow(pi, 2)) *
          std::cos(n * pi * dimensionless_position) *
          std::exp(-(std::pow(n, 2) * std::pow(pi, 2) +
                     dimensionless_exchange_current_density) *
                   dimensionless_time);
    }
    return dimensionless_current_density *
               (1.0 + ratio_of_solution_phase_to_matrix_phase_conductivities) *
               std::exp(-dimensionless_exchange_current_density *
                        dimensionless_time) /
               dimensionless_exchange_current_density -
           dimensionless_current_density *
               (std::cosh(std::sqrt(dimensionless_exchange_current_density) *
                          (1.0 - dimensionless_position)) +
                ratio_of_solution_phase_to_matrix_phase_conductivities *
                    std::cosh(
                        std::sqrt(dimensionless_exchange_current_density) *
                        dimensionless_position)) /
               (std::sqrt(dimensionless_exchange_current_density) *
                std::sinh(std::sqrt(dimensionless_exchange_current_density))) +
           2.0 * dimensionless_current_density *
               std::accumulate(&(coefficients[1]), &(coefficients[infty]), 0.0);
  };

  auto compute_dimensionless_potential_drop_across_the_electrode =
      [&compute_dimensionless_overpotential,
       &ratio_of_solution_phase_to_matrix_phase_conductivities,
       &dimensionless_current_density](double const dimensionless_time)
  {
    return (compute_dimensionless_overpotential(dimensionless_time, 0.0) +
            ratio_of_solution_phase_to_matrix_phase_conductivities *
                compute_dimensionless_overpotential(dimensionless_time, 1.0) -
            dimensionless_current_density *
                ratio_of_solution_phase_to_matrix_phase_conductivities) /
           (1.0 + ratio_of_solution_phase_to_matrix_phase_conductivities);
  };

  auto compute_dimensionless_interfacial_current_density =
      [infty, pi, &ratio_of_solution_phase_to_matrix_phase_conductivities,
       &dimensionless_exchange_current_density, &dimensionless_current_density](
          double const dimensionless_time, double const dimensionless_position)
  {
    std::vector<double> coefficients(infty);
    for (int n = 0; n < infty; ++n)
    {
      coefficients[n] =
          (ratio_of_solution_phase_to_matrix_phase_conductivities *
               std::cos(n * pi) +
           1.0) /
          (dimensionless_exchange_current_density +
           std::pow(n, 2) * std::pow(pi, 2)) /
          (ratio_of_solution_phase_to_matrix_phase_conductivities + 1.0) *
          std::pow(n, 2) * std::pow(pi, 2) *
          std::cos(n * pi * dimensionless_position) *
          std::exp(-(std::pow(n, 2) * std::pow(pi, 2) +
                     dimensionless_exchange_current_density) *
                   dimensionless_time);
    }
    return -2.0 * dimensionless_current_density *
               std::accumulate(&(coefficients[1]), &(coefficients[infty]),
                               0.0) -
           dimensionless_current_density *
               std::sqrt(dimensionless_exchange_current_density) *
               (std::cosh(std::sqrt(dimensionless_exchange_current_density) *
                          (1.0 - dimensionless_position)) +
                ratio_of_solution_phase_to_matrix_phase_conductivities *
                    std::cosh(
                        std::sqrt(dimensionless_exchange_current_density) *
                        dimensionless_position)) /
               ((ratio_of_solution_phase_to_matrix_phase_conductivities + 1.0) *
                std::sinh(std::sqrt(dimensionless_exchange_current_density)));
  };

  // exact vs computed
  double const charge_current = database->get<double>("charge_current");
  double const charge_time = database->get<double>("charge_time");
  double const time_step = database->get<double>("time_step");
  double const epsilon = time_step * 1.0e-4;
  double const cross_sectional_area =
      database->get<double>("cross_sectional_area");
  double const time_normalization_factor =
      database->get<double>("time_normalization_factor");
  double const voltage_normalization_factor =
      database->get<double>("voltage_normalization_factor");
  double potential_drop_across_the_separator =
      database->get<double>("potential_drop_across_the_separator");
  potential_drop_across_the_separator *= charge_current / cross_sectional_area;
  dimensionless_current_density *= charge_current / cross_sectional_area;

  std::cout << "delta=" << dimensionless_current_density << "\n";
  std::cout << "nu2  =" << dimensionless_exchange_current_density << "\n";
  std::cout << "beta ="
            << ratio_of_solution_phase_to_matrix_phase_conductivities << "\n";
  std::cout << "time step = " << time_step << std::endl;

  double computed_voltage;
  double exact_voltage;
  double const percent_tolerance = database->get<double>("percent_tolerance");
  for (double time = 0.0; time <= charge_time + epsilon; time += time_step)
  {
    double const dimensionless_time =
        (time + time_step) / time_normalization_factor;
    double const dimensionless_potential_drop_across_the_electrode =
        compute_dimensionless_potential_drop_across_the_electrode(
            dimensionless_time);
    exact_voltage = 2.0 * dimensionless_potential_drop_across_the_electrode *
                        voltage_normalization_factor +
                    potential_drop_across_the_separator;
    dev->evolve_one_time_step_constant_current(time_step, charge_current);
    dev->get_voltage(computed_voltage);
    if ((std::abs(time + time_step - 1e-3) < 1e-7) ||
        (std::abs(time + time_step - 2e-3) < 1e-7) ||
        (std::abs(time + time_step - 3e-3) < 1e-7) ||
        (std::abs(time + time_step - 4e-3) < 1e-7) ||
        (std::abs(time + time_step - 5e-3) < 1e-7) ||
        (std::abs(time + time_step - 6e-3) < 1e-7) ||
        (std::abs(time + time_step - 7e-3) < 1e-7) ||
        (std::abs(time + time_step - 8e-3) < 1e-7) ||
        (std::abs(time + time_step - 9e-3) < 1e-7) ||
        (std::abs(time + time_step - 10e-3) < 1e-7))
      os << boost::format("  %22.15e  %22.15e  %22.15e  \n") %
                (time + time_step) % exact_voltage % computed_voltage;
  }
}

} // end namespace cap

BOOST_AUTO_TEST_CASE(test_exact_transient_solution)
{
  // parse input file
  std::shared_ptr<boost::property_tree::ptree> input_database =
      std::make_shared<boost::property_tree::ptree>();
  boost::property_tree::info_parser::read_info("verification_problems.info",
                                               *input_database);

  // build an energy storage system
  std::shared_ptr<boost::property_tree::ptree> device_database =
      std::make_shared<boost::property_tree::ptree>(
          input_database->get_child("device"));
  std::shared_ptr<cap::EnergyStorageDevice> device =
      cap::EnergyStorageDevice::build(boost::mpi::communicator(),
                                      *device_database);

  // measure discharge curve
  std::fstream fout;
  fout.open("verification_problem_data", std::fstream::out);

  std::shared_ptr<boost::property_tree::ptree> verification_problem_database =
      std::make_shared<boost::property_tree::ptree>(
          input_database->get_child("verification_problem_subramanian"));

  cap::compute_parameters(device_database, verification_problem_database);

  cap::verification_problem(device, verification_problem_database, fout);

  fout.close();
}