#include <ctime>
#include <mutex>
#include <atomic>
#include <memory>
#include <iomanip>
#include <iostream>
#include <unordered_map>
#include <boost/thread.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <Eigen/Dense>
#include <pcl/io/pcd_io.h>

#include <ros/ros.h>
#include <geodesy/utm.h>
#include <geodesy/wgs84.h>
#include <pcl_ros/point_cloud.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_listener.h>

#include <std_msgs/Time.h>
#include <nav_msgs/Odometry.h>
#include <nmea_msgs/Sentence.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/PointCloud2.h>
#include <geographic_msgs/GeoPointStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <hdl_graph_slam/FloorCoeffs.h>

#include <hdl_graph_slam/SaveMap.h>
#include <hdl_graph_slam/DumpGraph.h>

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

#include <hdl_graph_slam/ros_utils.hpp>
#include <hdl_graph_slam/ros_time_hash.hpp>

#include <hdl_graph_slam/graph_slam.hpp>
#include <hdl_graph_slam/keyframe.hpp>
#include <hdl_graph_slam/keyframe_updater.hpp>
#include <hdl_graph_slam/loop_detector.hpp>
#include <hdl_graph_slam/information_matrix_calculator.hpp>
#include <hdl_graph_slam/map_cloud_generator.hpp>
#include <hdl_graph_slam/nmea_sentence_parser.hpp>

#include <g2o/types/slam3d/edge_se3.h>
#include <g2o/types/slam3d/vertex_se3.h>
#include <g2o/edge_se3_plane.hpp>
#include <g2o/edge_se3_priorxy.hpp>
#include <g2o/edge_se3_priorxyz.hpp>
#include <g2o/edge_se3_priorvec.hpp>
#include <g2o/edge_se3_priorquat.hpp>

//by li
#include <geometry_msgs/PoseStamped.h>

/**
#include "ctime"
#include "mutex"
#include "atomic"
#include <memory>
#include <iomanip>
#include <iostream>
#include <unordered_map>
#include <boost/thread.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <Eigen/Dense>
#include <pcl/io/pcd_io.h>

#include <ros/ros.h>
#include <geodesy/utm.h>
#include <geodesy/wgs84.h>
#include <pcl_ros/point_cloud.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_listener.h>

#include <std_msgs/Time.h>
#include <nav_msgs/Odometry.h>
#include <nmea_msgs/Sentence.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/PointCloud2.h>
#include <geographic_msgs/GeoPointStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <hdl_graph_slam/FloorCoeffs.h>

#include <hdl_graph_slam/SaveMap.h>
#include <hdl_graph_slam/DumpGraph.h>

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

#include <hdl_graph_slam/ros_utils.hpp>
#include <hdl_graph_slam/ros_time_hash.hpp>

#include <hdl_graph_slam/graph_slam.hpp>
#include <hdl_graph_slam/keyframe.hpp>
#include <hdl_graph_slam/keyframe_updater.hpp>
#include <hdl_graph_slam/loop_detector.hpp>
#include <hdl_graph_slam/information_matrix_calculator.hpp>
#include <hdl_graph_slam/map_cloud_generator.hpp>
#include <hdl_graph_slam/nmea_sentence_parser.hpp>

#include <g2o/types/slam3d/edge_se3.h>
#include <g2o/types/slam3d/vertex_se3.h>
#include <g2o/edge_se3_plane.hpp>
#include <g2o/edge_se3_priorxy.hpp>
#include <g2o/edge_se3_priorxyz.hpp>
#include <g2o/edge_se3_priorvec.hpp>
#include <g2o/edge_se3_priorquat.hpp>

//by li
#include <geometry_msgs/PoseStamped.h>
*/

using namespace std;

namespace hdl_graph_slam {

class HdlGraphSlamNodelet : public nodelet::Nodelet {
public:
  typedef pcl::PointXYZI PointT;
  typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry, sensor_msgs::PointCloud2> ApproxSyncPolicy;

  HdlGraphSlamNodelet() {}
  virtual ~HdlGraphSlamNodelet() {}

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

    // init parameters
    map_frame_id = private_nh.param<std::string>("map_frame_id", "map");
    odom_frame_id = private_nh.param<std::string>("odom_frame_id", "odom");
    map_cloud_resolution = private_nh.param<double>("map_cloud_resolution", 0.05);
    trans_odom2map.setIdentity();

    max_keyframes_per_update = private_nh.param<int>("max_keyframes_per_update", 10);

    //
    anchor_node = nullptr;
    anchor_edge = nullptr;
    floor_plane_node = nullptr;
    graph_slam.reset(new GraphSLAM(private_nh.param<std::string>("g2o_solver_type", "lm_var")));
    keyframe_updater.reset(new KeyframeUpdater(private_nh));
    loop_detector.reset(new LoopDetector(private_nh));
    map_cloud_generator.reset(new MapCloudGenerator());
    inf_calclator.reset(new InformationMatrixCalculator(private_nh));
    nmea_parser.reset(new NmeaSentenceParser());

    //by li 98-105
    pose_time_offset = private_nh.param<double>("pose_time_offset", 0.0);
    pose_edge_stddev_xy = private_nh.param<double>("pose_edge_stddev_xy", 10.0);
    pose_edge_stddev_z = private_nh.param<double>("pose_edge_stddev_z", 10.0);

    enable_pose_orientation = private_nh.param<bool>("enable_pose_orientation", false);
    pose_orientation_edge_stddev = private_nh.param<double>("pose_orientation_edge_stddev", 0.1);
    //

    gps_time_offset = private_nh.param<double>("gps_time_offset", 0.0);
    gps_edge_stddev_xy = private_nh.param<double>("gps_edge_stddev_xy", 10000.0);
    gps_edge_stddev_z = private_nh.param<double>("gps_edge_stddev_z", 10.0);
    floor_edge_stddev = private_nh.param<double>("floor_edge_stddev", 10.0);

    imu_time_offset = private_nh.param<double>("imu_time_offset", 0.0);
    enable_imu_orientation = private_nh.param<bool>("enable_imu_orientation", false);
    enable_imu_acceleration = private_nh.param<bool>("enable_imu_acceleration", false);
    imu_orientation_edge_stddev = private_nh.param<double>("imu_orientation_edge_stddev", 0.1);
    imu_acceleration_edge_stddev = private_nh.param<double>("imu_acceleration_edge_stddev", 3.0);

    points_topic = private_nh.param<std::string>("points_topic", "/velodyne_points");

    // subscribers
    odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(mt_nh, "/odom", 256));
    cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(mt_nh, "/filtered_points", 32));
    sync.reset(new message_filters::Synchronizer<ApproxSyncPolicy>(ApproxSyncPolicy(32), *odom_sub, *cloud_sub));
    sync->registerCallback(boost::bind(&HdlGraphSlamNodelet::cloud_callback, this, _1, _2));
    imu_sub = nh.subscribe("/gpsimu_driver/imu_data", 1024, &HdlGraphSlamNodelet::imu_callback, this);
    floor_sub = nh.subscribe("/floor_detection/floor_coeffs", 1024, &HdlGraphSlamNodelet::floor_coeffs_callback, this);

    //by li 128-134

    if(private_nh.param<bool>("enable_pose", true)) {
      pose_sub = nh.subscribe("/pose_data/pose", 1024, &HdlGraphSlamNodelet::pose_callback, this);
      posepos_sub = nh.subscribe("/pose_data/posepos", 1024, &HdlGraphSlamNodelet::posepos_callback, this);
      poseori_sub = nh.subscribe("/pose_data/poseori", 1024, &HdlGraphSlamNodelet::poseori_callback, this);
      //poseori_sub = nh.subscribe("/pose_data/pose_ori", 1024, &HdlGraphSlamNodelet::poseori_callback, this);
    }
    //

    if(private_nh.param<bool>("enable_gps", true)) {
      gps_sub = mt_nh.subscribe("/gps/geopoint", 1024, &HdlGraphSlamNodelet::gps_callback, this);
      nmea_sub = mt_nh.subscribe("/gpsimu_driver/nmea_sentence", 1024, &HdlGraphSlamNodelet::nmea_callback, this);
      navsat_sub = mt_nh.subscribe("/gps/navsat", 1024, &HdlGraphSlamNodelet::navsat_callback, this);
    }

    // publishers
    markers_pub = mt_nh.advertise<visualization_msgs::MarkerArray>("/hdl_graph_slam/markers", 16);
    odom2map_pub = mt_nh.advertise<geometry_msgs::TransformStamped>("/hdl_graph_slam/odom2pub", 16);
    map_points_pub = mt_nh.advertise<sensor_msgs::PointCloud2>("/hdl_graph_slam/map_points", 1, true);
    read_until_pub = mt_nh.advertise<std_msgs::Header>("/hdl_graph_slam/read_until", 32);

    dump_service_server = mt_nh.advertiseService("/hdl_graph_slam/dump", &HdlGraphSlamNodelet::dump_service, this);
    save_map_service_server = mt_nh.advertiseService("/hdl_graph_slam/save_map", &HdlGraphSlamNodelet::save_map_service, this);

    graph_updated = false;
    double graph_update_interval = private_nh.param<double>("graph_update_interval", 3.0);
    double map_cloud_update_interval = private_nh.param<double>("map_cloud_update_interval", 10.0);
    optimization_timer = mt_nh.createWallTimer(ros::WallDuration(graph_update_interval), &HdlGraphSlamNodelet::optimization_timer_callback, this);
    map_publish_timer = mt_nh.createWallTimer(ros::WallDuration(map_cloud_update_interval), &HdlGraphSlamNodelet::map_points_publish_timer_callback, this);
  }

