#include <ctime>
#include <mutex>
#include <atomic>
#include <memory>
#include <vector>
#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 <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>

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

#include <std_srvs/Empty.h>
#include <hdl_graph_slam/SaveMap.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 <amsi/gnss_tools.hpp>

namespace hdl_graph_slam {

class HdlGraphSlamNodelet : public nodelet::Nodelet {
public:
  typedef pcl::PointXYZI PointT;

  HdlGraphSlamNodelet() {
    preX = 0;
    preY = 0;
    initialLLH.resize(3,1);
    initialLLH(0) =  114.19289862;
    initialLLH(1) = 22.3181226456 ;
    initialLLH(2) = 0;

  }
  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);

    //
    graph_slam.reset(new GraphSLAM());
    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());

    gps_edge_stddev = private_nh.param<double>("gps_edge_stddev", 10000.0);
    floor_edge_stddev = private_nh.param<double>("floor_edge_stddev", 10.0);
    initialGNSSLon = private_nh.param<double>("initialGNSSLon", 114.19095395699307);
    initialGNSSLat = private_nh.param<double>("initialGNSSLat", 22.3216976234397251);
    last_gps_edge_stamp = ros::Time(0);

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

    if(private_nh.param<bool>("enable_gps", true)) {
      gps_sub = nh.subscribe("/gps/geopoint", 32, &HdlGraphSlamNodelet::gps_callback, this);
      nmea_sub = nh.subscribe("/nmea_sentence", 32, &HdlGraphSlamNodelet::nmea_callback, this);
    }

    // publishers
    markers_pub = nh.advertise<visualization_msgs::MarkerArray>("/hdl_graph_slam/markers", 16);
    odom2map_pub = 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);
    read_until_pub = nh.advertise<std_msgs::Header>("/hdl_graph_slam/read_until", 32);
    optimizedOdom = nh.advertise<nav_msgs::Odometry>("/optimizedOdom", 32); // the odom of the final
    map2odomTrans = nh.advertise<nav_msgs::Odometry>("/compsensateOdom", 32); // the odom of from /map to odom

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

    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.createTimer(ros::Duration(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 = odom_msg->header.stamp;
    Eigen::Isometry3d odom = odom2isometry(odom_msg);
    LiDAROdometry = *odom_msg;
    LiDAROdometryTime.push_back((double)LiDAROdometry.header.stamp.sec + double(LiDAROdometry.header.stamp.nsec)*1e-9);
    LiDAROdometryx.push_back(LiDAROdometry.pose.pose.position.x);
    LiDAROdometryy.push_back(LiDAROdometry.pose.pose.position.y);
    LiDAROdometryz.push_back(LiDAROdometry.pose.pose.position.z);

    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*cloud_msg, *cloud);

    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(30, 0);
        read_until.frame_id = "/velodyne_points";
        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
   * @return if true, at least one keyframe is 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.push_back(keyframe);

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

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

      // add edge between 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(prev_keyframe->cloud, keyframe->cloud, relative_pose);
      graph_slam->add_se3_edge(keyframe->node, prev_keyframe->node, relative_pose, information);
    }

    std_msgs::Header read_until;
    read_until.stamp = keyframe_queue[num_processed]->stamp + ros::Duration(30, 0);
    read_until.frame_id = "/velodyne_points";
    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;
  }

  void nmea_callback(const nmea_msgs::SentenceConstPtr& nmea_msg) {
    std::vector<std::string> str_vec_ptr;
    std::string token;
    std::stringstream ss(nmea_msg->sentence);
    bool find_SOL_COMPUTED =0;
    while (getline(ss, token, ' '))
    {
      if(token == "SOL_COMPUTED") // solutions are computed
      {
        // std::cout<<"message obtained"<<std::endl;
        find_SOL_COMPUTED = true;
      }
      if( find_SOL_COMPUTED ) // find flag SOL_COMPUTED
      {
        str_vec_ptr.push_back(token);
      }
    }
        if(find_SOL_COMPUTED)
    {
      sensor_msgs::NavSatFix navfix_ ;
      navfix_.header = nmea_msg->header;
      std::cout << std::setprecision(17);
      double lat = strtod((str_vec_ptr[2]).c_str(), NULL);
      double lon = strtod((str_vec_ptr[3]).c_str(), NULL);
      double alt = strtod((str_vec_ptr[4]).c_str(), NULL);
      std::cout << std::setprecision(17);

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

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

      // trans and rotation
      double prex_ = eigenENU(0);
      double prey_ = eigenENU(1);
      double theta = (68.5 )*( 3.141592 / 180.0 ); //
      eigenENU(0) = prex_ * cos(theta) - prey_ * sin(theta) ;
      eigenENU(1) = prex_ * sin(theta) + prey_ * cos(theta) ;

      geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped());
      gps_msg->header = nmea_msg->header;
      gps_msg->position.latitude = 1 * eigenENU(1);
      gps_msg->position.longitude = -1 * eigenENU(0);
      gps_msg->position.altitude = eigenENU(2);

      gps_callback(gps_msg);
      std::cout<<"push back message to gps_queue..."<<std::endl;
    }
  }

  /**
   * @brief received gps data is added to #gps_queue
   * @param gps_msg
   */
  void gps_callback(const geographic_msgs::GeoPointStampedConstPtr& gps_msg) {
    std::lock_guard<std::mutex> lock(gps_queue_mutex);
    gps_queue.push_back(gps_msg);
  }

  std::ofstream ofs;

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

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

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

    bool updated = false;
    auto seek = keyframes.begin();
    for(const auto& gps_msg : gps_queue) {
      seek = std::lower_bound(seek, keyframes.end(), gps_msg->header.stamp, [&](const KeyFrame::Ptr& key, const ros::Time& stamp) { return key->stamp < stamp; });
      if(seek == keyframes.end()) {
        break;
      }

      double  residual = ((*seek)->stamp - gps_msg->header.stamp).toSec(); // GPS-Keyframe and Gps frame time bias difference
      std::cout << "residual---------------------:" << residual<< std::endl;
      if(std::abs(residual) > 1.25 || (*seek)->utm_coord) { // 0.25
        continue;
      }

      if(gps_msg->header.stamp - last_gps_edge_stamp < ros::Duration(1.0)   ) { // constant update with GPS initially 30
        continue;
      }

      Eigen::MatrixXd enu_; // the enu for output
      enu_.resize(3, 1);
      double E_ = double(gps_msg->position.latitude); // this is the E in ENU coordinate system
      double N_ = double(gps_msg->position.longitude); // this is the N in ENU coordinate system
      double Covariance = double(gps_msg->position.altitude); // this is the positioning covariance of GNSS positioning
      enu_(0) = E_;
      enu_(1) = N_;
      enu_(2) = double(gps_msg->position.altitude);
      Eigen::Vector3d xyz(enu_(0), enu_(1), enu_(2)); // save the
      double prex_ = xyz(0);
      double prey_ = xyz(1);

      pt2GPS_ = xyz;
      (*seek)->utm_coord = xyz;

      Eigen::Matrix3d information_matrix = Eigen::Matrix3d::Identity() / (gps_edge_stddev ); // fixed : 13
      if(Covariance<3200000000) //  for open loop datasheet: set threshold 30            for closed loop datasheet: set threshold 40
      {
        graph_slam->add_se3_prior_xyz_edge((*seek)->node, xyz.head<3>(), information_matrix); // 2D position added into graph_slam
        std::cout << "xyz(0):" << xyz(0)<< std::endl;
        std::cout << "xyz(1):" << xyz(1)<< std::endl;
        std::cout << "xyz(2):" << xyz(2)<< std::endl;

      }

      last_gps_edge_stamp = gps_msg->header.stamp;

      updated = true;
    }

    auto remove_loc = std::upper_bound(gps_queue.begin(), gps_queue.end(), latest_keyframe_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;
  }

/*
author: WEN Weisong (17902061r@connect.polyu.hk)
function: llh to ecef
input: llh (Matrix3d)
output: ecef (Matrix3d)
*/
Eigen::MatrixXd llh2ecef(Eigen::MatrixXd data) // transform the llh to ecef
{
  Eigen::MatrixXd ecef; // the ecef for output
  ecef.resize(3, 1);
  double a = 6378137.0;
  double b = 6356752.314;
  double n, Rx, Ry, Rz;
  double lon = (double)data(0) * 3.1415926 / 180.0; // lon to radis
  double lat = (double)data(1) * 3.1415926 / 180.0; // lat to radis
  double alt = (double)data(2); // altitude
  n = a * a / sqrt(a * a * cos(lat) * cos(lat) + b * b * sin(lat) * sin(lat));
  Rx = (n + alt) * cos(lat) * cos(lon);
  Ry = (n + alt) * cos(lat) * sin(lon);
  Rz = (b * b / (a * a) * n + alt) * sin(lat);
  ecef(0) = Rx; // return value in ecef
  ecef(1) = Ry; // return value in ecef
  ecef(2) = Rz; // return value in ecef
  return ecef;

  /**************for test purpose*************************
  Eigen::MatrixXd llh;
  llh.resize(3, 1);
  Eigen::MatrixXd ecef;
  ecef.resize(3, 1);
  llh(0) = 114.1772621294604;
  llh(1) = 22.29842880200087;
  llh(2) = 58;
  ecef = llh2ecef(llh);
  cout << "ecef ->: " << ecef << "\n";
  */
}

