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

#include "cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h"

#include <memory>

#include "Eigen/Core"
#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/mapping/3d/hybrid_grid.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/internal/voxel_filter.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/rigid_transform_test_helpers.h"
#include "gtest/gtest.h"
#include "cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h"
#include <chrono>
#include <thread>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/gicp.h>
#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
//using namespace std::chrono_literals;
#include <map>
#include <boost/algorithm/string/trim.hpp>
#include <string.h>
#include <regex>
/* jwang 1/6/22
 *
 * TestFromInitialPose2()
 *
 * ptselection = 0|1|2
 * 0 for pcd file
 * 1 for dbg file
 * 2 for two pcd files: one for submap and one for curr scan
 * point_cloud_3_ from debug file
 * point_cloud_2_ from pcdfilename
 *
 *
 */
namespace cartographer {
namespace mapping {
namespace scan_matching {
namespace {
class CeresScanMatcher3DTest : public ::testing::Test {
 protected:
  CeresScanMatcher3DTest()
      : hybrid_grid_(0.1f),
        intensity_hybrid_grid_(0.1f),
        expected_pose_(
            transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) {
    std::vector<sensor::RangefinderPoint> points;
    std::vector<float> intensities;
    for (const Eigen::Vector3f& point :
         {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
          Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
          Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
          Eigen::Vector3f(-7.f, 3.f, 1.f)})
    {
      points.push_back({point});
      intensities.push_back(50);
      hybrid_grid_.SetProbability(
          hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() * point), 1.);
      intensity_hybrid_grid_.AddIntensity(
          intensity_hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() *
                                              point),
          50);
    }
    point_cloud_ = sensor::PointCloud(points, intensities);
    std::string filename2="testopt2.txt";
    std::ifstream stream2(filename2.c_str());
    std::string myoptstr2= std::string((std::istreambuf_iterator<char>(stream2)),
                     std::istreambuf_iterator<char>());
    char *token;
    token = strtok(const_cast<char*>(myoptstr2.c_str()), "\n");
    while (token != NULL) {
	    std::string s(token);
	    size_t pos = s.find(":");
	    std::string svalue = s.substr(pos + 1, std::string::npos);
	    boost::algorithm::trim(svalue);
	    mymap[s.substr(0, pos)] = svalue;
	    token = strtok(NULL, "\n");
    }

    //for (auto keyval : mymap)
    //	    std::cout << keyval.first << ":" << keyval.second << std::endl;
    //std::cout<< mymap["pcdfilename"] <<std::endl;

    std::string filename="testopt.lua";
    std::ifstream stream(filename.c_str());
    myoptstr= std::string((std::istreambuf_iterator<char>(stream)),
                     std::istreambuf_iterator<char>());
    //LOG(INFO)<< myoptstr;

    std::string testoptstr=R"text(
        return {
          occupied_space_weight_0 = 1.,
          intensity_cost_function_options_0 = {
            weight = 0.5,
            huber_scale = 55,
            intensity_threshold = 100,
          },
          translation_weight = 0.01,
          rotation_weight = 0.1,
          only_optimize_yaw = false,
          ceres_solver_options = {
            use_nonmonotonic_steps = true,
            max_num_iterations = 100,
            num_threads = 1,
          },
        })text";

    auto parameter_dictionary = common::MakeDictionary(myoptstr);
    options_ = CreateCeresScanMatcherOptions3D(parameter_dictionary.get());
    //LOG(INFO) << parameter_dictionary->GetKeys();
    //LOG(INFO) << parameter_dictionary->ToString();
    ceres_scan_matcher_.reset(new CeresScanMatcher3D(options_));
  }
/********************************************************************/
  void TestFromInitialPose(const transform::Rigid3d& initial_pose) {
    transform::Rigid3d pose;

    ceres::Solver::Summary summary;

    IntensityHybridGrid* intensity_hybrid_grid_ptr =
        point_cloud_.intensities().empty() ? nullptr : &intensity_hybrid_grid_;

    ceres_scan_matcher_->Match(
        initial_pose.translation(), initial_pose,
        {{&point_cloud_, &hybrid_grid_, intensity_hybrid_grid_ptr}}, &pose,
        &summary);
    LOG(INFO) << "estimated pose: "<< pose;
    EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();
    EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2));
  }

/* ******************************************************
 * convert initpose string to Rigid3d
 */
  transform::Rigid3d makepose(std::string posestring)
  {
	  std::cout<<"\nmake pose: " <<"\n";
	  std::istringstream ss(posestring);
	  float x,y,z,qw,qx,qy,qz;
	  ss>> x;
	  ss>> y;
	  ss>> z;
	  ss>> qw;
	  ss>> qx;
	  ss>> qy;
	  ss>> qz;
	  std::cout << "xyz: " <<x <<" " <<y<<" " <<z<<" quat " <<qw<<" " <<qx<<" " <<qy<<" " <<qz <<"\n";
	  Eigen::Matrix<double, 3, 1> trans;
	  Eigen::Vector3d trans2;
	  trans2 = Eigen::Vector3d(x, y,z);
	  //trans2 = Eigen::Vector3d(-0.95, -0.05, 0.05);
	  std::cout<<"\nmake pose: input pose: wxyz " <<qw<<" " << qx<<" "<< qy<<" " <<qz<<"\n";
	  Eigen::Quaternion<double> rotation(qw,qx,qy,qz);
	  Eigen::Quaternion<double> rotation3=rotation.normalized();
	  Eigen::Matrix3d mat3 = rotation3.toRotationMatrix();
	  Eigen::Quaternion<double> rotation2(mat3);
	  std::cout<<"\nmake pose: eigen pose normalized: wxyz " <<rotation3.w()<<" " << rotation3.x()<<" "<< rotation3.y()<<" " <<rotation3.z()<<"\n";
	  std::cout<<"\nmake pose: eigen pose recreated: wxyz " <<rotation2.w()<<" " << rotation2.x()<<" "<< rotation2.y()<<" " <<rotation2.z()<<"\n";
	  //Eigen::Quaternion<double> rotation(1,0,0,0);
	  return transform::Rigid3d(trans2, rotation2);
  }
  /*************** convert xyzi cloud t xyz cloud  ****/
  void convert_xyzi_2_xyz(pcl::PointCloud<pcl::PointXYZI>::Ptr pclcloud_i, pcl::PointCloud<pcl::PointXYZ>::Ptr pclcloud)
  {
	  pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
	  pclcloud->width = pclcloud_i->width;
	  pclcloud->height = 1;
	  pclcloud->is_dense = false;

	  pclcloud->points.resize (pclcloud->width * pclcloud->height);
	  for (int i =0; i< pclcloud->width; i++){
		  pclcloud->points[i].x= pclcloud_i->points[i].x;
		  pclcloud->points[i].y= pclcloud_i->points[i].y;
		  pclcloud->points[i].z= pclcloud_i->points[i].z;
	  }
	  //pclcloud= target_cloud; this not work
  }
/******************************************************
 * convert PCD XYZI cloud to carto's hybrid_grid
 */

  void PCDCloud_2_HybridGrid(pcl::PointCloud<pcl::PointXYZI>::Ptr pclcloud, HybridGrid & hybrid_grid, IntensityHybridGrid & intensity_hybrid_grid)
  {
    int ptcount=0;

    Eigen::Vector3f point;
    for (const pcl::PointXYZI& pointxyzi : pclcloud->points)
    {
	ptcount++;
	point(0)=pointxyzi.x;
	point(1)=pointxyzi.y;
	point(2)=pointxyzi.z;
	hybrid_grid.SetProbability(
          hybrid_grid.GetCellIndex(point), pointxyzi.intensity);
	intensity_hybrid_grid.AddIntensity(
          intensity_hybrid_grid.GetCellIndex(point),
          50);
    }
  }

/******************************************************
 * convert PCD cloud to carto's hybrid_grid
 */
  void PCDCloud_2_HybridGrid(pcl::PointCloud<pcl::PointXYZ>::Ptr pclcloud, HybridGrid & hybrid_grid, IntensityHybridGrid & intensity_hybrid_grid)
  {
    int ptcount=0;

    Eigen::Vector3f point;
    // pc2 points number: 33024
    for (const pcl::PointXYZ& pointxyz : pclcloud->points)
    {
	ptcount++;
	point(0)=pointxyz.x;
	point(1)=pointxyz.y;
	point(2)=pointxyz.z;
	hybrid_grid.SetProbability(
          hybrid_grid.GetCellIndex(point), 1.);
	intensity_hybrid_grid.AddIntensity(
          intensity_hybrid_grid.GetCellIndex(point),
          50);
    }
    std::cout<< "pcd RangefinderPoint: ";
  }

  // tbi
  void PCLCloud_2_Range(pcl::PointCloud<pcl::PointXYZ>::Ptr pclcloud)
  {
  }
/******************************************************
 * convert  carto's sensor::PointCloud to PCL/PCD cloud
 */
  void PointCloud_2_PCLCloud(sensor::PointCloud & point_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr pclcloud)
  {
	  int numpoints = point_cloud.size();
	  pclcloud->width=numpoints;
	  pclcloud->height=1;
	  pclcloud->points.resize (pclcloud->width * pclcloud->height);
	  auto points=point_cloud.points();
	  for (int i=0;i <numpoints; i++){
		  pclcloud->points[i].x=points[i].position(0);
		  pclcloud->points[i].y=points[i].position(1);
		  pclcloud->points[i].z=points[i].position(2);
	  }
  }
/******************************************************
 * convert PCD cloud to carto's sensor::PointCloud
 */
  void PCDCloud_2_PointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr pclcloud,sensor::PointCloud & point_cloud)
  {
    int ptcount=0;
    Eigen::Vector3f point;
    std::vector<float> intensities;
    std::vector<sensor::RangefinderPoint> points;
    // pc2 points number: 33024
    for (const pcl::PointXYZ& pointxyz : pclcloud->points)
    {
	ptcount++;
        //LOG(INFO) << "point: "<< pointxyz;
	point(0)=pointxyz.x;
	point(1)=pointxyz.y;
	point(2)=pointxyz.z;
      	points.push_back({point});
        intensities.push_back(50);
	/*
	hybrid_grid_2_.SetProbability(
          hybrid_grid_2_.GetCellIndex(expected_pose_.cast<float>() * point), 1.);
	intensity_hybrid_grid_2_.AddIntensity(
          intensity_hybrid_grid_2_.GetCellIndex(expected_pose_.cast<float>() *
                                              point),
          50);
	  */
    }
    std::cout<< "pcd RangefinderPoint: ";
    point_cloud = sensor::PointCloud(points, intensities);
  }

/***********************************************
 * read a pcd file, return cloud
  pcl::PointCloud<pcl::PointXYZI>::Ptr readPCD_file(std::string pcdfn1)
  {
        pcl::PointCloud<pcl::PointXYZI>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZI>);
	pcl::io::loadPCDFile<pcl::PointXYZI> (pcdfn1, *target_cloud);
	LOG(INFO) << *target_cloud;
	return target_cloud;
  }
 */
/***********************************************
 * read a pcd file, return cloud
 */
template <class T>
typename  pcl::PointCloud<T>::Ptr readPCD_file(std::string pcdfn1)
  {
        typename pcl::PointCloud<T>::Ptr target_cloud (new pcl::PointCloud<T>);
//	pcdfn1= "/home/student/Documents/AirSim/ros/src/hdl_graph_slam/mapdata_13.pcd";
	pcl::io::loadPCDFile<T> (pcdfn1, *target_cloud);
	LOG(INFO) << *target_cloud;
	return target_cloud;
  }

  /*********************************************************
   * reset scanmatch to use one pair of scan/submap
   */
  void scanmatch_reset_single()
  {
    myoptstr = std::regex_replace(myoptstr, std::regex("occupied_space_weight_1"), "occupied_space_weight_0");
  //  std::cout<<myoptstr;
    auto parameter_dictionary = common::MakeDictionary(myoptstr);
    options_ = CreateCeresScanMatcherOptions3D(parameter_dictionary.get());
    ceres_scan_matcher_.reset(new CeresScanMatcher3D(options_));
  }

  /*********************************************************
   * sweep init pose to test costfun
   */