private:
  /**
   * @brief received point clouds are pushed to #keyframe_queue
   * @param odom_msg
   * @param cloud_msg
   */
  void cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
    const ros::Time& stamp = cloud_msg->header.stamp;
    Eigen::Isometry3d odom = odom2isometry(odom_msg);

    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*cloud_msg, *cloud);
    if(base_frame_id.empty()) {
      base_frame_id = cloud_msg->header.frame_id;
    }

    if(!keyframe_updater->update(odom)) {
      std::lock_guard<std::mutex> lock(keyframe_queue_mutex);
      if(keyframe_queue.empty()) {
        std_msgs::Header read_until;
        read_until.stamp = stamp + ros::Duration(10, 0);
        read_until.frame_id = points_topic;
        read_until_pub.publish(read_until);
        read_until.frame_id = "/filtered_points";
        read_until_pub.publish(read_until);
      }

      return;
    }

    double accum_d = keyframe_updater->get_accum_distance();
    KeyFrame::Ptr keyframe(new KeyFrame(stamp, odom, accum_d, cloud));

    std::lock_guard<std::mutex> lock(keyframe_queue_mutex);
    keyframe_queue.push_back(keyframe);
  }

  /**
   * @brief this method adds all the keyframes in #keyframe_queue to the pose graph (odometry edges)
   * @return if true, at least one keyframe was added to the pose graph
   */
  bool flush_keyframe_queue() {
    std::lock_guard<std::mutex> lock(keyframe_queue_mutex);

    if(keyframe_queue.empty()) {
      return false;
    }

    trans_odom2map_mutex.lock();
    Eigen::Isometry3d odom2map(trans_odom2map.cast<double>());
    trans_odom2map_mutex.unlock();

    int num_processed = 0;
    for(int i = 0; i < std::min<int>(keyframe_queue.size(), max_keyframes_per_update); i++) {
      num_processed = i;

      const auto& keyframe = keyframe_queue[i];
      // new_keyframes will be tested later for loop closure
      new_keyframes.push_back(keyframe);

      // add pose node
      Eigen::Isometry3d odom = odom2map * keyframe->odom;
      keyframe->node = graph_slam->add_se3_node(odom);
      keyframe_hash[keyframe->stamp] = keyframe;

      // fix the first node
      if(keyframes.empty() && new_keyframes.size() == 1) {
        if(private_nh.param<bool>("fix_first_node", false)) {
          Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
          std::stringstream sst(private_nh.param<std::string>("fix_first_node_stddev", "1 1 1 1 1 1"));
          for(int i = 0; i < 6; i++) {
            double stddev = 1.0;
            sst >> stddev;
            inf(i, i) = 1.0 / stddev;
          }

          anchor_node = graph_slam->add_se3_node(Eigen::Isometry3d::Identity());
          anchor_node->setFixed(true);
          anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, Eigen::Isometry3d::Identity(), inf);
        }
      }

      if(i == 0 && keyframes.empty()) {
        continue;
      }

      // add edge between consecutive keyframes
      const auto& prev_keyframe = i == 0 ? keyframes.back() : keyframe_queue[i - 1];

      Eigen::Isometry3d relative_pose = keyframe->odom.inverse() * prev_keyframe->odom;
      Eigen::MatrixXd information = inf_calclator->calc_information_matrix(keyframe->cloud, prev_keyframe->cloud, relative_pose);
      auto edge = graph_slam->add_se3_edge(keyframe->node, prev_keyframe->node, relative_pose, information);
      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("odometry_edge_robust_kernel", "NONE"), private_nh.param<double>("odometry_edge_robust_kernel_size", 1.0));
    }

    std_msgs::Header read_until;
    read_until.stamp = keyframe_queue[num_processed]->stamp + ros::Duration(10, 0);
    read_until.frame_id = points_topic;
    read_until_pub.publish(read_until);
    read_until.frame_id = "/filtered_points";
    read_until_pub.publish(read_until);

    keyframe_queue.erase(keyframe_queue.begin(), keyframe_queue.begin() + num_processed + 1);
    return true;
  }
  //by li/////////////// 263-396
  /**
  void posepos_callback(const geometry_msgs::PointStampedConstPtr& posepos1_msg) {

      geometry_msgs::PoseStampedConstPtr posepos_msg(new geometry_msgs::PoseStamped());

      std::lock_guard<std::mutex> lock(posepos_queue_mutex);
      posepos_msg->header.stamp += ros::Duration(pose_time_offset);
      //posepos_msg->header = posepos1_msg->header.stamp;
      posepos_msg->pose.position.x = posepos1_msg->pose.position.x;//posepos_msg->position.z
      posepos_msg->pose.position.y = posepos1_msg->pose.position.y;
      posepos_msg->pose.position.z = posepos1_msg->pose.position.z;
      posepos_queue.push_back(posepos_msg);
    }
  void poseori_callback(const geometry_msgs::PoseStampedConstPtr& poseori1_msg) {
      geometry_msgs::PoseStampedConstPtr poseori_msg(new geometry_msgs::PoseStamped());
      //poseori_msg->header = poseori1_msg->header;
      std::lock_guard<std::mutex> lock(poseori_queue_mutex);
      poseori_msg->header = poseori1_msg->header.stamp;
      poseori_msg->pose.orientation.x = poseori1_msg->pose.orientation.x;//poseori_msg->orientation.w = poseori1_msg->pose.pose.orientation.w;
      poseori_msg->pose.orientation.y = poseori1_msg->pose.orientation.y;
      poseori_msg->pose.orientation.z = poseori1_msg->pose.orientation.z;
      poseori_msg->pose.orientation.w = poseori1_msg->pose.orientation.w;
      //pose_callback(pose_msg);
      poseori_queue.push_back(poseori_msg);
  }*/

  void pose_callback(const geometry_msgs::PoseStampedConstPtr& pose_msg){
  std::lock_guard<std::mutex> lock(pose_queue_mutex);
  pose_msg->header.stamp += ros::Duration(pose_time_offset);
  pose_queue.push_back(pose_msg);
  }
  /**void poseori_callback(const geometry_msgs::QuaternionStampedConstPtr& poseori_msg){
  std::lock_guard<std::mutex> lock(poseori_queue_mutex);
  poseori_msg->header.stamp += ros::Duration(pose_time_offset);
  poseori_queue.push_back(poseori_msg);
  }*/
      // //////
  bool flush_posepos_queue() {
    std::lock_guard<std::mutex> lock(pose_queue_mutex);

    /**
    if(keyframes.empty() || posepos_queue.empty()) {
      return false;
    }

    bool updated = false;
    auto posepos_cursor = posepos_queue.begin();

    for(auto& keyframe : keyframes) {
      if(keyframe->stamp > posepos_queue.back()->header.stamp) {
        break;
      }

      if(keyframe->stamp < (*posepos_cursor)->header.stamp ) {//|| keyframe->utm_coord || keyframe->acceleration) {
        continue;
      }

      // find the pose data which is closest to the keyframe
      auto closest_posepos = posepos_cursor;
      for(auto posepos = posepos_cursor; posepos != posepos_queue.end(); posepos++) {
        auto dt = ((*closest_posepos)->header.stamp - keyframe->stamp).toSec();
        auto dt2 = ((*posepos)->header.stamp - keyframe->stamp).toSec();
        if(std::abs(dt) < std::abs(dt2)) {
          break;
        }

        closest_posepos = posepos;
      }

      // if the time residual between the pose and keyframe is too large, skip it
      posepos_cursor = closest_posepos;
      if(0.2 < std::abs(((*closest_posepos)->header.stamp - keyframe->stamp).toSec())) {
        continue;
      }
      */

      //const auto& pose_pos = (*closest_pose)->pose.pose.position;
      //const auto& pose_ori = (*closest_pose)->pose.pose.orientation;
      /** // convert (latitude, longitude, altitude) -> (easting, northing, altitude) in UTM coordinate
      geodesy::UTMPoint utm;
      geodesy::fromMsg((*closest_gps)->position, utm);
      */
    // pose ///
    if(keyframes.empty() || pose_queue.empty()) {
      return false;
    }

    bool updated = false;
    auto pose_cursor = pose_queue.begin();

    for(auto& keyframe : keyframes) {
      if(keyframe->stamp > pose_queue.back()->header.stamp) {
        break;
      }

      if(keyframe->stamp < (*pose_cursor)->header.stamp ) {//|| keyframe->utm_coord || keyframe->acceleration) {
        continue;
      }

      // find the pose data which is closest to the keyframe
      auto closest_pose = pose_cursor;
      for(auto pose = pose_cursor; pose != pose_queue.end(); pose++) {
        auto dt = ((*closest_pose)->header.stamp - keyframe->stamp).toSec();
        auto dt2 = ((*pose)->header.stamp - keyframe->stamp).toSec();
        if(std::abs(dt) < std::abs(dt2)) {
          break;
        }

        closest_pose = pose;
      }

      // if the time residual between the pose and keyframe is too large, skip it
      pose_cursor = closest_pose;
      if(0.2 < std::abs(((*closest_pose)->header.stamp - keyframe->stamp).toSec())) {
        continue;
      }

      // pos ///
      //Eigen::Vector3d xyz(pose_msg->pose.position.x, (*closest_pose)->pose.position.y, (*closest_pose)->pose.position.z);
      //Eigen::Vector3d xyz(posepos_cursor.pose.position.x, posepos_cursor.pose.position.y, posepos_cursor.pose.position.z);
      Eigen::Vector3d xyz(pose_cursor.pose.position.x, pose_cursor.pose.position.y, pose_cursor.pose.position.z);

      // the first pose data position will be the origin of the map
      if(!zero_pos) {
        zero_pos = xyz;
      }
      xyz -= (*zero_pos); //keyframe和gps位姿直接相减得到先验约束加入到graph中

      keyframe->utm_coord = xyz;

      //ori//
      geometry_msgs::QuaternionStamped ori_pose; //quat_imu
      geometry_msgs::QuaternionStamped ori_base;

      ori_pose.header.frame_id = (*closest_pose)->header.frame_id;
      ori_pose.header.stamp = ros::Time(0);//ros::Time(0)表示使用缓冲中最新的tf数据
      //acc_imu.vector = (*closest_imu)->linear_acceleration;
      ori_pose.quaternion = (*closest_pose)->pose.orientation;

      try {
          tf_listener.transformQuaternion(base_frame_id, ori_pose, ori_base);
          } catch(std::exception& e) {
           std::cerr << "failed to find transform!!" << std::endl;
           return false;
          }

      // g2o pos ///
      g2o::OptimizableGraph::Edge* edge;
      if(std::isnan(xyz.z())) { //判断gps z轴是否有值，没有就只进行xy约束
        Eigen::Matrix2d information_matrix = Eigen::Matrix2d::Identity() / pose_edge_stddev_xy;
        edge = graph_slam->add_se3_prior_xy_edge(keyframe->node, xyz.head<2>(), information_matrix);
      } else {
        Eigen::Matrix3d information_matrix = Eigen::Matrix3d::Identity();
        information_matrix.block<2, 2>(0, 0) /= pose_edge_stddev_xy;
        information_matrix(2, 2) /= pose_edge_stddev_z;
        edge = graph_slam->add_se3_prior_xyz_edge(keyframe->node, xyz, information_matrix);
      }
      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("pose_edge_robust_kernel", "NONE"), private_nh.param<double>("pose_edge_robust_kernel_size", 1.0));

      // g2o ori ///
      keyframe->orientation = Eigen::Quaterniond(ori_base.quaternion.w, ori_base.quaternion.x, ori_base.quaternion.y, ori_base.quaternion.z);
      keyframe->orientation = keyframe->orientation;
      if(keyframe->orientation->w() < 0.0) {
        keyframe->orientation->coeffs() = -keyframe->orientation->coeffs();
      }

      if(enable_pose_orientation) { //if(x) x=true or false
        Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / pose_orientation_edge_stddev;
        auto edge = graph_slam->add_se3_prior_quat_edge(keyframe->node, *keyframe->orientation, info);
        graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("pose_orientation_edge_robust_kernel", "NONE"), private_nh.param<double>("pose_orientation_edge_robust_kernel_size", 1.0));
      }

      updated = true;
    }

    //auto remove_loc = std::upper_bound(posepos_queue.begin(), posepos_queue.end(), keyframes.back()->stamp, [=](const ros::Time& stamp, const geometry_msgs::PoseStampedConstPtr& posepos) { return stamp < posepos->header.stamp; });
    //posepos_queue.erase(posepos_queue.begin(), remove_loc);
    auto remove_loc = std::upper_bound(pose_queue.begin(), pose_queue.end(), keyframes.back()->stamp, [=](const ros::Time& stamp, const geometry_msgs::PoseStampedConstPtr& posepos) { return stamp < posepos->header.stamp; });
    pose_queue.erase(pose_queue.begin(), remove_loc);
    return updated;
  }

  // ori ///
  /**
  bool flush_poseori_queue() {
    std::lock_guard<std::mutex> lock(poseori_queue_mutex);

    if(keyframes.empty() || poseori_queue.empty()) {
      return false;
    }

    bool updated = false;
    auto poseori_cursor = poseori_queue.begin();

    for(auto& keyframe : keyframes) {
      if(keyframe->stamp > poseori_queue.back()->header.stamp) {
        break;
      }

      if(keyframe->stamp < (*poseori_cursor)->header.stamp ) {//|| keyframe->utm_coord || keyframe->acceleration) {
        continue;
      }

      // find the pose data which is closest to the keyframe
      auto closest_poseori = poseori_cursor;
      for(auto poseori = poseori_cursor; poseori != poseori_queue.end(); poseori++) {
        auto dt = ((*closest_poseori)->header.stamp - keyframe->stamp).toSec();
        auto dt2 = ((*poseori)->header.stamp - keyframe->stamp).toSec();
        if(std::abs(dt) < std::abs(dt2)) {
          break;
        }

        closest_poseori = poseori;
      }

      // if the time residual between the pose and keyframe is too large, skip it
      poseori_cursor = closest_poseori;
      if(0.2 < std::abs(((*closest_poseori)->header.stamp - keyframe->stamp).toSec())) {
        continue;
      }
     //geometry_msgs::Vector3Stamped pos_pose;
     //geometry_msgs::Vector3Stamped pos_base;
     geometry_msgs::QuaternionStamped ori_pose; //quat_imu
     geometry_msgs::QuaternionStamped ori_base;

     ori_pose.header.frame_id = (*closest_poseori)->header.frame_id;
     ori_pose.header.stamp = ros::Time(0);//ros::Time(0)表示使用缓冲中最新的tf数据
     //acc_imu.vector = (*closest_imu)->linear_acceleration;
     ori_pose.quaternion = (*closest_poseori)->pose.orientation;

     try {
         tf_listener.transformQuaternion(base_frame_id, ori_pose, ori_base);
         } catch(std::exception& e) {
          std::cerr << "failed to find transform!!" << std::endl;
          return false;
         }

  // g2o ori ///
  keyframe->orientation = Eigen::Quaterniond(ori_base.quaternion.w, ori_base.quaternion.x, ori_base.quaternion.y, ori_base.quaternion.z);
  keyframe->orientation = keyframe->orientation;
  if(keyframe->orientation->w() < 0.0) {
    keyframe->orientation->coeffs() = -keyframe->orientation->coeffs();
  }

  if(enable_pose_orientation) { //if(x) x=true or false
    Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / pose_orientation_edge_stddev;
    auto edge = graph_slam->add_se3_prior_quat_edge(keyframe->node, *keyframe->orientation, info);
    graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("pose_orientation_edge_robust_kernel", "NONE"), private_nh.param<double>("pose_orientation_edge_robust_kernel_size", 1.0));
  }
  updated = true;
  }
  auto remove_loc = std::upper_bound(poseori_queue.begin(), poseori_queue.end(), keyframes.back()->stamp, [=](const ros::Time& stamp, const geometry_msgs::PoseStampedConstPtr& poseori) { return stamp < poseori->header.stamp; });
  poseori_queue.erase(poseori_queue.begin(), remove_loc);

  return true;
  */
}

  // ////
  // //////////////////

  void nmea_callback(const nmea_msgs::SentenceConstPtr& nmea_msg) {
    GPRMC grmc = nmea_parser->parse(nmea_msg->sentence);

    if(grmc.status != 'A') {
      return;
    }

    geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped());
    gps_msg->header = nmea_msg->header;
    gps_msg->position.latitude = grmc.latitude;
    gps_msg->position.longitude = grmc.longitude;
    gps_msg->position.altitude = NAN;

    gps_callback(gps_msg);
  }

  void navsat_callback(const sensor_msgs::NavSatFixConstPtr& navsat_msg) {
    geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped());
    gps_msg->header = navsat_msg->header;
    gps_msg->position.latitude = navsat_msg->latitude;
    gps_msg->position.longitude = navsat_msg->longitude;
    gps_msg->position.altitude = navsat_msg->altitude;
    gps_callback(gps_msg);
  }

  /**
   * @brief received gps data is added to #gps_queue
   * @param gps_msg
   */
  void gps_callback(const geographic_msgs::GeoPointStampedPtr& gps_msg) {
    std::lock_guard<std::mutex> lock(gps_queue_mutex);
    gps_msg->header.stamp += ros::Duration(gps_time_offset);
    gps_queue.push_back(gps_msg);
  }

  /**
   * @brief
   * @return
   */
  bool flush_gps_queue() {
    std::lock_guard<std::mutex> lock(gps_queue_mutex);

    if(keyframes.empty() || gps_queue.empty()) {
      return false;
    }

    bool updated = false;
    auto gps_cursor = gps_queue.begin();

    for(auto& keyframe : keyframes) {
      if(keyframe->stamp > gps_queue.back()->header.stamp) {
        break;
      }

      if(keyframe->stamp < (*gps_cursor)->header.stamp || keyframe->utm_coord) {
        continue;
      }

      // find the gps data which is closest to the keyframe
      auto closest_gps = gps_cursor;
      for(auto gps = gps_cursor; gps != gps_queue.end(); gps++) {
        auto dt = ((*closest_gps)->header.stamp - keyframe->stamp).toSec();
        auto dt2 = ((*gps)->header.stamp - keyframe->stamp).toSec();
        if(std::abs(dt) < std::abs(dt2)) {
          break;
        }

        closest_gps = gps;
      }

      // if the time residual between the gps and keyframe is too large, skip it
      gps_cursor = closest_gps;
      if(0.2 < std::abs(((*closest_gps)->header.stamp - keyframe->stamp).toSec())) {
        continue;
      }

      // convert (latitude, longitude, altitude) -> (easting, northing, altitude) in UTM coordinate
      geodesy::UTMPoint utm;
      geodesy::fromMsg((*closest_gps)->position, utm);
      Eigen::Vector3d xyz(utm.easting, utm.northing, utm.altitude);

      // the first gps data position will be the origin of the map
      if(!zero_utm) {
        zero_utm = xyz;
      }
      xyz -= (*zero_utm);

      keyframe->utm_coord = xyz;

      g2o::OptimizableGraph::Edge* edge;
      if(std::isnan(xyz.z())) {
        Eigen::Matrix2d information_matrix = Eigen::Matrix2d::Identity() / gps_edge_stddev_xy;
        edge = graph_slam->add_se3_prior_xy_edge(keyframe->node, xyz.head<2>(), information_matrix);
      } else {
        Eigen::Matrix3d information_matrix = Eigen::Matrix3d::Identity();
        information_matrix.block<2, 2>(0, 0) /= gps_edge_stddev_xy;
        information_matrix(2, 2) /= gps_edge_stddev_z;
        edge = graph_slam->add_se3_prior_xyz_edge(keyframe->node, xyz, information_matrix);
      }
      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("gps_edge_robust_kernel", "NONE"), private_nh.param<double>("gps_edge_robust_kernel_size", 1.0));

      updated = true;
    }

    auto remove_loc = std::upper_bound(gps_queue.begin(), gps_queue.end(), keyframes.back()->stamp, [=](const ros::Time& stamp, const geographic_msgs::GeoPointStampedConstPtr& geopoint) { return stamp < geopoint->header.stamp; });
    gps_queue.erase(gps_queue.begin(), remove_loc);
    return updated;
  }

  void imu_callback(const sensor_msgs::ImuPtr& imu_msg) {
    if(!enable_imu_orientation && !enable_imu_acceleration) {
      return;
    }

    std::lock_guard<std::mutex> lock(imu_queue_mutex);
    imu_msg->header.stamp += ros::Duration(imu_time_offset);
    imu_queue.push_back(imu_msg);
  }

  bool flush_imu_queue() {
    std::lock_guard<std::mutex> lock(imu_queue_mutex);
    if(keyframes.empty() || imu_queue.empty() || base_frame_id.empty()) {
      return false;
    }

    bool updated = false;
    auto imu_cursor = imu_queue.begin();

    for(auto& keyframe : keyframes) {
      if(keyframe->stamp > imu_queue.back()->header.stamp) {
        break;
      }

      if(keyframe->stamp < (*imu_cursor)->header.stamp || keyframe->acceleration) {
        continue;
      }

      // find imu data which is closest to the keyframe
      auto closest_imu = imu_cursor;
      for(auto imu = imu_cursor; imu != imu_queue.end(); imu++) {
        auto dt = ((*closest_imu)->header.stamp - keyframe->stamp).toSec();
        auto dt2 = ((*imu)->header.stamp - keyframe->stamp).toSec();
        if(std::abs(dt) < std::abs(dt2)) {
          break;
        }

        closest_imu = imu;
      }

      imu_cursor = closest_imu;
      if(0.2 < std::abs(((*closest_imu)->header.stamp - keyframe->stamp).toSec())) {
        continue;
      }

      const auto& imu_ori = (*closest_imu)->orientation;
      const auto& imu_acc = (*closest_imu)->linear_acceleration;

      geometry_msgs::Vector3Stamped acc_imu;
      geometry_msgs::Vector3Stamped acc_base;
      geometry_msgs::QuaternionStamped quat_imu;
      geometry_msgs::QuaternionStamped quat_base;

      quat_imu.header.frame_id = acc_imu.header.frame_id = (*closest_imu)->header.frame_id;
      quat_imu.header.stamp = acc_imu.header.stamp = ros::Time(0);
      acc_imu.vector = (*closest_imu)->linear_acceleration;
      quat_imu.quaternion = (*closest_imu)->orientation;

      try {
        tf_listener.transformVector(base_frame_id, acc_imu, acc_base);
        tf_listener.transformQuaternion(base_frame_id, quat_imu, quat_base);
      } catch(std::exception& e) {
        std::cerr << "failed to find transform!!" << std::endl;
        return false;
      }

      keyframe->acceleration = Eigen::Vector3d(acc_base.vector.x, acc_base.vector.y, acc_base.vector.z);
      keyframe->orientation = Eigen::Quaterniond(quat_base.quaternion.w, quat_base.quaternion.x, quat_base.quaternion.y, quat_base.quaternion.z);
      keyframe->orientation = keyframe->orientation;
      if(keyframe->orientation->w() < 0.0) {
        keyframe->orientation->coeffs() = -keyframe->orientation->coeffs();
      }

      if(enable_imu_orientation) {
        Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / imu_orientation_edge_stddev;
        auto edge = graph_slam->add_se3_prior_quat_edge(keyframe->node, *keyframe->orientation, info);
        graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("imu_orientation_edge_robust_kernel", "NONE"), private_nh.param<double>("imu_orientation_edge_robust_kernel_size", 1.0));
      }

      if(enable_imu_acceleration) {
        Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / imu_acceleration_edge_stddev;
        g2o::OptimizableGraph::Edge* edge = graph_slam->add_se3_prior_vec_edge(keyframe->node, -Eigen::Vector3d::UnitZ(), *keyframe->acceleration, info);
        graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("imu_acceleration_edge_robust_kernel", "NONE"), private_nh.param<double>("imu_acceleration_edge_robust_kernel_size", 1.0));
      }
      updated = true;
    }

    auto remove_loc = std::upper_bound(imu_queue.begin(), imu_queue.end(), keyframes.back()->stamp, [=](const ros::Time& stamp, const sensor_msgs::ImuConstPtr& imu) { return stamp < imu->header.stamp; });
    imu_queue.erase(imu_queue.begin(), remove_loc);

    return true;
  }

  /**
   * @brief received floor coefficients are added to #floor_coeffs_queue
   * @param floor_coeffs_msg
   */
  void floor_coeffs_callback(const hdl_graph_slam::FloorCoeffsConstPtr& floor_coeffs_msg) {
    if(floor_coeffs_msg->coeffs.empty()) {
      return;
    }

    std::lock_guard<std::mutex> lock(floor_coeffs_queue_mutex);
    floor_coeffs_queue.push_back(floor_coeffs_msg);
  }

  /**
   * @brief this methods associates floor coefficients messages with registered keyframes, and then adds the associated coeffs to the pose graph
   * @return if true, at least one floor plane edge is added to the pose graph
   */
  bool flush_floor_queue() {
    std::lock_guard<std::mutex> lock(floor_coeffs_queue_mutex);

    if(keyframes.empty()) {
      return false;
    }

    const auto& latest_keyframe_stamp = keyframes.back()->stamp;

    bool updated = false;
    for(const auto& floor_coeffs : floor_coeffs_queue) {
      if(floor_coeffs->header.stamp > latest_keyframe_stamp) {
        break;
      }

      auto found = keyframe_hash.find(floor_coeffs->header.stamp);
      if(found == keyframe_hash.end()) {
        continue;
      }

      if(!floor_plane_node) {
        floor_plane_node = graph_slam->add_plane_node(Eigen::Vector4d(0.0, 0.0, 1.0, 0.0));
        floor_plane_node->setFixed(true);
      }

      const auto& keyframe = found->second;

      Eigen::Vector4d coeffs(floor_coeffs->coeffs[0], floor_coeffs->coeffs[1], floor_coeffs->coeffs[2], floor_coeffs->coeffs[3]);
      Eigen::Matrix3d information = Eigen::Matrix3d::Identity() * (1.0 / floor_edge_stddev);
      auto edge = graph_slam->add_se3_plane_edge(keyframe->node, floor_plane_node, coeffs, information);
      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("floor_edge_robust_kernel", "NONE"), private_nh.param<double>("floor_edge_robust_kernel_size", 1.0));

      keyframe->floor_coeffs = coeffs;

      updated = true;
    }

    auto remove_loc = std::upper_bound(floor_coeffs_queue.begin(), floor_coeffs_queue.end(), latest_keyframe_stamp, [=](const ros::Time& stamp, const hdl_graph_slam::FloorCoeffsConstPtr& coeffs) { return stamp < coeffs->header.stamp; });
    floor_coeffs_queue.erase(floor_coeffs_queue.begin(), remove_loc);

    return updated;
  }

  /**
   * @brief generate map point cloud and publish it
   * @param event
   */
  void map_points_publish_timer_callback(const ros::WallTimerEvent& event) {
    if(!map_points_pub.getNumSubscribers() || !graph_updated) {
      return;
    }

    std::vector<KeyFrameSnapshot::Ptr> snapshot;

    keyframes_snapshot_mutex.lock();
    snapshot = keyframes_snapshot;
    keyframes_snapshot_mutex.unlock();

    auto cloud = map_cloud_generator->generate(snapshot, 0.05);
    if(!cloud) {
      return;
    }

    cloud->header.frame_id = map_frame_id;
    cloud->header.stamp = snapshot.back()->cloud->header.stamp;

    sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
    pcl::toROSMsg(*cloud, *cloud_msg);

    map_points_pub.publish(cloud_msg);
  }

  /**
   * @brief this methods adds all the data in the queues to the pose graph, and then optimizes the pose graph
   * @param event
   */
  void optimization_timer_callback(const ros::WallTimerEvent& event) {
    std::lock_guard<std::mutex> lock(main_thread_mutex);

    // add keyframes and floor coeffs in the queues to the pose graph
    bool keyframe_updated = flush_keyframe_queue();

    if(!keyframe_updated) {
      std_msgs::Header read_until;
      read_until.stamp = ros::Time::now() + ros::Duration(30, 0);
      read_until.frame_id = points_topic;
      read_until_pub.publish(read_until);
      read_until.frame_id = "/filtered_points";
      read_until_pub.publish(read_until);
    }

    if(!keyframe_updated & !flush_floor_queue() & !flush_gps_queue() & !flush_imu_queue()) {
      return;
    }

    // loop detection
    std::vector<Loop::Ptr> loops = loop_detector->detect(keyframes, new_keyframes, *graph_slam);
    for(const auto& loop : loops) {
      Eigen::Isometry3d relpose(loop->relative_pose.cast<double>());
      Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose);
      auto edge = graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix);
      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("loop_closure_edge_robust_kernel", "NONE"), private_nh.param<double>("loop_closure_edge_robust_kernel_size", 1.0));
    }

    std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes));
    new_keyframes.clear();

    // move the first node anchor position to the current estimate of the first node pose
    // so the first node moves freely while trying to stay around the origin
    if(anchor_node && private_nh.param<bool>("fix_first_node_adaptive", true)) {
      Eigen::Isometry3d anchor_target = static_cast<g2o::VertexSE3*>(anchor_edge->vertices()[1])->estimate();
      anchor_node->setEstimate(anchor_target);
    }

    // optimize the pose graph
    int num_iterations = private_nh.param<int>("g2o_solver_num_iterations", 1024);
    graph_slam->optimize(num_iterations);

    // publish tf
    const auto& keyframe = keyframes.back();
    Eigen::Isometry3d trans = keyframe->node->estimate() * keyframe->odom.inverse();
    trans_odom2map_mutex.lock();
    trans_odom2map = trans.matrix().cast<float>();
    trans_odom2map_mutex.unlock();

    std::vector<KeyFrameSnapshot::Ptr> snapshot(keyframes.size());
    std::transform(keyframes.begin(), keyframes.end(), snapshot.begin(), [=](const KeyFrame::Ptr& k) { return std::make_shared<KeyFrameSnapshot>(k); });

    keyframes_snapshot_mutex.lock();
    keyframes_snapshot.swap(snapshot);
    keyframes_snapshot_mutex.unlock();
    graph_updated = true;

    if(odom2map_pub.getNumSubscribers()) {
      geometry_msgs::TransformStamped ts = matrix2transform(keyframe->stamp, trans.matrix().cast<float>(), map_frame_id, odom_frame_id);
      odom2map_pub.publish(ts);
    }

    if(markers_pub.getNumSubscribers()) {
      auto markers = create_marker_array(ros::Time::now());
      markers_pub.publish(markers);
    }
  }

  /**
   * @brief create visualization marker
   * @param stamp
   * @return
   */
  visualization_msgs::MarkerArray create_marker_array(const ros::Time& stamp) const {
    visualization_msgs::MarkerArray markers;
    markers.markers.resize(4);

    // node markers
    visualization_msgs::Marker& traj_marker = markers.markers[0];
    traj_marker.header.frame_id = "map";
    traj_marker.header.stamp = stamp;
    traj_marker.ns = "nodes";
    traj_marker.id = 0;
    traj_marker.type = visualization_msgs::Marker::SPHERE_LIST;

    traj_marker.pose.orientation.w = 1.0;
    traj_marker.scale.x = traj_marker.scale.y = traj_marker.scale.z = 0.5;

    visualization_msgs::Marker& imu_marker = markers.markers[1];
    imu_marker.header = traj_marker.header;
    imu_marker.ns = "imu";
    imu_marker.id = 1;
    imu_marker.type = visualization_msgs::Marker::SPHERE_LIST;

    imu_marker.pose.orientation.w = 1.0;
    imu_marker.scale.x = imu_marker.scale.y = imu_marker.scale.z = 0.75;

    traj_marker.points.resize(keyframes.size());
    traj_marker.colors.resize(keyframes.size());
    for(int i = 0; i < keyframes.size(); i++) {
      Eigen::Vector3d pos = keyframes[i]->node->estimate().translation();
      traj_marker.points[i].x = pos.x();
      traj_marker.points[i].y = pos.y();
      traj_marker.points[i].z = pos.z();

      double p = static_cast<double>(i) / keyframes.size();
      traj_marker.colors[i].r = 1.0 - p;
      traj_marker.colors[i].g = p;
      traj_marker.colors[i].b = 0.0;
      traj_marker.colors[i].a = 1.0;

      if(keyframes[i]->acceleration) {
        Eigen::Vector3d pos = keyframes[i]->node->estimate().translation();
        geometry_msgs::Point point;
        point.x = pos.x();
        point.y = pos.y();
        point.z = pos.z();

        std_msgs::ColorRGBA color;
        color.r = 0.0;
        color.g = 0.0;
        color.b = 1.0;
        color.a = 0.1;

        imu_marker.points.push_back(point);
        imu_marker.colors.push_back(color);
      }
    }

    // edge markers
    visualization_msgs::Marker& edge_marker = markers.markers[2];
    edge_marker.header.frame_id = "map";
    edge_marker.header.stamp = stamp;
    edge_marker.ns = "edges";
    edge_marker.id = 2;
    edge_marker.type = visualization_msgs::Marker::LINE_LIST;

    edge_marker.pose.orientation.w = 1.0;
    edge_marker.scale.x = 0.05;

    edge_marker.points.resize(graph_slam->graph->edges().size() * 2);
    edge_marker.colors.resize(graph_slam->graph->edges().size() * 2);

    auto edge_itr = graph_slam->graph->edges().begin();
    for(int i = 0; edge_itr != graph_slam->graph->edges().end(); edge_itr++, i++) {
      g2o::HyperGraph::Edge* edge = *edge_itr;
      g2o::EdgeSE3* edge_se3 = dynamic_cast<g2o::EdgeSE3*>(edge);
      if(edge_se3) {
        g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_se3->vertices()[0]);
        g2o::VertexSE3* v2 = dynamic_cast<g2o::VertexSE3*>(edge_se3->vertices()[1]);
        Eigen::Vector3d pt1 = v1->estimate().translation();
        Eigen::Vector3d pt2 = v2->estimate().translation();

        edge_marker.points[i * 2].x = pt1.x();
        edge_marker.points[i * 2].y = pt1.y();
        edge_marker.points[i * 2].z = pt1.z();
        edge_marker.points[i * 2 + 1].x = pt2.x();
        edge_marker.points[i * 2 + 1].y = pt2.y();
        edge_marker.points[i * 2 + 1].z = pt2.z();

        double p1 = static_cast<double>(v1->id()) / graph_slam->graph->vertices().size();
        double p2 = static_cast<double>(v2->id()) / graph_slam->graph->vertices().size();
        edge_marker.colors[i * 2].r = 1.0 - p1;
        edge_marker.colors[i * 2].g = p1;
        edge_marker.colors[i * 2].a = 1.0;
        edge_marker.colors[i * 2 + 1].r = 1.0 - p2;
        edge_marker.colors[i * 2 + 1].g = p2;
        edge_marker.colors[i * 2 + 1].a = 1.0;

        if(std::abs(v1->id() - v2->id()) > 2) {
          edge_marker.points[i * 2].z += 0.5;
          edge_marker.points[i * 2 + 1].z += 0.5;
        }

        continue;
      }

      g2o::EdgeSE3Plane* edge_plane = dynamic_cast<g2o::EdgeSE3Plane*>(edge);
      if(edge_plane) {
        g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_plane->vertices()[0]);
        Eigen::Vector3d pt1 = v1->estimate().translation();
        Eigen::Vector3d pt2(pt1.x(), pt1.y(), 0.0);

        edge_marker.points[i * 2].x = pt1.x();
        edge_marker.points[i * 2].y = pt1.y();
        edge_marker.points[i * 2].z = pt1.z();
        edge_marker.points[i * 2 + 1].x = pt2.x();
        edge_marker.points[i * 2 + 1].y = pt2.y();
        edge_marker.points[i * 2 + 1].z = pt2.z();

        edge_marker.colors[i * 2].b = 1.0;
        edge_marker.colors[i * 2].a = 1.0;
        edge_marker.colors[i * 2 + 1].b = 1.0;
        edge_marker.colors[i * 2 + 1].a = 1.0;

        continue;
      }

      g2o::EdgeSE3PriorXY* edge_priori_xy = dynamic_cast<g2o::EdgeSE3PriorXY*>(edge);
      if(edge_priori_xy) {
        g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_priori_xy->vertices()[0]);
        Eigen::Vector3d pt1 = v1->estimate().translation();
        Eigen::Vector3d pt2 = Eigen::Vector3d::Zero();
        pt2.head<2>() = edge_priori_xy->measurement();

        edge_marker.points[i * 2].x = pt1.x();
        edge_marker.points[i * 2].y = pt1.y();
        edge_marker.points[i * 2].z = pt1.z() + 0.5;
        edge_marker.points[i * 2 + 1].x = pt2.x();
        edge_marker.points[i * 2 + 1].y = pt2.y();
        edge_marker.points[i * 2 + 1].z = pt2.z() + 0.5;

        edge_marker.colors[i * 2].r = 1.0;
        edge_marker.colors[i * 2].a = 1.0;
        edge_marker.colors[i * 2 + 1].r = 1.0;
        edge_marker.colors[i * 2 + 1].a = 1.0;

        continue;
      }

      g2o::EdgeSE3PriorXYZ* edge_priori_xyz = dynamic_cast<g2o::EdgeSE3PriorXYZ*>(edge);
      if(edge_priori_xyz) {
        g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_priori_xyz->vertices()[0]);
        Eigen::Vector3d pt1 = v1->estimate().translation();
        Eigen::Vector3d pt2 = edge_priori_xyz->measurement();

        edge_marker.points[i * 2].x = pt1.x();
        edge_marker.points[i * 2].y = pt1.y();
        edge_marker.points[i * 2].z = pt1.z() + 0.5;
        edge_marker.points[i * 2 + 1].x = pt2.x();
        edge_marker.points[i * 2 + 1].y = pt2.y();
        edge_marker.points[i * 2 + 1].z = pt2.z();

        edge_marker.colors[i * 2].r = 1.0;
        edge_marker.colors[i * 2].a = 1.0;
        edge_marker.colors[i * 2 + 1].r = 1.0;
        edge_marker.colors[i * 2 + 1].a = 1.0;

        continue;
      }
    }

    // sphere
    visualization_msgs::Marker& sphere_marker = markers.markers[3];
    sphere_marker.header.frame_id = "map";
    sphere_marker.header.stamp = stamp;
    sphere_marker.ns = "loop_close_radius";
    sphere_marker.id = 3;
    sphere_marker.type = visualization_msgs::Marker::SPHERE;

    if(!keyframes.empty()) {
      Eigen::Vector3d pos = keyframes.back()->node->estimate().translation();
      sphere_marker.pose.position.x = pos.x();
      sphere_marker.pose.position.y = pos.y();
      sphere_marker.pose.position.z = pos.z();
    }
    sphere_marker.pose.orientation.w = 1.0;
    sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z = loop_detector->get_distance_thresh() * 2.0;

    sphere_marker.color.r = 1.0;
    sphere_marker.color.a = 0.3;

    return markers;
  }

  /**
   * @brief dump all data to the current directory
   * @param req
   * @param res
   * @return
   */
  bool dump_service(hdl_graph_slam::DumpGraphRequest& req, hdl_graph_slam::DumpGraphResponse& res) {
    std::lock_guard<std::mutex> lock(main_thread_mutex);

    std::string directory = req.destination;

    if(directory.empty()) {
      std::array<char, 64> buffer;
      buffer.fill(0);
      time_t rawtime;
      time(&rawtime);
      const auto timeinfo = localtime(&rawtime);
      strftime(buffer.data(), sizeof(buffer), "%d-%m-%Y %H:%M:%S", timeinfo);
      std::string directory(buffer.data());
    }

    if(!boost::filesystem::is_directory(directory)) {
      boost::filesystem::create_directory(directory);
    }

    std::cout << "all data dumped to:" << directory << std::endl;

    graph_slam->save(directory + "/graph.g2o");
    for(int i = 0; i < keyframes.size(); i++) {
      std::stringstream sst;
      sst << boost::format("%s/%06d") % directory % i;

      keyframes[i]->save(sst.str());
    }

    if(zero_utm) {
      std::ofstream zero_utm_ofs(directory + "/zero_utm");
      zero_utm_ofs << *zero_utm << std::endl;
    }

    std::ofstream ofs(directory + "/special_nodes.csv");
    ofs << "anchor_node " << (anchor_node == nullptr ? -1 : anchor_node->id()) << std::endl;
    ofs << "anchor_edge " << (anchor_edge == nullptr ? -1 : anchor_edge->id()) << std::endl;
    ofs << "floor_node " << (floor_plane_node == nullptr ? -1 : floor_plane_node->id()) << std::endl;

    res.success = true;
    return true;
  }

  /**
   * @brief save map data as pcd
   * @param req
   * @param res
   * @return
   */
  bool save_map_service(hdl_graph_slam::SaveMapRequest& req, hdl_graph_slam::SaveMapResponse& res) {
    std::vector<KeyFrameSnapshot::Ptr> snapshot;

    keyframes_snapshot_mutex.lock();
    snapshot = keyframes_snapshot;
    keyframes_snapshot_mutex.unlock();

    auto cloud = map_cloud_generator->generate(snapshot, req.resolution);
    if(!cloud) {
      res.success = false;
      return true;
    }

    if(zero_utm && req.utm) {
      for(auto& pt : cloud->points) {
        pt.getVector3fMap() += (*zero_utm).cast<float>();
      }
    }

    cloud->header.frame_id = map_frame_id;
    cloud->header.stamp = snapshot.back()->cloud->header.stamp;

    if(zero_utm) {
      std::ofstream ofs(req.destination + ".utm");
      ofs << (*zero_utm).transpose() << std::endl;
    }

    int ret = pcl::io::savePCDFileBinary(req.destination, *cloud);
    res.success = ret == 0;

    return true;
  }