/*
author: WEN Weisong (17902061r@connect.polyu.hk)
function: ecef to enu
input: original llh, and current ecef (Matrix3d)
output: enu (Matrix3d)
*/
Eigen::MatrixXd ecef2enu(Eigen::MatrixXd originllh, Eigen::MatrixXd ecef) // transform the ecef to enu
{
  double pi = 3.1415926; // pi
  double DEG2RAD = pi / 180.0;
  double RAD2DEG = 180.0 / pi;

  Eigen::MatrixXd enu; // the enu for output
  enu.resize(3, 1); // resize to 3X1
  Eigen::MatrixXd oxyz; // the original position
  oxyz.resize(3, 1); // resize to 3X1

  double x, y, z; // save the x y z in ecef
  x = ecef(0);
  y = ecef(1);
  z = ecef(2);

  double ox, oy, oz; // save original reference position in ecef
  oxyz = llh2ecef(originllh);
  ox = oxyz(0); // obtain x in ecef
  oy = oxyz(1); // obtain y in ecef
  oz = oxyz(2); // obtain z in ecef

  double dx, dy, dz;
  dx = x - ox;
  dy = y - oy;
  dz = z - oz;

  double lonDeg, latDeg, _; // save the origin lon alt in llh
  lonDeg = originllh(0);
  latDeg = originllh(1);
  double lon = lonDeg * DEG2RAD;
  double lat = latDeg * DEG2RAD;

  //save ENU
  enu(0) = -sin(lon) * dx + cos(lon) * dy;
  enu(1) = -sin(lat) * cos(lon) * dx - sin(lat) * sin(lon) * dy + cos(lat) * dz;
  enu(2) = cos(lat) * cos(lon) * dx + cos(lat) * sin(lon) * dy + sin(lat) * dz;
  return enu;

  /**************for test purpose*****suqare distance is about 37.4 meters********************
  Eigen::MatrixXd llh;  //original
  llh.resize(3, 1);
  llh(0) = 114.1775072541416;
  llh(1) = 22.29817969722738;
  llh(2) = 58;
  Eigen::MatrixXd ecef;
  ecef.resize(3, 1);
  ecef(0) = -2418080.9387265667;
  ecef(1) = 5386190.3905763263;
  ecef(2) = 2405041.9305451373;
  Eigen::MatrixXd enu;
  enu.resize(3, 1);
  enu = ecef2enu(llh, ecef);
  cout << "enu ->: " << enu << "\n";
  */
}

  /**
   * @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;
      }

      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);
      graph_slam->add_se3_plane_edge(keyframe->node, graph_slam->floor_plane_node, coeffs, information);

      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 a map point cloud and publish it
   * @param event
   */
  void map_points_publish_timer_callback(const ros::WallTimerEvent& event) {
    // if(!map_points_pub.getNumSubscribers()) {
    //   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::TimerEvent& event) {
    std::lock_guard<std::mutex> lock(main_thread_mutex);

    // add keyframes and floor coeffs in the queues to the pose graph
    if(!flush_keyframe_queue() & !flush_floor_queue() & !flush_gps_queue()) {
      std_msgs::Header read_until;
      read_until.stamp = event.current_real + ros::Duration(30, 0);
      read_until.frame_id = "/velodyne_points";
      read_until_pub.publish(read_until);
      read_until.frame_id = "/filtered_points";
      read_until_pub.publish(read_until);

      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);
      graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix);
    }

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

    // optimize the pose graph
    graph_slam->optimize();

    // 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();

    // publish pose of the final optimization
    nav_msgs::Odometry odom_;
    nav_msgs::Odometry comOdom;
    // odom_.header.stamp = event.current_real; //stamp
    odom_.header.stamp = keyframe->stamp; //stamp
    odom_.header.frame_id = "velodyne";

    Eigen::Vector3d pos = keyframe->node->estimate().translation(); // this is

    double graph_time = ((double)odom_.header.stamp.sec + double(odom_.header.stamp.nsec)*1e-9); // time stamp of the keyframe
    double bias =100000;
    int index_ =0;
    for(int index= 0; index<LiDAROdometryTime.size(); index++)
    {
      if(fabs (graph_time - LiDAROdometryTime[index]) <bias)
      {
        index_ = index;
        bias = fabs (graph_time - LiDAROdometryTime[index]);
      }

    }
    odom_.pose.pose.position.x = pos.x() - LiDAROdometryx[index_];
    odom_.pose.pose.position.y = pos.y() - LiDAROdometryy[index_];
    odom_.pose.pose.position.z = pos.z() - LiDAROdometryz[index_];
    std::cout << "time bias:" << fabs (graph_time - LiDAROdometryTime[index_])  << std::endl;

    odom_.child_frame_id = "map";
    odom_.twist.twist.linear.x = 0.0;
    odom_.twist.twist.linear.y = 0.0;
    odom_.twist.twist.angular.z = 0.0;

    optimizedOdom.publish(odom_);

    if(map_points_pub.getNumSubscribers()) {
      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);
      });

      std::lock_guard<std::mutex> lock(keyframes_snapshot_mutex);
      keyframes_snapshot.swap(snapshot);
    }

//     if(odom2map_pub.getNumSubscribers()) {
    if(1) {
      geometry_msgs::TransformStamped ts = matrix2transform(keyframe->stamp, trans.matrix().cast<float>(), map_frame_id, odom_frame_id);
      odom2map_pub.publish(ts);
      comOdom.pose.pose.position.x = ts.transform.translation.x;
      comOdom.pose.pose.position.y = ts.transform.translation.y;
      comOdom.pose.pose.position.z = ts.transform.translation.z;

      comOdom.child_frame_id = "map";
      comOdom.twist.twist.linear.x = 0.0;
      comOdom.twist.twist.linear.y = 0.0;
      comOdom.twist.twist.angular.z = 0.0;
      map2odomTrans.publish(comOdom);
    }

    if(markers_pub.getNumSubscribers()) {
      auto markers = create_marker_array(event.current_real);
      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 = 2; // initially 0.5

    traj_marker.points.resize(keyframes.size());
    traj_marker.colors.resize(keyframes.size());
    for(int i=0; i<keyframes.size(); i++) {  // Node
      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].r = 1.0;
      traj_marker.colors[i].g = 0.0;
      traj_marker.colors[i].b = 0.0;
      traj_marker.colors[i].a = 1.0;
    }

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

    edge_marker.pose.orientation.w = 1.0;
    edge_marker.scale.x = 0.9; // initially 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 = 0.0 ;
        edge_marker.colors[i*2].g = 0.0;
	edge_marker.colors[i*2].b = 1.0;
        edge_marker.colors[i*2].a = 1.0;
        edge_marker.colors[i*2 + 1].r = 0;
        edge_marker.colors[i*2 + 1].g = 0;
	edge_marker.colors[i*2 + 1].b = 1.0;
        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); // Plane
      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); // GPS
      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();
        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();
        // std::cout << "pt2(0)->:" << pt2(0)<< std::endl;
        // std::cout << "pt2(1)->:" << pt2(1)<< std::endl;

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

        continue;
      }
    }

    // sphere for loop closure
    // 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 = 0;
    // 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(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& res) {
    std::lock_guard<std::mutex> lock(main_thread_mutex);

    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:" << std::flush;
    system("pwd");

    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]->dump(sst.str());
    }

    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;
    }

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

    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::Timer 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::TimeSynchronizer<nav_msgs::Odometry, sensor_msgs::PointCloud2>> sync;

  ros::Subscriber gps_sub;
  ros::Subscriber nmea_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;

  ros::Publisher read_until_pub;
  ros::Publisher map_points_pub;
  ros::Publisher optimizedOdom;
  ros::Publisher map2odomTrans;

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

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

  // gps queue
  double gps_edge_stddev;
  double preX;
  double preY;
  Eigen::Vector3d pt2GPS_ ;
  boost::optional<Eigen::Vector3d> zero_utm;
  boost::optional<Eigen::Vector3d> zero_utm_;
  ros::Time last_gps_edge_stamp;
  std::mutex gps_queue_mutex;
  std::deque<geographic_msgs::GeoPointStampedConstPtr> gps_queue;

  double initialGNSSLon; // initial gps longitude
  double initialGNSSLat; // initial gps latitude
  Eigen::MatrixXd initialLLH; // the ecef for output
  nav_msgs::Odometry LiDAROdometry; // LiDAR odometry information
  std::vector <double> LiDAROdometryTime;
  std::vector <double> LiDAROdometryx;
  std::vector <double> LiDAROdometryy;
  std::vector <double> LiDAROdometryz;

  // gps sentence
  sensor_msgs::NavSatFix ini_navf ; // initial sensor msg
  // gnss_tools
  GNSS_Tools gnss_tools_;
  Eigen::MatrixXd originllh; // origin llh

  // 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
  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;

  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;
};

}

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