void sweeptest(const transform::Rigid3d& initial_pose, sensor::PointCloud &point_cloud, HybridGrid * hybrid_grid)
{
    std::ofstream outFile("/home/student//Documents/cartographer/test_ceres_pcd/sweep4.txt");
    std::ofstream outFile2("/home/student//Documents/cartographer/test_ceres_pcd/sweep4.csv"); //gnuplot csv
    outFile2 << "# pose_x, y, z, costfun, ceres_initcost, ceres_finalcost\n";
    std::ostringstream ssout;
    transform::Rigid3d pose;
    ceres::Solver::Summary summary;
    double *residual = new double [point_cloud_5_.size()];
    double trans[3]= {initial_pose.translation().x(), initial_pose.translation().y(),initial_pose.translation().z()};

    double quat[4] ={initial_pose.rotation().w(),initial_pose.rotation().x(),initial_pose.rotation().y(),initial_pose.rotation().z()};
    //OccupiedSpaceCostFunction3D::scan_matching_verbose=1;
    OccupiedSpaceCostFunction3D  costfun (1.0, point_cloud, *hybrid_grid);
    double sweepstep,sweeprange;
    std::istringstream(mymap["sweepstep"])>>sweepstep;
    std::istringstream(mymap["sweeprange"])>>sweeprange;
    int sweepcnt = sweeprange/sweepstep;
    for (int j = -sweepcnt; j<sweepcnt; j++){
	for(int k=-sweepcnt; k<sweepcnt; k++){
	    trans[0]= sweepstep*j;
	    trans[1]= sweepstep*k;
	    transform::Rigid3d pose2(Eigen::Matrix<double, 3, 1>(trans[0],trans[1],trans[2]),initial_pose.rotation());
	    costfun(trans,quat, residual);
	    double sqsum_residual=0;
	    for (int i =0; i< point_cloud.size();i++){
		    sqsum_residual +=residual[i]*residual[i];
	    }
	    summary = run_lowreso_match(pose2,point_cloud,hybrid_grid, pose);
	    ssout.str("");
	    ssout<<"\n\t init_xyz: "<<trans[0]<<" "<<trans[1]<<" "<<trans[2]<<" residual "
			    << sqsum_residual <<" ceres init/final cost " << summary.initial_cost<<" " << summary.final_cost <<" final pose "
			    <<pose.translation().x() <<" " << pose.translation().y() <<" " <<pose.translation().z()<<"";
	    outFile << ssout.str();
	    outFile2 << trans[0]<<", "<<trans[1]<<", "<<trans[2]<<", " <<sqsum_residual<<", " << summary.initial_cost<<", " << summary.final_cost<<"\n";
	    std::cout<< ssout.str();
	}
    }
}
  /*********************************************************
   * run low resolution scanmatch
   */
  ceres::Solver::Summary run_lowreso_match(const transform::Rigid3d& initial_pose, sensor::PointCloud &point_cloud, HybridGrid * hybrid_grid, transform::Rigid3d & pose_o)
  {
    transform::Rigid3d pose_1;
    ceres::Solver::Summary summary_1;
    std::vector<PointCloudAndHybridGridsPointers> point_clouds_and_hybrid_grids_1;
    scanmatch_reset_single();
    point_clouds_and_hybrid_grids_1={{&point_cloud, hybrid_grid, nullptr}};
    ceres_scan_matcher_->Match(initial_pose.translation(), initial_pose, point_clouds_and_hybrid_grids_1, &pose_1,&summary_1);

    pose_o=pose_1;
    return summary_1;
  }
  /*********************************************************
 * jwang: new scan match test code from here
 * ptselection: 0|1|2
 * 2 for testing two pcd files scan match
 * *********************************************************/
  void TestFromInitialPose2(const transform::Rigid3d& initial_pose) {
    transform::Rigid3d pose;

     Eigen::Transform<double, 3, Eigen::Affine > pose_transform;
     Eigen::Matrix3f mat3 = Eigen::Quaternionf(1, 0,0,0).toRotationMatrix();
    ceres::Solver::Summary summary;
    std::vector<PointCloudAndHybridGridsPointers> point_clouds_and_hybrid_grids;

//--------------- select which one to run ceres on: pcd points or debug points
    int ptselection = std::stoi(mymap["ptselection"]);
    if (ptselection ==0){
	    //use single pcd file
	std::cout <<"use pcd file points\n";
	IntensityHybridGrid* intensity_hybrid_grid_ptr =
        point_cloud_2_.intensities().empty() ? nullptr : &intensity_hybrid_grid_2_;
	ceres_scan_matcher_->Match(
        initial_pose.translation(), initial_pose,
        {{&point_cloud_2_, &hybrid_grid_2_, intensity_hybrid_grid_ptr}}, &pose,
        &summary);
    }
    else if (ptselection ==1){
	    // use single ebug txt filea
	std::cout <<"use debug txt file points";

	IntensityHybridGrid* intensity_hybrid_grid_ptr =
        	point_cloud_3_.intensities().empty() ? nullptr : &intensity_hybrid_grid_3_;

	point_clouds_and_hybrid_grids={{&point_cloud_3_, &hybrid_grid_3_, intensity_hybrid_grid_ptr}};
	std::cout << "point_clouds_and_hybrid_grids.size(): "<<point_clouds_and_hybrid_grids.size() << "\npoint_cloud\n" << point_clouds_and_hybrid_grids[0].point_cloud <<std::endl;
	std::cout << "\nhybrid_grid:\n " << point_clouds_and_hybrid_grids[0].hybrid_grid <<std::endl;

	OccupiedSpaceCostFunction3D::scan_matching_verbose=1;

	ceres_scan_matcher_->Match(
        initial_pose.translation(), initial_pose,
        {{&point_cloud_3_, &hybrid_grid_3_, intensity_hybrid_grid_ptr}}, &pose,
        &summary);
    }else if (ptselection ==2){
            // use two pcd files: pcdfilename1 pcdfilename2
	std::cout <<"use two pcd file points\n";
	    std::string pcdf1, pcdf2, pcdf3, pcdf4;
	    transform::Rigid3d initial_pose2;
	    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1;
	    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);
	    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud2_i;
	    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud4;
	    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud5(new pcl::PointCloud<pcl::PointXYZ>);
	    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud5_i;
	    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud3(new pcl::PointCloud<pcl::PointXYZ>);
	    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud6(new pcl::PointCloud<pcl::PointXYZ>);
	    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1_edgefiltered(new pcl::PointCloud<pcl::PointXYZ>);
	    pcdf1 =  mymap["pcdfilename1"]; //scan pcd high res
	    pcdf2 =  mymap["pcdfilename2"]; //submap pcd high res
	    pcdf3 =  mymap["pcdfilename3"]; //scan pcd low res
	    pcdf4 =  mymap["pcdfilename4"]; //submap pcd low res
	    initial_pose2 = makepose(mymap["initpose"]);

	    int hgridxyzi = std::stoi(mymap["hgridxyzi"]);
	    //read scan data, high and low res
	    cloud1 = readPCD_file<pcl::PointXYZ>(pcdf1);
	    PCDCloud_2_PointCloud(cloud1,point_cloud_4_);
	    	//convert to point_cloud_4_  carto's pointcloud class
	    cloud4 = readPCD_file<pcl::PointXYZ>(pcdf3);
	    PCDCloud_2_PointCloud(cloud4,point_cloud_5_);

    //	    std::vector<RangefinderPoint> cloud1_ranges = PCLcloud_2_Range(cloud1);

	  //  std::vector<PointCloud>
	  auto cloud1_filtered_carto =
	    sensor::VoxelFilter(
      point_cloud_4_, 1.5f );
	  auto cloud1_edgefiltered_carto =
	    sensor::VoxelFilterEdge(
      point_cloud_4_, 0.5f, 0.6f );
	  PointCloud_2_PCLCloud(cloud1_filtered_carto,cloud1_filtered);
	  PointCloud_2_PCLCloud(cloud1_edgefiltered_carto,cloud1_edgefiltered);
	    show_pcl_2cloud(cloud1,cloud1_filtered, "high res scan and voxel filtered 0.5");
	    show_pcl_2cloud(cloud1,cloud1_edgefiltered, "high res scan and voxel edgefiltered 0.5");
	    show_pcl_cloud(cloud1, "high res scan cloud1");
	    show_pcl_cloud(cloud1_edgefiltered, "high res scan voxel edgefiltered 0.5");
	    //read hbrid data, xyzi or xyz type, high and low res
	    if (hgridxyzi==1){
	    	cloud2_i=readPCD_file<pcl::PointXYZI>(pcdf2);
	    	cloud5_i = readPCD_file<pcl::PointXYZI>(pcdf4);
		std::cout<<"\n converted cloud2: " <<cloud2<<"\n";
		convert_xyzi_2_xyz(cloud2_i, cloud2);
		std::cout<<" converted cloud2: " <<cloud2<<"\n";
		convert_xyzi_2_xyz(cloud5_i, cloud5);
	    }
	    else{
	    	cloud2 = readPCD_file<pcl::PointXYZ>(pcdf2);
	    	cloud5 = readPCD_file<pcl::PointXYZ>(pcdf4);
	    }

	    show_pcl_2cloud(cloud1,cloud2, "high res scan and submap no transform");
	    transform_cloud (cloud1, cloud3, initial_pose2.translation(), initial_pose2.rotation());
	    show_pcl_2cloud(cloud3, cloud2, "high res scan and submap, transform initial_pose2");
	    float hgrid_res_h;
	    float hgrid_res_l;
	    std::istringstream ss_h(mymap["hgrid_res_h"]);
	    std::istringstream ss_l(mymap["hgrid_res_l"]);
	    ss_h >> hgrid_res_h; //submap pcd
	    ss_l >> hgrid_res_l; //submap pcd
	    hybrid_grid_4_ = new HybridGrid(hgrid_res_h);
	    hybrid_grid_5_ = new HybridGrid(hgrid_res_l);

	    //convert pcl cloud to hbrid, there are two version of PCDCloud_2_HybridGrid
	    if (hgridxyzi==0){
	    	PCDCloud_2_HybridGrid(cloud2,*hybrid_grid_4_,intensity_hybrid_grid_4_);
	    	PCDCloud_2_HybridGrid(cloud5,*hybrid_grid_5_,intensity_hybrid_grid_5_);
	    }else{
		//hgrid pcd is xyzi format with prob value.
	    	PCDCloud_2_HybridGrid(cloud2_i,*hybrid_grid_4_,intensity_hybrid_grid_4_);
	    	PCDCloud_2_HybridGrid(cloud5_i,*hybrid_grid_5_,intensity_hybrid_grid_5_);
	    }

	    IntensityHybridGrid* intensity_hybrid_grid_ptr =
		    point_cloud_2_.intensities().empty() ?
		    nullptr : &intensity_hybrid_grid_2_;
	    if (mymap["usehighres"] =="1" && mymap["uselowres"] =="1")
	    {
		    std::cout<<"******************  usehighres and uselowres***\n\n";
		    if (mymap["usehighres"] =="1" ){
			    std::cout<<"******************  twostep:  ***\n\n";
			    scanmatch_reset_single();
			    point_clouds_and_hybrid_grids={{&point_cloud_5_, hybrid_grid_5_, nullptr}};

			    ceres_scan_matcher_->Match(initial_pose2.translation(), initial_pose2, point_clouds_and_hybrid_grids, &pose,&summary);
			    std::cout<<"******************  twostep: phase 1 low res ***\n\tpose: "<< pose<<"\n";
			    point_clouds_and_hybrid_grids={{&point_cloud_4_, hybrid_grid_4_, intensity_hybrid_grid_ptr}};

			    ceres_scan_matcher_->Match(pose.translation(), pose, point_clouds_and_hybrid_grids, &pose,&summary);
			    std::cout<<"******************  twostep: phase 2 high res ***\n\tpose: "<< pose<<"\n";

		    }else{
			    std::cout<<"******************  twostep:0 high/low in once Match call ***\n\n";
			    point_clouds_and_hybrid_grids={{&point_cloud_4_, hybrid_grid_4_, intensity_hybrid_grid_ptr},{&point_cloud_5_,hybrid_grid_5_,nullptr}};
			    ceres_scan_matcher_->Match(initial_pose2.translation(), initial_pose2, point_clouds_and_hybrid_grids, &pose,&summary);
		    }
	    }
	    if (mymap["usehighres"] =="1" && mymap["uselowres"] =="0")
	    {
		    std::cout<<"******************  usehighres ***\n\n";
		    scanmatch_reset_single();
		    point_clouds_and_hybrid_grids={{&point_cloud_4_, hybrid_grid_4_, intensity_hybrid_grid_ptr}};
		    ceres_scan_matcher_->Match(initial_pose2.translation(), initial_pose2, point_clouds_and_hybrid_grids, &pose,&summary);

	    }
	    if (mymap["usehighres"] =="0" && mymap["uselowres"] =="1")
	    {
		    std::cout<<"******************  uselowres ***\n\n";
		    summary = run_lowreso_match(initial_pose2,point_cloud_5_,hybrid_grid_5_, pose);
		    /*
	    LOG(INFO) << "\nestimated pose: "<< pose;
	    LOG(INFO) << summary.FullReport();
		    std::cout<<"******************  uselowres ***\n\n";
		    scanmatch_reset_single();
		    point_clouds_and_hybrid_grids={{&point_cloud_5_, hybrid_grid_5_, nullptr}};
		    ceres_scan_matcher_->Match(initial_pose2.translation(), initial_pose2, point_clouds_and_hybrid_grids, &pose,&summary);
		    */
	    }
	    if (mymap["usehighres"] =="0" && mymap["uselowres"] =="0")
	    {
		    double *residual = new double [point_cloud_5_.size()];
		    double trans[3]= {initial_pose2.translation().x(), initial_pose2.translation().y(),initial_pose2.translation().z()};
		    double quat[4] ={initial_pose2.rotation().w(),initial_pose2.rotation().x(),initial_pose2.rotation().y(),initial_pose2.rotation().z()};

		    std::cout<<"****************** both usehighres and uselowres are false, perform sweep func ***\n\ttrans" << trans[0]<<" "<<trans[1]<<" "<<trans[2] <<" quat "<< quat[0]<<" " << quat[1]<<" " <<quat[2]<<" " <<quat[3] <<"\n";

		    sweeptest(initial_pose2, point_cloud_5_, hybrid_grid_5_);
	    }

	    //ceres_scan_matcher_->Match(initial_pose2.translation(), initial_pose2, point_clouds_and_hybrid_grids, &pose,&summary);

	    LOG(INFO) << "\nestimated pose: "<< pose;
	    LOG(INFO) << summary.FullReport();

	    //transform cloud1 (scan) to cloud3 using estimated pose
	    transform_cloud (cloud1, cloud3, pose.translation(), pose.rotation());
	    transform_cloud (cloud4, cloud6, pose.translation(), pose.rotation());
	    // show high res submap and transformed scan
	    show_pcl_2cloud(cloud3,cloud2, "high res scan/submap aligned");
	    // show low res submap and transformed scan
	    show_pcl_2cloud(cloud6,cloud5, "low res scan/submap aligned");
	    return;
    }//end if ptselection

    OccupiedSpaceCostFunction3D::scan_matching_verbose=0;
    LOG(INFO) << "estimated pose: "<< pose;
    LOG(INFO) << summary.FullReport();
    //EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();
    //EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2));
  }

  void transform_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out, Eigen::Matrix<double, 3, 1> trans, Eigen::Quaternion<double> quat0)
  {
        Eigen::Quaterniond quat(1, 0,0,0);

        Eigen::Matrix3d mat3 = Eigen::Quaterniond(1, 0,0,0).toRotationMatrix();
        Eigen::Matrix3d mat4 = quat0.toRotationMatrix();

	Eigen::Vector3d v3d(-3., 0., 0.);

	Eigen::Transform<double, 3, Eigen::Affine > pose_trans;
//	pose_trans.translation()=v3d;
	pose_trans.translation()=trans;
	pose_trans.linear()=mat4;
	std::cout<<"transform_cloud quat0 wxyz: "<< quat0.w()<<" "<< quat0.x()<<" "<< quat0.y()<<" "<<quat0.z() <<"\n";
	std::cout<<"Eigen::Transform: "<< pose_trans.translation() <<"\n";
	std::cout<<"Eigen::Transform: rotation"<< pose_trans.rotation() <<"\n";
	pcl::transformPointCloud (*cloud_in, *cloud_out, pose_trans);
  }