private:
  // ROS
  ros::NodeHandle nh;
  ros::NodeHandle mt_nh;
  ros::NodeHandle private_nh;
  ros::WallTimer optimization_timer;
  ros::WallTimer map_publish_timer;

  std::unique_ptr<message_filters::Subscriber<nav_msgs::Odometry>> odom_sub;
  std::unique_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>> cloud_sub;
  std::unique_ptr<message_filters::Synchronizer<ApproxSyncPolicy>> sync;

  //by li ///
  ros::Subscriber pose_sub;
  ros::Subscriber posepos_sub;
  ros::Subscriber poseori_sub;
  //
  ros::Subscriber gps_sub;
  ros::Subscriber nmea_sub;
  ros::Subscriber navsat_sub;

  ros::Subscriber imu_sub;
  ros::Subscriber floor_sub;

  ros::Publisher markers_pub;

  std::string map_frame_id;
  std::string odom_frame_id;

  std::mutex trans_odom2map_mutex;
  Eigen::Matrix4f trans_odom2map;
  ros::Publisher odom2map_pub;

  std::string points_topic;
  ros::Publisher read_until_pub;
  ros::Publisher map_points_pub;

  tf::TransformListener tf_listener;

  ros::ServiceServer dump_service_server;
  ros::ServiceServer save_map_service_server;

  // keyframe queue
  std::string base_frame_id;
  std::mutex keyframe_queue_mutex;
  std::deque<KeyFrame::Ptr> keyframe_queue;

  // gps queue
  double gps_time_offset;
  double gps_edge_stddev_xy;
  double gps_edge_stddev_z;
  boost::optional<Eigen::Vector3d> zero_utm;
  std::mutex gps_queue_mutex;
  std::deque<geographic_msgs::GeoPointStampedConstPtr> gps_queue;

  //by li//////
  // pose queue
  double pose_time_offset;
  double pose_edge_stddev_xy;
  double pose_edge_stddev_z;
  bool enable_pose_orientation;
  double pose_orientation_edge_stddev;
  boost::optional<Eigen::Vector3d> zero_pos;
  std::mutex pose_queue_mutex;
  std::deque<geometry_msgs::PoseStampedConstPtr> pose_queue;
  std::mutex posepos_queue_mutex;
  std::deque<geometry_msgs::PoseStampedConstPtr> posepos_queue;
  std::mutex poseori_queue_mutex;
  std::deque<geometry_msgs::PoseStampedConstPtr> poseori_queue;

  // imu queue
  double imu_time_offset;
  bool enable_imu_orientation;
  double imu_orientation_edge_stddev;
  bool enable_imu_acceleration;
  double imu_acceleration_edge_stddev;
  std::mutex imu_queue_mutex;
  std::deque<sensor_msgs::ImuConstPtr> imu_queue;

  // floor_coeffs queue
  double floor_edge_stddev;
  std::mutex floor_coeffs_queue_mutex;
  std::deque<hdl_graph_slam::FloorCoeffsConstPtr> floor_coeffs_queue;

  // for map cloud generation
  std::atomic_bool graph_updated;
  double map_cloud_resolution;
  std::mutex keyframes_snapshot_mutex;
  std::vector<KeyFrameSnapshot::Ptr> keyframes_snapshot;
  std::unique_ptr<MapCloudGenerator> map_cloud_generator;

  // graph slam
  // all the below members must be accessed after locking main_thread_mutex
  std::mutex main_thread_mutex;

  int max_keyframes_per_update;
  std::deque<KeyFrame::Ptr> new_keyframes;

  g2o::VertexSE3* anchor_node;
  g2o::EdgeSE3* anchor_edge;
  g2o::VertexPlane* floor_plane_node;
  std::vector<KeyFrame::Ptr> keyframes;
  std::unordered_map<ros::Time, KeyFrame::Ptr, RosTimeHash> keyframe_hash;

  std::unique_ptr<GraphSLAM> graph_slam;
  std::unique_ptr<LoopDetector> loop_detector;
  std::unique_ptr<KeyframeUpdater> keyframe_updater;
  std::unique_ptr<NmeaSentenceParser> nmea_parser;

  std::unique_ptr<InformationMatrixCalculator> inf_calclator;
};

}  // namespace hdl_graph_slam