/* json_parsing_test.cc
   Jeremy Barnes, 21 February 2007
   Copyright (c) 2007 Jeremy Barnes.  All rights reserved.

   Test for the environment functions.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

#include "jml/utils/json_parsing.h"
#include "jml/utils/info.h"

#include <boost/test/unit_test.hpp>
#include <boost/test/auto_unit_test.hpp>
#include <math.h>

using namespace ML;

using boost::unit_test::test_suite;

void testUnsigned(const std::string & str, unsigned long long expected)
{
    Parse_Context context(str, str.c_str(), str.c_str() + str.size());
    auto val = expectJsonNumber(context);
    context.expect_eof();
    BOOST_CHECK_EQUAL(val.uns, expected);
    BOOST_CHECK_EQUAL(val.type, JsonNumber::UNSIGNED_INT);
}

void testSigned(const std::string & str, long long expected)
{
    Parse_Context context(str, str.c_str(), str.c_str() + str.size());
    auto val = expectJsonNumber(context);
    context.expect_eof();
    BOOST_CHECK_EQUAL(val.uns, expected);
    BOOST_CHECK_EQUAL(val.type, JsonNumber::SIGNED_INT);
}

void testFp(const std::string & str, double expected)
{
    Parse_Context context(str, str.c_str(), str.c_str() + str.size());
    auto val = expectJsonNumber(context);
    context.expect_eof();
    BOOST_CHECK_EQUAL(val.fp, expected);
    BOOST_CHECK_EQUAL(val.type, JsonNumber::FLOATING_POINT);
}

void testHex4(const std::string & str, long long expected)
{
    Parse_Context context(str, str.c_str(), str.c_str() + str.size());
    auto val = context.expect_hex4();
    context.expect_eof();
    BOOST_CHECK_EQUAL(val, expected);
}

BOOST_AUTO_TEST_CASE( test1 )
{
    testUnsigned("0", 0);
    testSigned("-0", 0);
    testSigned("-1", -1);
    testFp("0.", 0.0);
    testFp(".1", 0.1);
    testFp("-.1", -0.1);
    testFp("0.0", 0.0);
    testFp("1e0", 1e0);
    testFp("-1e0", -1e0);
    testFp("-1e+0", -1e+0);
    testFp("-1e-0", -1e-0);
    testFp("-1E+3", -1e+3);
    testFp("1.0E-3", 1.0E-3);
    testFp("Inf", INFINITY);
    testFp("-Inf", -INFINITY);

    testHex4("0026", 38);
    testHex4("001A", 26);

    JML_TRACE_EXCEPTIONS(false);
    BOOST_CHECK_THROW(testFp(".", 0.1), std::exception);
    BOOST_CHECK_THROW(testFp("", 0.1), std::exception);
    BOOST_CHECK_THROW(testFp("e3", 0.1), std::exception);
    BOOST_CHECK_THROW(testFp("3e", 0.1), std::exception);
    BOOST_CHECK_THROW(testFp("3.1aade", 0.1), std::exception);

    BOOST_CHECK_THROW(testHex4("002", 2), std::exception);
    BOOST_CHECK_THROW(testHex4("002G", 2), std::exception);
    BOOST_CHECK_THROW(testHex4("002.", 2), std::exception);
}


#include <gtest/gtest.h>
#include "headers/speed_constraints.h"
#include "headers/field_functions.h"
#include <boost/filesystem.hpp>
#include <chrono>

TEST(corners_10_robots_1, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 5);
  Robot r7(Position_t(8.0, 15.0), 6);
  Robot r8(Position_t(15.0, 8.0), 7);
  Robot r9(Position_t(2.5, 0.0), 8);
  Robot r10(Position_t(0.0, 2.5), 9);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);
  boost::add_vertex(r5, *rg);
  boost::add_vertex(r6, *rg);
  boost::add_vertex(r7, *rg);
  boost::add_vertex(r8, *rg);
  boost::add_vertex(r9, *rg);
  boost::add_vertex(r10, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 5; i < 31; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 4.0), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.0), *og);
  }
  for (int i = 31; i < 41; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.0), *og);
  }

  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(39 - i, 49 + i), 4.0), *og);
    boost::add_vertex(Obstacle(Position_t(59 - i, 49 + i), 4.0), *og);
  }
  for (int i = 0; i < 10; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(44 - i, 64 + i), 4.0), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("corners_10_robots_1");

  for (int i = 0; i < 2500; i++)
  {
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 49)
    {
      leaderV = Vector_t(-sqrt(2), sqrt(2));
    }
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 74)
    {
      leaderV = Vector_t(0, 1);
    }

    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printWidePlotWithArrows("corners_10_robots_1/corners_10_robots_1_" + std::to_string(i) + ".png",
                              "corners_10_robots_1", 0, 90, 1, vg.getRobotGraph(), std::function(&overallPotential),
                              vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(corners_10_robots_2, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 5);
  Robot r7(Position_t(8.0, 15.0), 6);
  Robot r8(Position_t(15.0, 8.0), 7);
  Robot r9(Position_t(2.5, 0.0), 8);
  Robot r10(Position_t(0.0, 2.5), 9);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);
  boost::add_vertex(r5, *rg);
  boost::add_vertex(r6, *rg);
  boost::add_vertex(r7, *rg);
  boost::add_vertex(r8, *rg);
  boost::add_vertex(r9, *rg);
  boost::add_vertex(r10, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 5; i < 41; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 4.0), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.0), *og);
  }
  for (int i = 41; i < 51; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.0), *og);
  }
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(49 - i, 59 + i), 4.0), *og);
    boost::add_vertex(Obstacle(Position_t(69 - i, 59 + i), 4.0), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("corners_10_robots_2");

  for (int i = 0; i < 2500; i++)
  {
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 59)
    {
      leaderV = Vector_t(-sqrt(2), sqrt(2));
    }
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 74)
    {
      leaderV = Vector_t(0, 1);
    }

    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printWidePlotWithArrows("corners_10_robots_2/corners_10_robots_2_" + std::to_string(i) + ".png",
                              "corners_10_robots_2", 0, 90, 1, vg.getRobotGraph(), std::function(&overallPotential),
                              vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(corners_10_robots_3, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 5);
  Robot r7(Position_t(8.0, 15.0), 6);
  Robot r8(Position_t(15.0, 8.0), 7);
  Robot r9(Position_t(2.5, 0.0), 8);
  Robot r10(Position_t(0.0, 2.5), 9);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);
  boost::add_vertex(r5, *rg);
  boost::add_vertex(r6, *rg);
  boost::add_vertex(r7, *rg);
  boost::add_vertex(r8, *rg);
  boost::add_vertex(r9, *rg);
  boost::add_vertex(r10, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 5; i < 41; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 4.0), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.0), *og);
  }
  for (int i = 41; i < 51; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.0), *og);
  }
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(49 - i, 59 + i), 4.0), *og);
    boost::add_vertex(Obstacle(Position_t(69 - i, 59 + i), 4.0), *og);
  }
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(34 + i, 74 + i), 4.0), *og);
    boost::add_vertex(Obstacle(Position_t(54 + i, 74 + i), 4.0), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("corners_10_robots_3");

  for (int i = 0; i < 2500; i++)
  {
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 59)
    {
      leaderV = Vector_t(-sqrt(2), sqrt(2));
    }

    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 74)
    {
      leaderV = Vector_t(sqrt(2), sqrt(2));
    }

    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printWidePlotWithArrows("corners_10_robots_3/corners_10_robots_3_" + std::to_string(i) + ".png",
                              "corners_10_robots_3", 0, 90, 1, vg.getRobotGraph(), std::function(&overallPotential),
                              vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(corners_10_robots_4, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 5);
  Robot r7(Position_t(8.0, 15.0), 6);
  Robot r8(Position_t(15.0, 8.0), 7);
  Robot r9(Position_t(2.5, 0.0), 8);
  Robot r10(Position_t(0.0, 2.5), 9);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);
  boost::add_vertex(r5, *rg);
  boost::add_vertex(r6, *rg);
  boost::add_vertex(r7, *rg);
  boost::add_vertex(r8, *rg);
  boost::add_vertex(r9, *rg);
  boost::add_vertex(r10, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 5; i < 41; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 4.0), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.0), *og);
  }
  for (int i = 41; i < 51; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.0), *og);
  }
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(49 - i, 59 + i), 4.0), *og);
    boost::add_vertex(Obstacle(Position_t(69 - i, 59 + i), 4.0), *og);
  }
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(34, 74 + i), 4.0), *og);
  }
  for (int i = 0; i < 30; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(34 + i * sqrt(2), 89), 4.0), *og);
    boost::add_vertex(Obstacle(Position_t(54 + i * sqrt(2), 74), 4.0), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("corners_10_robots_4");

  for (int i = 0; i < 2500; i++)
  {
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 59)
    {
      leaderV = Vector_t(-sqrt(2), sqrt(2));
    }

    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 74)
    {
      leaderV = Vector_t(0, 1);
    }

    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 81)
    {
      leaderV = Vector_t(1, 0);
    }

    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printWidePlotWithArrows("corners_10_robots_4/corners_10_robots_4_" + std::to_string(i) + ".png",
                              "corners_10_robots_4", 0, 90, 1, vg.getRobotGraph(), std::function(&overallPotential),
                              vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(corners_10_robots_5, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 5);
  Robot r7(Position_t(8.0, 15.0), 6);
  Robot r8(Position_t(15.0, 8.0), 7);
  Robot r9(Position_t(2.5, 0.0), 8);
  Robot r10(Position_t(0.0, 2.5), 9);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);
  boost::add_vertex(r5, *rg);
  boost::add_vertex(r6, *rg);
  boost::add_vertex(r7, *rg);
  boost::add_vertex(r8, *rg);
  boost::add_vertex(r9, *rg);
  boost::add_vertex(r10, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 5; i < 41; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 3.0), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 3.0), *og);
  }
  for (int i = 41; i < 51; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 3.0), *og);
  }
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(49 - i, 59 + i), 3.0), *og);
    boost::add_vertex(Obstacle(Position_t(69 - i, 59 + i), 3.0), *og);
  }
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(34, 74 + i), 5.0), *og);
  }
  for (int i = 0; i < 30; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(34 + i * sqrt(2), 89), 5.0), *og);
    boost::add_vertex(Obstacle(Position_t(54 + i * sqrt(2), 74), 5.0), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("corners_10_robots_5");

  for (int i = 0; i < 2300; i++)
  {
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 59)
    {
      leaderV = Vector_t(-sqrt(2), sqrt(2));
    }

    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 74)
    {
      leaderV = Vector_t(0, 1);
    }

    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 81)
    {
      leaderV = Vector_t(1, 0);
    }

    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printWidePlotWithArrows("corners_10_robots_5/corners_10_robots_5_" + std::to_string(i) + ".png",
                              "corners_10_robots_5", 0, 90, 1, vg.getRobotGraph(), std::function(&overallPotential),
                              vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#ifndef LBP_VARLBP_HPP
#define LBP_VARLBP_HPP

#include <lbp/defs.hpp>
#include <lbp/detail/neighborhoods.hpp>
#include <lbp/detail/sampling.hpp>

#include <opencv2/core.hpp>

#include <boost/hana/fold.hpp>
#include <boost/hana/size.hpp>
#include <boost/hana/tuple.hpp>

//
// @inproceedings{Ojala:2001:GLB:646260.685274,
//  author = {Ojala, Timo and Pietik\"{a}inen, Matti and M\"{a}enp\"{a}\"{a}, Topi},
//  title = {A Generalized Local Binary Pattern Operator for Multiresolution Gray Scale and Rotation Invariant Texture Classification},
//  booktitle = {Proceedings of the Second International Conference on Advances in Pattern Recognition},
//  series = {ICAPR '01},
//  year = {2001},
//  isbn = {3-540-41767-2},
//  pages = {397--406},
//  numpages = {10},
//  url = {http://dl.acm.org/citation.cfm?id=646260.685274},
//  acmid = {685274},
//  publisher = {Springer-Verlag},
//  address = {London, UK, UK},
// }
//

namespace lbp {
namespace varlbp_detail {

const auto varlbp = [](auto N, auto S) {
    const auto n = boost::hana::size (N).value - 1;

    return [=](const cv::Mat& src, size_t i, size_t j) {
        using namespace hana::literals;

        double d = 0., m = 0.;

        const auto s = boost::hana::fold_left (
            N, 0, [&, k = 0](auto accum, auto c) mutable {
                const auto x = S (src, i + c [0_c], j + c [1_c]);

                d = x - m;
                m += d / ++k;

                return accum + d * (x - m);
            });

        return s / n;
    };
};

} // namespace varlbp_detail

template< typename T, size_t R, size_t P >
auto varlbp = [](const cv::Mat& src) {
    LBP_ASSERT (CV_32FC1 == src.type () || CV_64FC1 == src.type ());

    cv::Mat dst (src.size (), src.type (), cv::Scalar (0));

    auto op = varlbp_detail::varlbp (
        detail::circular_neighborhood< R, P >,
        detail::bilinear_sampler< T >);

#pragma omp parallel for
    for (size_t i = R; i < src.rows - R - 1; ++i) {
        for (size_t j = R; j < src.cols - R - 1; ++j) {
            dst.at< T > (i, j) = op (src, i, j);
        }
    }

    return dst;
};

} // namespace lbp

#endif // LBP_VARLBP_HPP


#include "dSFMTsearch.hpp"
#include "AlgorithmDSFMTEquidistribution.hpp"
#include <MTToolBox/AlgorithmEquidistribution.hpp>
#include <MTToolBox/AlgorithmReducibleRecursionSearch.hpp>
#include <MTToolBox/period.hpp>
#include <NTL/GF2X.h>
#include <errno.h>
#include <stdlib.h>
#include <getopt.h>
#include "Annihilate.h"

using namespace MTToolBox;
using namespace std;

class options {
public:
    bool verbose;
    bool reverse;
    uint64_t seed;
    dSFMT_param params;
};

static bool parse_opt(options& opt, int argc, char **argv);
static void output_help(string& pgm);

int main(int argc, char * argv[])
{
    options opt;
    if (!parse_opt(opt, argc, argv)) {
        return -1;
    }
    dSFMT sf(opt.params);
    //cout << sf.getParamString() << endl;
    //w128_t wseed = {{0x1234, 0x5678, 0x9abc, 0xdef0}};
    //w128_t wseed = {{0x12345, 0x56789, 0x9abcd, 0xdef01}};
    w128_t wseed;
    wseed.u64[0] = opt.seed;
    sf.seed(wseed);
#if defined(DEBUG)
    sf.d_p();
    cout << endl;
    for (int w = 2; w >= 1; w--) {
        cout << "weight = " << dec << w << endl;
        sf.setWeightMode(w);
        for (int i = 0; i < 10; i++) {
            w128_t tmp = sf.generate();
            cout << hex << setw(16) << setfill('0') << tmp.u64[0] << " ";
            cout << hex << setw(16) << setfill('0') << tmp.u64[1] << endl;
        }
        cout << endl;
        sf.seed(wseed);
    }
    sf.d_p();
#endif
    if (!anni(sf)) {
        return -1;
    }
    int delta52 = 0;
    int veq52[52];
    DSFMTInfo info;
    info.bitSize = 128; // IMPORTANT
    info.elementNo = 2;
    delta52 = calc_dSFMT_equidistribution<w128_t, dSFMT>(sf, veq52, 52, info,
                                                         opt.params.mexp);
    cout << sf.getParamString();
    cout << dec << delta52 << endl;
    if (opt.verbose) {
        cout << "52bit dimension of equidistribution at v-bit accuracy k(v)"
             << endl;
        for (int j = 0; j < 52; j++) {
            cout << "k(" << dec << (j + 1) << ") = " << dec << veq52[j];
            cout << "\td(" << dec << (j + 1) << ") = " << dec
                 << (opt.params.mexp / (j + 1) - veq52[j]) << endl;
        }
    }
    return 0;
}

#if 0
static void printBinary(FILE *fp, GF2X& poly)
{
    int i;
    if (deg(poly) < 0) {
        fprintf(fp, "0deg=-1\n");
        return;
    }
    for(i = 0; i <= deg(poly); i++) {
        if(rep(coeff(poly, i)) == 1) {
            fprintf(fp, "1");
        } else {
            fprintf(fp, "0");
        }
        if ((i % 32) == 31) {
            fprintf(fp, "\n");
        }
    }
    fprintf(fp, "deg=%ld\n", deg(poly));
}
#endif
/**
 * command line option parser
 * @param opt a structure to keep the result of parsing
 * @param argc number of command line arguments
 * @param argv command line arguments
 * @param start default start value
 * @return command line options have error, or not
 */