/********************************************************************/
  /* read from txt file debugging cloud point */
  void add_dbg_pts(int dbgpt, std::vector<sensor::RangefinderPoint> &points, std::vector<float> &intensities)
  {

    //std::vector<sensor::RangefinderPoint> points;
    //std::vector<float> intensities;
    Eigen::Vector3f point;
    std::string dbgfilename = mymap["dbgfilename"];
    std::ifstream stream2(dbgfilename.c_str());
    std::string mypt_str= std::string((std::istreambuf_iterator<char>(stream2)),
                     std::istreambuf_iterator<char>());
    char *token;
    //std::cout<< "strings from dbg point file:\n" << mypt_str <<"\n";
    token = strtok(const_cast<char*>(mypt_str.c_str()), "\n");
    while (token != NULL) {
	    std::string svalue(token);
	    size_t pos = svalue.find(",");
            std::string svalue2 = svalue.substr(pos + 1, std::string::npos);
	    size_t pos2 = svalue2.find(",");
            std::string svalue3 = svalue2.substr(pos2 + 1, std::string::npos);
	    //std::cout<<"dbg " << pos<<" "<<pos2 <<" "<<svalue.substr(0, pos)<<" " <<svalue2.substr(0, pos2) << " " << svalue3 << "\n";
	    point(0) = std::stof(svalue.substr(0, pos));
	    point(1) = std::stof(svalue2.substr(0, pos2));
	    point(2) = std::stof(svalue3);

	    points.push_back({point});
	    intensities.push_back(50);
	    token = strtok(NULL, "\n");
// std::cout<<NULL will cause exception and terminate the func call, but the rest of the parent func still run.
	    if (token == NULL) std::cout<< "token is NULL" << (token ==NULL)<<"\n";
	    else std::cout<< "line " << token <<" ----\n";
    }
  }

/********************************************************************/
  // setup test points from either pcd cloud or debug file
  void initpc2(pcl::PointCloud<pcl::PointXYZ>::Ptr pclcloud) {
    std::vector<sensor::RangefinderPoint> points;
    std::vector<sensor::RangefinderPoint> points_dbg;
    std::vector<float> intensities;
    std::vector<float> intensities_dbg;
    int maxpt = std::stoi(mymap["ptcount"]);
    maxpt = std::min(maxpt, int(pclcloud->width * pclcloud->height));
    int dbgpt = std::stoi(mymap["dbgpt"]);

    //----------------------read the dbg points from file
    add_dbg_pts(dbgpt, points_dbg, intensities_dbg);
    std::cout<< "dbg RangefinderPoint:\n";
    for(int i=0;i<dbgpt;i++){
    	std::cout<<  points_dbg[i].position(0) << ", " <<points_dbg[i].position(1) <<", "<<points_dbg[i].position(2) <<std::endl;
      hybrid_grid_3_.SetProbability(
          hybrid_grid_3_.GetCellIndex(expected_pose_.cast<float>() * points_dbg[i].position), 1.);
      intensity_hybrid_grid_3_.AddIntensity(
          intensity_hybrid_grid_3_.GetCellIndex(expected_pose_.cast<float>() *
                                              points_dbg[i].position),
          50);
    }
    point_cloud_3_ = sensor::PointCloud(points_dbg, intensities_dbg);

    //--------------------- pcd file data -----------------

    int ptcount=0;
    // pc2 points number: 33024
    for (const pcl::PointXYZ& pointxyz : pclcloud->points)
    {
	    ptcount++;
	    if (ptcount > maxpt) break;
	Eigen::Vector3f point;
        //LOG(INFO) << "point: "<< pointxyz;
	point(0)=pointxyz.x;
	point(1)=pointxyz.y;
	point(2)=pointxyz.z;
      	points.push_back({point});
        intensities.push_back(50);

      hybrid_grid_2_.SetProbability(
          hybrid_grid_2_.GetCellIndex(expected_pose_.cast<float>() * point), 1.);
      intensity_hybrid_grid_2_.AddIntensity(
          intensity_hybrid_grid_2_.GetCellIndex(expected_pose_.cast<float>() *
                                              point),
          50);
    }
    std::cout<< "pcd RangefinderPoint: ";
    for(int i=0;i<10;i++){
    	std::cout<<  points[i].position(0) << ", " <<points[i].position(1) <<", "<<points[i].position(2) <<std::endl;
    }
    point_cloud_2_ = sensor::PointCloud(points, intensities);
  }

  void init_pcl_viewer(std::string title)
  {
	  // Initializing point cloud visualizer
 // pcl::visualization::PCLVisualizer::Ptr
  viewer_final.reset( new pcl::visualization::PCLVisualizer (title));
  viewer_final->setBackgroundColor (0, 0, 0);
  // Coloring and visualizing target cloud (red).
//  viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
  // Starting visualizer
  viewer_final->addCoordinateSystem (1.0, "global");
  viewer_final->initCameraParameters ();
  viewer_final->setCameraPosition(0, 0, -4, 10, 5, 10, 10, 0, 10);

  }

  void wait_pcl()
  {
  // Wait until visualizer window is closed.
	std::chrono::milliseconds dura( 100 );
	while (!viewer_final->wasStopped ())
	{
	     	viewer_final->spinOnce (100);
		std::this_thread::sleep_for(dura);
	}
  }

  void show_pcl_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, std::string title)
  {
	init_pcl_viewer(title); //need to init pcl every time it was closed.
  	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
		target_color (target_cloud, 255, 0, 0);

	viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");

	wait_pcl();
	/*
  // Wait until visualizer window is closed.
	std::chrono::milliseconds dura( 100 );
	while (!viewer_final->wasStopped ())
	{
	     	viewer_final->spinOnce (100);
		std::this_thread::sleep_for(dura);
	}
	*/
  }

  void show_pcl_2cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2, std::string title)
  {
	  init_pcl_viewer(title); //need to init pcl every time it was closed.

	  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
  		color1 (cloud1, 255, 0, 0);

	  viewer_final->addPointCloud<pcl::PointXYZ> (cloud1, color1, "target cloud");
	  viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                  1, "target cloud");
	  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
  		color2 (cloud2, 0, 255, 0);
	  viewer_final->addPointCloud<pcl::PointXYZ> (cloud2, color2, "output cloud");
	  viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                  1, "output cloud");
	  wait_pcl();

  }

/********************************************************************/
  HybridGrid hybrid_grid_;
  IntensityHybridGrid intensity_hybrid_grid_;
  transform::Rigid3d expected_pose_;
  sensor::PointCloud point_cloud_;
  proto::CeresScanMatcherOptions3D options_;
  std::unique_ptr<CeresScanMatcher3D> ceres_scan_matcher_;

  pcl::visualization::PCLVisualizer::Ptr viewer_final;

  HybridGrid hybrid_grid_2_ = HybridGrid(1.f);
  IntensityHybridGrid intensity_hybrid_grid_2_ = IntensityHybridGrid(1.f);
  HybridGrid hybrid_grid_3_ = HybridGrid(1.f);
  IntensityHybridGrid intensity_hybrid_grid_3_ = IntensityHybridGrid(1.f);
  sensor::PointCloud point_cloud_2_;
  sensor::PointCloud point_cloud_3_;
  sensor::PointCloud point_cloud_4_; // for scan pcd high res
  HybridGrid * hybrid_grid_4_; // = HybridGrid(0.4f); // for submap pcd high res
  HybridGrid * hybrid_grid_5_; // = HybridGrid(0.4f); // for submap pcd low res
  IntensityHybridGrid intensity_hybrid_grid_4_ = IntensityHybridGrid(1.f);
  IntensityHybridGrid intensity_hybrid_grid_5_ = IntensityHybridGrid(1.f);
  sensor::PointCloud point_cloud_5_; // for scan pcd low res

    std::map<std::string, std::string> mymap;
    std::string myoptstr;
};

/********************************************************************/
/********************************************************************/
TEST_F(CeresScanMatcher3DTest, PerfectEstimate) {
	LOG(INFO) << "test ceres scan matching with pcd files :";
	        // Loading first scan, as reference or target cloud
        pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
        std::string pcdfn1;
	pcdfn1 = mymap["pcdfilename"];
//	pcdfn1= "/home/student/Documents/AirSim/ros/src/hdl_graph_slam/mapdata_13.pcd";
	pcl::io::loadPCDFile<pcl::PointXYZ> (pcdfn1, *target_cloud);
	LOG(INFO) << *target_cloud;
	initpc2(target_cloud);

	//init_pcl_viewer();
	show_pcl_cloud(target_cloud, pcdfn1);

	LOG(INFO) << "new test case: ";

	TestFromInitialPose2(
		transform::Rigid3d::Translation(Eigen::Vector3d(-1.5, 0., 0.)));

	LOG(INFO) << "orig test case: ";

	TestFromInitialPose(
      		transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.)));
}

TEST_F(CeresScanMatcher3DTest, AlongX) {
  ceres_scan_matcher_.reset(new CeresScanMatcher3D(options_));
  TestFromInitialPose(
      transform::Rigid3d::Translation(Eigen::Vector3d(-2.8, 0., 0.)));
}

TEST_F(CeresScanMatcher3DTest, AlongZ) {
  TestFromInitialPose(
      transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2)));
}

TEST_F(CeresScanMatcher3DTest, AlongXYZ) {
  TestFromInitialPose(
      transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2)));
}

TEST_F(CeresScanMatcher3DTest, FullPoseCorrection) {
  // We try to find the rotation around z...
  const auto additional_transform = transform::Rigid3d::Rotation(
      Eigen::AngleAxisd(0.05, Eigen::Vector3d(0., 0., 1.)));
  point_cloud_ = sensor::TransformPointCloud(
      point_cloud_, additional_transform.cast<float>());
  expected_pose_ = expected_pose_ * additional_transform.inverse();
  // ...starting initially with rotation around x.
  TestFromInitialPose(
      transform::Rigid3d(Eigen::Vector3d(-0.95, -0.05, 0.05),
                         Eigen::AngleAxisd(0.05, Eigen::Vector3d(1., 0., 0.))));
}

}  // namespace
}  // namespace scan_matching
}  // namespace mapping
}  // namespace cartographer


// -*- C++ -*-
//
// Package:     MVAComputer
// Class  :     ProcSort
//

// Implementation:
//     Sorts the input variables. Each input variable must appear in the
//     multiplicity as the others. One variable is the sorting "leader" by
//     which all variables are reordered the same way. The ordering is
//     determined by either ascending or descending order of the leader.
//
// Author:      Christophe Saout
// Created:     Sun Sep 16 14:52 CEST 2007
//

#include <cstdlib>
#include <algorithm>
#include <iterator>
#include <vector>

#include <boost/iterator/transform_iterator.hpp>