PLUGINLIB_EXPORT_CLASS(hdl_graph_slam::HdlGraphSlamNodelet, nodelet::Nodelet)


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology 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 HOLDER 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110206    D. Dirkx          First version of file.
 *      110208    D. Dirkx          Finalized for code check.
 *      110212    J. Melman         Fixed many minor formatting issues.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      120326    D. Dirkx          Changed raw pointers to shared pointers.
 *      120628    A. Ronse          Boostified unit test.
 *
 *    References
 *      Craidon, C.B. A Desription of the Langley Wireframe Geometry Standard (LaWGS) format, NASA
 *          TECHNICAL MEMORANDUM 85767.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

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

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Mathematics/GeometricShapes/lawgsPartGeometry.h"
#include "Tudat/Mathematics/GeometricShapes/sphereSegment.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_Lawgs_Surface_Geometry )

//! Test implementation of Lawgs surface geometry.
BOOST_AUTO_TEST_CASE( testLawgsSurfaceGeometry )
{
    using namespace tudat;
    using namespace geometric_shapes;

    // Create a full sphere as test geometry, with a radius of 2.0.
    const double sphereRadius = 2.0;
    boost::shared_ptr< SphereSegment > sphere = boost::make_shared< SphereSegment >(
                sphereRadius );

    // Create a Lawgs mesh of the sphere.
    LawgsPartGeometry lawgsSurface;
    const int numberOfLines = 21;
    const int numberOfPoints = 21;
    lawgsSurface.setMesh( sphere, numberOfLines, numberOfPoints );

    // Retrieve the total surface area and check if it is sufficiently close
    // to the expected value.
    using mathematical_constants::PI;
    const double totalArea = lawgsSurface.getTotalArea( );
    BOOST_CHECK_SMALL( std::fabs( totalArea - 4.0 * PI
                                  * ( std::pow( sphereRadius, 2.0 ) ) ), 0.6 );

    // Test if number of lines on mesh is correct.
    BOOST_CHECK_EQUAL( lawgsSurface.getNumberOfLines( ), numberOfLines );

    // Test if number of points per line on mesh is correct.
    BOOST_CHECK_EQUAL( lawgsSurface.getNumberOfPoints( ), numberOfPoints );

    // Set part name.
    std::string partName = "sphere";
    lawgsSurface.setName( partName );

    // Test if part name is properly retrieved.
    BOOST_CHECK_EQUAL( lawgsSurface.getName( ), partName );

    // Retrieve normal and centroid for panel 0, 0.
    Eigen::Vector3d testNormal = lawgsSurface.getPanelSurfaceNormal( 0, 0 );
    Eigen::Vector3d testCentroid = lawgsSurface.getPanelCentroid( 0, 0 );

    // Test whether centroid and normal are collinear for panel 0, 0.
    BOOST_CHECK_SMALL( std::fabs( testCentroid.normalized( ).dot(
                                      testNormal.normalized( ) ) ) - 1.0, 1.0e-5 );

    // Test if the position of the x- and y-coordinate of panel 0, 0 is correct.
    BOOST_CHECK_SMALL( std::fabs( std::atan( testCentroid.y( ) / testCentroid.x( ) ) - PI / 20.0 ),
                       std::numeric_limits< double >::epsilon( ) );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/**
 * @file TerrainDescription.hpp
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-08
 */

#pragma once

#include <array>
#include <vector>
#include <memory>
#include <iostream>
#include <Eigen/Eigen>

#include <momentumopt/setting/Definitions.hpp>

namespace momentumopt {

  /**
   * This class is a container for all variables required to define a terrain region
   */
  class TerrainRegion
  {
    public:
      TerrainRegion() : terrain_id_(-1) {}
      ~TerrainRegion(){}

      /*! Getter and setter methods */
      int& terrainId() { return terrain_id_; }
      Eigen::Vector3d& pointOnTerrain() { return point_; }
      Eigen::Vector3d& normalToTerrain() { return normal_; }
      Eigen::Matrix3d& terrainRotation() { return rotation_; }
      Eigen::VectorXd& terrainDescriptionVector() { return b_; }
      Eigen::Matrix<double,Eigen::Dynamic,3>& terrainDescriptionMatrix() { return A_; }

      const int& terrainId() const { return terrain_id_; }
      const Eigen::Vector3d& pointOnTerrain() const { return point_; }
      const Eigen::Vector3d& normalToTerrain() const { return normal_; }
      const Eigen::Matrix3d& terrainRotation() const { return rotation_; }
      const Eigen::VectorXd& terrainDescriptionVector() const { return b_; }
      const Eigen::Matrix<double,Eigen::Dynamic,3>& terrainDescriptionMatrix() const { return A_; }
      const Eigen::Quaternion<double> terrainOrientation() const { return Eigen::Quaternion<double>(rotation_); }

      std::string toString() const;
      friend std::ostream& operator<<(std::ostream &os, const TerrainRegion& obj) { return os << obj.toString(); }

    private:
      friend class TerrainDescription;

      /* initialization function only available to terrain description */
      void initialize(const Eigen::Matrix<double,Eigen::Dynamic,3>& ccwvertices, int terrain_id);

    private:
      int terrain_id_;
      Eigen::VectorXd b_;
      Eigen::Matrix3d rotation_;
      Eigen::Vector3d point_, normal_;
      Eigen::Matrix<double,Eigen::Dynamic,3> A_;
  };

  /**
   * This class is a container for a terrain description in terms of terrain surfaces
   */
  class TerrainDescription
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    public:
      TerrainDescription() : num_terrain_regions_(-1) {}
      ~TerrainDescription(){}

      void addTerrainRegion(const Eigen::Matrix<double,Eigen::Dynamic,3>& ccwvertices);
      void loadFromFile(const std::string cfg_file, const std::string terrain_description_name = "terrain_description");

      /*! Getter and setter methods */
      const int numRegions() const { return terrain_regions_.size(); }
      TerrainRegion& terrainRegion(int terrain_id) { return terrain_regions_[terrain_id]; }
      const TerrainRegion& terrainRegion(int terrain_id) const { return terrain_regions_[terrain_id]; }

      std::string toString() const;
      friend std::ostream& operator<<(std::ostream &os, const TerrainDescription& obj) { return os << obj.toString(); }

    private:
      int num_terrain_regions_;
      std::vector<TerrainRegion> terrain_regions_;
  };

}