static bool parse_opt(options& opt, int argc, char **argv) {
    opt.verbose = false;
    opt.seed = (uint64_t)clock();
    int c;
    bool error = false;
    string pgm = argv[0];
    static struct option longopts[] = {
        {"verbose", no_argument, NULL, 'v'},
        {"seed", required_argument, NULL, 's'},
        {NULL, 0, NULL, 0}};
    errno = 0;
    for (;;) {
        c = getopt_long(argc, argv, "vs:", longopts, NULL);
        if (error) {
            break;
        }
        if (c == -1) {
            break;
        }
        switch (c) {
        case 's':
            opt.seed = strtoull(optarg, NULL, 0);
            if (errno) {
                error = true;
                cerr << "seed must be a number" << endl;
            }
            break;
        case 'v':
            opt.verbose = true;
            break;
        case '?':
        default:
            error = true;
            break;
        }
    }
    argc -= optind;
    argv += optind;
    if (argc < 1) {
        error = true;
    } else {
        char * para = argv[0];
        opt.params.mexp = strtoul(para, &para, 10);
        para++;
        opt.params.pos1 = strtoul(para, &para, 10);
        para++;
        opt.params.sl1 = strtoul(para, &para, 10);
        para++;
        opt.params.msk1 = strtoull(para, &para, 16);
        para++;
        opt.params.msk2 = strtoull(para, &para, 16);
        para++;
        opt.params.fix1 = strtoull(para, &para, 16);
        para++;
        opt.params.fix2 = strtoull(para, &para, 16);
        para++;
        opt.params.parity1 = strtoull(para, &para, 16);
        para++;
        opt.params.parity2 = strtoull(para, &para, 16);
    }
    if (error) {
        output_help(pgm);
        return false;
    }
    return true;
}
/**
 * showing help message
 * @param pgm program name
 */
static void output_help(string& pgm)
{
    cerr << "usage:" << endl;
    cerr << pgm
         << " [-v] [-r]"
         << " mexp,pos1,sl1,msk1,msk2,fix1,fix2,"
         << "parity1,parity2"
         << endl;
    static string help_string1 = "\n"
"--verbose, -v        Verbose mode. Output detailed information.\n"
        ;
    cerr << help_string1 << endl;
}


#include <boost/numeric/conversion/converter.hpp>


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, AASS Research Center, Orebro University.
 *  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 Willow Garage, Inc. 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 OWNER 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.
 *
 */
#ifndef CEVENT_COUNTER_DATA_H
#define CEVENT_COUNTER_DATA_H
#include<cmath>
#include <iostream>
#include <cstdio>
#include <Eigen/Dense>
#include <inttypes.h>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
///Defines for presets
#define EVENTMAP_OCCU  255
#define EVENTMAP_UNKNOWN  127
#define EVENTMAP_FREE  0
#define EVENTMAP_NUMBER_OF_EVENTS_STORED 128

///Recency filtering stuff
#define EVENTMAP_USE_RECENCY_FILTERING
#define EVENTMAP_OBSERVATION_LIMIT 30000.0
#define Wforget (5000.0/5001.0)

/**
* Structure in the map
**/
struct TEventData
{
  uint8_t 	occval; 			///<Occupancy value
  float 		a_exit_event; ///Number of exit events ( OCC 2 EMP)
  float  		b_exit_event; ///Number of times the cell is perceived as occupied

  float 		a_entry_event;
  float  		b_entry_event; ///Number of times the cell is perceived as empty
  uint64_t  events;        /// A storage for the last 64 events (as bits)

  TEventData():
    occval(127),
    a_exit_event(1.0),
    b_exit_event(1.0),
    a_entry_event(1.0),
    b_entry_event(1.0),
    events(0)
  {
  };

  ~TEventData()
  {
  }

  const TEventData &operator =(const TEventData &copy)
  {
    occval = copy.occval;
    a_exit_event = copy.a_exit_event;
    b_exit_event = copy.b_exit_event;
    a_entry_event = copy.a_entry_event;
    b_entry_event = copy.b_entry_event;
    events = copy.events;

    return *this;
  }