#include "PhysicsTools/MVAComputer/interface/VarProcessor.h"
#include "PhysicsTools/MVAComputer/interface/Calibration.h"

using namespace PhysicsTools;

namespace {  // anonymous

  class ProcSort : public VarProcessor {
  public:
    typedef VarProcessor::Registry::Registry<ProcSort, Calibration::ProcSort> Registry;

    ProcSort(const char *name, const Calibration::ProcSort *calib, const MVAComputer *computer);
    ~ProcSort() override {}

    void configure(ConfIterator iter, unsigned int n) override;
    void eval(ValueIterator iter, unsigned int n) const override;
    std::vector<double> deriv(ValueIterator iter, unsigned int n) const override;

  private:
    unsigned int leader;
    bool descending;
  };

  ProcSort::Registry registry("ProcSort");

  ProcSort::ProcSort(const char *name, const Calibration::ProcSort *calib, const MVAComputer *computer)
      : VarProcessor(name, calib, computer), leader(calib->sortByIndex), descending(calib->descending) {}

  void ProcSort::configure(ConfIterator iter, unsigned int n) {
    if (leader >= n)
      return;

    iter << iter;
    while (iter)
      iter << iter++(Variable::FLAG_ALL);
  }

  namespace {  // anonymous
    struct LeaderLookup {
      inline LeaderLookup() {}
      inline LeaderLookup(const double *values) : values(values) {}

      inline double operator()(int index) const { return values[index]; }

      const double *values;
    };
  }  // anonymous namespace

  void ProcSort::eval(ValueIterator iter, unsigned int n) const {
    ValueIterator leaderIter = iter;
    for (unsigned int i = 0; i < leader; i++, leaderIter++)
      ;
    unsigned int size = leaderIter.size();
    LeaderLookup lookup(leaderIter.begin());

    int *sort = (int *)alloca(size * sizeof(int));
    for (unsigned int i = 0; i < size; i++)
      sort[i] = (int)i;

    boost::transform_iterator<LeaderLookup, int *> begin(sort, lookup);
    boost::transform_iterator<LeaderLookup, int *> end = begin;

    for (unsigned int i = 0; i < size; i++, end++) {
      unsigned int pos = std::lower_bound(begin, end, leaderIter[i]) - begin;
      std::memmove(sort + (pos + 1), sort + pos, (i - pos) * sizeof(*sort));
      sort[pos] = i;
    }

    if (descending)
      std::reverse(sort, sort + size);

    for (unsigned int i = 0; i < size; i++)
      iter << (double)sort[i];
    iter();

    while (iter) {
      for (unsigned int i = 0; i < size; i++)
        iter << iter[sort[i]];
      iter();
      iter++;
    }
  }

  std::vector<double> ProcSort::deriv(ValueIterator iter, unsigned int n) const {
    unsigned int in = 0;
    for (ValueIterator iter2 = iter; iter2; ++iter2)
      in += iter2.size();

    ValueIterator leaderIter = iter;
    for (unsigned int i = 0; i < leader; i++, leaderIter++)
      ;
    unsigned int size = leaderIter.size();
    LeaderLookup lookup(leaderIter.begin());

    std::vector<int> sort;
    for (unsigned int i = 0; i < size; i++)
      sort.push_back((int)i);

    boost::transform_iterator<LeaderLookup, std::vector<int>::const_iterator> begin(sort.begin(), lookup);
    boost::transform_iterator<LeaderLookup, std::vector<int>::const_iterator> end = begin;

    for (unsigned int i = 0; i < size; i++, end++) {
      unsigned int pos = std::lower_bound(begin, end, leaderIter[i]) - begin;
      std::memmove(&sort.front() + (pos + 1), &sort.front() + pos, (i - pos) * sizeof(sort.front()));
      sort[pos] = i;
    }

    if (descending)
      std::reverse(sort.begin(), sort.end());

    std::vector<double> result(size * in, 0.0);

    for (unsigned int pos = 0; iter; pos += (iter++).size()) {
      for (unsigned int i = 0; i < size; i++) {
        unsigned int row = result.size();
        result.resize(row + in);
        result[row + pos + sort[i]] = 1.0;
      }
    }

    return result;
  }

}  // anonymous namespace


#include "mj_sim_impl.h"
#include "mj_utils.h"

#include <cassert>
#include <chrono>
#include <type_traits>

#include "MujocoClient.h"
#include "config.h"

#include "backends/imgui_impl_glfw.h"
#include "backends/imgui_impl_opengl3.h"

#include "implot.h"

#include "ImGuizmo.h"

#include "MujocoClient.h"

#include <boost/filesystem.hpp>
namespace bfs = boost::filesystem;

#include <mc_rtc/version.h>