/**
 * Written by Joris Guerry (joris.guerry@onera.fr) 2015/09
 * with snippets of code from:
 * http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages
 * published under license: CC BY 3.0 http://creativecommons.org/licenses/by/3.0/
 *
 * Modified:
 * 2015/09 timestamps in filenames / Bertrand le Saux (bertrand.le_saux@onera.fr)
 * 2016/09 adding buffering / Joris Guerry (joris.guerry@onera.fr)
 */

#include <boost/thread/thread.hpp>
#include <boost/filesystem.hpp>

// pour ROS et OpenCV
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

// pour le traitement

#include <stdio.h>
#include <dirent.h>
#include <ios>
#include <stdexcept>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/ml/ml.hpp>

// Les classiques
#include <algorithm>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <stdlib.h>

using namespace std;
using std::string;
using namespace boost::filesystem;

class ExportImage
{
		ros::NodeHandle nh;
		image_transport::ImageTransport it;
		image_transport::Subscriber sub_video_png_input;
		int counter;
		int tampon;
    double timebase;
		string flux;
		string path;
		string format;

	public:
  ExportImage( string _flux, string _path, string _fmt, int _tampon ): it( nh ) //constructeur qui initialise un objet ImageTransport (it) avec la handle du noeud ROS (nh)
		{

			flux = _flux;
			path = _path;
      format = _fmt;
			tampon = _tampon;
			boost::filesystem::path dir( _path.c_str() );
			if( boost::filesystem::create_directory( dir ) )
			{
				std::cerr << "Directory Created: " << _path << std::endl;
			}

			sub_video_png_input = it.subscribe( flux, tampon, &ExportImage::video_png_input_Cb, this );

			counter = 0;
			timebase = 0;

		}