  TEventData(const TEventData& copy):
    occval(copy.occval),
    a_exit_event(copy.a_exit_event),
    b_exit_event(copy.b_exit_event),
    a_entry_event(copy.a_entry_event),
    b_entry_event(copy.b_entry_event),
    events( copy.events)
  {
  }

  void Reset()
  {
    occval = 127;
    a_exit_event = 1;
    b_exit_event = 1;
    a_entry_event = 1;
    b_entry_event = 1;
  }

  bool getBit(int ind)
  {
    if((events & (1<<ind))>0) return true;
    return false;
  }

  float computeShortTermOccupancy()
  {
    static float OCC_LOG_PROP = log(0.6/0.4);
    static float EMP_LOG_PROP = log(0.3/0.7);
    uint bs=0;

    if( (b_entry_event+b_exit_event-2) > 63) bs=64;
    else bs = b_entry_event+b_exit_event-2;
    uint cnt=0;
    float log_ogg_prob  = 0 ;
    for(uint i = 0; i<bs; i++)
    {
      if(getBit(i))
      {
        log_ogg_prob += OCC_LOG_PROP;
        cnt++;
      }
      else
      {
        log_ogg_prob += EMP_LOG_PROP;
      }
    }
    return (1.0- 1.0/(1.0+exp(log_ogg_prob)));
  }

  int getObservations()
  {
    return (b_entry_event+b_exit_event);
  }

  /**
    * Updates a new measurement
    */
  void updateSimple(uint8_t meas)
  {
    events = events<<1;
    if(meas > EVENTMAP_UNKNOWN)  ///OCC
    {
      events |= 1;
    }
    if(occval > EVENTMAP_UNKNOWN)  ///cell is considered Occupied
    {
      updateExitEvent(meas, 1.0);
    }
    else if(occval < EVENTMAP_UNKNOWN)
    {
      updateEntryEvent(meas,1.0);
    }
    else
    {
      occval = meas; ///Just update
    }
  }

  /**
    * Updates a new measurement
    */
  void updateSimple(uint8_t meas, float w)
  {
    events = events<<1;
    if(meas > EVENTMAP_UNKNOWN)  ///OCC
    {
      events |= 1;
    }
    if(occval > EVENTMAP_UNKNOWN)  ///cell is considered Occupied
    {
      updateExitEvent(meas, w);
    }
    else if(occval < EVENTMAP_UNKNOWN)
    {
      updateEntryEvent(meas,w);
    }
    else
    {
      occval = meas; ///Just update
    }
#ifdef EVENTMAP_USE_RECENCY_FILTERING
    performRecencyFiltering();
#endif
  }

  void performRecencyFiltering()
  {
    if(occval > EVENTMAP_UNKNOWN)  ///We are in Exit state
    {
      if(b_exit_event>EVENTMAP_OBSERVATION_LIMIT)
      {
        float w = EVENTMAP_OBSERVATION_LIMIT / b_exit_event;
        b_exit_event *= w;
        a_exit_event *= w;
      }
      a_entry_event = 1.0 + (a_entry_event-1.0) * Wforget;
      b_entry_event = 1.0 + (b_entry_event-1.0) * Wforget;
    }
    else
    {
      if(b_entry_event>EVENTMAP_OBSERVATION_LIMIT)
      {
        float w = EVENTMAP_OBSERVATION_LIMIT / b_entry_event;
        b_entry_event *= w;
        a_entry_event *= w;
      }
      a_exit_event = 1.0 + (a_exit_event-1.0) * Wforget;
      b_exit_event = 1.0 + (b_exit_event-1.0) * Wforget;
    }
  }

  /**
    * Update Exit event (cell is OCC)
    */
  void updateExitEvent(uint8_t meas, float w)
  {
    if(meas>EVENTMAP_UNKNOWN)  ///OCC
    {
      b_exit_event+=1.0;
    }
    else
    {
      if(w>1.0) fprintf(stderr,"EXIT W larger that one = %f\n ",w);
      a_exit_event+=w;
      b_exit_event+=1.0;
      occval = EVENTMAP_FREE;
    }
  }
  /**
    * Update Entry event (Cell is Free)
    */
  void updateEntryEvent(uint8_t meas, float w)
  {
    if(meas>EVENTMAP_UNKNOWN)  ///OCC
    {
      b_entry_event+=1.0;
      if(w>1.0) fprintf(stderr,"W larger that one = %f\n ",w);
      a_entry_event+=w;
      occval = EVENTMAP_OCCU;
    }
    else
    {
      b_entry_event+=1.0;
    }
  }

  ///Expected number of observations/event
  int getEntryN()
  {
    return (int)((float)b_entry_event/(float)a_entry_event +0.5);
  }
  ///Expected number of observations/event
  int getExitN()
  {
    return (int)((float)b_exit_event/(float)a_exit_event +0.5);
  }

  double entryL()
  {
    if(b_entry_event == 0) fprintf(stderr,"B Zero, which is not possible!!\n");
    return ((double)a_entry_event/(double)b_entry_event );
  }
  double exitL()
  {
    if(b_exit_event == 0) fprintf(stderr,"B Zero, which is not possible!!\n");
    return ((double)a_exit_event/(double)b_exit_event);
  }
  double L()
  {
    if(b_exit_event == 0) fprintf(stderr,"B Zero, which is not possible!!\n");
    return ((double)(a_exit_event+a_entry_event)/(double)(b_exit_event+b_entry_event));
  }

  double fac(int n)
  {
    double t=1;
    for (int i=n; i>1; i--)
      t*=i;
    return t;
  }

  double Bin(int n,double p,int r)
  {
    return fac(n)/(fac(n-r)*fac(r))*pow(p,r)*pow(1-p,n-r);
  }

  float binaryBayesUpdate(float Pold, float P)
  {
    return (( Pold*P) / ( Pold*P + (1.0f-Pold)*(1.0f-P))); 	///< Updated probability
  }

  /**
    * To prevent numerical difficulties
    */
  void normalizeProb(float &p)
  {
    if(p>0.999) p = 0.999;
    if(p<0.001) p = 0.001;

  }
  /////////////////////////////////////////////////////////////////////////////////////////////77
  /// Compute different probabilities for the cell
  /////////////////////////////////////////////////////////////////////////////////////////////77
  /////////////////////////////////////////////////////////////////////////////////////////////77
  /**
    * Compute the probability of this cell being Static occupied.
    */
  float getOccStaticLikelihood()
  {
    if( (b_entry_event + b_exit_event)<20.0 )
    {
      return 0.5f;
    }
    Eigen::Matrix2f P;
    Eigen::Vector2f u1(0.5, 0.5);
    Eigen::Vector2f P0;
    float Lex = exitL();
    float Len = entryL();
    P(0,0) = (1.0-Len);
    P(0,1) =  Len;
    P(1,0)	 = Lex;
    P(1,1) = (1-Lex);

    P0 = u1.transpose() * P;
    return (P0(1));
  }

  /**
    * Returns the probability of static empty
    */
  float getFreeStaticLikelihood()
  {
    if( (b_entry_event + b_exit_event)<20.0 )
    {
      return 0.5f;
    }
    Eigen::Matrix2f P;
    Eigen::Vector2f P0;
    Eigen::Vector2f u1(0.5, 0.5);
    float Lex = exitL();
    float Len = entryL();
    P(0,0) = (1.0-Len);
    P(0,1) =  Len;
    P(1,0)	 = Lex;
    P(1,1) = (1-Lex);

    P0 = u1.transpose() * P;
    return (P0(0));
  }

  /**
    * Returns the probability of the cell having dynamics after 2^N
    * Observations. The larger the N, and higher the value, the more "Semi-static"
    * is the cell. Note that the filter returns
    * 0.5 >= P <=1.0 and P = 0.0 only if there is not enough evidence about the
    * Behaviour
    */
  float computeSemiStaticLikelihood(int N)
  {
    if( (b_entry_event + b_exit_event)<20.0 )
    {
      return 0.0f;
    }

    Eigen::Matrix2f P;
    Eigen::Vector2f u2(0, 1.0);
    Eigen::Vector2f u3(1.0, 0);
    float Lex = exitL();
    float Len = entryL();
    Eigen::Vector2f P2,P3;
    P(0,0) = (1.0-Len);
    P(0,1) =  Len;
    P(1,0)	 = Lex;
    P(1,1) = (1-Lex);
    for(int i=0; i<N; i++) P = P*P;
    P2 = u2.transpose() * P;
    P3 = u3.transpose() * P;

    float Po = P2(1);
    float Pu = binaryBayesUpdate(Po, P3(0));
    normalizeProb(Pu);

    return Pu;
  }

  float getOccupancyNow()
  {
    if( (b_entry_event + b_exit_event)<50.0)
    {
      return 0.5f;
    }
    float Po = 0;

    Po = computeShortTermOccupancy();
    float Lex = exitL();
    float Len = entryL();
    Eigen::Matrix2f P;
    P(0,0) = (1.0-Len);
    P(0,1) =  Len;
    P(1,0)	 = Lex;
    P(1,1) = (1-Lex);
    Eigen::Vector2f u1(1.0-Po, Po);
    Eigen::Vector2f u = u1.transpose() *P;
    return u(1);
  }

  /////////////////////////////////////////////////////////////////////////////////////////////77
  /////////////////////////////////////////////////////////////////////////////////////////////77
  ///Predict
  /////////////////////////////////////////////////////////////////////////////////////////////77
  /////////////////////////////////////////////////////////////////////////////////////////////77