namespace mc_mujoco
{

double MjRobot::PD(double jnt_id, double q_ref, double q, double qdot_ref, double qdot)
{
  double p_error = q_ref - q;
  double v_error = qdot_ref - qdot;
  double ret = (kp[jnt_id] * p_error + kd[jnt_id] * v_error);
  return ret;
}

/* Load PD gains from file (taken from RobotHardware/robot.cpp) */
bool MjRobot::loadGain(const std::string & path_to_pd, const std::vector<std::string> & joints)
{
  std::ifstream strm(path_to_pd.c_str());
  if(!strm.is_open())
  {
    mc_rtc::log::error_and_throw<std::runtime_error>("[mc_mujoco] Cannot open PD gains file for {} at {}", name,
                                                     path_to_pd);
  }

  int num_joints = joints.size();
  if(!num_joints)
  {
    return false;
  }
  std::vector<double> default_pgain(num_joints, 0);
  std::vector<double> default_dgain(num_joints, 0);
  for(int i = 0; i < num_joints; i++)
  {
    std::string str;
    bool getlinep;
    while((getlinep = !!(std::getline(strm, str))))
    {
      if(str.empty())
      {
        continue;
      }
      if(str[0] == '#')
      {
        continue;
      }
      double tmp;
      std::istringstream sstrm(str);
      sstrm >> tmp;
      default_pgain[i] = tmp;
      if(sstrm.eof()) break;

      sstrm >> tmp;
      default_dgain[i] = tmp;
      if(sstrm.eof()) break;
      break;
    }
    if(!getlinep)
    {
      if(i < num_joints)
      {
        mc_rtc::log::error(
            "[mc_mujoco] loadGain error: size of gains reading from file ({}) does not match size of joints",
            path_to_pd);
      }
      break;
    }
  }

  strm.close();
  mc_rtc::log::info("[mc_mujoco] Gains for {}", name);
  for(unsigned int i = 0; i < num_joints; i++)
  {
    mc_rtc::log::info("[mc_mujoco] {}, pgain = {}, dgain = {}", joints[i], default_pgain[i], default_dgain[i]);
    // push to kp and kd
    default_kp.push_back(default_pgain[i]);
    default_kd.push_back(default_dgain[i]);
    kp.push_back(default_pgain[i]);
    kd.push_back(default_dgain[i]);
  }
  return true;
}

MjSimImpl::MjSimImpl(const MjConfiguration & config)
: controller(std::make_unique<mc_control::MCGlobalController>(config.mc_config)), config(config)
{
  auto get_robot_cfg_path = [&](const std::string & robot_name) -> std::string {
    if(bfs::exists(bfs::path(mc_mujoco::USER_FOLDER) / (robot_name + ".yaml")))
    {
      return (bfs::path(mc_mujoco::USER_FOLDER) / (robot_name + ".yaml")).string();
    }
    else if(bfs::exists(bfs::path(mc_mujoco::SHARE_FOLDER) / (robot_name + ".yaml")))
    {
      return (bfs::path(mc_mujoco::SHARE_FOLDER) / (robot_name + ".yaml")).string();
    }
    else
    {
      return "";
    }
  };

  std::vector<std::string> mujRobots;
  std::vector<std::string> xmlFiles;
  std::vector<std::string> pdGainsFiles;
#if MC_RTC_VERSION_MAJOR > 1
  for(const auto & r_ptr : controller->robots())
  {
    const auto & r = *r_ptr;
#else
  for(const auto & r : controller->robots())
  {
#endif
    const auto & robot_cfg_path = get_robot_cfg_path(r.module().name);
    if(robot_cfg_path.size())
    {
      auto robot_cfg = mc_rtc::Configuration(robot_cfg_path);
      if(!robot_cfg.has("xmlModelPath"))
      {
        mc_rtc::log::error_and_throw<std::runtime_error>("Missing xmlModelPath in {}", robot_cfg_path);
      }
      mujRobots.push_back(r.name());
      xmlFiles.push_back(static_cast<std::string>(robot_cfg("xmlModelPath")));
      pdGainsFiles.push_back(robot_cfg("pdGainsPath", std::string("")));
      if(!bfs::exists(xmlFiles.back()))
      {
        mc_rtc::log::error_and_throw<std::runtime_error>("[mc_mujoco] XML model cannot be found at {}",
                                                         xmlFiles.back());
      }
    }
  }

  if(!xmlFiles.size())
  {
    mc_rtc::log::error_and_throw<std::runtime_error>("No Mujoco model associated to any robots in the controller");
  }

  // initial mujoco here and load XML model
  bool initialized = mujoco_init(this, mujRobots, xmlFiles);
  if(!initialized)
  {
    mc_rtc::log::error_and_throw<std::runtime_error>("[mc_mujoco] Initialized failed.");
  }

  // read PD gains from file
  for(size_t i = 0; i < robots.size(); ++i)
  {
    auto & r = robots[i];
    bool has_motor =
        std::any_of(r.mj_mot_names.begin(), r.mj_mot_names.end(), [](const std::string & m) { return m.size() != 0; });
    const auto & robot = controller->robot(r.name);
    if(robot.mb().nrDof() == 0 || (robot.mb().nrDof() == 6 && robot.mb().joint(0).dof() == 6) || !has_motor)
    {
      continue;
    }
    if(!bfs::exists(pdGainsFiles[i]))
    {
      mc_rtc::log::error_and_throw<std::runtime_error>("[mc_mujoco] PD gains file for {} cannot be found at {}", r.name,
                                                       pdGainsFiles.back());
    }
    r.loadGain(pdGainsFiles[i], controller->robots().robot(r.name).module().ref_joint_order());
  }

  if(config.with_visualization)
  {
    mujoco_create_window(this);
    if(config.with_mc_rtc_gui)
    {
      client = std::make_unique<MujocoClient>();
    }
  }
  mc_rtc::log::info("[mc_mujoco] Initialized successful.");
}

void MjSimImpl::cleanup()
{
  mujoco_cleanup(this);
}

void MjRobot::initialize(mjModel * model, const mc_rbdyn::Robot & robot)
{
  mj_jnt_ids.resize(0);
  for(const auto & j : mj_jnt_names)
  {
    mj_jnt_ids.push_back(mj_name2id(model, mjOBJ_JOINT, j.c_str()));
  }
  auto fill_acuator_ids = [&](const std::vector<std::string> & names, std::vector<int> & ids) {
    ids.resize(0);
    for(const auto & n : names)
    {
      if(n.size())
      {
        ids.push_back(mj_name2id(model, mjOBJ_ACTUATOR, n.c_str()));
      }
      else
      {
        ids.push_back(-1);
      }
    }
  };
  fill_acuator_ids(mj_mot_names, mj_mot_ids);
  fill_acuator_ids(mj_pos_act_names, mj_pos_act_ids);
  fill_acuator_ids(mj_vel_act_names, mj_vel_act_ids);
  if(root_body.size())
  {
    root_body_id = mj_name2id(model, mjOBJ_BODY, root_body.c_str());
  }
  auto init_sensor_id = [&](const char * mj_name, const char * mc_name, const std::string & sensor_name,
                            const char * suffix, mjtSensor type, std::unordered_map<std::string, int> & mapping) {
    auto mj_sensor = prefixed(fmt::format("{}_{}", sensor_name, suffix));
    auto sensor_id = mujoco_get_sensor_id(*model, mj_sensor, type);
    if(sensor_id == -1)
    {
      mc_rtc::log::error("[mc_mujoco] No MuJoCo {} for {} {} in {}, expected to find a {} named {}", mj_name,
                         sensor_name, mc_name, name, mj_name, mj_sensor);
    }
    mapping[sensor_name] = sensor_id;
  };
  for(const auto & fs : robot.module().forceSensors())
  {
    wrenches[fs.name()] = sva::ForceVecd(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0));
    init_sensor_id("force sensor", "force sensor", fs.name(), "fsensor", mjSENS_FORCE, mc_fs_to_mj_fsensor_id);
    init_sensor_id("torque sensor", "force sensor", fs.name(), "tsensor", mjSENS_TORQUE, mc_fs_to_mj_tsensor_id);
  }
  for(const auto & bs : robot.bodySensors())
  {
    if(bs.name() == "FloatingBase" || bs.name().empty())
    {
      continue;
    }
    gyros[bs.name()] = Eigen::Vector3d::Zero();
    accelerometers[bs.name()] = Eigen::Vector3d::Zero();
    init_sensor_id("gyro sensor", "body sensor", bs.name(), "gyro", mjSENS_GYRO, mc_bs_to_mj_gyro_id);
    init_sensor_id("accelerometer sensor", "body sensor", bs.name(), "accelerometer", mjSENS_ACCELEROMETER,
                   mc_bs_to_mj_accelerometer_id);
  }
  reset(robot);
}

void MjRobot::reset(const mc_rbdyn::Robot & robot)
{
  const auto & mbc = robot.mbc();
  const auto & rjo = robot.module().ref_joint_order();
  if(rjo.size() != mj_jnt_names.size())
  {
    mc_rtc::log::error_and_throw<std::runtime_error>(
        "[mc_mujoco] Missmatch in model for {}, reference joint order has {} joints but MuJoCo models has {} joints",
        name, rjo.size(), mj_jnt_names.size());
  }
  mj_to_mbc.resize(0);
  mj_prev_ctrl_q.resize(0);
  mj_prev_ctrl_alpha.resize(0);
  mj_jnt_to_rjo.resize(0);
  mj_to_mbc.resize(0);
  encoders = std::vector<double>(rjo.size(), 0.0);
  alphas = std::vector<double>(rjo.size(), 0.0);
  torques = std::vector<double>(rjo.size(), 0.0);
  for(const auto & mj_jn : mj_jnt_names)
  {
    const auto & jn = [&]() {
      if(prefix.size())
      {
        return mj_jn.substr(prefix.size() + 1);
      }
      return mj_jn;
    }();
    auto rjo_it = std::find(rjo.begin(), rjo.end(), jn);
    int rjo_idx = -1;
    if(rjo_it != rjo.end())
    {
      rjo_idx = std::distance(rjo.begin(), rjo_it);
    }
    mj_jnt_to_rjo.push_back(rjo_idx);
    if(robot.hasJoint(jn))
    {
      auto jIndex = robot.jointIndexByName(jn);
      mj_to_mbc.push_back(jIndex);
      if(robot.mb().joint(jIndex).dof() != 1)
      {
        mc_rtc::log::error_and_throw<std::runtime_error>(
            "[mc_mujoco] Only support revolute and prismatic joint for control");
      }
      mj_prev_ctrl_q.push_back(robot.mbc().q[jIndex][0]);
      mj_prev_ctrl_alpha.push_back(robot.mbc().alpha[jIndex][0]);
      if(rjo_idx != -1)
      {
        encoders[rjo_idx] = mj_prev_ctrl_q.back();
        alphas[rjo_idx] = mj_prev_ctrl_alpha.back();
      }
    }
    else
    {
      mj_to_mbc.push_back(-1);
    }
  }
  mj_ctrl = mj_prev_ctrl_q;
  mj_next_ctrl_q = mj_prev_ctrl_q;
  mj_next_ctrl_alpha = mj_prev_ctrl_alpha;

  // reset the PD gains to default values
  kp = default_kp;
  kd = default_kd;
}

void MjSimImpl::setSimulationInitialState()
{
  if(controller)
  {
    qInit.resize(0);
    alphaInit.resize(0);
    for(auto & r : robots)
    {
      const auto & robot = controller->robots().robot(r.name);
      r.initialize(model, robot);
      if(r.root_joint.size())
      {
        r.root_qpos_idx = qInit.size();
        r.root_qvel_idx = alphaInit.size();
        if(robot.mb().joint(0).dof() == 6)
        {
          const auto & t = robot.posW().translation();
          for(size_t i = 0; i < 3; ++i)
          {
            qInit.push_back(t[i]);
            // push linear/angular velocities
            alphaInit.push_back(0);
            alphaInit.push_back(0);
          }
          Eigen::Quaterniond q = Eigen::Quaterniond(robot.posW().rotation()).inverse();
          qInit.push_back(q.w());
          qInit.push_back(q.x());
          qInit.push_back(q.y());
          qInit.push_back(q.z());
        }
      }
      else if(r.root_body_id != -1)
      {
        const auto & t = robot.posW().translation();
        model->body_pos[3 * r.root_body_id + 0] = t.x();
        model->body_pos[3 * r.root_body_id + 1] = t.y();
        model->body_pos[3 * r.root_body_id + 2] = t.z();
        Eigen::Quaterniond q = Eigen::Quaterniond(robot.posW().rotation()).inverse();
        model->body_quat[4 * r.root_body_id + 0] = q.w();
        model->body_quat[4 * r.root_body_id + 1] = q.x();
        model->body_quat[4 * r.root_body_id + 2] = q.y();
        model->body_quat[4 * r.root_body_id + 3] = q.z();
      }
      for(size_t i = 0; i < r.mj_jnt_names.size(); ++i)
      {
        qInit.push_back(r.encoders[r.mj_jnt_to_rjo[i]]);
        alphaInit.push_back(r.alphas[r.mj_jnt_to_rjo[i]]);
      }
    }
  }
  // set initial qpos, qvel in mujoco
  if(!mujoco_set_const(model, data, qInit, alphaInit))
  {
    mc_rtc::log::error_and_throw<std::runtime_error>("[mc_mujoco] Set inital state failed.");
  }
  mj_forward(model, data);
}

void MjSimImpl::makeDatastoreCalls()
{
  for(auto & r : robots)
  {
    // make_call for setting pd gains (for all joints)
    controller->controller().datastore().make_call(
        r.name + "::SetPDGains", [this, &r](const std::vector<double> & p_vec, const std::vector<double> & d_vec) {
          const auto & rjo = controller->robots().robot(r.name).module().ref_joint_order();
          if(p_vec.size() != rjo.size())
          {
            mc_rtc::log::warning("[mc_mujoco] {}::SetPDGains failed. p_vec size({})!=ref_joint_order size({})", r.name,
                                 p_vec.size(), rjo.size());
            return false;
          }
          if(d_vec.size() != rjo.size())
          {
            mc_rtc::log::warning("[mc_mujoco] {}::SetPDGains failed. d_vec size({})!=ref_joint_order size({})", r.name,
                                 d_vec.size(), rjo.size());
            return false;
          }
          r.kp = p_vec;
          r.kd = d_vec;
          return true;
        });

    // make_call for setting pd gains (by name)
    controller->controller().datastore().make_call(
        r.name + "::SetPDGainsByName", [this, &r](const std::string & jn, double p, double d) {
          const auto & rjo = controller->robots().robot(r.name).module().ref_joint_order();
          auto rjo_it = std::find(rjo.begin(), rjo.end(), jn);
          if(rjo_it == rjo.end())
          {
            mc_rtc::log::warning("[mc_mujoco] {}::SetPDGainsByName failed. Joint {} not found in ref_joint_order.",
                                 r.name, jn);
            return false;
          }
          int rjo_idx = std::distance(rjo.begin(), rjo_it);
          r.kp[rjo_idx] = p;
          r.kd[rjo_idx] = d;
          return true;
        });

    // make_call for reading pd gains (for all joints)
    controller->controller().datastore().make_call(
        r.name + "::GetPDGains", [this, &r](std::vector<double> & p_vec, std::vector<double> & d_vec) {
          p_vec.resize(0);
          d_vec.resize(0);
          p_vec = r.kp;
          d_vec = r.kd;
          const auto & rjo = controller->robots().robot(r.name).module().ref_joint_order();
          if(p_vec.size() != rjo.size())
          {
            mc_rtc::log::warning("[mc_mujoco] {}::GetPDGains failed. p_vec size({})!=ref_joint_order size({})", r.name,
                                 p_vec.size(), rjo.size());
            return false;
          }
          if(d_vec.size() != rjo.size())
          {
            mc_rtc::log::warning("[mc_mujoco] {}::GetPDGains failed. d_vec size({})!=ref_joint_order size({})", r.name,
                                 d_vec.size(), rjo.size());
            return false;
          }
          return true;
        });

    // make_call for reading pd gains (by name)
    controller->controller().datastore().make_call(
        r.name + "::GetPDGainsByName", [this, &r](const std::string & jn, double & p, double & d) {
          const auto & rjo = controller->robots().robot(r.name).module().ref_joint_order();
          auto rjo_it = std::find(rjo.begin(), rjo.end(), jn);
          if(rjo_it == rjo.end())
          {
            mc_rtc::log::warning("[mc_mujoco] {}::GetPDGainsByName failed. Joint {} not found in ref_joint_order.",
                                 r.name, jn);
            return false;
          }
          int rjo_idx = std::distance(rjo.begin(), rjo_it);
          p = r.kp[rjo_idx];
          d = r.kd[rjo_idx];
          return true;
        });
  }
}

void MjSimImpl::startSimulation()
{
  setSimulationInitialState();
  if(!config.with_controller)
  {
    controller.reset();
    return;
  }

  makeDatastoreCalls();

  // get sim timestep and set the frameskip parameter
  double simTimestep = model->opt.timestep;
  frameskip_ = std::round(controller->timestep() / simTimestep);
  mc_rtc::log::info("[mc_mujoco] MC-RTC timestep: {}. MJ timestep: {}", controller->timestep(), simTimestep);
  mc_rtc::log::info("[mc_mujoco] Hence, Frameskip: {}", frameskip_);

  for(const auto & r : robots)
  {
    controller->setEncoderValues(r.name, r.encoders);
  }
  controller->init(robots[0].encoders);
  controller->running = true;
}

void MjRobot::updateSensors(mc_control::MCGlobalController * gc, mjModel * model, mjData * data)
{
  for(size_t i = 0; i < mj_jnt_ids.size(); ++i)
  {
    if(mj_jnt_to_rjo[i] == -1)
    {
      continue;
    }
    encoders[mj_jnt_to_rjo[i]] = data->qpos[model->jnt_qposadr[mj_jnt_ids[i]]];
    alphas[mj_jnt_to_rjo[i]] = data->qvel[model->jnt_dofadr[mj_jnt_ids[i]]];
  }
  for(size_t i = 0; i < mj_mot_ids.size(); ++i)
  {
    if(mj_jnt_to_rjo[i] == -1)
    {
      continue;
    }
    torques[mj_jnt_to_rjo[i]] = data->qfrc_actuator[model->jnt_dofadr[mj_jnt_ids[i]]];
  }
  if(!gc)
  {
    return;
  }
  auto & robot = gc->controller().robots().robot(name);

  // Body sensor updates
  if(root_qpos_idx != -1)
  {
    root_pos = Eigen::Map<Eigen::Vector3d>(&data->qpos[root_qpos_idx]);
    root_ori.w() = data->qpos[root_qpos_idx + 3];
    root_ori.x() = data->qpos[root_qpos_idx + 4];
    root_ori.y() = data->qpos[root_qpos_idx + 5];
    root_ori.z() = data->qpos[root_qpos_idx + 6];
    root_ori = root_ori.inverse();
    root_linvel = Eigen::Map<Eigen::Vector3d>(&data->qvel[root_qvel_idx]);
    root_angvel = Eigen::Map<Eigen::Vector3d>(&data->qvel[root_qvel_idx + 3]);
    root_linacc = Eigen::Map<Eigen::Vector3d>(&data->qacc[root_qvel_idx]);
    root_angacc = Eigen::Map<Eigen::Vector3d>(&data->qacc[root_qvel_idx + 3]);
    if(robot.hasBodySensor("FloatingBase"))
    {
      gc->setSensorPositions(name, {{"FloatingBase", root_pos}});
      gc->setSensorOrientations(name, {{"FloatingBase", root_ori}});
      gc->setSensorLinearVelocities(name, {{"FloatingBase", root_linvel}});
      gc->setSensorAngularVelocities(name, {{"FloatingBase", root_angvel}});
      gc->setSensorLinearAccelerations(name, {{"FloatingBase", root_linacc}});
      // FIXME Not implemented in mc_rtc
      // gc->setSensorAngularAccelerations(name, {{"FloatingBase", root_angacc}});
    }
  }

  // Gyro update
  for(auto & gyro : gyros)
  {
    mujoco_get_sensordata(*model, *data, mc_bs_to_mj_gyro_id[gyro.first], gyro.second.data());
  }
  gc->setSensorAngularVelocities(name, gyros);

  // Accelerometers update
  for(auto & accelerometer : accelerometers)
  {
    mujoco_get_sensordata(*model, *data, mc_bs_to_mj_accelerometer_id[accelerometer.first],
                          accelerometer.second.data());
  }
  gc->setSensorLinearAccelerations(name, accelerometers);

  // Force sensor update
  for(auto & fs : wrenches)
  {
    mujoco_get_sensordata(*model, *data, mc_fs_to_mj_fsensor_id[fs.first], fs.second.force().data());
    mujoco_get_sensordata(*model, *data, mc_fs_to_mj_tsensor_id[fs.first], fs.second.couple().data());
    fs.second *= -1;
  }
  gc->setWrenches(name, wrenches);

  // Joint sensor updates
  gc->setEncoderValues(name, encoders);
  gc->setEncoderVelocities(name, alphas);
  gc->setJointTorques(name, torques);
}

void MjSimImpl::updateData()
{
  for(auto & r : robots)
  {
    r.updateSensors(controller.get(), model, data);
  }
}

void MjRobot::updateControl(const mc_rbdyn::Robot & robot)
{
  mj_prev_ctrl_q = mj_next_ctrl_q;
  mj_prev_ctrl_alpha = mj_next_ctrl_alpha;
  size_t ctrl_idx = 0;
  for(size_t i = 0; i < mj_to_mbc.size(); ++i)
  {
    auto jIndex = mj_to_mbc[i];
    if(jIndex != -1)
    {
      mj_next_ctrl_q[ctrl_idx] = robot.mbc().q[jIndex][0];
      mj_next_ctrl_alpha[ctrl_idx] = robot.mbc().alpha[jIndex][0];
      ctrl_idx++;
    }
  }
}

void MjRobot::sendControl(const mjModel & model, mjData & data, size_t interp_idx, size_t frameskip_)
{
  for(size_t i = 0; i < mj_ctrl.size(); ++i)
  {
    auto mot_id = mj_mot_ids[i];
    auto pos_act_id = mj_pos_act_ids[i];
    auto vel_act_id = mj_vel_act_ids[i];
    auto rjo_id = mj_jnt_to_rjo[i];
    if(rjo_id == -1)
    {
      continue;
    }
    // compute desired q using interpolation
    double q_ref = (interp_idx + 1) * (mj_next_ctrl_q[i] - mj_prev_ctrl_q[i]) / frameskip_;
    q_ref += mj_prev_ctrl_q[i];
    // compute desired alpha using interpolation
    double alpha_ref = (interp_idx + 1) * (mj_next_ctrl_alpha[i] - mj_prev_ctrl_alpha[i]) / frameskip_;
    alpha_ref += mj_prev_ctrl_alpha[i];
    if(mot_id != -1)
    {
      // compute desired torque using PD control
      mj_ctrl[i] = PD(i, q_ref, encoders[rjo_id], alpha_ref, alphas[rjo_id]);
      double ratio = model.actuator_gear[6 * mot_id];
      data.ctrl[mot_id] = mj_ctrl[i] / ratio;
    }
    if(pos_act_id != -1)
    {
      data.ctrl[pos_act_id] = q_ref;
    }
    if(vel_act_id != -1)
    {
      data.ctrl[vel_act_id] = alpha_ref;
    }
  }
}

bool MjSimImpl::controlStep()
{
  auto interp_idx = iterCount_ % frameskip_;
  // After every frameskip iters
  if(config.with_controller && interp_idx == 0)
  {
    // run the controller
    if(!controller->run())
    {
      return true;
    }
    for(auto & r : robots)
    {
      r.updateControl(controller->robots().robot(r.name));
    }
  }
  // On each control iter
  for(auto & r : robots)
  {
    r.sendControl(*model, *data, interp_idx, frameskip_);
  }
  iterCount_++;
  return false;
}

void MjSimImpl::simStep()
{
  // clear old perturbations, apply new
  mju_zero(data->xfrc_applied, 6 * model->nbody);
  mjv_applyPerturbPose(model, data, &pert, 0); // move mocap bodies only
  mjv_applyPerturbForce(model, data, &pert);

  // take one step in simulation
  // model.opt.timestep will be used here
  mj_step(model, data);
}

void MjSimImpl::resetSimulation(const std::map<std::string, std::vector<double>> & reset_qs,
                                const std::map<std::string, sva::PTransformd> & reset_pos)
{
  iterCount_ = 0;
  reset_simulation_ = false;
  if(controller)
  {
    controller->reset(reset_qs, reset_pos);
    for(auto & robot : robots)
    {
      robot.reset(controller->robot(robot.name));
    }
    controller->running = true;
  }
  mj_resetData(model, data);
  setSimulationInitialState();
  makeDatastoreCalls();
}

bool MjSimImpl::stepSimulation()
{
  if(reset_simulation_)
  {
    resetSimulation({}, {});
  }
  auto start_step = clock::now();
  // Only run the GUI update if the simulation is paused
  if(config.step_by_step && rem_steps == 0)
  {
    if(controller)
    {
      controller->running = false;
      controller->run();
      controller->running = true;
    }
    mj_sim_start_t = start_step;
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    return false;
  }
  if(iterCount_ > 0)
  {
    duration_us dt = start_step - mj_sim_start_t;
    mj_sync_delay += duration_us(1e6 * model->opt.timestep) - dt;
    mj_sim_dt[(iterCount_ - 1) % mj_sim_dt.size()] = dt.count();
  }
  mj_sim_start_t = start_step;
  auto do_step = [this, &start_step]() {
    {
      std::lock_guard<std::mutex> lock(rendering_mutex_);
      simStep();
    }
    updateData();
    return controlStep();
  };
  bool done = false;
  if(!config.step_by_step)
  {
    done = do_step();
  }
  if(config.step_by_step && rem_steps > 0)
  {
    // Doing 'frameskip_' steps of sim + control
    // (But controller.run() will execute only when interp_idx == 0)
    for(size_t i = 0; i < frameskip_; i++)
    {
      done = do_step() && done;
    }
    rem_steps--;
  }
  if(config.sync_real_time)
  {
    std::this_thread::sleep_until(start_step + duration_us(1e6 * model->opt.timestep) + mj_sync_delay);
  }
  return done;
}

void MjSimImpl::updateScene()
{
  // update scene and render
  std::lock_guard<std::mutex> lock(rendering_mutex_);
  mjv_updateScene(model, data, &options, &pert, &camera, mjCAT_ALL, &scene);

  if(client)
  {
    client->updateScene(scene);
  }

  // process pending GUI events, call GLFW callbacks
  glfwPollEvents();
}

bool MjSimImpl::render()
{
  if(!config.with_visualization)
  {
    return true;
  }

  // mj render
  mjr_render(uistate.rect[0], &scene, &context);

  // Render ImGui
  ImGui_ImplOpenGL3_NewFrame();
  ImGui_ImplGlfw_NewFrame();
  ImGui::NewFrame();
  ImGuizmo::BeginFrame();
  ImGuiIO & io = ImGui::GetIO();
  ImGuizmo::AllowAxisFlip(false);
  ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y);
  if(client)
  {
    client->update();
    client->draw2D(window);
    client->draw3D();
  }
  {
    auto right_margin = 5.0f;
    auto top_margin = 5.0f;
    auto width = io.DisplaySize.x - 2 * right_margin;
    auto height = io.DisplaySize.y - 2 * top_margin;
    ImGui::SetNextWindowPos({0.8f * width - right_margin, top_margin}, ImGuiCond_FirstUseEver);
    ImGui::SetNextWindowSize({0.2f * width, 0.3f * height}, ImGuiCond_FirstUseEver);
#if mjVERSION_HEADER <= 210
    ImGui::Begin(fmt::format("mc_mujoco (MuJoCo {})", mj_version()).c_str());
#else
    ImGui::Begin(fmt::format("mc_mujoco (MuJoCo {})", mj_versionString()).c_str());
#endif
    size_t nsamples = std::min(mj_sim_dt.size(), iterCount_);
    mj_sim_dt_average = 0;
    for(size_t i = 0; i < nsamples; ++i)
    {
      mj_sim_dt_average += mj_sim_dt[i] / nsamples;
    }
    ImGui::Text("Average sim time: %.2fμs", mj_sim_dt_average);
    ImGui::Text("Simulation/Real time: %.2f", mj_sim_dt_average / (1e6 * model->opt.timestep));
    if(ImGui::Checkbox("Sync with real-time", &config.sync_real_time))
    {
      if(config.sync_real_time)
      {
        mj_sync_delay = duration_us(0);
      }
    }
    ImGui::Checkbox("Step-by-step", &config.step_by_step);
    if(config.step_by_step)
    {
      auto doNStepsButton = [&](size_t n, bool final_) {
        size_t n_ms = std::ceil(n * 1000 * (controller ? controller->timestep() : model->opt.timestep));
        if(ImGui::Button(fmt::format("+{}ms", n_ms).c_str()))
        {
          rem_steps = n;
        }
        if(!final_)
        {
          ImGui::SameLine();
        }
      };
      doNStepsButton(1, false);
      doNStepsButton(5, false);
      doNStepsButton(10, false);
      doNStepsButton(50, false);
      doNStepsButton(100, true);
    }
    auto flag_to_gui = [&](const char * label, mjtVisFlag flag) {
      bool show = options.flags[flag];
      if(ImGui::Checkbox(label, &show))
      {
        options.flags[flag] = show;
      }
    };
    flag_to_gui("Show contact points [C]", mjVIS_CONTACTPOINT);
    flag_to_gui("Show contact forces [F]", mjVIS_CONTACTFORCE);
    flag_to_gui("Make Transparent [T]", mjVIS_TRANSPARENT);
    auto group_to_checkbox = [&](size_t group, bool last) {
      bool show = options.geomgroup[group];
      if(ImGui::Checkbox(fmt::format("{}", group).c_str(), &show))
      {
        options.geomgroup[group] = show;
      }
      if(!last)
      {
        ImGui::SameLine();
      }
    };
    ImGui::Text("%s", fmt::format("Visible layers [0-{}]", mjNGROUP).c_str());
    for(size_t i = 0; i < mjNGROUP; ++i)
    {
      group_to_checkbox(i, i == mjNGROUP - 1);
    }
    if(ImGui::Button("Reset simulation", ImVec2(-FLT_MIN, 0.0f)))
    {
      reset_simulation_ = true;
    }
    ImGui::End();
  }
  ImGui::Render();
  ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData());

  // swap OpenGL buffers (blocking call due to v-sync)
  glfwSwapBuffers(window);

  return !glfwWindowShouldClose(window);
}