		~ExportImage()
		{
			cout << "Compteur : " << counter << " images sauvegardées." << endl;
		}

		void video_png_input_Cb( const sensor_msgs::ImageConstPtr& msg )
		{
      //
			if (counter==0) {
        cout << "msg->encoding :" << msg->encoding << endl;
      }
	    // cv_bridge::CvImageConstPtr cv_ptr;
	    cv_bridge::CvImagePtr cv_ptr;
			try
			{
          sensor_msgs::Image img;
          img.header = msg->header;
          img.height = msg->height;
          img.width = msg->width;
          img.is_bigendian = msg->is_bigendian;
          img.step = msg->step;
          img.data = msg->data;
          // img.encoding = "mono16";
          // cv_ptr = cv_bridge::toCvCopy( img , sensor_msgs::image_encodings::MONO16 );
          img.encoding = "32FC1";
          cv_ptr = cv_bridge::toCvCopy( img , sensor_msgs::image_encodings::TYPE_32FC1 );

					// scale from 0-8000mm to 0 255 uint.
					// cv_ptr->image
					// THRESH_TOZERO_INV

					float min_disparity = 0;
					float max_disparity = 10;// max value in meter before tresholding to zero
					float multiplier = 255.0f / (max_disparity - min_disparity);
					cv::threshold(cv_ptr->image, cv_ptr->image, max_disparity, 0, cv::THRESH_TOZERO_INV);
					cv_ptr->image=multiplier*cv_ptr->image;

          // cv_ptr = cv_bridge::toCvCopy( img , sensor_msgs::image_encodings::CV_8UC1 );

			}
			catch( cv_bridge::Exception& e )
			{
				ROS_ERROR( "cv_bridge exception: %s", e.what() );
				return;
			}
			counter++;

      /* with timebase, understandable timestamp */
      if (counter==1) {
        timebase=0 ;//msg->header.stamp.toSec();
      }
      double timestampsec = msg->header.stamp.toSec() - timebase;
      unsigned int timestamp = static_cast<unsigned int> ( timestampsec * 100000);

                        /* timestamp based on micro-seconds timestamp since beginning */
			std::stringstream saveLocationFlux;
			cout << path << "/" << std::setw( 7 ) << std::setfill( '0' ) << counter << "-" << std::setw( 12 ) << std::setfill( '0' ) << timestamp << "." << format << "\xd";
			saveLocationFlux << path << "/" << std::setw( 7 ) << std::setfill( '0' ) << counter << "-" << std::setw( 12 ) << std::setfill( '0' ) << timestamp << "." << format;

			std::string saveLocation = saveLocationFlux.str();

      if ( (!format.compare(string("png"))) || (!format.compare(string("jpg"))) || (!format.compare(string("jpeg"))) ) {
        cv::imwrite( saveLocation, cv_ptr->image );
      } else {
        ROS_ERROR( "cv::imwrite error: unknown image format" );
        return;
      }

		}
};

int main( int argc, char** argv )
{
	std::cout << "###########################################################################" << std::endl << std::endl ;
	std::cout << "Export image" <<  std::endl << std::endl;
	std::cout << "###########################################################################" << std::endl << std::endl ;

	std::string flux;
	std::string path;
	std::string format;
	int tampon;

	ros::init( argc, argv, "ExportImageDepth" );
	if( argc == 4 )
	{
		format = "png";
		flux = argv[1];
		path = argv[2];
		tampon = std::stoi(argv[3]);
	}
	else
	{
		ROS_ERROR( "Parameters invalid" );
		return 1;
	}

	std::cout <<"Save " << flux << " to " << path << " ["<< format <<"] with a buffer size of " << tampon <<"."<< std::endl;

	ExportImage toImage( flux, path, format, tampon );
	ros::spin();

	return 0;

}


//
// Add --log_level=message to see the messages!
//
#include <boost/test/unit_test.hpp>

#include <sstream>

#include "layers/s2slayer.h"
#include "layers/lstmlayer.h"
#include "core/training.h"
#include "core/utils.h"
#include "core/functions.h"

#include "timer.h"
#include "test_utils.h"
#include "test_layers.h"

using namespace std;
using namespace boost;
using namespace boost::unit_test;
using namespace yann;
using namespace yann::test;

struct Seq2SeqLayerTestFixture
{
  Seq2SeqLayerTestFixture()
  {

  }
  ~Seq2SeqLayerTestFixture()
  {

  }
};
// struct Seq2SeqLayerTestFixture

BOOST_FIXTURE_TEST_SUITE(Seq2SeqLayerTest, Seq2SeqLayerTestFixture);

BOOST_AUTO_TEST_CASE(IO_Test)
{
  BOOST_TEST_MESSAGE("*** Seq2SeqLayer IO test ...");

  const MatrixSize input_size = 4;
  const MatrixSize output_size = 3;

  auto one = Seq2SeqLayer::create_lstm(input_size, output_size, make_unique<SigmoidFunction>());
  BOOST_CHECK(one);
  one->init(Layer::InitMode_Random, boost::none);

  BOOST_TEST_MESSAGE("Seq2SeqLayer before writing to file: " << "\n" << *one);
  ostringstream oss;
  oss << *one;
  BOOST_CHECK(!oss.fail());

  auto two = Seq2SeqLayer::create_lstm(input_size, output_size, make_unique<SigmoidFunction>());
  BOOST_CHECK(two);
  std::istringstream iss(oss.str());
  iss >> *two;
  BOOST_CHECK(!iss.fail());
  BOOST_TEST_MESSAGE("Seq2SeqLayer after loading from file: " << "\n" << *two);

  BOOST_CHECK(one->is_equal(*two, TEST_TOLERANCE));
}

BOOST_AUTO_TEST_CASE(FeedForward_Test)
{
  BOOST_TEST_MESSAGE("*** Seq2SeqLayer FeedForward test ...");

  const MatrixSize input_size = 4;
  const MatrixSize output_size = 3;
  const MatrixSize batch_size = 2;

  Matrix ww_x_enc[LstmLayer::Gate_Max], ww_x_dec[LstmLayer::Gate_Max];
  Matrix ww_h_enc[LstmLayer::Gate_Max], ww_h_dec[LstmLayer::Gate_Max];
  Vector bb_enc[LstmLayer::Gate_Max], bb_dec[LstmLayer::Gate_Max];

  for(auto ii = 0; ii < LstmLayer::Gate_Max; ++ii) {
    ww_x_enc[ii].resize(input_size, output_size);
    ww_h_enc[ii].resize(output_size, output_size);
    bb_enc[ii].resize(output_size);

    ww_x_dec[ii].resize(output_size, output_size);
    ww_h_dec[ii].resize(output_size, output_size);
    bb_dec[ii].resize(output_size);

    ww_x_enc[ii].setConstant((ii + 1) / (Value)10);
    ww_h_enc[ii].setConstant((ii + 1) / (Value)10);
    bb_enc[ii].setConstant((ii + 1) / (Value)10);

    ww_x_dec[ii].setConstant((ii + 1) / (Value)100);
    ww_h_dec[ii].setConstant((ii + 1) / (Value)100);
    bb_dec[ii].setConstant((ii + 1) / (Value)100);
  }

  VectorBatch input, expected_output;
  resize_batch(input, batch_size, input_size);
  resize_batch(expected_output, batch_size, output_size);

  input <<
      0.1, 0.2, 0.3, 0.4,
      0.5, 0.6, 0.7, 0.8;

  // identity
  auto layer_identity = Seq2SeqLayer::create_lstm(input_size, output_size, make_unique<IdentityFunction>());
  BOOST_CHECK(layer_identity);
  auto encoder_identity = dynamic_cast<LstmLayer*>(layer_identity->get_encoder());
  auto decoder_identity = dynamic_cast<LstmLayer*>(layer_identity->get_decoder());
  BOOST_CHECK(encoder_identity);
  BOOST_CHECK(decoder_identity);
  encoder_identity->set_values(ww_x_enc, ww_h_enc, bb_enc);
  decoder_identity->set_values(ww_x_dec, ww_h_dec, bb_dec);

  expected_output <<
      0.00016147813, 0.00016147813, 0.00016147813,
      9.805934e-06, 9.805934e-06, 9.805934e-06;
  test_layer_feedforward(*layer_identity, input, expected_output);

  // sigmoid
  auto layer_sigmoid = Seq2SeqLayer::create_lstm(input_size, output_size, make_unique<SigmoidFunction>());
  BOOST_CHECK(layer_sigmoid);
  auto encoder_sigmoid = dynamic_cast<LstmLayer*>(layer_sigmoid->get_encoder());
  auto decoder_sigmoid = dynamic_cast<LstmLayer*>(layer_sigmoid->get_decoder());
  BOOST_CHECK(encoder_sigmoid);
  BOOST_CHECK(decoder_sigmoid);
  encoder_sigmoid->set_values(ww_x_enc, ww_h_enc, bb_enc);
  decoder_sigmoid->set_values(ww_x_dec, ww_h_dec, bb_dec);

  expected_output <<
      0.2979, 0.2979, 0.2979,
      0.3155, 0.3155, 0.3155;
  test_layer_feedforward(*layer_sigmoid, input, expected_output);
}