  float predictOccupancy(int N)
  {
    if( (b_entry_event + b_exit_event)<20.0)
    {
      return 0.5f;
    }
    ///Testing
    float ps = getOccStaticLikelihood();
    if(ps>0.6) return 0.9;
    ///end testing

    float Po = computeShortTermOccupancy();
    float Lex = exitL();
    float Len = entryL();
    Eigen::Matrix2f P;
    P(0,0) = (1.0-Len);
    P(0,1) =  Len;
    P(1,0)	 = Lex;
    P(1,1) = (1-Lex);
    Eigen::Vector2f u1(1.0-Po, Po);
    for(int i=0; i<N; i++) P = P*P;
    Eigen::Vector2f u = u1.transpose() *P;

    return u(1);
  }

};
namespace boost {
namespace serialization {

template<class Archive>
void serialize(Archive & ar, TEventData & g, const unsigned int version)
{
  ar & 	g.occval; 			///<Occupancy value
  ar &  g.a_exit_event; ///Number of exit events ( OCC 2 EMP)
  ar &  g.b_exit_event; ///Number of times the cell is perceived as occupied
  ar & 	g.a_entry_event;
  ar & 	g.b_entry_event; ///Number of times the cell is perceived as empty
  ar &  g.events;
}

} // namespace serialization
} // namespace boost
#endif


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::int64_t, 2, 2> test_long_2_2(const Eigen::Matrix<boost::int64_t, 2, 2>& M) { return M; }
void export_long_2_2() { boost::python::def("test_long_2_2", test_long_2_2); }


//
// Copyright (c) 2015-2020 CNRS INRIA
//

/*
 * Unittest of the RNE algorithm. The code simply test that the algorithm does
 * not cause any serious errors. The numerical values are not cross validated
 * in any way.
 *
 */

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"

#include <iostream>

//#define __SSE3__
#include <fenv.h>

#ifdef __SSE3__
#include <pmmintrin.h>
#endif

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_rnea )
{
  #ifdef __SSE3__
    _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
  #endif
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);

  pinocchio::Data data(model);
  data.v[0] = Motion::Zero();
  data.a[0] = -model.gravity;

  VectorXd q = randomConfiguration(model);
  VectorXd v = VectorXd::Random(model.nv);
  VectorXd a = VectorXd::Random(model.nv);

  #ifdef NDEBUG
    const size_t NBT = 10000;
  #else
    const size_t NBT = 1;
    std::cout << "(the time score in debug mode is not relevant)  " ;
  #endif

  PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
  SMOOTH(NBT)
    {
      rnea(model,data,q,v,a);
    }
  timer.toc(std::cout,NBT);

}

BOOST_AUTO_TEST_CASE ( test_nle_vs_rnea )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);

  pinocchio::Data data_nle(model);
  pinocchio::Data data_rnea(model);

  VectorXd q = randomConfiguration(model);
  VectorXd v = VectorXd::Random(model.nv);

  VectorXd tau_nle (VectorXd::Zero (model.nv));
  VectorXd tau_rnea (VectorXd::Zero (model.nv));

  // -------
  q.tail(model.nq-7).setZero();
  v.setZero();

  tau_nle = nonLinearEffects(model,data_nle,q,v);
  tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));

  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));

  // -------
  q.tail(model.nq-7).setZero();
  v.setOnes();

  tau_nle = nonLinearEffects(model,data_nle,q,v);
  tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));

  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));

  // -------
  q.tail(model.nq-7).setOnes();
  v.setOnes();

  tau_nle = nonLinearEffects(model,data_nle,q,v);
  tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));

  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));

  // -------
  q = randomConfiguration(model);
  v.setRandom();

  tau_nle = nonLinearEffects(model,data_nle,q,v);
  tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));

  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
}

BOOST_AUTO_TEST_CASE (test_rnea_with_fext)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);

  Data data_rnea_fext(model);
  Data data_rnea(model);

  VectorXd q = randomConfiguration(model);

  VectorXd v (VectorXd::Random(model.nv));
  VectorXd a (VectorXd::Random(model.nv));

  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext(model.joints.size(), Force::Zero());

  JointIndex rf = model.getJointId("rleg6_joint"); Force Frf = Force::Random();
  fext[rf] = Frf;
  JointIndex lf = model.getJointId("lleg6_joint"); Force Flf = Force::Random();
  fext[lf] = Flf;

  rnea(model,data_rnea,q,v,a);
  VectorXd tau_ref(data_rnea.tau);
  Data::Matrix6x Jrf(Data::Matrix6x::Zero(6,model.nv));
  computeJointJacobian(model,data_rnea,q,rf,Jrf);
  tau_ref -= Jrf.transpose() * Frf.toVector();

  Data::Matrix6x Jlf(Data::Matrix6x::Zero(6,model.nv));
  computeJointJacobian(model,data_rnea,q,lf,Jlf);
  tau_ref -= Jlf.transpose() * Flf.toVector();

  rnea(model,data_rnea_fext,q,v,a,fext);

  BOOST_CHECK(tau_ref.isApprox(data_rnea_fext.tau));
}

BOOST_AUTO_TEST_CASE(test_compute_gravity)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);

  Data data_rnea(model);
  Data data(model);

  VectorXd q = randomConfiguration(model);

  rnea(model,data_rnea,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv));
  computeGeneralizedGravity(model,data,q);

  BOOST_CHECK(data_rnea.tau.isApprox(data.g));

  // Compare with Jcom
  crba(model,data_rnea,q);
  Data::Matrix3x Jcom = getJacobianComFromCrba(model,data_rnea);

  VectorXd g_ref(-data_rnea.mass[0]*Jcom.transpose()*Model::gravity981);

  BOOST_CHECK(g_ref.isApprox(data.g));
}

BOOST_AUTO_TEST_CASE(test_compute_static_torque)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);

  Data data_rnea(model);
  Data data(model);

  VectorXd q = randomConfiguration(model);

  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
  ForceVector fext((size_t)model.njoints);
  for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
    (*it).setRandom();

  rnea(model,data_rnea,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv),fext);
  computeStaticTorque(model,data,q,fext);

  BOOST_CHECK(data_rnea.tau.isApprox(data.tau));

  // Compare with Jcom + Jacobian of joint
  crba(model,data_rnea,q);
  Data::Matrix3x Jcom = getJacobianComFromCrba(model,data_rnea);

  VectorXd static_torque_ref = -data_rnea.mass[0]*Jcom.transpose()*Model::gravity981;
  computeJointJacobians(model,data_rnea,q);

  Data::Matrix6x J_local(6,model.nv);
  for(JointIndex joint_id = 1; joint_id < (JointIndex)(model.njoints); ++joint_id)
  {
    J_local.setZero();
    getJointJacobian(model, data_rnea, joint_id, LOCAL, J_local);
    static_torque_ref -= J_local.transpose() * fext[joint_id].toVector();
  }

  BOOST_CHECK(static_torque_ref.isApprox(data.tau));
}

BOOST_AUTO_TEST_CASE(test_compute_coriolis)
{
  using namespace Eigen;
  using namespace pinocchio;

  const double prec = Eigen::NumTraits<double>::dummy_precision();

  Model model;
  buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);

  Data data_ref(model);
  Data data(model);

  VectorXd q = randomConfiguration(model);

  VectorXd v (VectorXd::Random(model.nv));
  computeCoriolisMatrix(model,data,q,Eigen::VectorXd::Zero(model.nv));
  BOOST_CHECK(data.C.isZero(prec));

  model.gravity.setZero();
  rnea(model,data_ref,q,v,VectorXd::Zero(model.nv));
  computeJointJacobiansTimeVariation(model,data_ref,q,v);
  computeCoriolisMatrix(model,data,q,v);

  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
  BOOST_CHECK(data.J.isApprox(data_ref.J));

  VectorXd tau = data.C * v;
  BOOST_CHECK(tau.isApprox(data_ref.tau,prec));

  dccrba(model,data_ref,q,v);
  crba(model,data_ref,q);

  const Data::Vector3 & com = data_ref.com[0];
  Motion vcom(data_ref.vcom[0],Data::Vector3::Zero());
  SE3 cM1(data.oMi[1]); cM1.translation() -= com;

  BOOST_CHECK((cM1.toDualActionMatrix()*data_ref.M.topRows<6>()).isApprox(data_ref.Ag,prec));

  Force dh_ref = cM1.act(Force(data_ref.tau.head<6>()));
  Force dh(data_ref.dAg * v);
  BOOST_CHECK(dh.isApprox(dh_ref,prec));

  {
    Data data_ref(model), data_ref_plus(model);
    Eigen::MatrixXd dM(data.C + data.C.transpose());

    const double alpha = 1e-8;
    Eigen::VectorXd q_plus(model.nq);
    q_plus = integrate(model,q,alpha*v);

    crba(model,data_ref,q);
    data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
    crba(model,data_ref_plus,q_plus);
    data_ref_plus.M.triangularView<Eigen::StrictlyLower>() = data_ref_plus.M.transpose().triangularView<Eigen::StrictlyLower>();

    Eigen::MatrixXd dM_ref = (data_ref_plus.M - data_ref.M)/alpha;
    BOOST_CHECK(dM.isApprox(dM_ref,sqrt(alpha)));
  }

}

BOOST_AUTO_TEST_SUITE_END()


#include <geometry_msgs/PoseArray.h>
#include <mav_planning_common/path_visualization.h>
#include <mav_planning_common/utils.h>
#include <mav_trajectory_generation/polynomial_optimization_nonlinear.h>
#include <mav_trajectory_generation/timing.h>
#include <mav_trajectory_generation_ros/feasibility_analytic.h>
#include <voxblox/utils/planning_utils.h>

#include <mav_msgs/default_topics.h>

#include "voxblox_rrt_planner/rrt_planner.h"
#include <mav_planning_msgs/PlannerServiceResponse.h>

#include <boost/format.hpp>
#include <istream>