void MjSimImpl::stopSimulation() {}

void MjSimImpl::saveGUISettings()
{
  auto user_path = bfs::path(USER_FOLDER);
  if(!bfs::exists(user_path))
  {
    if(!bfs::create_directories(user_path))
    {
      mc_rtc::log::critical("Failed to create the user directory: {}. GUI configuration will not be saved",
                            user_path.string());
      return;
    }
  }
  mc_rtc::Configuration config;
  auto camera_c = config.add("camera");
  auto lookat = camera_c.array("lookat", 3);
  for(size_t i = 0; i < 3; ++i)
  {
    lookat.push(camera.lookat[i]);
  }
  camera_c.add("distance", camera.distance);
  camera_c.add("azimuth", camera.azimuth);
  camera_c.add("elevation", camera.elevation);
  auto visualize_c = config.add("visualize");
  visualize_c.add("collisions", static_cast<bool>(options.geomgroup[0]));
  visualize_c.add("visuals", static_cast<bool>(options.geomgroup[1]));
  visualize_c.add("contact-points", static_cast<bool>(options.flags[mjVIS_CONTACTPOINT]));
  visualize_c.add("contact-forces", static_cast<bool>(options.flags[mjVIS_CONTACTFORCE]));
  auto path_out = (user_path / "mc_mujoco.yaml").string();
  config.save(path_out);
  mc_rtc::log::success("[mc_mujoco] Configuration saved to {}", path_out);
}

MjSim::MjSim(const MjConfiguration & config) : impl(new MjSimImpl(config))
{
  impl->startSimulation();
}

MjSim::~MjSim()
{
  impl->cleanup();
}

bool MjSim::stepSimulation()
{
  return impl->stepSimulation();
}

void MjSim::stopSimulation()
{
  impl->stopSimulation();
}

void MjSim::updateScene()
{
  impl->updateScene();
}

void MjSim::resetSimulation(const std::map<std::string, std::vector<double>> & reset_qs,
                            const std::map<std::string, sva::PTransformd> & reset_pos)
{
  impl->resetSimulation(reset_qs, reset_pos);
}

bool MjSim::render()
{
  return impl->render();
}

mc_control::MCGlobalController * MjSim::controller() noexcept
{
  return impl->get_controller();
}

} // namespace mc_mujoco


#ifndef DATASET_HPP
#define DATASET_HPP

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

#include "Eigen/Core"
#include <Eigen/Geometry>

class DataSet
{
public:
  DataSet(const std::string& folder);
  ~DataSet();

  bool get_next_point_cloud(Eigen::MatrixXd& P, Eigen::MatrixXd &colors, Eigen::Matrix4d& t_camera);
  Eigen::Affine3f findPlaneRotation(Eigen::MatrixXd& points);
private:
  bool operational;
  int next_file_idx;
  std::string folder_path;
  std::vector<std::string> depth_files;
  std::string rgb_ref_file_name;
  std::fstream camera_ref_file;
  Eigen::Matrix4d rOffset;

  bool get_next_camera(Eigen::Matrix4d& cam, const double timestamp);
  std::string get_next_rgb(const double timestamp);
};

#endif // DATASET_HPP


// Copyright David Abrahams 2002.
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
#include <boost/python/object/inheritance.hpp>
#include <boost/python/type_id.hpp>
#include <boost/graph/breadth_first_search.hpp>
#if _MSC_FULL_VER >= 13102171 && _MSC_FULL_VER <= 13102179
# include <boost/graph/reverse_graph.hpp>
#endif
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/bind.hpp>
#include <boost/integer_traits.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_comparison.hpp>
#include <queue>
#include <vector>
#include <functional>

//
// Procedure:
//
//      The search is a BFS over the space of (type,address) pairs
//      guided by the edges of the casting graph whose nodes
//      correspond to classes, and whose edges are traversed by
//      applying associated cast functions to an address. We use
//      vertex distance to the goal node in the cast_graph to rate the
//      paths. The vertex distance to any goal node is calculated on
//      demand and outdated by the addition of edges to the graph.