BOOST_AUTO_TEST_CASE(Backprop_Test)
{
  BOOST_TEST_MESSAGE("*** Seq2SeqLayer backprop test ...");

  const MatrixSize input_size = 4;
  const MatrixSize output_size = 3;
  const MatrixSize batch_size = 2;
  const double learning_rate = 10;
  const size_t epochs = 10000;

  Matrix ww_x_enc[LstmLayer::Gate_Max], ww_x_dec[LstmLayer::Gate_Max];
  Matrix ww_h_enc[LstmLayer::Gate_Max], ww_h_dec[LstmLayer::Gate_Max];
  Vector bb_enc[LstmLayer::Gate_Max], bb_dec[LstmLayer::Gate_Max];

  for(auto ii = 0; ii < LstmLayer::Gate_Max; ++ii) {
    ww_x_enc[ii].resize(input_size, output_size);
    ww_h_enc[ii].resize(output_size, output_size);
    bb_enc[ii].resize(output_size);

    ww_x_dec[ii].resize(output_size, output_size);
    ww_h_dec[ii].resize(output_size, output_size);
    bb_dec[ii].resize(output_size);

    ww_x_enc[ii].setConstant((ii + 1) / (Value)10);
    ww_h_enc[ii].setConstant((ii + 1) / (Value)10);
    bb_enc[ii].setConstant((ii + 1) / (Value)10);

    ww_x_dec[ii].setConstant((ii + 1) / (Value)100);
    ww_h_dec[ii].setConstant((ii + 1) / (Value)100);
    bb_dec[ii].setConstant((ii + 1) / (Value)100);
  }

  VectorBatch input, expected_output;
  resize_batch(input, batch_size, input_size);
  resize_batch(expected_output, batch_size, output_size);

  input <<
      0, 0, 0, 0, // was 0.1, 0.2, 0.3, 0.4,
      0, 0, 0, 0; // was 0.5, 0.6, 0.7, 0.8;

  expected_output <<
      0.2979, 0.2979, 0.2979,
      0.3155, 0.3155, 0.3155;

  auto layer = Seq2SeqLayer::create_lstm(input_size, output_size, make_unique<SigmoidFunction>());
  BOOST_CHECK(layer);
  auto encoder = dynamic_cast<LstmLayer*>(layer->get_encoder());
  auto decoder = dynamic_cast<LstmLayer*>(layer->get_decoder());
  BOOST_CHECK(encoder);
  BOOST_CHECK(decoder);

  encoder->set_values(ww_x_enc, ww_h_enc, bb_enc);
  decoder->set_values(ww_x_dec, ww_h_dec, bb_dec);

  test_layer_backprop(
      *layer,
      input,
      boost::none,
      expected_output,
      make_unique<CrossEntropyCost>(),
      learning_rate,
      epochs
  );
}

BOOST_AUTO_TEST_CASE(RandomBackprop_Test)
{
  BOOST_TEST_MESSAGE("*** Seq2SeqLayer backprop with random inputs and weights ...");

  const MatrixSize input_size = 4;
  const MatrixSize output_size = 3;
  const MatrixSize batch_size = 2;
  const double learning_rate = 1.0;
  const size_t epochs = 10000;

  // create layer
  auto layer = Seq2SeqLayer::create_lstm(input_size, output_size, make_unique<SigmoidFunction>());
  BOOST_CHECK(layer);
  layer->init(Layer::InitMode_Random, Layer::InitContext(123));

  // test
  test_layer_backprop_from_random(
      *layer,
      batch_size,
      make_unique<CrossEntropyCost>(),
      learning_rate,
      epochs
  );
}

BOOST_AUTO_TEST_CASE(Training_WithSigmoid_Test)
{
  BOOST_TEST_MESSAGE("*** Seq2SeqLayer with Sigmoid activation training test ...");

  const MatrixSize input_size = 4;
  const MatrixSize output_size = 3;
  const MatrixSize batch_size = 2;
  const double learning_rate = 0.75;
  const size_t epochs = 500;

  VectorBatch input, expected;
  resize_batch(input, batch_size, input_size);
  resize_batch(expected, batch_size, output_size);

  input <<
      0.1, 0.2, 0.3, 0.4,
      0.5, 0.6, 0.7, 0.8;

  expected <<
      0.2979, 0.2979, 0.2979,
      0.3155, 0.3155, 0.3155;

  // create layer
  auto layer = Seq2SeqLayer::create_lstm(input_size, output_size, make_unique<SigmoidFunction>());
  BOOST_CHECK(layer);
  layer->init(Layer::InitMode_Random, Layer::InitContext(123));

  // test
  test_layer_training(
      *layer,
      input,
      expected,
      1, // tests num
      make_unique<CrossEntropyCost>(),
      learning_rate,
      epochs
  );
}

BOOST_AUTO_TEST_CASE(RandomTraining_Test)
{
  BOOST_TEST_MESSAGE("*** Seq2SeqLayer training with random inputs ...");

  const MatrixSize input_size = 10;
  const MatrixSize output_size = 7;
  const MatrixSize batch_size = 5;
  const double learning_rate = 1.0;
  const size_t epochs = 50000;

  // create layer
  auto layer = Seq2SeqLayer::create_lstm(input_size, output_size, make_unique<SigmoidFunction>());
  BOOST_CHECK(layer);

  test_layer_training_from_random(
      *layer,
      batch_size,
      1, // tests num
      make_unique<CrossEntropyCost>(),
      learning_rate,
      epochs
  );
}

BOOST_AUTO_TEST_SUITE_END()


#include "transport.hpp"

#include <assert.h>
#include <limits>
#include <vector>

#include <boost/intrusive/list.hpp>
#include <boost/intrusive/slist.hpp>

#include "union-find.hpp"

template <typename T>
class TransportGraph {
    public:
        TransportGraph(int sizeSupply, int sizeDemand)
            : _sizeSupply(sizeSupply)
            , _sizeDemand(sizeDemand)
            , _nodes(sizeSupply+sizeDemand)
            , _treeEdges(2*(sizeSupply+sizeDemand - 1))
        {
            for (int i = 0; i < _sizeSupply+_sizeDemand; ++i)
                _nodes[i].id = i;
        }

        typedef boost::intrusive::list_base_hook<
            boost::intrusive::link_mode<boost::intrusive::normal_link>
            > ListHook;
        struct Edge : public ListHook {
            int target;
            int edgeIdx;
            Edge* reverse;
        };

        typedef boost::intrusive::list<Edge,
                boost::intrusive::base_hook<ListHook>> EdgeList;

        typedef boost::intrusive::slist_member_hook<
            boost::intrusive::link_mode<boost::intrusive::normal_link>
            > StackHook;

        struct Node {
            int id = 0;
            T potential = 0;
            EdgeList outEdges;
            typename EdgeList::iterator dfsOutEdge;
            StackHook stackHook;
            bool inDfsStack = false;
        };
        typedef boost::intrusive::slist<Node,
                boost::intrusive::member_hook<Node,
                                              StackHook,
                                              &Node::stackHook>
                > NodeStack;

        void addTreeEdge(int supply, int demand) {
            assert(_numTreeEdges+2 <= static_cast<int>(_treeEdges.size()));
            Edge& e1 = _treeEdges[_numTreeEdges];
            Edge& e2 = _treeEdges[_numTreeEdges+1];
            _numTreeEdges += 2;

            int edgeIdx = supply*_sizeDemand+demand;
            e1.target = demand+_sizeSupply;
            e1.edgeIdx = edgeIdx;
            e1.reverse = &e2;
            e2.target = supply;
            e2.edgeIdx = edgeIdx;
            e2.reverse = &e1;

            _nodes[supply].outEdges.push_back(e1);
            _nodes[demand+_sizeSupply].outEdges.push_back(e2);
        }

        void augment(int supply, int demand, T* flows) {
            int edgeIdx = supply*_sizeDemand+demand;

            for (auto& n : _nodes)
                n.inDfsStack = false;

            NodeStack stack;
            {
                Node& startNode = _nodes.at(supply);
                stack.push_front(startNode);
                startNode.dfsOutEdge = startNode.outEdges.begin();
                startNode.inDfsStack = true;
            }
            while (!stack.empty() && stack.front().id != demand+_sizeSupply) {
                auto& n = stack.front();
                if (n.dfsOutEdge == n.outEdges.end()) {
                    stack.pop_front();
                    assert(!stack.empty());
                    auto& nextN = stack.front();
                    assert(nextN.dfsOutEdge != nextN.outEdges.end());
                    assert(nextN.dfsOutEdge->target == n.id);
                    nextN.dfsOutEdge++;
                    continue;
                } else {
                    int target = n.dfsOutEdge->target;
                    auto& nextN = _nodes.at(target);
                    if (nextN.inDfsStack) {
                        n.dfsOutEdge++;
                        continue;
                    } else {
                        stack.push_front(nextN);
                        nextN.dfsOutEdge = nextN.outEdges.begin();
                        nextN.inDfsStack = true;
                        continue;
                    }
                }
            }
            assert(!stack.empty());
            assert(stack.front().id == demand+_sizeSupply);
            assert((stack.size() % 2) == 0);
            stack.pop_front();

            T bottleneckFlow = std::numeric_limits<T>::max();
            Edge* bottleneckEdge = nullptr;
            bool parity = true;
            for (auto& n : stack) {
                if (parity) {
                    auto& e = *n.dfsOutEdge;
                    auto f = flows[e.edgeIdx];
                    if (f < bottleneckFlow) {
                        bottleneckFlow = f;
                        bottleneckEdge = &e;
                    }
                }
                parity = !parity;
            }
            assert(bottleneckFlow < std::numeric_limits<T>::max());
            assert(bottleneckEdge != nullptr);

            parity = true;
            for (auto& n : stack) {
                auto& e = *n.dfsOutEdge;
                if (parity)
                    flows[e.edgeIdx] -= bottleneckFlow;
                else
                    flows[e.edgeIdx] += bottleneckFlow;
                parity = !parity;
            }
            flows[edgeIdx] += bottleneckFlow;

            Edge& e1 = *bottleneckEdge;
            Edge& e2 = *e1.reverse;
            {
                Node& n1 = _nodes[e2.target];
                Node& n2 = _nodes[e1.target];
                n1.outEdges.erase(n1.outEdges.iterator_to(e1));
                n2.outEdges.erase(n2.outEdges.iterator_to(e2));
            }
            {
                Node& n1 = _nodes[supply];
                Node& n2 = _nodes[demand+_sizeSupply];
                e1.target = demand+_sizeSupply;
                e1.edgeIdx = edgeIdx;
                e1.reverse = &e2;
                e2.target = supply;
                e2.edgeIdx = edgeIdx;
                e2.reverse = &e1;
                n1.outEdges.push_back(e1);
                n2.outEdges.push_back(e2);
            }
        }

        void updatePotentials(const T* costs) {
            for (auto& n : _nodes) {
                n.potential = 0;
                n.inDfsStack = false;
            }

            NodeStack stack;
            {
                Node& startNode = _nodes.at(0);
                stack.push_front(startNode);
                startNode.dfsOutEdge = startNode.outEdges.begin();
                startNode.inDfsStack = true;
            }
            while (!stack.empty()) {
                auto& n = stack.front();
                if (n.dfsOutEdge == n.outEdges.end()) {
                    stack.pop_front();
                    if(!stack.empty()) {
                        auto& nextN = stack.front();
                        assert(nextN.dfsOutEdge != nextN.outEdges.end());
                        assert(nextN.dfsOutEdge->target == n.id);
                        nextN.dfsOutEdge++;
                    }
                    continue;
                } else {
                    int target = n.dfsOutEdge->target;
                    auto& nextN = _nodes.at(target);
                    if (nextN.inDfsStack) {
                        n.dfsOutEdge++;
                        continue;
                    } else {
                        int edgeIdx = n.dfsOutEdge->edgeIdx;
                        T c = costs[edgeIdx];
                        nextN.potential = c - n.potential;

                        stack.push_front(nextN);
                        nextN.dfsOutEdge = nextN.outEdges.begin();
                        nextN.inDfsStack = true;
                        continue;
                    }
                }
            }
        }