namespace mav_planning {

RrtPlanner::RrtPlanner(const ros::NodeHandle& nh,
                       const ros::NodeHandle& nh_private)
    : nh_(nh),
      nh_private_(nh_private),
      frame_id_("odom"),
      visualize_(true),
      do_smoothing_(true),
      last_trajectory_valid_(false) {
}

void RrtPlanner::getParametersFromRos() {
  constraints_.setParametersFromRos(nh_private_);
  nh_private_.param("visualize", visualize_, visualize_);
  nh_private_.param("frame_id", frame_id_, frame_id_);
  nh_private_.param("do_smoothing", do_smoothing_, do_smoothing_);
}
void RrtPlanner::advertiseTopics() {
  path_marker_pub_ =
      nh_private_.advertise<visualization_msgs::MarkerArray>("path", 1, true);
  polynomial_trajectory_pub_ =
      nh_.advertise<mav_planning_msgs::PolynomialTrajectory4D>(
          "polynomial_trajectory", 1);

  waypoint_list_pub_ =
      nh_.advertise<geometry_msgs::PoseArray>("waypoint_list", 1);

  planner_srv_ = nh_private_.advertiseService(
      "plan", &RrtPlanner::plannerServiceCallback, this);
  path_pub_srv_ = nh_private_.advertiseService(
      "publish_path", &RrtPlanner::publishPathCallback, this);
  ROS_INFO("[RrtPlanner] advertized topics.");
}
void RrtPlanner::subscribeToTopics() {}

bool RrtPlanner::publishPathCallback(std_srvs::EmptyRequest& request,
                                     std_srvs::EmptyResponse& response) {
  if (!last_trajectory_valid_) {
    ROS_ERROR("Can't publish trajectory, marked as invalid.");
    return false;
  }

  ROS_INFO("Publishing path.");

  if (!do_smoothing_) {
    geometry_msgs::PoseArray pose_array;
    pose_array.poses.reserve(last_waypoints_.size());
    for (const mav_msgs::EigenTrajectoryPoint& point : last_waypoints_) {
      geometry_msgs::PoseStamped pose_stamped;
      mav_msgs::msgPoseStampedFromEigenTrajectoryPoint(point, &pose_stamped);
      pose_array.poses.push_back(pose_stamped.pose);
    }

    pose_array.header.frame_id = frame_id_;
    waypoint_list_pub_.publish(pose_array);
  } else {
    mav_planning_msgs::PolynomialTrajectory4D msg;
    mav_trajectory_generation::trajectoryToPolynomialTrajectoryMsg(
        last_trajectory_, &msg);

    msg.header.stamp = ros::Time::now();
    msg.header.frame_id = frame_id_;
    polynomial_trajectory_pub_.publish(msg);
  }
  return true;
}

bool RrtPlanner::plannerServiceCallback(
    mav_planning_msgs::PlannerServiceRequest& request,
    mav_planning_msgs::PlannerServiceResponse& response) {
  mav_msgs::EigenTrajectoryPoint start_pose, goal_pose;

  mav_msgs::eigenTrajectoryPointFromPoseMsg(request.start_pose, &start_pose);
  mav_msgs::eigenTrajectoryPointFromPoseMsg(request.goal_pose, &goal_pose);

  // Setup latest copy of map.
  if (!map_->isMapInitialized()) {
    return false;
  }
  // Figure out map bounds!
  Eigen::Vector3d lower_bound, upper_bound;
  map_->computeMapBounds(&lower_bound, &upper_bound);

  ROS_INFO_STREAM("Map bounds: " << lower_bound.transpose() << " to "
                                 << upper_bound.transpose() << " size: "
                                 << (upper_bound - lower_bound).transpose());
  setupRrtPlanner();

  ROS_INFO("Planning path.");

  if (map_->getMapDistance(start_pose.position_W) < constraints_.robot_radius) {
    ROS_ERROR("Start pose occupied!");
    return false;
  }
  if (map_->getMapDistance(goal_pose.position_W) < constraints_.robot_radius) {
    ROS_ERROR("Goal pose occupied!");
    return false;
  }

  mav_msgs::EigenTrajectoryPoint::Vector waypoints;
  mav_trajectory_generation::timing::Timer rrtstar_timer("plan/rrt_star");

  bool success = planRrt(start_pose, goal_pose, &waypoints);
  rrtstar_timer.Stop();
  double path_length = computePathLength(waypoints);
  int num_vertices = waypoints.size();
  ROS_INFO("RRT* Success? %d Path length: %f Vertices: %d", success,
           path_length, num_vertices);

  if (!success) {
    return false;
  }

  visualization_msgs::MarkerArray marker_array;
  if (visualize_) {
    marker_array.markers.push_back(createMarkerForPath(
        waypoints, frame_id_, mav_visualization::Color::Green(), "rrt_star",
        0.075));
    marker_array.markers.push_back(createMarkerForWaypoints(
        waypoints, frame_id_, mav_visualization::Color::Green(),
        "rrt_star_waypoints", 0.15));
  }

  last_waypoints_ = waypoints;

  if (!do_smoothing_) {
    last_trajectory_valid_ = true;
  } else {
    mav_msgs::EigenTrajectoryPointVector poly_path;
    mav_trajectory_generation::timing::Timer poly_timer("plan/poly");
    bool poly_has_collisions =
        !generateFeasibleTrajectory(waypoints, &poly_path);
    poly_timer.Stop();

    mav_msgs::EigenTrajectoryPointVector loco_path;
    mav_trajectory_generation::timing::Timer loco_timer("plan/loco");
    bool loco_has_collisions =
        !generateFeasibleTrajectoryLoco(waypoints, &loco_path);
    loco_timer.Stop();

    mav_msgs::EigenTrajectoryPointVector loco2_path;
    mav_trajectory_generation::timing::Timer loco2_timer("plan/loco2");
    bool loco2_has_collisions =
        !generateFeasibleTrajectoryLoco2(waypoints, &loco2_path);
    loco2_timer.Stop();

    ROS_INFO(
        "Poly Smoothed Path has collisions? %d Loco Path has collisions? %d "
        "Loco 2 has collisions? %d",
        poly_has_collisions, loco_has_collisions, loco2_has_collisions);

    if (!poly_has_collisions) {
      last_trajectory_valid_ = true;
    }

    if (visualize_) {
      marker_array.markers.push_back(createMarkerForPath(
          poly_path, frame_id_, mav_visualization::Color::Orange(), "poly",
          0.075));
      marker_array.markers.push_back(
          createMarkerForPath(loco_path, frame_id_,
                              mav_visualization::Color::Pink(), "loco", 0.075));
      marker_array.markers.push_back(createMarkerForPath(
          loco2_path, frame_id_, mav_visualization::Color::Teal(), "loco2",
          0.075));
    }
  }

  if (visualize_) {
    path_marker_pub_.publish(marker_array);
  }

  response.success = success;

  ROS_INFO_STREAM("All timings: "
                  << std::endl
                  << mav_trajectory_generation::timing::Timing::Print());
  ROS_INFO_STREAM("Finished planning with start point: "
                  << start_pose.position_W.transpose()
                  << " and goal point: " << goal_pose.position_W.transpose());
  return success;
}

bool RrtPlanner::generateFeasibleTrajectory(
    const mav_msgs::EigenTrajectoryPointVector& coordinate_path,
    mav_msgs::EigenTrajectoryPointVector* path) {
  smoother_.getPathBetweenWaypoints(coordinate_path, path);

  bool path_in_collision = checkPathForCollisions(*path, NULL);

  if (path_in_collision) {
    return false;
  }
  return true;
}

bool RrtPlanner::generateFeasibleTrajectoryLoco(
    const mav_msgs::EigenTrajectoryPointVector& coordinate_path,
    mav_msgs::EigenTrajectoryPointVector* path) {
  loco_smoother_.setResampleTrajectory(false);
  loco_smoother_.setAddWaypoints(false);

  loco_smoother_.getPathBetweenWaypoints(coordinate_path, path);

  bool path_in_collision = checkPathForCollisions(*path, NULL);

  if (path_in_collision) {
    return false;
  }
  return true;
}

bool RrtPlanner::generateFeasibleTrajectoryLoco2(
    const mav_msgs::EigenTrajectoryPointVector& coordinate_path,
    mav_msgs::EigenTrajectoryPointVector* path) {
  loco_smoother_.setResampleTrajectory(true);
  loco_smoother_.setAddWaypoints(false);

  loco_smoother_.getPathBetweenWaypoints(coordinate_path, path);

  bool path_in_collision = checkPathForCollisions(*path, NULL);

  if (path_in_collision) {
    return false;
  }
  return true;
}

bool RrtPlanner::checkPathForCollisions(
    const mav_msgs::EigenTrajectoryPointVector& path, double* t) const {
  for (const mav_msgs::EigenTrajectoryPoint& point : path) {
    if (map_->getMapDistance(point.position_W) < constraints_.robot_radius) {
      if (t != NULL) {
        *t = mav_msgs::nanosecondsToSeconds(point.time_from_start_ns);
      }
      return true;
    }
  }
  return false;
}

bool RrtPlanner::checkPhysicalConstraints(
    const mav_trajectory_generation::Trajectory& trajectory) {
  // Check min/max manually.
  // Evaluate min/max extrema
  std::vector<int> dimensions = {0, 1, 2};  // Evaluate dimensions in x, y and z
  mav_trajectory_generation::Extremum v_min, v_max, a_min, a_max;
  trajectory.computeMinMaxMagnitude(
      mav_trajectory_generation::derivative_order::VELOCITY, dimensions, &v_min,
      &v_max);
  trajectory.computeMinMaxMagnitude(
      mav_trajectory_generation::derivative_order::ACCELERATION, dimensions,
      &a_min, &a_max);

  ROS_INFO("V min/max: %f/%f, A min/max: %f/%f", v_min.value, v_max.value,
           a_min.value, a_max.value);

  // Create input constraints.
  // TODO(helenol): just store these as members...
  typedef mav_trajectory_generation::InputConstraintType ICT;
  mav_trajectory_generation::InputConstraints input_constraints;
  input_constraints.addConstraint(
      ICT::kFMin, (mav_msgs::kGravity -
                   constraints_.a_max));  // maximum acceleration in [m/s/s].
  input_constraints.addConstraint(
      ICT::kFMax, (mav_msgs::kGravity +
                   constraints_.a_max));  // maximum acceleration in [m/s/s].
  input_constraints.addConstraint(
      ICT::kVMax, constraints_.v_max);  // maximum velocity in [m/s].

  // Create feasibility object of choice (FeasibilityAnalytic,
  // FeasibilitySampling, FeasibilityRecursive).
  mav_trajectory_generation::FeasibilityAnalytic feasibility_check(
      input_constraints);
  feasibility_check.settings_.setMinSectionTimeS(0.01);

  mav_trajectory_generation::InputFeasibilityResult feasibility =
      feasibility_check.checkInputFeasibilityTrajectory(trajectory);
  if (feasibility !=
      mav_trajectory_generation::InputFeasibilityResult::kInputFeasible) {
    ROS_ERROR_STREAM(
        "Trajectory is input infeasible: "
        << mav_trajectory_generation::getInputFeasibilityResultName(
            feasibility));
    return false;
  }
  return true;
}

}  // namespace mav_planning