namespace boost {
namespace
{
  enum edge_cast_t { edge_cast = 8010 };
  template <class T> inline void unused_variable(const T&) { }
}

// Install properties
BOOST_INSTALL_PROPERTY(edge, cast);

namespace
{
  typedef void*(*cast_function)(void*);

  //
  // Here we put together the low-level data structures of the
  // casting graph representation.
  //
  typedef python::type_info class_id;

  // represents a graph of available casts

#if 0
  struct cast_graph
      :
#else
        typedef
#endif
        adjacency_list<vecS,vecS, bidirectionalS, no_property

      // edge index property allows us to look up edges in the connectivity matrix
      , property<edge_index_t,std::size_t

                 // The function which casts a void* from the edge's source type
                 // to its destination type.
                 , property<edge_cast_t,cast_function> > >
#if 0
  {};
#else
  cast_graph;
#endif

  typedef cast_graph::vertex_descriptor vertex_t;
  typedef cast_graph::edge_descriptor edge_t;

  struct smart_graph
  {
      typedef std::vector<std::size_t>::const_iterator node_distance_map;

      typedef std::pair<cast_graph::out_edge_iterator
                        , cast_graph::out_edge_iterator> out_edges_t;

      // Return a map of the distances from any node to the given
      // target node
      node_distance_map distances_to(vertex_t target) const
      {
          std::size_t n = num_vertices(m_topology);
          if (m_distances.size() != n * n)
          {
              m_distances.clear();
              m_distances.resize(n * n, (std::numeric_limits<std::size_t>::max)());
              m_known_vertices = n;
          }

          std::vector<std::size_t>::iterator to_target = m_distances.begin() + n * target;

          // this node hasn't been used as a target yet
          if (to_target[target] != 0)
          {
              typedef reverse_graph<cast_graph> reverse_cast_graph;
              reverse_cast_graph reverse_topology(m_topology);

              to_target[target] = 0;

              breadth_first_search(
                  reverse_topology, target
                  , visitor(
                      make_bfs_visitor(
                          record_distances(
                              make_iterator_property_map(
                                  to_target
                                  , get(vertex_index, reverse_topology)
# ifdef BOOST_NO_STD_ITERATOR_TRAITS
                                  , *to_target
# endif
                                  )
                              , on_tree_edge()
                              ))));
          }

          return to_target;
      }

      cast_graph& topology() { return m_topology; }
      cast_graph const& topology() const { return m_topology; }

      smart_graph()
          : m_known_vertices(0)
      {}

   private:
      cast_graph m_topology;
      mutable std::vector<std::size_t> m_distances;
      mutable std::size_t m_known_vertices;
  };

  smart_graph& full_graph()
  {
      static smart_graph x;
      return x;
  }

  smart_graph& up_graph()
  {
      static smart_graph x;
      return x;
  }

  //
  // Our index of class types
  //
  using boost::python::objects::dynamic_id_function;
  typedef tuples::tuple<
      class_id               // static type
      , vertex_t             // corresponding vertex
      , dynamic_id_function  // dynamic_id if polymorphic, or 0
      >
  index_entry_interface;
  typedef index_entry_interface::inherited index_entry;
  enum { ksrc_static_t, kvertex, kdynamic_id };

  typedef std::vector<index_entry> type_index_t;

  type_index_t& type_index()
  {
      static type_index_t x;
      return x;
  }

  template <class Tuple>
  struct select1st
  {
      typedef typename tuples::element<0, Tuple>::type result_type;

      result_type const& operator()(Tuple const& x) const
      {
          return tuples::get<0>(x);
      }
  };

  // map a type to a position in the index
  inline type_index_t::iterator type_position(class_id type)
  {
      typedef index_entry entry;

      return std::lower_bound(
          type_index().begin(), type_index().end()
          , boost::make_tuple(type, vertex_t(), dynamic_id_function(0))
          , boost::bind<bool>(std::less<class_id>()
               , boost::bind<class_id>(select1st<entry>(), _1)
               , boost::bind<class_id>(select1st<entry>(), _2)));
  }

  inline index_entry* seek_type(class_id type)
  {
      type_index_t::iterator p = type_position(type);
      if (p == type_index().end() || tuples::get<ksrc_static_t>(*p) != type)
          return 0;
      else
          return &*p;
  }

  // Get the entry for a type, inserting if necessary
  inline type_index_t::iterator demand_type(class_id type)
  {
      type_index_t::iterator p = type_position(type);

      if (p != type_index().end() && tuples::get<ksrc_static_t>(*p) == type)
          return p;

      vertex_t v = add_vertex(full_graph().topology());
      vertex_t v2 = add_vertex(up_graph().topology());
      unused_variable(v2);
      assert(v == v2);
      return type_index().insert(p, boost::make_tuple(type, v, dynamic_id_function(0)));
  }

  // Map a two types to a vertex in the graph, inserting if necessary
  typedef std::pair<type_index_t::iterator, type_index_t::iterator>
        type_index_iterator_pair;

  inline type_index_iterator_pair
  demand_types(class_id t1, class_id t2)
  {
      // be sure there will be no reallocation
      type_index().reserve(type_index().size() + 2);
      type_index_t::iterator first = demand_type(t1);
      type_index_t::iterator second = demand_type(t2);
      if (first == second)
          ++first;
      return std::make_pair(first, second);
  }

  struct q_elt
  {
      q_elt(std::size_t distance
            , void* src_address
            , vertex_t target
            , cast_function cast
            )
          : distance(distance)
          , src_address(src_address)
          , target(target)
          , cast(cast)
      {}

      std::size_t distance;
      void* src_address;
      vertex_t target;
      cast_function cast;

      bool operator<(q_elt const& rhs) const
      {
          return distance < rhs.distance;
      }
  };

  // Optimization:
  //
  // Given p, src_t, dst_t
  //
  // Get a pointer pd to the most-derived object
  //    if it's polymorphic, dynamic_cast to void*
  //    otherwise pd = p
  //
  // Get the most-derived typeid src_td
  //
  // ptrdiff_t offset = p - pd
  //
  // Now we can keep a cache, for [src_t, offset, src_td, dst_t] of
  // the cast transformation function to use on p and the next src_t
  // in the chain.  src_td, dst_t don't change throughout this
  // process. In order to represent unreachability, when a pair is
  // found to be unreachable, we stick a 0-returning "dead-cast"
  // function in the cache.

  // This is needed in a few places below
  inline void* identity_cast(void* p)
  {
      return p;
  }

  void* search(smart_graph const& g, void* p, vertex_t src, vertex_t dst)
  {
      // I think this test was thoroughly bogus -- dwa
      // If we know there's no path; bail now.
      // if (src > g.known_vertices() || dst > g.known_vertices())
      //    return 0;

      smart_graph::node_distance_map d(g.distances_to(dst));

      if (d[src] == (std::numeric_limits<std::size_t>::max)())
          return 0;

      typedef property_map<cast_graph,edge_cast_t>::const_type cast_map;
      cast_map casts = get(edge_cast, g.topology());

      typedef std::pair<vertex_t,void*> search_state;
      typedef std::vector<search_state> visited_t;
      visited_t visited;
      std::priority_queue<q_elt> q;

      q.push(q_elt(d[src], p, src, identity_cast));
      while (!q.empty())
      {
          q_elt top = q.top();
          q.pop();

          // Check to see if we have a real state
          void* dst_address = top.cast(top.src_address);
          if (dst_address == 0)
              continue;

          if (top.target == dst)
              return dst_address;

          search_state s(top.target,dst_address);

          visited_t::iterator pos = std::lower_bound(
              visited.begin(), visited.end(), s);

          // If already visited, continue
          if (pos != visited.end() && *pos == s)
              continue;

          visited.insert(pos, s); // mark it

          // expand it:
          smart_graph::out_edges_t edges = out_edges(s.first, g.topology());
          for (cast_graph::out_edge_iterator p = edges.first
                   , finish = edges.second
                   ; p != finish
                   ; ++p
              )
          {
              edge_t e = *p;
              q.push(q_elt(
                         d[target(e, g.topology())]
                         , dst_address
                         , target(e, g.topology())
                         , boost::get(casts, e)));
          }
      }
      return 0;
  }

  struct cache_element
  {
      typedef tuples::tuple<
          class_id              // source static type
          , class_id            // target type
          , std::ptrdiff_t      // offset within source object
          , class_id            // source dynamic type
          >::inherited key_type;

      cache_element(key_type const& k)
          : key(k)
          , offset(0)
      {}

      key_type key;
      std::ptrdiff_t offset;

      BOOST_STATIC_CONSTANT(
          std::ptrdiff_t, not_found = integer_traits<std::ptrdiff_t>::const_min);

      bool operator<(cache_element const& rhs) const
      {
          return this->key < rhs.key;
      }

      bool unreachable() const
      {
          return offset == not_found;
      }
  };

  enum { kdst_t = ksrc_static_t + 1, koffset, ksrc_dynamic_t };
  typedef std::vector<cache_element> cache_t;

  cache_t& cache()
  {
      static cache_t x;
      return x;
  }

  inline void* convert_type(void* const p, class_id src_t, class_id dst_t, bool polymorphic)
  {
      // Quickly rule out unregistered types
      index_entry* src_p = seek_type(src_t);
      if (src_p == 0)
          return 0;

      index_entry* dst_p = seek_type(dst_t);
      if (dst_p == 0)
          return 0;

      // Look up the dynamic_id function and call it to get the dynamic
      // info
      boost::python::objects::dynamic_id_t dynamic_id = polymorphic
          ? tuples::get<kdynamic_id>(*src_p)(p)
          : std::make_pair(p, src_t);

      // Look in the cache first for a quickie address translation
      std::ptrdiff_t offset = (char*)p - (char*)dynamic_id.first;

      cache_element seek(boost::make_tuple(src_t, dst_t, offset, dynamic_id.second));
      cache_t& c = cache();
      cache_t::iterator const cache_pos
          = std::lower_bound(c.begin(), c.end(), seek);

      // if found in the cache, we're done
      if (cache_pos != c.end() && cache_pos->key == seek.key)
      {
          return cache_pos->offset == cache_element::not_found
              ? 0 : (char*)p + cache_pos->offset;
      }

      // If we are starting at the most-derived type, only look in the up graph
      smart_graph const& g = polymorphic && dynamic_id.second != src_t
          ? full_graph() : up_graph();

      void* result = search(
          g, p, tuples::get<kvertex>(*src_p)
          , tuples::get<kvertex>(*dst_p));

      // update the cache
      c.insert(cache_pos, seek)->offset
          = (result == 0) ? cache_element::not_found : (char*)result - (char*)p;

      return result;
  }
}

namespace python { namespace objects {

BOOST_PYTHON_DECL void* find_dynamic_type(void* p, class_id src_t, class_id dst_t)
{
    return convert_type(p, src_t, dst_t, true);
}

BOOST_PYTHON_DECL void* find_static_type(void* p, class_id src_t, class_id dst_t)
{
    return convert_type(p, src_t, dst_t, false);
}

BOOST_PYTHON_DECL void add_cast(
    class_id src_t, class_id dst_t, cast_function cast, bool is_downcast)
{
    // adding an edge will invalidate any record of unreachability in
    // the cache.
    static std::size_t expected_cache_len = 0;
    cache_t& c = cache();
    if (c.size() > expected_cache_len)
    {
        c.erase(std::remove_if(
                    c.begin(), c.end(),
                    mem_fn(&cache_element::unreachable))
                , c.end());

        // If any new cache entries get added, we'll have to do this
        // again when the next edge is added
        expected_cache_len = c.size();
    }

    type_index_iterator_pair types = demand_types(src_t, dst_t);
    vertex_t src = tuples::get<kvertex>(*types.first);
    vertex_t dst = tuples::get<kvertex>(*types.second);

    cast_graph* const g[2] = { &up_graph().topology(), &full_graph().topology() };

    for (cast_graph*const* p = g + (is_downcast ? 1 : 0); p < g + 2; ++p)
    {
        edge_t e;
        bool added;

        tie(e, added) = add_edge(src, dst, **p);
        assert(added);

        put(get(edge_cast, **p), e, cast);
        put(get(edge_index, **p), e, num_edges(full_graph().topology()) - 1);
    }
}

BOOST_PYTHON_DECL void register_dynamic_id_aux(
    class_id static_id, dynamic_id_function get_dynamic_id)
{
    tuples::get<kdynamic_id>(*demand_type(static_id)) = get_dynamic_id;
}

}}} // namespace boost::python::objects


#include <chrono>
#include <thread>
#include <boost/test/unit_test.hpp>
#include "instruments/meter.hpp"
#include "common.hpp"

namespace mock{
	class ticker;
	static std::vector<ticker*> one_minute_tickers;
	static std::vector<ticker*> five_minute_tickers;
	static std::vector<ticker*> fifteen_minute_tickers;

	class ticker {
	public:
		typedef std::chrono::duration<double, std::chrono::nanoseconds::period> time_type;