        std::tuple<int, int, T> findPivot(const T* costs) {
            std::tuple<int, int, T> minEdge
                = {0, 0, std::numeric_limits<T>::max()};
            for (int i = 0; i < _sizeSupply; ++i) {
                for (int j = 0; j < _sizeDemand; ++j) {
                    int edgeIdx = i*_sizeDemand+j;
                    T resCost = costs[edgeIdx]
                        - _nodes[i].potential
                        - _nodes[j+_sizeSupply].potential;
                    if (resCost < std::get<2>(minEdge))
                        minEdge = {i, j, resCost};
                }
            }
            return minEdge;
        }

        int _sizeSupply;
        int _sizeDemand;
        int _numTreeEdges = 0;
        std::vector<Node> _nodes;
        std::vector<Edge> _treeEdges;
};

template <typename T>
double solveTransport(int sizeSupply, int sizeDemand, const T* costs,
        const T* supply, const T* demand, T* flow) {
    T sumSupply = 0;
    for (int i = 0; i < sizeSupply; ++i)
        sumSupply += supply[i];
    T sumDemand = 0;
    for (int j = 0; j < sizeDemand; ++j)
        sumDemand += demand[j];
    assert(fabs(sumSupply - sumDemand) < 1e-5);

    for (int k = 0; k < sizeSupply*sizeDemand; ++k)
        flow[k] = 0;

    std::vector<T> resSupply(supply, supply+sizeSupply);
    std::vector<T> resDemand(demand, demand+sizeDemand);

    TransportGraph<T> graph{sizeSupply, sizeDemand};

    int numNodes = sizeSupply+sizeDemand;
    auto uf = UnionFind{numNodes};
    for (int k = 0; k < numNodes-1; ++k) {
        T minCost = std::numeric_limits<T>::max();
        int minI = 0;
        int minJ = 0;
        for (int i = 0; i < sizeSupply; ++i) {
            if (resSupply.at(i) <= 0)
                continue;
            auto comp_i = uf.Find(i);
            for (int j = 0; j < sizeDemand; ++j) {
                if (resDemand.at(j) <= 0)
                    continue;
                auto comp_j = uf.Find(j+sizeSupply);
                if (comp_i == comp_j)
                    continue;
                if (costs[i*sizeDemand+j] < minCost) {
                    minCost = costs[i*sizeDemand+j];
                    minI = i;
                    minJ = j;
                }
            }
        }
        if (minCost == std::numeric_limits<T>::max())
            break;
        assert(uf.Find(minI) != uf.Find(minJ+sizeSupply));
        T f = std::min(resSupply.at(minI), resDemand.at(minJ));
        resSupply.at(minI) -= f;
        resDemand.at(minJ) -= f;
        flow[minI*sizeDemand+minJ] += f;
        graph.addTreeEdge(minI, minJ);
        uf.Merge(minI, minJ+sizeSupply);
    }
    // Add any unconnected components to connect to 0
    for (int i = 0; i < sizeSupply; ++i) {
        if (uf.Find(i) != uf.Find(0+sizeSupply)) {
            graph.addTreeEdge(i, 0);
            uf.Merge(i, 0+sizeSupply);
        }
    }
    for (int j = 0; j < sizeDemand; ++j) {
        if (uf.Find(0) != uf.Find(j+sizeSupply)) {
            graph.addTreeEdge(0, j);
            uf.Merge(0, j+sizeSupply);
        }
    }

#ifndef NDEBUG
    for (auto s : resSupply) {
        assert(fabs(s) < 1e-5);
    }
    for (auto d : resDemand)
        assert(fabs(d) < 1e-5);
#endif

    while (true) {
        graph.updatePotentials(costs);
        std::tuple<int, int, T> minEdge = graph.findPivot(costs);
        if (std::get<2>(minEdge) < 0)
            graph.augment(std::get<0>(minEdge), std::get<1>(minEdge), flow);
        else
            break;
    }

    double cost = 0;
    for (int i = 0; i < sizeSupply; ++i) {
        for (int j = 0; j < sizeDemand; ++j) {
            int idx = i*sizeDemand + j;
            cost += flow[idx] * costs[idx];
        }
    }
    return cost;
}

#define INSTANTIATE_TRANSPORT(T) \
    template double solveTransport<T>(int sizeSupply, int sizeDemand, \
            const T* costs, const T* supply, const T* demand, T* flow);
INSTANTIATE_TRANSPORT(double)
INSTANTIATE_TRANSPORT(int32_t)
INSTANTIATE_TRANSPORT(int64_t)
#undef INSTANTIATE_TRANSPORT


///////////////////////////////////////////////////////////////
//  Copyright 2017 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

//
// Check results of truncated overflow.
//

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/multiprecision/cpp_int.hpp>
#include "test.hpp"
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>

template <class T>
T generate_random(unsigned bits_wanted)
{
   static boost::random::mt19937 gen;
   typedef boost::random::mt19937::result_type random_type;

   T max_val;
   unsigned digits;
   if(std::numeric_limits<T>::is_bounded && (bits_wanted == (unsigned)std::numeric_limits<T>::digits))
   {
      max_val = (std::numeric_limits<T>::max)();
      digits = std::numeric_limits<T>::digits;
   }
   else
   {
      max_val = T(1) << bits_wanted;
      digits = bits_wanted;
   }

   unsigned bits_per_r_val = std::numeric_limits<random_type>::digits - 1;
   while((random_type(1) << bits_per_r_val) > (gen.max)()) --bits_per_r_val;

   unsigned terms_needed = digits / bits_per_r_val + 1;

   T val = 0;
   for(unsigned i = 0; i < terms_needed; ++i)
   {
      val *= (gen.max)();
      val += gen();
   }
   val %= max_val;
   return val;
}

template <class Number>
void test()
{
   using namespace boost::multiprecision;
   typedef Number test_type;

   for(unsigned i = 30; i < std::numeric_limits<test_type>::digits; ++i)
   {
      for(unsigned j = std::numeric_limits<test_type>::digits - i - 1; j < std::numeric_limits<test_type>::digits; ++j)
      {
         for(unsigned k = 0; k < 10; ++k)
         {
            test_type a = static_cast<test_type>(generate_random<cpp_int>(i));
            test_type b = static_cast<test_type>(generate_random<cpp_int>(j));
            test_type c = static_cast<test_type>(cpp_int(a) * cpp_int(b));
            test_type d = a * b;
            BOOST_CHECK_EQUAL(c, d);

            if((k == 0) && (j == 0))
            {
               for(unsigned s = 1; s < std::numeric_limits<test_type>::digits; ++s)
                  BOOST_CHECK_EQUAL(a << s, test_type(cpp_int(a) << s));
            }
         }
      }
   }
}

int main()
{
   using namespace boost::multiprecision;

   test<int512_t>();
   test<uint512_t>();

   //
   // We also need to test type with "odd" bit counts in order to ensure full code coverage:
   //
   test<number<cpp_int_backend<528, 528, signed_magnitude, unchecked, void> > >();
   test<number<cpp_int_backend<528, 528, unsigned_magnitude, unchecked, void> > >();
   test<number<cpp_int_backend<48, 48, signed_magnitude, unchecked, void> > >();
   test<number<cpp_int_backend<48, 48, unsigned_magnitude, unchecked, void> > >();
   return boost::report_errors();
}


#ifndef SAMPML_INCLUDE_FEATURE_VECTOR_HPP
#define SAMPML_INCLUDE_FEATURE_VECTOR_HPP

#include <dlib/matrix.h>

#include "common.hpp"

namespace SAMPML_NAMESPACE {
    template <typename T, std::size_t N>
    using feature_vector = dlib::matrix<T, N, 1>;
}

#endif /* SAMPML_INCLUDE_COMMON_HPP */


#include <fstream>
#include <iostream>
#include <algorithm>
#include <vector>
#include <set>
#include <map>

#include <boost/lexical_cast.hpp>

using namespace std;
using namespace boost;

typedef set<size_t> Vertices;
typedef map<size_t, Vertices> Edges;

Edges edges, redges;
Vertices vertices;
map<size_t, bool> marks;
map<size_t, int> component;

vector<size_t> queue;
vector<set<int> > cedges;

void dfs(size_t u)
{
    if (marks[u])
        return;
    marks[u] = true;

    for (Vertices::const_iterator it = edges[u].begin(), end = edges[u].end();
            it != end; ++it)
    {
        dfs(*it);
    }

    queue.push_back(u);
}

size_t dfs2(size_t u, int c)
{
    if (component[u] != -1)
        return 0;
    size_t res = 1;
    component[u] = c;

    for (Vertices::const_iterator it = redges[u].begin(), end = redges[u].end();
            it != end; ++it)
    {
        res += dfs2(*it, c);
    }
    return res;
}

int main(int argc, char ** argv)
{
    if (argc > 1)
    {
        cerr << "usage: dense_graph < in_graph > out_graph\n";
        return 1;
    }
    size_t edges_num = 0;
    while (1)
    {
        size_t from, to;
        if (!(cin >> from >> to))
            break;

        ++edges_num;

        edges[from].insert(to);
        redges[to].insert(from);
        vertices.insert(from);
        vertices.insert(to);
    }

    cerr << edges_num << " edges read\n";

    queue.reserve(vertices.size());
    for (Vertices::const_iterator it = vertices.begin(), end = vertices.end(); it != end; ++it)
    {
        size_t i = *it;;
        dfs(i);
        component[i] = -1;
    }

    cerr << "dfsed\n";

    reverse(queue.begin(), queue.end());
    int cur_comp = 0;
    for (size_t i = 0, sz = queue.size(); i < sz; ++i)
    {
        size_t u = queue[i];
        if (component[u] != -1)
            continue;

        dfs2(u, cur_comp++);
    }

    cerr << cur_comp  << " components\n";
    vector<vector<size_t> > components(cur_comp);
    for (Vertices::const_iterator it = vertices.begin(), end = vertices.end(); it != end; ++it)
    {
        size_t i = *it;;
        components[component[i]].push_back(i);
    }
    cedges.resize(cur_comp);

    for (Edges::const_iterator it1 = edges.begin(), end1 = edges.end(); it1 != end1; ++it1)
    {
        size_t i = it1->first;
        if (component[i] == -1)
            continue;
        for (Vertices::const_iterator it = it1->second.begin(), end = it1->second.end();
                it != end; ++it)
        {
            cedges[component[i]].insert(component[*it]);
        }
    }

    // printing result
    cout << cur_comp << "\n";

    int non_trivial = 0;
    for (int i = 0; i < cur_comp; ++i)
    {
        size_t sz = components[i].size();
        if (sz != 1)
        {
            ++non_trivial;
        }

        cout << sz << " :";
        for (size_t j = 0; j < sz; ++j)
        {
            cout << " " << components[i][j];
        }
        cout << "\n";
    }

    cerr << non_trivial << " non-trivial\n";
    for (int i = 0; i < cur_comp; ++i)
    {
        for (set<int>::const_iterator it = cedges[i].begin(), end = cedges[i].end();
                it != end; ++it)
        {
            cout << i << " " << *it << "\n";
        }
    }
}


#ifndef INCLUDED_util_meta_intersect_HH
#define INCLUDED_util_meta_intersect_HH

#include <boost/mpl/copy.hpp>
#include <boost/mpl/copy_if.hpp>
#include <boost/mpl/set.hpp>
#include <boost/mpl/has_key.hpp>
#include <boost/mpl/insert.hpp>
// #include <boost/mpl/inserter.hpp>

namespace scheme {
namespace util {
namespace meta {

namespace m = boost::mpl;

template <typename SeqA, typename SeqB>
struct intersect {
	typedef typename
		m::copy<
			SeqB ,
			m::inserter< m::set<>, m::insert< m::_1, m::_2 > >
		>::type set_seq2;
	typedef typename
		m::copy_if<
			SeqA,
			m::has_key< set_seq2, m::_1 >
		>::type type;
};

}
}
}

#endif


#include <boost/compute/algorithm/set_difference.hpp>