#include "board.h"
#include <fstream>
#include <chrono>
#include <thread>
#include <atomic>
#include <mutex>
#include <vector>
#include <ios>
#include <boost/program_options.hpp>
namespace po = boost::program_options;

typedef std::vector<char> vC;
typedef std::vector<vC> vvC;

constexpr size_t bytesize(uint64_t sz) {
  return ((sz + 7) / 8);
}

void set_table(std::vector<uint8_t> &table, uint64_t pos) {
  table[pos / 8] |= (1 << (pos % 8));
}

bool test_table(std::vector<uint8_t> const& table, uint64_t pos) {
  return (table[pos / 8] & (1 << (pos % 8))) != 0;
}

class BitReader {
  std::ifstream ifs;
  static constexpr int BUFSIZE() {
    return 0x1000;
  }
  vC buf;
  int i, j;
public:
  BitReader(std::string const& fname) :ifs(fname, std::ios::binary), buf(BUFSIZE()), i(BUFSIZE()), j(0) {
  }
  int read() {
    if (j >= 8) {
      i++;
      j = 0;
    }
    if (i >= 0x1000) {
      ifs.read((char *)&buf[0], BUFSIZE());
      i = 0;
    }
    int r = (buf[i] >> j) & 1;
    j++;
    return r;
  }
};

std::mutex io_lock;

/*
 * capter_type : 0 - brown can capture a black piece at anywhere
 *               1 - brown can capture a black piece at one of four corners
 *               2 - brown can capture a black piece at the corner(0, 0)
 *               3 - brown can capture a black piece at (1, 0)
 *               4 - brown can capture a black piece at (2, 0)
 */
template<int SIZE, int capture_type = 0>
class Merger {
  static constexpr bool NO_SYMMETRY() {
    if (capture_type == 2 || capture_type == 3 || capture_type == 4) return true;
    else return false;
  }
  static constexpr size_t pos_size() {
    return (1ull << (SIZE - 1));
  }
  static constexpr size_t h_table_size() {
    if (NO_SYMMETRY())
      return pos_size() * SIZE;
    else
      return pos_size() * Board<SIZE>::HSIZE();
  }
  static constexpr size_t table_size() {
    return h_table_size() * 2;
  }

/*
  0 - unknown
  1 - (black turn) black cannot move (brown win)
  2 - (brown turn) brown has move to state 1
 2*n - (brown turn) brown has move to state 2*n - 1
 2*n + 1 - (black turn) all black move lead to state 2*m (m <= n)
 */

//   std::vector<uint8_t> table_brown, table_black;

public:
  Merger() {}

  std::string out_file_name() {
    std::string fname = "count_" + std::to_string(SIZE) + "_" + std::to_string(capture_type) + ".bin";
    return fname;
  }

  std::string file_name(int step) {
    if (step % 2 == 1) {
      std::string fname = "black_" + std::to_string(SIZE) + "_" + std::to_string(capture_type) + "_" + std::to_string(step) + ".bin";
      return fname;
    }
    else {
      std::string fname = "brown_" + std::to_string(SIZE) + "_" + std::to_string(capture_type) + "_" + std::to_string(step) + ".bin";
      return fname;
    }
  }

  bool check_step(int step) {
    std::ifstream ifs(file_name(step));
    return ifs.is_open();
  }
  int step_limit() {
    for (int step = 1; step < 256; step++)
      if (!check_step(step)) return step;
    return 256;
  }

  char read_black(std::vector<BitReader> &streams) {
    char r = 0;
    for (size_t i = 0; i < streams.size(); i += 2) {
      int v = streams[i].read();
      if (r == 0 && v > 0) r = i + 1;
    }
    return r;
  }

  char read_brown(std::vector<BitReader> &streams) {
    char r = 0;
    for (size_t i = 1; i < streams.size(); i += 2) {
      int v = streams[i].read();
      if (r == 0 && v > 0) r = i + 1;
    }
    return r;
  }

  void merge() {
    // std::string prefix="/mnt/sda1/ktanaka/";
    std::cerr << "start mearging SIZE=" << SIZE << ", capture_type=" << capture_type << std::endl;
    std::ofstream ofs(out_file_name(), std::ios::binary|std::ios::trunc);
    int limit = step_limit();
    std::vector<BitReader> streams;
    for (int step = 1; step < limit; step++) {
      streams.emplace_back(BitReader(file_name(step)));
    }
    for (uint64_t v = 0; v < table_size(); v++ ) {
      if (v % 10000000 == 0) {
	std::lock_guard<std::mutex> l_(io_lock);
	std::cerr << "step=" << v << std::endl;
      }
      Board<SIZE> b(v);
      if (b.turn() == Board<SIZE>::black) {
	ofs << read_black(streams);
      }
      else {
	ofs << read_brown(streams);
      }
    }
  }
};

int main(int ac, char **ag) {
  int board_size;
  int capture_type;

  po::options_description options("all_options");
  options.add_options()
    ("board-size,n",
     po::value<int>(&board_size)->default_value(25),
     "board size (25, 31, 33)")
    ("capture-type,t",
     po::value<int>(&capture_type)->default_value(0),
     "capture type : 0 (ALL), 1(any corner), 2(one corner), 3(1,0), 4(2,0)")
    ;
  po::variables_map vm;
  try
    {
      po::store(po::parse_command_line(ac, ag, options), vm);
      po::notify(vm);
    }
  catch (std::exception& e)
    {
      std::cerr << "error in parsing options" << std::endl
		<< e.what() << std::endl;
      std::cerr << options << std::endl;
      return 1;
    }
  if (vm.count("help")) {
    std::cerr << options << std::endl;
    return 0;
  }
  if (board_size == 25) {
    if (capture_type == 0)
      Merger<25, 0>().merge();
    else if (capture_type == 1)
      Merger<25, 1>().merge();
    else if (capture_type == 2)
      Merger<25, 2>().merge();
    else if (capture_type == 3)
      Merger<25, 3>().merge();
    else if (capture_type == 4)
      Merger<25, 4>().merge();
    else {
      std::cerr << options << std::endl;
      return 0;
    }
  }
  else if (board_size == 31) {
    if (capture_type == 0)
      Merger<31, 0>().merge();
  }
  else if (board_size == 33) {
    if (capture_type == 0)
      Merger<33, 0>().merge();
  }
  else {
    std::cerr << options << std::endl;
    return 0;
  }
}


/*
 * fcnn.cpp
 *
 */
#include <string>

#include <boost/assert.hpp>

#include "core/utils.h"
#include "layers/fclayer.h"
#include "fcnn.h"

using namespace std;
using namespace yann;

////////////////////////////////////////////////////////////////////////////////////////////////
//
// FullyConnectedNetwork implementation
//
yann::FullyConnectedNetwork::Params::Params() :
    _input_size(0),
    _output_size(0)
{
}

unique_ptr<Network>yann::FullyConnectedNetwork::create(const vector<MatrixSize> & layer_sizes)
{
  YANN_CHECK_GE(layer_sizes.size(), 2); // should have at least 2 layers: input and output

  auto fcnn = make_unique<Network>();
  YANN_CHECK(fcnn);

  for (size_t ii = 1; ii < layer_sizes.size(); ++ii) {
    YANN_CHECK_GT(layer_sizes[ii - 1], 0);
    YANN_CHECK_GT(layer_sizes[ii], 0);
    auto layer = make_unique<FullyConnectedLayer>(layer_sizes[ii - 1], layer_sizes[ii]);
    YANN_CHECK(layer);
    fcnn->append_layer(std::move(layer));
  }

  return fcnn;
}

unique_ptr<Network>yann::FullyConnectedNetwork::create(const vector<Params> & params)
{
  YANN_CHECK_GE(params.size(), 1); // should have at least 1 layer for input/output

  auto fcnn = make_unique<Network>();
  YANN_CHECK(fcnn);

  for (const auto & param : params) {
    YANN_CHECK_GT(param._input_size, 0);
    YANN_CHECK_GT(param._output_size, 0);
    auto layer = make_unique<FullyConnectedLayer>(param._input_size, param._output_size);
    if(param._activation_function) {
      layer->set_activation_function(param._activation_function);
    }
    fcnn->append_layer(std::move(layer));
  }

  return fcnn;
}