		template<typename DurationAll, typename DurationTick>
		ticker(const DurationAll& footprint,const DurationTick& tick)
		: m_tick(std::chrono::duration_cast<std::chrono::nanoseconds>(tick)),
		  m_tick_count(0),
		  m_mark_count(0),
		  m_footprint(std::chrono::duration_cast<std::chrono::minutes>(footprint))
		{
			if(m_footprint == std::chrono::minutes(1)){
				one_minute_tickers.push_back(this);
			} else if(m_footprint == std::chrono::minutes(5)){
				five_minute_tickers.push_back(this);
			} else if(m_footprint == std::chrono::minutes(15)){
				fifteen_minute_tickers.push_back(this);
			} else {
				throw std::invalid_argument("mock ticker received a non-standard footprint.");
			}
		}

		void tick(const unsigned int& count){
			m_mark_count += count;
			++m_tick_count;
		}

		template<typename Duration = std::chrono::minutes>
	    double rate(const Duration& unit = std::chrono::minutes(1)) {
	        return 0.0;
	    }

	    time_type interval(){
	    	return std::chrono::nanoseconds(5000);
	    }

	    int count(){
	    	return m_mark_count;
	    }

	    int ticks(){
	    	return m_tick_count;
	    }
	    std::chrono::nanoseconds m_tick;
	    std::chrono::minutes m_footprint;
	    unsigned int m_tick_count;
	    unsigned int m_mark_count;
	};
}

BOOST_AUTO_TEST_CASE(meter_counting_test){
	metrics::instruments::meter m;
	BOOST_CHECK_EQUAL(m.count(),0);

	// test mark by 1, by n
	m.mark();
	BOOST_CHECK_EQUAL(m.count(),1);

	// test increment-by-delta
	m.mark(7);
	BOOST_CHECK_EQUAL(m.count(),8);
}

BOOST_AUTO_TEST_CASE(meter_rate_test){
	mock::clock::set_time(0);
	metrics::instruments::clocked_meter<mock::clock> m;
	mock::clock::set_time(1);
	BOOST_CHECK_EQUAL(m.mean_rate(),0);

	mock::clock::set_time(5);
	m.mark();
	BOOST_CHECK_CLOSE(m.mean_rate(),0.2,1e-8);

	mock::clock::set_time(100);
	m.mark(99);
	BOOST_CHECK_CLOSE(m.mean_rate(),1,1e-8);
}

BOOST_AUTO_TEST_CASE(meter_ticker_test){
	using mock::ticker;

	mock::clock::set_time(0);
	typedef metrics::instruments::clocked_meter<mock::clock,mock::ticker> mock_meter;
	mock_meter m;
	BOOST_ASSERT(mock::one_minute_tickers.size() == 1);
	BOOST_ASSERT(mock::five_minute_tickers.size() == 1);
	BOOST_ASSERT(mock::fifteen_minute_tickers.size() == 1);

	ticker& ticker_1 = *mock::one_minute_tickers[0];
	ticker& ticker_5 = *mock::five_minute_tickers[0];
	ticker& ticker_15 = *mock::fifteen_minute_tickers[0];

	BOOST_CHECK_EQUAL(ticker_1.count(),0);
	BOOST_CHECK_EQUAL(ticker_5.count(),0);
	BOOST_CHECK_EQUAL(ticker_15.count(),0);
	BOOST_CHECK_EQUAL(ticker_1.ticks(),0);
	BOOST_CHECK_EQUAL(ticker_5.ticks(),0);
	BOOST_CHECK_EQUAL(ticker_15.ticks(),0);

	mock::clock::set_time(4);

	BOOST_CHECK_EQUAL(ticker_1.count(),0);
	BOOST_CHECK_EQUAL(ticker_5.count(),0);
	BOOST_CHECK_EQUAL(ticker_15.count(),0);
	BOOST_CHECK_EQUAL(ticker_1.ticks(),0);
	BOOST_CHECK_EQUAL(ticker_5.ticks(),0);
	BOOST_CHECK_EQUAL(ticker_15.ticks(),0);

	m.mark(5);

	BOOST_CHECK_EQUAL(ticker_1.count(),0);
	BOOST_CHECK_EQUAL(ticker_5.count(),0);
	BOOST_CHECK_EQUAL(ticker_15.count(),0);
	BOOST_CHECK_EQUAL(ticker_1.ticks(),0);
	BOOST_CHECK_EQUAL(ticker_5.ticks(),0);
	BOOST_CHECK_EQUAL(ticker_15.ticks(),0);

	mock::clock::set_time(5);

	// regardless of implementation
	// getting the latest value
	// has to flush ticks
	double r = m.one_minute_rate();

	BOOST_CHECK_EQUAL(ticker_1.count(),5);
	BOOST_CHECK_EQUAL(ticker_5.count(),5);
	BOOST_CHECK_EQUAL(ticker_15.count(),5);
	BOOST_CHECK_EQUAL(ticker_1.ticks(),1);
	BOOST_CHECK_EQUAL(ticker_5.ticks(),1);
	BOOST_CHECK_EQUAL(ticker_15.ticks(),1);

	m.mark(95);
	mock::clock::set_time(100);

	r = m.fifteen_minute_rate();

	BOOST_CHECK_EQUAL(ticker_1.count(),100);
	BOOST_CHECK_EQUAL(ticker_5.count(),100);
	BOOST_CHECK_EQUAL(ticker_15.count(),100);
	BOOST_CHECK_EQUAL(ticker_1.ticks(),20);
	BOOST_CHECK_EQUAL(ticker_5.ticks(),20);
	BOOST_CHECK_EQUAL(ticker_15.ticks(),20);

	mock::clock::set_time(104);
	m.mark(1);
	mock::clock::set_time(105);
	m.mark(1);
	r = m.fifteen_minute_rate();

	// test the extra unit didn't get included in the previous interval
	BOOST_CHECK_EQUAL(ticker_1.count(),101);
	BOOST_CHECK_EQUAL(ticker_5.count(),101);
	BOOST_CHECK_EQUAL(ticker_15.count(),101);
	BOOST_CHECK_EQUAL(ticker_1.ticks(),21);
	BOOST_CHECK_EQUAL(ticker_5.ticks(),21);
	BOOST_CHECK_EQUAL(ticker_15.ticks(),21);

	mock::clock::set_time(110);
	r = m.fifteen_minute_rate();

	// test the extra unit didn't get forgotten
	BOOST_CHECK_EQUAL(ticker_1.count(), 102);
	BOOST_CHECK_EQUAL(ticker_5.count(), 102);
	BOOST_CHECK_EQUAL(ticker_15.count(), 102);
	BOOST_CHECK_EQUAL(ticker_1.ticks(), 22);
	BOOST_CHECK_EQUAL(ticker_5.ticks(), 22);
	BOOST_CHECK_EQUAL(ticker_15.ticks(), 22);

}

BOOST_AUTO_TEST_CASE(meter_ewma_test){
	mock::clock::set_time(0);
	metrics::instruments::clocked_meter<mock::clock> m;
	// mean_rate returns nan
	// is this good? It's accurate.
	//BOOST_CHECK_EQUAL(m.mean_rate(),0);
	//BOOST_CHECK_EQUAL(m.one_minute_rate(),0);
	//BOOST_CHECK_EQUAL(m.five_minute_rate(),0);
	//BOOST_CHECK_EQUAL(m.fifteen_minute_rate(),0);

	mock::clock::set_time(1);
	BOOST_CHECK_EQUAL(m.mean_rate(),0);
	BOOST_CHECK_EQUAL(m.one_minute_rate(),0);
	BOOST_CHECK_EQUAL(m.five_minute_rate(),0);
	BOOST_CHECK_EQUAL(m.fifteen_minute_rate(),0);

	bool terminate = false;
	auto force_count = [&terminate,&m](std::atomic<int>& time){
		while(!terminate){
			int local_time = mock::clock::time();
			if(time != local_time){
				int diff = local_time - time;
				m.mark(5 * diff);
				time = (int)local_time;
			}
		}
	};

	std::atomic<int> time_0((int)mock::clock::time());
	std::atomic<int> time_1((int)mock::clock::time());

	std::thread marker_0(force_count,std::ref(time_0));
	std::thread marker_1(force_count,std::ref(time_1));

	BOOST_CHECK_EQUAL(m.count(),0);
	BOOST_CHECK_EQUAL(m.mean_rate(),0);
	BOOST_CHECK_EQUAL(m.one_minute_rate(),0);
	BOOST_CHECK_EQUAL(m.five_minute_rate(),0);
	BOOST_CHECK_EQUAL(m.fifteen_minute_rate(),0);

	mock::clock::set_time(2);

	auto verify_2= [&time_0,&time_1]()->bool{
			return (time_0 == 2) && (time_1 == 2);
	};

	auto verify_4 = [&time_0,&time_1]()->bool{
			return (time_0 == 4) && (time_1 == 4);
	};

	wait_for(verify_2);

	BOOST_CHECK_EQUAL(m.count(),10);
	BOOST_CHECK_EQUAL(m.mean_rate(),10 / mock::clock::time());
	BOOST_CHECK_EQUAL(m.one_minute_rate(),0);
	BOOST_CHECK_EQUAL(m.five_minute_rate(),0);
	BOOST_CHECK_EQUAL(m.fifteen_minute_rate(),0);

	mock::clock::set_time(4);

	wait_for(verify_4);

	int count_before_interval = m.count();
	BOOST_CHECK_EQUAL(count_before_interval,30);
	terminate = true;

	marker_0.join();
	marker_1.join();

	mock::clock::set_time(5);

	BOOST_CHECK_EQUAL(time_0, 4);
	BOOST_CHECK_EQUAL(time_1, 4);
	BOOST_CHECK_EQUAL(m.count(), count_before_interval);
	BOOST_CHECK_EQUAL(mock::clock::time(), 5);
	BOOST_CHECK_EQUAL(m.mean_rate(), count_before_interval / (double)mock::clock::time());
	BOOST_CHECK_EQUAL(m.one_minute_rate(),(count_before_interval / mock::clock::time()) * 60);
	BOOST_CHECK_EQUAL(m.five_minute_rate(),(count_before_interval / mock::clock::time()) * 60);
	BOOST_CHECK_EQUAL(m.fifteen_minute_rate(),(count_before_interval / mock::clock::time()) * 60);

}


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/xray/each_hkl_gradients_direct.h>
#include <boost/python/class.hpp>

namespace cctbx { namespace xray { namespace structure_factors {
namespace boost_python {

namespace {

  struct each_hkl_gradients_direct_wrappers
  {
    typedef each_hkl_gradients_direct<> w_t;
    typedef w_t::scatterer_type scatterer_type;
    typedef w_t::float_type float_type;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("each_hkl_gradients_direct", no_init)
        .def(init<uctbx::unit_cell const&,
                  sgtbx::space_group const&,
                  af::const_ref<miller::index<> > const&,
                  af::const_ref<scatterer_type> const&,
                  af::const_ref<float_type> const&,
                  scattering_type_registry const&,
                  sgtbx::site_symmetry_table const&,
                  std::size_t>())
        .def(init<math::cos_sin_table<double> const&,
                  uctbx::unit_cell const&,
                  sgtbx::space_group const&,
                  af::const_ref<miller::index<> > const&,
                  af::const_ref<scatterer_type> const&,
                  af::const_ref<float_type> const&,
                  scattering_type_registry const&,
                  sgtbx::site_symmetry_table const&,
                  std::size_t>())
        .def("d_fcalc_d_fp", &w_t::d_fcalc_d_fp)
        .def("d_fcalc_d_fdp", &w_t::d_fcalc_d_fdp)
      ;
    }
  };

} // namespace <anoymous>

}} // namespace structure_factors::boost_python

namespace boost_python {

  void wrap_each_hkl_gradients_direct()
  {
    structure_factors::boost_python::each_hkl_gradients_direct_wrappers::wrap();
  }

}}} // namespace cctbx::xray::boost_python


#ifndef BIGGLES_DETAIL_PHYSICS_HPP___
#define BIGGLES_DETAIL_PHYSICS_HPP___

#include <Eigen/Dense>

#include "../observation.hpp"

namespace biggles { namespace detail
{

/// @brief This is the default maximum separation of observations to be considered for observations to be neighbours.
static const time_stamp last_delta_t_ = 31;

/// @brief The default maximum separation of observations to be considered for extending a track.
static const time_stamp extend_last_delta_t_ = 2;

/// @brief The default maximum separation of observations to be considered for extending a track.
static const float extend_speed_of_light_ = 1.2f;

/// @brief This is the default speed of light.
///
/// @note If this were any other algorithm, specifying a 'default' speed of light would be a bit of a WTF.
static const float speed_of_light_ = 3.f;

inline Eigen::Matrix4f initQ() {
    Eigen::Matrix4f Q(Eigen::Matrix4f::Zero());
    Q(0,0) = Q(2,2) = 1e-1f * 1e-1f * speed_of_light_ * speed_of_light_;
    Q(1,1) = Q(3,3) = 1e-2f * 1e-2f * speed_of_light_ * speed_of_light_;
    return Q;
}

} }

#endif // BIGGLES_DETAIL_PHYSICS_HPP___