#include <iostream>
#include <caffe/caffe.hpp>
#include <boost/lexical_cast.hpp>
#include "boost/algorithm/string.hpp"
// opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <algorithm>
#include <iosfwd>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <fstream>
#include <unistd.h>

// definitions
#define NO_OPERATION      0
#define MEAN_SUBTRACTION  1
#define SCALE_DATA        2

using namespace caffe;
using namespace std;
using std::string;

void WriteCVMatToFile(string fileName, cv::Mat data);
cv::Mat ReadCVMatFromFile(string filename);
cv::Mat ReadInputFile(string filename);
/*************************************************/
class CaffeForward {
 public:
  CaffeForward( const string& model_file,const string& weight_file,const string& preprocess,const string& preprocess_data);

  std::vector<std::vector<float> > ForwardProcess(std::vector <cv::Mat> &inputs);
  void SaveChannels(std::vector<std::vector<float> > outputs , const string& output_dir);
  private:
  void WrapInputLayer(std::vector<cv::Mat>* input_channels, int input_idx);
  void SetMean(const string& mean_file);
  void Preprocess(const cv::Mat& img, std::vector<cv::Mat>* input_channels, int input_idx);

 public:
 	int num_outputs_, num_inputs_;
 private:
  shared_ptr<Net<float> > net_;
  std::vector<cv::Size> input_geometry_;
  std::vector<int> num_channels_;
  cv::Mat mean_;
  double scale_;
  unsigned char preprocess_;
};
/*************************************************/
/* CAFFE FORWARD */
CaffeForward::CaffeForward( const string& model_file,const string& weight_file,const string& preprocess,const string& preprocess_data)
{
  #ifdef CPU_ONLY
    Caffe::set_mode(Caffe::CPU);
  #else
    Caffe::set_mode(Caffe::GPU);
  #endif
	/* Load the network. */
  net_.reset(new Net<float>(model_file, TEST));
  std::vector<std::string> model_names;
  boost::split(model_names, weight_file, boost::is_any_of(",") );
  for (int i = 0; i < model_names.size(); ++i) {
    net_->CopyTrainedLayersFrom(model_names[i]);
  }

  num_outputs_ = net_->num_outputs();
  num_inputs_ = net_->num_inputs();
	//cout << "net inputs x outputs dimension: " << num_inputs_ << "x" << num_outputs_ << endl;

  for(int i = 0; i < num_inputs_; i++)
  {
    Blob<float>* input_layer = net_->input_blobs()[i];
    num_channels_.push_back(input_layer->channels());
    input_geometry_.push_back(cv::Size(input_layer->width(), input_layer->height()));
    //cout << "input " << i << " (channels x width x height): " << num_channels_[i] <<"x"<<input_geometry_[i].width<<"x"<<input_geometry_[i].height << endl;
    CHECK(num_channels_[i] == 3 || num_channels_[i] == 1) << "Input layer should have 1 or 3 channels.";
  }
  /*
  for(int i = 0; i < num_outputs_; i++)
  {
    Blob<float>* output_layer = net_->output_blobs()[i];
    cout << "output " << i << " (channels x width x height): " << output_layer->channels()<< "x" << output_layer->width() << "x" << output_layer->height() << endl;
  }*/

  if ( preprocess == "MEAN_SUBTRACTION" )
  {
    SetMean(preprocess_data);
    preprocess_ = MEAN_SUBTRACTION;
  }
  else if( preprocess == "SCALE_DATA" )
  {
    scale_  = boost::lexical_cast<double>(preprocess_data);
    preprocess_ = SCALE_DATA;
  }
  else
    preprocess_ = NO_OPERATION;
}

/*************************************************/
/* SET MEAN */
void CaffeForward::SetMean(const string& mean_file) {
  /*cv::Mat mean_img = cv::imread(mean_file.c_str(), -1);
  cv::resize(mean_img, mean_, input_geometry_);
  mean_.convertTo(mean_, CV_32FC3);
  return;*/
  /*
  BlobProto blob_proto;
  ReadProtoFromBinaryFileOrDie(mean_file.c_str(), &blob_proto);
  // Convert from BlobProto to Blob<float>
  Blob<float> mean_blob;
  mean_blob.FromProto(blob_proto);
  CHECK_EQ(mean_blob.channels(), num_channels_)
    << "Number of channels of mean file doesn't match input layer.";
  // The format of the mean file is planar 32-bit float BGR or grayscale.
  std::vector<cv::Mat> channels;
  float* data = mean_blob.mutable_cpu_data();
  for (int i = 0; i < num_channels_; ++i) {
    // Extract an individual channel.
    cv::Mat channel(mean_blob.height(), mean_blob.width(), CV_32FC1, data);
    channels.push_back(channel);
    data += mean_blob.height() * mean_blob.width();
  }
  // Merge the separate channels into a single image.
  cv::Mat mean;
  cv::merge(channels, mean);
  cv::imwrite("/home/ali/Codes/Caffe/mean.PNG", mean);
  // Compute the global mean pixel value and create a mean image filled with this value.
  cv::Scalar channel_mean = cv::mean(mean);
  mean_ = cv::Mat(input_geometry_, mean.type(), channel_mean);
  mean_ = mean;
  cv::imwrite("/home/ali/Codes/Caffe/mean2.PNG", mean_);*/
}

/*************************************************/
/* FORWARD PROCESS */
std::vector <std::vector<float> > CaffeForward::ForwardProcess(std::vector <cv::Mat> & inputs)
{
  for (int i = 0; i < inputs.size(); i++)
  {
    Blob<float>* input_layer = net_->input_blobs()[i];
    input_layer->Reshape(1, input_layer->channels(), input_layer->height(), input_layer->width());
  }
  net_->Reshape();
  for (int i = 0; i < inputs.size(); i++)
  {
    std::vector<cv::Mat> input_channels;
    WrapInputLayer(&input_channels, i);
    Preprocess(inputs[i], &input_channels, i);
  }

  net_->Forward();

  std::vector<std::vector<float> > outputs;

  for (int i = 0; i < num_outputs_; i++)
  {
    Blob<float>* output_layer = net_->output_blobs()[i];
    // cout << output_layer->width() << "x"<< output_layer->height() << "x" <<output_layer->channels() << endl;
  	const float* begin = output_layer->cpu_data();
  	const float* end = begin + output_layer->channels()*output_layer->width()*output_layer->height();
		outputs.push_back(std::vector<float>(begin, end));
  }
  return outputs;
}

/*************************************************/
/* SAVE CHANNELS */
void CaffeForward::SaveChannels(std::vector<std::vector<float> > outputs, const string& output_dir )
{
  for (int i = 0; i < outputs.size(); i++)
  {
    Blob<float>* output_layer = net_->output_blobs()[i];
    char filename[100];
    for (int ch = 0; ch < output_layer->channels(); ch++)
    {
      std::vector<float> channel;
      int p_start = ch*output_layer->width()*output_layer->height();
      int length = output_layer->width()*output_layer->height();
      channel.assign(outputs[i].begin()+p_start, outputs[i].begin()+p_start+length);
      cv::Mat img = cv::Mat(channel).reshape(0,output_layer->height());
      sprintf(filename,"%s/o_%02d_ch_%02d.txt", output_dir.c_str(),i,ch);
      WriteCVMatToFile(filename, img);
      double min, max;
      cv::minMaxLoc(img, &min, &max);
      img = img - min;
      img = img / (max - min);
      img.convertTo(img, CV_8UC1, 255.0);
      sprintf(filename,"%s/o_%02d_ch_%02d.jpg", output_dir.c_str(),i,ch);
      cv::imwrite(filename, img);
    }
  }
}
 /*************************************************/
 /* WRAP INPUT LAYER */
void CaffeForward::WrapInputLayer(std::vector<cv::Mat>* input_channels, int input_idx)
{
  Blob<float>* input_layer = net_->input_blobs()[input_idx];
  int width = input_layer->width();
  int height = input_layer->height();
  float* input_data = input_layer->mutable_cpu_data();
  for (int i = 0; i < input_layer->channels(); ++i) {
    cv::Mat channel(height, width, CV_32FC1, input_data);
    input_channels->push_back(channel);
    input_data += width * height;
  }
}
/*************************************************/
/* PREPROCESS */
void CaffeForward::Preprocess(const cv::Mat& img, std::vector<cv::Mat>* input_channels, int input_idx)
{
  cv::Mat sample;
  if (img.channels() == 3 && num_channels_[input_idx] == 1)
    cv::cvtColor(img, sample, cv::COLOR_BGR2GRAY);
  else if (img.channels() == 4 && num_channels_[input_idx] == 1)
    cv::cvtColor(img, sample, cv::COLOR_BGRA2GRAY);
  else if (img.channels() == 4 && num_channels_[input_idx] == 3)
    cv::cvtColor(img, sample, cv::COLOR_BGRA2BGR);
  else if (img.channels() == 1 && num_channels_[input_idx] == 3)
    cv::cvtColor(img, sample, cv::COLOR_GRAY2BGR);
  else
    sample = img;
  cv::Mat sample_resized;
  if (sample.size() != input_geometry_[input_idx])
  {
    cout << "dimension mismatch" << endl;
    cout << "input geometry: " << input_geometry_[input_idx].width << " x "<< input_geometry_[input_idx].height << endl;
    cout << "input data:     " << sample.size().width << " x " << sample.size().height << endl;
    cv::resize(sample, sample_resized, input_geometry_[input_idx]);
  }
  else
    sample_resized = sample;

  cv::Mat sample_float;
  if (num_channels_[input_idx] == 3)
    sample_resized.convertTo(sample_float, CV_32FC3);
  else
    sample_resized.convertTo(sample_float, CV_32FC1);

  cv::Mat sample_preprocessed;

  if (preprocess_ == SCALE_DATA)
    sample_preprocessed = (1.0 / 256.0) * sample_float;
  else if(preprocess_ == MEAN_SUBTRACTION)
    cv::subtract(sample_float, mean_, sample_preprocessed);
  else
    sample_preprocessed = sample_float;

  cv::split(sample_preprocessed, *input_channels);
  CHECK(reinterpret_cast<float*>(input_channels->at(0).data)
        == net_->input_blobs()[input_idx]->cpu_data())
    << "Input channels are not wrapping the input layer of the network.";
}

/*************************************************/
/* MAIN */
int main(int argc, char** argv)
{
	::google::InitGoogleLogging(argv[0]);

  string model_file       = argv[1];    // deploy file
  string weight_file      = argv[2];    // weights
  string preprocess       = argv[3];    // preprocess operation
  string preprocess_data  = argv[4];    // preprocess file
  string input_files      = argv[5];    // input files, each separated by ','
  string output_dir       = argv[6];    // output directory to save the files

  std::vector<cv::Mat> inputs;
  string filename;
  std::size_t fname_pos1 = 0;
  std::size_t fname_pos2 = 0;
  bool break_loop = false;
  // open input_files one by one
  while(!break_loop)
  {
    fname_pos2 = input_files.find(",", fname_pos1);
    if(fname_pos2 != string::npos)
      filename = input_files.substr(fname_pos1, fname_pos2-fname_pos1);
    else
    {
      filename = input_files.substr(fname_pos1);
      break_loop = true;
    }
    cout << filename << endl;
    cv::Mat input = ReadInputFile(filename);
    CHECK(!input.empty()) << "Unable to decode input: " << filename;
    inputs.push_back(input);
    fname_pos1 = fname_pos2+1;
  }

  CaffeForward CaffeForward(model_file, weight_file, preprocess, preprocess_data);

  std::vector< std::vector<float> > outputs = CaffeForward.ForwardProcess(inputs);
  CaffeForward.SaveChannels(outputs, output_dir);

  return 0;
}

cv::Mat ReadInputFile(string filename)
{
  if(filename.find(".txt") != string::npos)
    return ReadCVMatFromFile(filename);
  else
    return cv::imread(filename, -1);
}

void WriteCVMatToFile(string fileName, cv::Mat data)
{
    int nRow = data.size().height;
    int nCol = data.size().width;
    ofstream fileWriter;
    fileWriter.open(fileName.c_str());

    fileWriter << nRow << endl;
    fileWriter << nCol << endl;

    for(int i = 0; i < nRow; i++)
    {
      for(int j = 0; j < nCol; j++)
      {
        fileWriter << std::fixed << std::setprecision(20) << data.at<float>(i,j) << endl;
      }
    }
    fileWriter.close();
}
cv::Mat ReadCVMatFromFile(string filename)
{
  ifstream reader(filename.c_str());
  int rows, cols;
  string line;

  getline(reader, line);
  rows = boost::lexical_cast<int>(line);
  getline(reader, line);
  cols = boost::lexical_cast<int>(line);

  cv::Mat im( cv::Size(rows, cols), CV_32FC1, cv::Scalar(255) );

  for(int i = 0; i < rows; i++)
  {
    for(int j = 0; j < cols; j++)
    {
      getline(reader, line);
      im.at<float>(j, i) = boost::lexical_cast<float>(line);
    }
  }

  reader.close();
  return im;
}


#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <vector>

#include <Eigen/Core>
#include <pcl/common/angles.h>
#include <pcl/common/distances.h>
#include <pcl/filters/random_sample.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <vector_types.h>

#include "impl/cycle_iterator.hpp"
#include "impl/scene_generation.hpp"
#include "impl/util.hpp"
#include "kernel.h"
#include "linalg.h"
#include "ppf.h"
#include "vector_ops.h"

// Workaround for a bug in pcl::geometry::distance.
template <typename PointT>
inline float
distance (const PointT &p1, const PointT &p2)
{
  Eigen::Vector3f diff = p1.getVector3fMap () - p2.getVector3fMap ();
  return (diff.norm ());
}

template <typename Point>
typename pcl::PointCloud<Point>::Ptr
randomDownsample (typename pcl::PointCloud<Point>::Ptr cloud, float p)
{
  pcl::RandomSample<Point> rs;
  typename pcl::PointCloud<Point>::Ptr filtered_cloud (new pcl::PointCloud<Point>);
  rs.setSample (p * cloud->size ());
  rs.setInputCloud (cloud);
  rs.filter (*filtered_cloud);
  return filtered_cloud;
}

template <typename Point>
typename pcl::PointCloud<Point>::Ptr
sequentialDownsample (typename pcl::PointCloud<Point>::Ptr cloud, int n)
{
  typename pcl::PointCloud<Point>::Ptr filtered_cloud (new pcl::PointCloud<Point>);
  int i = 0;
  for (typename pcl::PointCloud<Point>::iterator it = cloud->begin (); it != cloud->end (); ++it, ++i)
  {
    if (i % n == 0)
    {
      filtered_cloud->push_back (*it);
    }
  }
  return filtered_cloud;
}

template <typename Point>
typename pcl::PointCloud<Point>::Ptr
voxelGridDownsample (typename pcl::PointCloud<Point>::Ptr cloud, float leaf)
{
  typename pcl::PointCloud<Point>::Ptr filtered_cloud (new pcl::PointCloud<Point>);
  pcl::VoxelGrid<Point> sor;
  sor.setInputCloud (cloud);
  sor.setLeafSize (leaf, leaf, leaf);
  sor.filter (*filtered_cloud);
  return filtered_cloud;
}

int
run (int argc, char **argv)
{
  srand (time (0));
  po::variables_map vm = configure_options (argc, argv);
  init_logging (vm);

  std::string argv_string;
  for (int i = 0; i < argc; i++)
  {
    argv_string.append (argv[i]);
    argv_string.append (std::string (" "));
  }
  BOOST_LOG_TRIVIAL (info) << argv_string;

  float scene_leaf_size = vm["scene_leaf_size"].as<float> ();
  std::vector<float> scene_d_dists;
  std::vector<pcl::PointCloud<pcl::PointNormal>::Ptr> scene_clouds;
  CommaSeparatedVector scene_files = vm["scene_files"].as<CommaSeparatedVector> ();
  for (std::string scene_file : scene_files.values)
  {
    scene_clouds.push_back (pcl::PointCloud<pcl::PointNormal>::Ptr (new pcl::PointCloud<pcl::PointNormal>));
    BOOST_LOG_TRIVIAL (info) << boost::format ("Loading scene point cloud: %s") % scene_file.c_str ();
    if (pcl::io::loadPLYFile<pcl::PointNormal> (scene_file, *scene_clouds.back ()) < 0)
    {
      BOOST_LOG_TRIVIAL (error) << "Error loading scene file!";
      exit (1);
    }

    scene_d_dists.push_back (scene_leaf_size);
  }

  std::vector<float> tau_d;
  CommaSeparatedVector tau_d_strs = vm["tau_d"].as<CommaSeparatedVector> ();
  for (std::string tau_d_str : tau_d_strs.values)
  {
    tau_d.push_back (std::atof (tau_d_str.c_str ()));
  }

  std::vector<pcl::PointCloud<pcl::PointNormal>::Ptr> model_clouds;
  std::vector<float> model_d_dists;
  CommaSeparatedVector model_files = vm["model_files"].as<CommaSeparatedVector> ();

  if (tau_d.size () != model_files.values.size ())
  {
    BOOST_LOG_TRIVIAL (error) << "Each model must have an associated tau_d.";
    exit (1);
  }
  for (int i = 0; i < model_files.values.size (); i++)
  {
    std::string model_file = model_files.values[i];
    model_clouds.push_back (pcl::PointCloud<pcl::PointNormal>::Ptr (new pcl::PointCloud<pcl::PointNormal>));

    BOOST_LOG_TRIVIAL (info) << boost::format ("Loading model point cloud: %s") % model_file.c_str ();
    if (pcl::io::loadPLYFile<pcl::PointNormal> (model_file, *model_clouds.back ()) < 0)
    {
      BOOST_LOG_TRIVIAL (error) << "Error loading model file!";
      exit (1);
    }

    pcl::PointNormal minPt, maxPt;
    // Finding the max pairwise distance is epxensive, so
    // approximate it with the max difference between coords.
    pcl::getMinMax3D (*model_clouds.back (), minPt, maxPt);
    float3 model_diam = {maxPt.x - minPt.x, maxPt.y - minPt.y, maxPt.z - minPt.z};
    model_d_dists.push_back (tau_d[i] * max (model_diam));
    BOOST_LOG_TRIVIAL (debug) << boost::format ("model_diam, d_dist: (%f, %f, %f), %f") % model_diam.x % model_diam.y
                                   % model_diam.z % model_d_dists.back ();
  }

  // Downsample
  BOOST_LOG_TRIVIAL (info) << "Downsampling...";

  // shared_ptr &operator=(shared_ptr const &r) is equivalent to
  // shared_ptr(r).swap(*this), so, in each loop, the original cloud gets
  // de-allocated when new_* goes out of scope.
  //
  // http://www.boost.org/doc/libs/1_60_0/libs/smart_ptr/shared_ptr.htm#assignment
  for (pcl::PointCloud<pcl::PointNormal>::Ptr &scene : scene_clouds)
  {
    BOOST_LOG_TRIVIAL (info) << boost::format ("Scene size before filtering: %u (%u x %u)") % scene->size ()
                                  % scene->width % scene->height;
    pcl::PointCloud<pcl::PointNormal>::Ptr new_scene = voxelGridDownsample<pcl::PointNormal> (scene, scene_leaf_size);
    scene = new_scene;
    BOOST_LOG_TRIVIAL (info) << boost::format ("Scene size after filtering: %u (%u x %u)") % scene->size ()
                                  % scene->width % scene->height;
  }

  for (int i = 0; i < model_clouds.size (); i++)
  {
    pcl::PointCloud<pcl::PointNormal>::Ptr model = model_clouds[i];
    BOOST_LOG_TRIVIAL (info) << boost::format ("Model size before filtering: %u (%u x %u)") % model->size ()
                                  % model->width % model->height;
    pcl::PointCloud<pcl::PointNormal>::Ptr new_model = voxelGridDownsample<pcl::PointNormal> (model, model_d_dists[i]);
    BOOST_LOG_TRIVIAL (info) << boost::format ("Model size after filtering: %u (%u x %u)") % new_model->size ()
                                  % new_model->width % new_model->height;
    model_clouds[i] = new_model;
  }

  std::vector<std::vector<Eigen::Matrix4f>> results = ppf_registration (
    scene_clouds, model_clouds, model_d_dists, vm["ref_point_df"].as<unsigned int> (),
    vm["vote_count_threshold"].as<float> (), vm["cpu_clustering"].as<bool> (), vm["use_l1_norm"].as<bool> (),
    vm["use_averaged_clusters"].as<bool> (), vm["dev"].as<int> (), NULL);

  return 0;
}


/*
 * Copyright 2017, Thomas Flayols, LAAS-CNRS
 *
 * This file is part of sot-torque-control.
 * sot-torque-control is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-torque-control is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef __sot_torque_control_base_estimator_H__
#define __sot_torque_control_base_estimator_H__

/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#if defined(WIN32)
#if defined(base_estimator_EXPORTS)
#define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllexport)
#else
#define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllimport)
#endif
#else
#define TALOS_BASE_ESTIMATOR_EXPORT
#endif

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#include <pinocchio/fwd.hpp>
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <map>
#include "boost/assign.hpp"
//#include <boost/random/normal_distribution.hpp>
#include <boost/math/distributions/normal.hpp>  // for normal_distribution

/* Pinocchio */
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <sot/core/robot-utils.hh>

namespace dynamicgraph {
namespace sot {
namespace talos_balance {

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/** Compute s12 as an intermediate transform between s1 and s2 SE3 transforms**/
void se3Interp(const pinocchio::SE3& s1, const pinocchio::SE3& s2, const double alpha, pinocchio::SE3& s12);

/** Convert from Roll, Pitch, Yaw to transformation Matrix. */
void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d& R);

/** Convert from Roll, Pitch, Yaw to transformation Matrix. */
void rpyToMatrix(const Eigen::Vector3d& rpy, Eigen::Matrix3d& R);

/**  Convert from Transformation Matrix to Roll, Pitch, Yaw */
void matrixToRpy(const Eigen::Matrix3d& M, Eigen::Vector3d& rpy);

/** Multiply to quaternions stored in (w,x,y,z) format */
// void quanternionMult(const Eigen::Vector4d & q1, const Eigen::Vector4d & q2,  Eigen::Vector4d & q12);

/** Rotate a point or a vector by a quaternion stored in (w,x,y,z) format */
// void pointRotationByQuaternion(const Eigen::Vector3d & point,const Eigen::Vector4d & quat, Eigen::Vector3d &
// rotatedPoint);

/** Avoids singularity while taking the mean of euler angles**/
double eulerMean(double a1, double a2);

/** Avoids singularity while taking the mean of euler angles**/
double wEulerMean(double a1, double a2, double w1, double w2);

class TALOS_BASE_ESTIMATOR_EXPORT TalosBaseEstimator : public ::dynamicgraph::Entity {
  typedef TalosBaseEstimator EntityClassName;
  typedef pinocchio::SE3 SE3;
  typedef Eigen::Vector2d Vector2;
  typedef Eigen::Vector3d Vector3;
  typedef Eigen::Vector4d Vector4;
  typedef Vector6d Vector6;
  typedef Vector7d Vector7;
  typedef Eigen::Matrix3d Matrix3;
  typedef boost::math::normal normal;

  DYNAMIC_GRAPH_ENTITY_DECL();

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /* --- CONSTRUCTOR ---- */
  TalosBaseEstimator(const std::string& name);

  void init(const double& dt, const std::string& urdfFile);

  void set_fz_stable_windows_size(const int& ws);
  void set_alpha_w_filter(const double& a);
  void set_alpha_DC_acc(const double& a);
  void set_alpha_DC_vel(const double& a);
  void reset_foot_positions();
  void set_imu_weight(const double& w);
  void set_zmp_std_dev_right_foot(const double& std_dev);
  void set_zmp_std_dev_left_foot(const double& std_dev);
  void set_normal_force_std_dev_right_foot(const double& std_dev);
  void set_normal_force_std_dev_left_foot(const double& std_dev);
  void set_stiffness_right_foot(const dynamicgraph::Vector& k);
  void set_stiffness_left_foot(const dynamicgraph::Vector& k);
  void set_right_foot_sizes(const dynamicgraph::Vector& s);
  void set_left_foot_sizes(const dynamicgraph::Vector& s);
  void set_zmp_margin_right_foot(const double& margin);
  void set_zmp_margin_left_foot(const double& margin);
  void set_normal_force_margin_right_foot(const double& margin);
  void set_normal_force_margin_left_foot(const double& margin);

  void reset_foot_positions_impl(const Vector6& ftlf, const Vector6& ftrf);
  void compute_zmp(const Vector6& w, Vector2& zmp);
  double compute_zmp_weight(const Vector2& zmp, const Vector4& foot_sizes, double std_dev, double margin);
  double compute_force_weight(double fz, double std_dev, double margin);
  void kinematics_estimation(const Vector6& ft, const Vector6& K, const SE3& oMfs, const pinocchio::FrameIndex foot_id,
                             SE3& oMff, SE3& oMfa, SE3& fsMff);

  /* --- SIGNALS --- */
  DECLARE_SIGNAL_IN(joint_positions, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(joint_velocities, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(imu_quaternion, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(forceLLEG, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(forceRLEG, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(dforceLLEG, dynamicgraph::Vector);  /// derivative of left force torque sensor
  DECLARE_SIGNAL_IN(dforceRLEG, dynamicgraph::Vector);  /// derivative of right force torque sensor
  DECLARE_SIGNAL_IN(w_lf_in, double);                   /// weight of the estimation coming from the left foot
  DECLARE_SIGNAL_IN(w_rf_in, double);                   /// weight of the estimation coming from the right foot
  DECLARE_SIGNAL_IN(
      K_fb_feet_poses,
      double);  /// feed back gain to correct feet position according to last base estimation and kinematic
  DECLARE_SIGNAL_IN(lf_ref_xyzquat, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(rf_ref_xyzquat, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);

  DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector);

  DECLARE_SIGNAL_OUT(q, dynamicgraph::Vector);      /// n+6 robot configuration with base6d in RPY
  DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);      /// n+6 robot velocities
  DECLARE_SIGNAL_OUT(v_kin, dynamicgraph::Vector);  /// 6d robot velocities from kinematic only   (encoders derivative)
  DECLARE_SIGNAL_OUT(v_flex,
                     dynamicgraph::Vector);  /// 6d robot velocities from flexibility only (force sensor derivative)
  DECLARE_SIGNAL_OUT(v_imu,
                     dynamicgraph::Vector);  /// 6d robot velocities form imu only (accelerometer integration + gyro)
  DECLARE_SIGNAL_OUT(v_gyr, dynamicgraph::Vector);  /// 6d robot velocities form gyroscope only (as if gyro measured
                                                    /// the pure angular ankle velocities)
  DECLARE_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector);  /// left foot pose
  DECLARE_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector);  /// right foot pose
  DECLARE_SIGNAL_OUT(a_ac, dynamicgraph::Vector);  /// acceleration of the base in the world with DC component removed
  DECLARE_SIGNAL_OUT(v_ac, dynamicgraph::Vector);  /// velocity of the base in the world with DC component removed

  DECLARE_SIGNAL_OUT(q_lf, dynamicgraph::Vector);   /// n+6 robot configuration with base6d in RPY
  DECLARE_SIGNAL_OUT(q_rf, dynamicgraph::Vector);   /// n+6 robot configuration with base6d in RPY
  DECLARE_SIGNAL_OUT(q_imu, dynamicgraph::Vector);  /// n+6 robot configuration with base6d in RPY
  DECLARE_SIGNAL_OUT(w_lf, double);                 /// weight of the estimation coming from the left foot
  DECLARE_SIGNAL_OUT(w_rf, double);                 /// weight of the estimation coming from the right foot
  DECLARE_SIGNAL_OUT(w_lf_filtered, double);        /// filtered weight of the estimation coming from the left foot
  DECLARE_SIGNAL_OUT(w_rf_filtered, double);        /// filtered weight of the estimation coming from the right foot

  /* --- COMMANDS --- */
  /* --- ENTITY INHERITANCE --- */
  virtual void display(std::ostream& os) const;

 protected:
  bool m_initSucceeded;   /// true if the entity has been successfully initialized
  bool m_reset_foot_pos;  /// true after the command resetFootPositions is called
  double m_dt;            /// sampling time step
  RobotUtilShrPtr m_robot_util;

  bool m_left_foot_is_stable;    /// True if left foot as been stable for the last 'm_fz_stable_windows_size' samples
  bool m_right_foot_is_stable;   /// True if right foot as been stable for the last 'm_fz_stable_windows_size' samples
  int m_fz_stable_windows_size;  /// size of the windows used to detect that feet did not leave the ground
  int m_lf_fz_stable_cpt;        /// counter for detecting for how long the feet has been stable
  int m_rf_fz_stable_cpt;        /// counter for detecting for how long the feet has been stable
  /* Estimator parameters */
  double m_w_imu;              /// weight of IMU for sensor fusion
  double m_zmp_std_dev_rf;     /// standard deviation of ZMP measurement errors for right foot
  double m_zmp_std_dev_lf;     /// standard deviation of ZMP measurement errors for left foot
  double m_fz_std_dev_rf;      /// standard deviation of normal force measurement errors for right foot
  double m_fz_std_dev_lf;      /// standard deviation of normal force measurement errors for left foot
  Vector4 m_left_foot_sizes;   /// sizes of the left foot (pos x, neg x, pos y, neg y)
  Vector4 m_right_foot_sizes;  /// sizes of the left foot (pos x, neg x, pos y, neg y)
  double m_zmp_margin_lf;      /// margin used for computing zmp weight
  double m_zmp_margin_rf;      /// margin used for computing zmp weight
  double m_fz_margin_lf;       /// margin used for computing normal force weight
  double m_fz_margin_rf;       /// margin used for computing normal force weight
  Vector6 m_K_rf;              /// 6d stiffness of right foot spring
  Vector6 m_K_lf;              /// 6d stiffness of left foot spring

  Eigen::VectorXd m_v_kin;   /// 6d robot velocities from kinematic only   (encoders derivative)
  Eigen::VectorXd m_v_flex;  /// 6d robot velocities from flexibility only (force sensor derivative)
  Eigen::VectorXd m_v_imu;   /// 6d robot velocities form imu only (accelerometer integration + gyro)
  Eigen::VectorXd
      m_v_gyr;  /// 6d robot velocities form gyroscope only (as if gyro measured the pure angular ankle velocities)

  Vector3 m_v_ac;  /// velocity of the base in the world with DC component removed
  Vector3 m_a_ac;  /// acceleration of the base in the world with DC component removed

  double m_alpha_DC_acc;  /// alpha parameter for DC blocker filter on acceleration data
  double m_alpha_DC_vel;  /// alpha parameter for DC blocker filter on velocity data

  double m_alpha_w_filter;  /// filter parameter to filter weights (1st order low pass filter)

  double m_w_lf_filtered;  /// filtered weight of the estimation coming from the left foot
  double m_w_rf_filtered;  /// filtered weight of the estimation coming from the right foot

  pinocchio::Model m_model;    /// Pinocchio robot model
  pinocchio::Data* m_data;     /// Pinocchio robot data
  pinocchio::SE3 m_torsoMimu;  /// chest to imu transformation
  pinocchio::SE3 m_oMff_lf;    /// world-to-base transformation obtained through left foot
  pinocchio::SE3 m_oMff_rf;    /// world-to-base transformation obtained through right foot
  SE3 m_oMlfs;                 /// transformation from world to left foot sole
  SE3 m_oMrfs;                 /// transformation from world to right foot sole
  Vector7 m_oMlfs_xyzquat;
  Vector7 m_oMrfs_xyzquat;
  SE3 m_oMlfs_default_ref;  /// Default reference for left foot pose to use if no ref is pluged
  SE3 m_oMrfs_default_ref;  /// Default reference for right foot pose to use if no ref is pluged
  normal m_normal;          /// Normal distribution

  bool m_isFirstIterFlag;  /// flag to detect first iteration and initialise velocity filters

  SE3 m_sole_M_ftSens;  /// foot sole to F/T sensor transformation

  pinocchio::FrameIndex m_right_foot_id;
  pinocchio::FrameIndex m_left_foot_id;
  pinocchio::FrameIndex m_torso_id;
  pinocchio::FrameIndex m_IMU_frame_id;
  Eigen::VectorXd m_q_pin;  /// robot configuration according to pinocchio convention
  Eigen::VectorXd m_q_sot;  /// robot configuration according to SoT convention
  Eigen::VectorXd m_v_pin;  /// robot velocities according to pinocchio convention
  Eigen::VectorXd m_v_sot;  /// robot velocities according to SoT convention
  Matrix3 m_oRtorso;        /// chest orientation in the world from angular fusion
  Matrix3 m_oRff;           /// base orientation in the world

  /* Filter buffers*/
  Vector3 m_last_vel;
  Vector3 m_last_DCvel;
  Vector3 m_last_DCacc;

};  // class TalosBaseEstimator

}  // namespace talos_balance
}  // namespace sot
}  // namespace dynamicgraph

#endif  // #ifndef __sot_torque_control_free_flyer_locator_H__


#ifndef __abstract_MSSMNoFV_onshell_soft_parameters_gm2calc_1_2_0_hpp__
#define __abstract_MSSMNoFV_onshell_soft_parameters_gm2calc_1_2_0_hpp__

#include "gambit/Backends/abstractbase.hpp"
#include "forward_decls_abstract_classes.hpp"
#include "forward_decls_wrapper_classes.hpp"
#include "wrapper_MSSMNoFV_onshell_susy_parameters_decl.hpp"
#include <Eigen/Core>
#include <ostream>
#include <cstddef>
#include <iostream>

#include "identification.hpp"

namespace CAT_3(BACKENDNAME,_,SAFE_VERSION)
{

   namespace gm2calc
   {
      class Abstract_MSSMNoFV_onshell_soft_parameters : virtual public gm2calc::Abstract_MSSMNoFV_onshell_susy_parameters
      {
         public:

            virtual void print(::std::basic_ostream<char, std::char_traits<char> >&) const =0;

            virtual void clear() =0;

            virtual void set_TYd(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&) =0;

            virtual void set_TYd(int, int, double) =0;

            virtual void set_TYe(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&) =0;

            virtual void set_TYe(int, int, double) =0;

            virtual void set_TYu(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&) =0;

            virtual void set_TYu(int, int, double) =0;

            virtual void set_BMu(double) =0;

            virtual void set_mq2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&) =0;

            virtual void set_mq2(int, int, double) =0;

            virtual void set_ml2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&) =0;

            virtual void set_ml2(int, int, double) =0;

            virtual void set_mHd2(double) =0;

            virtual void set_mHu2(double) =0;

            virtual void set_md2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&) =0;

            virtual void set_md2(int, int, double) =0;

            virtual void set_mu2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&) =0;

            virtual void set_mu2(int, int, double) =0;

            virtual void set_me2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&) =0;

            virtual void set_me2(int, int, double) =0;

            virtual void set_MassB(double) =0;

            virtual void set_MassWB(double) =0;

            virtual void set_MassG(double) =0;

            virtual const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_TYd() const =0;

            virtual double get_TYd(int, int) const =0;

            virtual const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_TYe() const =0;

            virtual double get_TYe(int, int) const =0;

            virtual const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_TYu() const =0;

            virtual double get_TYu(int, int) const =0;

            virtual double get_BMu() const =0;

            virtual const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_mq2() const =0;

            virtual double get_mq2(int, int) const =0;

            virtual const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_ml2() const =0;

            virtual double get_ml2(int, int) const =0;

            virtual double get_mHd2() const =0;

            virtual double get_mHu2() const =0;

            virtual const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_md2() const =0;

            virtual double get_md2(int, int) const =0;

            virtual const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_mu2() const =0;

            virtual double get_mu2(int, int) const =0;

            virtual const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_me2() const =0;

            virtual double get_me2(int, int) const =0;

            virtual double get_MassB() const =0;

            virtual double get_MassWB() const =0;

            virtual double get_MassG() const =0;

         public:
            using gm2calc::Abstract_MSSMNoFV_onshell_susy_parameters::pointer_assign__BOSS;
            virtual void pointer_assign__BOSS(Abstract_MSSMNoFV_onshell_soft_parameters*) =0;
            virtual Abstract_MSSMNoFV_onshell_soft_parameters* pointer_copy__BOSS() =0;

         private:
            MSSMNoFV_onshell_soft_parameters* wptr;
            bool delete_wrapper;
         public:
            MSSMNoFV_onshell_soft_parameters* get_wptr() { return wptr; }
            void set_wptr(MSSMNoFV_onshell_soft_parameters* wptr_in) { wptr = wptr_in; }
            bool get_delete_wrapper() { return delete_wrapper; }
            void set_delete_wrapper(bool del_wrp_in) { delete_wrapper = del_wrp_in; }

         public:
            Abstract_MSSMNoFV_onshell_soft_parameters()
            {
               wptr = 0;
               delete_wrapper = false;
            }

            Abstract_MSSMNoFV_onshell_soft_parameters(const Abstract_MSSMNoFV_onshell_soft_parameters& in) :
               gm2calc::Abstract_MSSMNoFV_onshell_susy_parameters(in)
            {
               wptr = 0;
               delete_wrapper = false;
            }

            Abstract_MSSMNoFV_onshell_soft_parameters& operator=(const Abstract_MSSMNoFV_onshell_soft_parameters&) { return *this; }

            virtual void init_wrapper() =0;

            MSSMNoFV_onshell_soft_parameters* get_init_wptr()
            {
               init_wrapper();
               return wptr;
            }

            MSSMNoFV_onshell_soft_parameters& get_init_wref()
            {
               init_wrapper();
               return *wptr;
            }

            virtual ~Abstract_MSSMNoFV_onshell_soft_parameters() =0;
      };
   }

}

#include "gambit/Backends/backend_undefs.hpp"

#endif /* __abstract_MSSMNoFV_onshell_soft_parameters_gm2calc_1_2_0_hpp__ */


//
// test/ibstream.hpp
//
// Copyright (c) Chris Glover, 2017-2018
//
// 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)
//

#define BOOST_TEST_MODULE TestInBitstream
#include <boost/test/unit_test.hpp>

#include <boost/radix/detail/ibitstream.hpp>
#include <vector>

#include "common.hpp"

BOOST_AUTO_TEST_CASE(read_bytes)
{
    std::vector<char> buf = generate_bytes(3 * 8 * 2);
    boost::radix::detail::ibitstream<std::vector<char>::iterator> s(
        buf.begin(), buf.end());
    boost::radix::detail::dynamic_bit_reader reader(3);
    BOOST_TEST(!s.finished());
    for(std::size_t i = 0; i < buf.size() * 8; i += reader.num_bits())
    {
        char val = 0;
        s.read_bits(val, reader);
        BOOST_TEST(val == get_bits<char>(buf, i, reader.num_bits()));
    }

    BOOST_TEST(s.finished());
}

BOOST_AUTO_TEST_CASE(read_ints)
{
    std::vector<char> buf = generate_bytes(11 * 8 * 2);
    boost::radix::detail::ibitstream<std::vector<char>::iterator> s(
        buf.begin(), buf.end());
    boost::radix::detail::dynamic_bit_reader reader(11);
    BOOST_TEST(!s.finished());
    for(std::size_t i = 0; i < buf.size() * 8; i += reader.num_bits())
    {
        int val = 0;
        s.read_bits(val, reader);
        BOOST_TEST(val == get_bits<int>(buf, i, reader.num_bits()));
    }
    BOOST_TEST(s.finished());
}


#ifndef TISSOT_CONVERTER_IMW_P_HPP
#define TISSOT_CONVERTER_IMW_P_HPP

// Tissot, converts projecton source code (Proj4) to Boost.Geometry
// (or potentially other source code)
//
// Copyright (c) 2015 Barend Gehrels, Amsterdam, the Netherlands.
//
// Use, modification and distribution is subject to 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 "tissot_structs.hpp"
#include "tissot_util.hpp"
#include "converter_base.hpp"
#include <boost/algorithm/string/replace.hpp>

namespace boost { namespace geometry { namespace proj4converter
{

class converter_cpp_bg_imw_p : public converter_cpp_bg_default
{
    public :
        converter_cpp_bg_imw_p(projection_properties& prop)
            : m_prop(prop)
        {
        }

        void replace_if_found(std::vector<std::string>& lines, std::string const& line, std::string const& term, std::string const& replacement)
        {
            std::vector<std::string>::iterator it =
                std::find_if(lines.begin(), lines.end(),
                    functor_trimmed_equals(line));

            if (it != lines.end())
            {
                boost::replace_all(*it, term, replacement);
            }
        }

        void pre_convert()
        {
            // Move the free function xy, after inverse, to inlined_functions
            BOOST_FOREACH(projection& proj, m_prop.projections)
            {
                std::vector<std::string>::iterator it
                    = std::find_if(proj.lines.begin(), proj.lines.end(),
                                           functor_trimmed_equals("static void"));
                if (it != proj.lines.end())
                {
                    // Move all further lines
                    for (std::vector<std::string>::iterator fit = it; fit != proj.lines.end(); ++fit)
                    {
                        m_prop.inlined_functions.push_back(*fit);
                    }
                    proj.lines.erase(it, proj.lines.end());
                }
            }
        }

        void convert()
        {
            m_prop.extra_structs.push_back("struct XY { double x, y; }; // specific for IMW_P");

            // First adapt projections and move xy to inlined_functions
            BOOST_FOREACH(projection& proj, m_prop.projections)
            {
                replace_if_found(proj.lines, "double yc;", ";", " = 0;");
                replace_if_found(proj.lines, "xy = loc_for(lp, P, &yc);", "xy =", "XY xy =");
                BOOST_FOREACH(std::string& line, proj.lines)
                {
                    boost::replace_all(line, "lp, P", "lp_lon, lp_lat, this->m_par, m_proj_parm");
                }
                if (proj.direction == "forward")
                {
                    proj.lines.push_back(tab1 + "xy_x = xy.x; xy_y = xy.y;");
                }
            }

            BOOST_FOREACH(std::string& line, m_prop.inlined_functions)
            {
                boost::replace_all(line, "LP lp", "double const& lp_lam, double const& lp_phi");
                boost::replace_all(line, "lp.lam", "lp_lam");
                boost::replace_all(line, "lp.phi", "lp_phi");
                boost::replace_all(line, "Parameters& par,", "Parameters& par, par_imw_p& proj_parm,");
                if (boost::contains(line, "Parameters")
                    && (boost::contains(line, "loc_for") || boost::contains(line, "xy")))
                {
                    // Make parameters const in this function definition
                    boost::replace_all(line, "& par", " const& par");
                    boost::replace_all(line, "& proj_parm", " const& proj_parm");
                }

                // Set translation (done in converter) to proj_parm, but not for .params and .es
                boost::replace_all(line, "par.", "proj_parm.");
                boost::replace_all(line, "this->m_proj_parm.", "proj_parm.");

                boost::replace_all(line, "proj_parm.params", "par.params");
                boost::replace_all(line, "proj_parm.es", "par.es");
            }

            // See also lsat/sconics
            BOOST_FOREACH(derived& d, m_prop.derived_projections)
            {
                BOOST_FOREACH(std::string& line, d.constructor_lines)
                {
                    boost::replace_last(line, "(P,", "(par, proj_parm,");
                }
            }

        }

    private :

        projection_properties& m_prop;
};

}}} // namespace boost::geometry::proj4converter

#endif // TISSOT_CONVERTER_IMW_P_HPP


/*
 * Copyright (c) 2017, Doug Smith, KEBA Corp
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *
 *  Created on: Aug 25, 2017
 *      Author: Doug Smith
 */

#include "rmi_driver/util.h"

#include <boost/algorithm/string.hpp>
#include <boost/tokenizer.hpp>

#include <boost/spirit/include/qi.hpp>

namespace rmi_driver
{
namespace util
{
double radToDeg(double rad)
{
  return rad * (180.0 / M_PI);
}

double degToRad(double degrees)
{
  return degrees * (M_PI / 180.0);
}

std::string floatToStringNoTrailing(float fval, int precision)
{
  // Convert the value to a string with fixed precision
  std::ostringstream oss;
  oss << std::setprecision(precision) << std::fixed;
  oss << fval;
  auto str = oss.str();
  // Remove extra trailing 0s or .
  boost::trim_right_if(str, boost::is_any_of("0"));
  boost::trim_right_if(str, boost::is_any_of("."));

  return str;
}

using boost::spirit::qi::double_;
using boost::spirit::qi::parse;

namespace qi = boost::spirit::qi;

std::vector<double> stringToDoubleVec(const std::string& s)
{
  std::vector<double> doubleVec;

  // So, boost::split is the least efficient thing ever.  Use qi instead.

  // std::vector<std::string> strVec;

  //
  //  // Split the string at spaces into a vector of strings
  //  // boost::split(strVec, s, boost::is_any_of(" "), boost::token_compress_on);
  //  boost::split(strVec, s, boost::is_space(), boost::token_compress_on);
  //  t.stop();
  //
  //  auto time_total = t.getTime();
  //  std::cout << "duration boost::split " << t.getTime() << " ";
  //
  //  t.reset();
  //  t.start();
  //  // Insert the double value of each entry
  //
  //  doubleVec.reserve(10);
  //  std::transform(strVec.begin(), strVec.end(), std::back_inserter(doubleVec),
  //                 [](const std::string& val) { return boost::lexical_cast<double>(val); });
  //  t.stop();
  //
  //  time_total += t.getTime();
  //  std::cout << "duration transform: " << t.getTime() << " total: " << time_total << " (" << s << ")" << std::endl;
  //
  //  // Clock::time_point t0 = Clock::now();
  //
  //  {
  //    std::vector<double> doubleVec;
  //    t.reset();
  //    t.start();
  //    auto s_trim = boost::trim_copy(s);
  //    doubleVec.reserve(10);
  //    std::stringstream ss(s_trim);
  //
  //    double d;
  //    while (ss >> d)
  //      doubleVec.push_back(d);
  //
  //    t.stop();
  //    std::cout << "duration ss: " << t.getTime() << std::endl;
  //  }

  std::string::const_iterator start = s.begin();
  std::string::const_iterator end = s.end();

  std::string s_trim;

  // If it starts or ends with whitespace, trim it
  if (*start == ' ' || *end == ' ')
  {
    s_trim = boost::trim_copy(s);
    start = s_trim.begin();
    end = s_trim.end();
  }

  bool r;

  r = qi::parse(start, end, (double_ % ' '), doubleVec);

  return doubleVec;
}

bool usedAndNotEqualIdx(int entry_index, const std::vector<float>& sample, const std::vector<float>& msg)
{
  // If there is a message then there must be a valid index
  // @todo This seems safer, but then it's not "ignoring" the entry.
  //  if (msg.size() > 0)
  //  {
  //    if (entry_index < 0 || entry_index >= sample.size())
  //      return true;
  //  }

  // Check if the sample has anything
  if (sample.size() == 0)
    return false;

  // Make sure the index is valid
  if (entry_index < 0 || entry_index >= sample.size())
    return true;

  // Get the value stored in sample[entry_index] and compare it with the size of msg
  auto expected_size = std::lround(sample[entry_index]);

  return msg.size() != expected_size;
}

bool usedAndNotEqual(const std::string& sample, const std::string& msg, int* entry_index)
{
  if (entry_index)
    *entry_index = 0;

  if (sample.length() <= 0)
    return false;

  boost::char_separator<char> sep("|", "", boost::keep_empty_tokens);
  boost::tokenizer<boost::char_separator<char>, std::string::const_iterator, std::string> tok(sample, sep);

  // Eclipse complains about auto&& with a tokenizer?
  for (const std::string& entry : tok)
  {
    // Found an entry that matches.
    if (entry.compare(msg) == 0)
      return false;

    if (entry_index)
      (*entry_index)++;
  }

  // A sample was given but no match was found.
  if (entry_index)
    *entry_index = -1;  // Redundant since commandhandler matching will stop after the 1st fail?

  return true;
}

}  // namespace util

}  // namespace rmi_driver


/**
 * Copyright Soramitsu Co., Ltd. All Rights Reserved.
 * SPDX-License-Identifier: Apache-2.0
 */

#ifndef KAGOME_SCALE_UTIL_HPP
#define KAGOME_SCALE_UTIL_HPP

#include <algorithm>
#include <array>
#include <cstdint>
#include <vector>

#include <boost/endian/arithmetic.hpp>
#include "common/outcome_throw.hpp"
#include "macro/unreachable.hpp"
#include "scale/scale_error.hpp"

namespace kagome::scale::detail {
  /**
   * encodeInteger encodes any integer type to little-endian representation
   * @tparam T integer type
   * @tparam S output stream type
   * @param value integer value
   * @return byte array representation of value
   */
  template <class T,
            class S,
            typename I = std::decay_t<T>,
            typename = std::enable_if_t<std::is_integral<I>::value>>
  void encodeInteger(T value, S &out) {  // no need to take integers by &&
    constexpr size_t size = sizeof(I);
    constexpr size_t bits = size * 8;
    boost::endian::endian_buffer<boost::endian::order::little, I, bits> buf{};
    buf = value;  // cannot initialize, only assign
    for (size_t i = 0; i < size; ++i) {
      // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic)
      out << buf.data()[i];
    }
  }

  /**
   * @brief decodeInteger function decodes integer from stream
   * @tparam T integer type
   * @param stream source stream
   * @return decoded value or error
   */
  template <class T,
            class S,
            typename I = std::decay_t<T>,
            typename = std::enable_if_t<std::is_integral_v<I>>>
  I decodeInteger(S &stream) {
    constexpr size_t size = sizeof(I);
    static_assert(size <= 8);

    // clang-format off
    // sign bit = 2^(num_bits - 1)
    static constexpr std::array<uint64_t, 8> sign_bit = {
            0x80,               // 1 byte
            0x8000,             // 2 bytes
            0x800000,           // 3 bytes
            0x80000000,         // 4 bytes
            0x8000000000,       // 5 bytes
            0x800000000000,     // 6 bytes
            0x80000000000000,   // 7 bytes
            0x8000000000000000  // 8 bytes
    };

    static constexpr std::array<uint64_t, 8> multiplier = {
            0x1,                // 2^0
            0x100,              // 2^8
            0x10000,            // 2^16
            0x1000000,          // 2^24
            0x100000000,        // 2^32
            0x10000000000,      // 2^40
            0x1000000000000,    // 2^48
            0x100000000000000   // 2^56
    };
    // clang-format on

    if (!stream.hasMore(size)) {
      common::raise(DecodeError::NOT_ENOUGH_DATA);
      UNREACHABLE
    }

    // get integer as 4 bytes from little-endian stream
    // and represent it as native-endian unsigned int eger
    uint64_t v = 0u;

    for (size_t i = 0; i < size; ++i) {
      // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
      v += multiplier[i] * static_cast<uint64_t>(stream.nextByte());
    }
    // now we have uint64 native-endian value
    // which can be signed or unsigned under the cover

    // if it is unsigned, we know that is not greater than max value for type T
    // so static_cast<T>(v) is safe

    // if it is signed, but positive it is also ok
    // we can be sure that it is less than max_value<T>/2
    // to check whether is is negative we check if the sign bit present
    // in unsigned form it means that value is more than
    // a value 2^(bits_number-1)
    bool is_positive_signed = v < sign_bit[size - 1];
    if (std::is_unsigned<I>() || is_positive_signed) {
      return static_cast<I>(v);
    }

    // T is signed integer type and the value v is negative
    // value is negative signed means ( - x )
    // where x is positive unsigned < sign_bits[size-1]
    // find this x, safely cast to signed and negate result
    // the bitwise negation operation affects higher bits as well
    // but it doesn't spoil the result
    // static_cast to smaller size cuts them off
    T sv = -static_cast<I>((~v) + 1);

    return sv;
  }
}  // namespace kagome::scale::detail

#endif  // KAGOME_SCALE_UTIL_HPP


/*
 * Copyright (c) 2011, Mattia Penati <mattia.penati@gmail.com>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright notice,
 *       this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice,
 *       this list of conditions and the following disclaimer in the documentation
 *       and/or other materials provided with the distribution.
 *     * Neither the name of the Politecnico di Milano nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMA_TENSOR_DETAIL_TENSOR_CWISE_BINARY_HPP
#define AMA_TENSOR_DETAIL_TENSOR_CWISE_BINARY_HPP 1

#include <ama/tensor/detail/is_same_tensor.hpp>
#include <ama/tensor/detail/tensor_base.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/if.hpp>

namespace ama
{
  namespace tensor_
  {

    /* this class represent a unary component-wise operation */
    template <typename LEFT, typename RIGHT, typename OPERATOR> class tensor_cwise_binary;

    /* specialization of tensor_traits */
    template <typename LEFT, typename RIGHT, typename OPERATOR>
    struct tensor_traits< tensor_cwise_binary<LEFT, RIGHT, OPERATOR> >
    {
      typedef typename OPERATOR::result_type value_type;

      typedef typename LEFT::dimension_type dimension_type;

      typedef typename LEFT::controvariant_type controvariant_type;
      typedef typename LEFT::covariant_type covariant_type;

      typedef ::boost::mpl::false_ is_assignable;
      typedef ::boost::mpl::true_ is_temporary;
    };

    /* class declaration */
    template <typename LEFT, typename RIGHT, typename OPERATOR>
    class tensor_cwise_binary:
        public tensor_base< tensor_cwise_binary<LEFT, RIGHT, OPERATOR> >
    {
      BOOST_MPL_ASSERT_MSG(
            (tensor_::is_same_tensor<LEFT, RIGHT>::value)
          , COMPONENT_WISE_BINARY_OPERATION_BETWEEN_DIFFERENT_TENSORS_ARE_NOT_ALLOWED
          , (LEFT, RIGHT));

    protected:
      typedef tensor_base< tensor_cwise_binary<LEFT, RIGHT, OPERATOR> > base_type;
      typedef tensor_cwise_binary<LEFT, RIGHT, OPERATOR> derived_type;

    protected:
      typedef LEFT left_operand_type;
      typedef RIGHT right_operand_type;
      typedef OPERATOR operator_type;

    public:
      typedef typename base_type::value_type value_type;

    public:
      /* constructor */
      tensor_cwise_binary(left_operand_type const & left_operand,
                          right_operand_type const & right_operand,
                          operator_type const & op = operator_type())
          : m_left_operand(left_operand),
            m_right_operand(right_operand),
            m_operator(op) { }

    public:
      /* retrieve the value */
      template <typename ILIST>
      value_type at() const
      {
        return m_operator(
                 m_left_operand.template at<ILIST>(),
                 m_right_operand.template at<ILIST>());
      }

    protected:
      /* if the operand is temporary we save a copy, otherwise a reference */
      typedef typename LEFT::is_temporary left_operand_is_temporary;
      typedef typename ::boost::mpl::if_<
            left_operand_is_temporary
          , left_operand_type const
          , left_operand_type const &
          >::type const_left_operand_type;

      /* if the operand is temporary we save a copy, otherwise a reference */
      typedef typename RIGHT::is_temporary right_operand_is_temporary;
      typedef typename ::boost::mpl::if_<
            right_operand_is_temporary
          , right_operand_type const
          , right_operand_type const &
          >::type const_right_operand_type;

      /* members */
      const_left_operand_type m_left_operand;
      const_right_operand_type m_right_operand;
      operator_type const m_operator;
    };

  }
}

#endif /* AMA_TENSOR_DETAIL_TENSOR_CWISE_BINARY_HPP */


#pragma once

#include <Eigen/Dense>
#include <dart/dart.hpp>
#include <dart/gui/GLFuncs.hpp>
#include <dart/gui/osg/osg.hpp>

class EnvInterface;
class DracoSensorData;
class DracoCommand;

class DracoWorldNode : public dart::gui::osg::WorldNode {
   private:
    EnvInterface* Interface_;
    DracoSensorData* SensorData_;
    DracoCommand* Command_;

    void GetContactSwitchData_(bool& rfoot_contact, bool& lfoot_contact);
    void SetParams_();
    void PlotMPCResult_();
    void GetForceTorqueData_();

    dart::simulation::WorldPtr world_;
    dart::dynamics::SkeletonPtr robot_;
    dart::dynamics::SkeletonPtr ground_;

    Eigen::VectorXd trq_cmd_;

    int count_;
    double t_;
    double servo_rate_;
    int n_dof_;
    Eigen::VectorXd kp_;
    Eigen::VectorXd kd_;
    Eigen::VectorXd trq_lb_;
    Eigen::VectorXd trq_ub_;

    bool b_plot_mpc_result_;

   public:
    DracoWorldNode(const dart::simulation::WorldPtr& world);
    virtual ~DracoWorldNode();

    void customPreStep() override;
};


// Author(s): Jeroen Keiren
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file untime_test.cpp
/// \brief Add your file description here.

#define BOOST_TEST_MODULE untime_test
#include <boost/test/included/unit_test_framework.hpp>

#include "mcrl2/lps/linearise.h"
#include "mcrl2/lps/untime.h"

using namespace mcrl2;
using namespace mcrl2::data;
using namespace mcrl2::lps;

/*
 * Trivial test: LPS should be left as is, because there is no time involved.
 */
void test_case_1()
{
  const std::string text(
    "act a,b;\n"
    "proc P = a . b . P;\n"
    "init P;\n"
  );

  specification s0=remove_stochastic_operators(linearise(text));
  specification s1 = s0;
  rewriter r;
  lps::untime_algorithm<specification>(s1, false, false, r).run();
  const action_summand_vector& summands1 = s1.process().action_summands();
  for (action_summand_vector::const_iterator i = summands1.begin(); i != summands1.end(); ++i)
  {
    BOOST_CHECK(!i->has_time());
  }

  BOOST_CHECK(s0 == s1);

  if (s0 != s1)
  {
    std::clog << "Input specification  : " << lps::pp(s0) << std::endl
              << "Output specification : " << lps::pp(s1) << std::endl;
  }
}

/*
 * An extra process parameter (say "lat") of type Real is introduced
 * time is removed from the actions, and the condition
 * is weakened with time > lat (i.e. 2 > lat or 3 > lat in this case).
 * Furthermore a summand true->delta is introduced.
 */
void test_case_2()
{
  const std::string text(
    "act a,b;\n"
    "proc P = a@2 . b@3 . P;\n"
    "init P;\n"
  );

  specification s0=remove_stochastic_operators(linearise(text));
  specification s1 = s0;
  rewriter r;
  lps::untime_algorithm<specification>(s1, true, false, r).run();
  const action_summand_vector& summands0 = s0.process().action_summands();
  const action_summand_vector& summands1 = s1.process().action_summands();
  BOOST_CHECK(s0.process().process_parameters().size() == s1.process().process_parameters().size() - 1);
  for (action_summand_vector::const_iterator i = summands1.begin(); i != summands1.end(); ++i)
  {
    BOOST_CHECK(!i->has_time());
  }
}

/*
 * An extra process parameter (say "lat") of type Real is introduced
 * time is removed from the actions, and the condition
 * is weakened with time > lat (i.e. 2 > lat or 3 > lat in this case).
 * In the untimed summand, a summation over Real (say sum tv:Real) is introduced,
 * and the condition is weakened with tv > lat.
 * Furthermore a summand true->delta is introduced.
 */
void test_case_3()
{
  const std::string text(
    "act a,b;\n"
    "proc P = a@2 . b@3 . P\n"
    "       + a . P;\n"
    "init P;\n"
  );

  specification s0=remove_stochastic_operators(linearise(text));
  specification s1 = s0;
  rewriter r;
  lps::untime_algorithm<specification>(s1, true, false, r).run();
  const action_summand_vector& summands0 = s0.process().action_summands();
  const action_summand_vector& summands1 = s1.process().action_summands();
  BOOST_CHECK(s0.process().process_parameters().size() == s1.process().process_parameters().size() - 1);
  int sumvar_count = 0;
  for (action_summand_vector::const_iterator i = summands1.begin(); i != summands1.end(); ++i)
  {
    BOOST_CHECK(!i->has_time());
    sumvar_count += i->summation_variables().size();
  }
  BOOST_CHECK(sumvar_count == 1);
}

/*
 * An extra process parameter (say "lat") of type Real is introduced
 * time is removed from the actions, and the condition
 * is weakened with time > lat (i.e. 2 > lat or 3 > lat in this case).
 * In the untimed summand, a summation over Real (say sum tv:Real) is introduced,
 * and the condition is weakened with tv > lat.
 */
void test_case_4()
{
  const std::string text(
    "act a,b;\n"
    "proc P = a@2 . b@3 . P\n"
    "       + a . P\n"
    "       + true -> delta;\n"
    "init P;\n"
  );

  specification s0=remove_stochastic_operators(linearise(text));
  specification s1 = s0;
  rewriter r;
  lps::untime_algorithm<specification>(s1, true, false, r).run();
  const action_summand_vector& summands0 = s0.process().action_summands();
  const action_summand_vector& summands1 = s1.process().action_summands();
  BOOST_CHECK(s0.process().process_parameters().size() == s1.process().process_parameters().size() - 1);
  int sumvar_count = 0;
  for (action_summand_vector::const_iterator i = summands1.begin(); i != summands1.end(); ++i)
  {
    BOOST_CHECK(!i->has_time());
    sumvar_count += i->summation_variables().size();
  }
  BOOST_CHECK(sumvar_count == 1);
}

BOOST_AUTO_TEST_CASE(test_main)
{
  test_case_1();
  test_case_2();
  test_case_3();
  test_case_4();
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_COMMON_HMSB_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_COMMON_HMSB_HPP_INCLUDED
#include <boost/simd/reduction/functions/hmsb.hpp>
#include <boost/simd/include/functions/simd/bits.hpp>
#include <boost/simd/include/functions/simd/genmask.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <climits>
#include <cstddef>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::hmsb_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<arithmetic_<A0>,X>))
                                    )
  {
    typedef std::size_t result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type stype;

      result_type z = 0;
      const result_type N = meta::cardinal_of<A0>::value;
      for(result_type i = 0; i != N; ++i)
      {
        z |= (bits(a0[i]) >> (sizeof(stype)*CHAR_BIT - 1)) << i;
      }
      return z;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::hmsb_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<logical_<A0>,X>))
                                    )
  {
    typedef std::size_t result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return boost::simd::hmsb(boost::simd::genmask(a0));
    }
  };
} } }

#endif


/**  \file ublas_elementwise.hpp \brief Elementwise operations for ublas vector/matrix */
/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#ifndef  _ELEMENTWISE_UBLAS_HPP_
#define  _ELEMENTWISE_UBLAS_HPP_

#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>

namespace bayesopt
{
  namespace utils
  {

    /**
     * Computes the elementwise product of two vectors or matrices.
     *             c_i = a_i * b_i
     */
    template <class v1, class v2>
    v1 ublas_elementwise_prod(const v1& a, const v2& b)
    {
      typedef typename v1::value_type D;
      v1 c(a.size());
      std::transform(a.begin(),a.end(),b.begin(),c.begin(),std::multiplies<D>());
      return c;
    }

    /**
     * Computes the elementwise division of two vectors or matrices.
     *            c_i = a_i / b_i
     */
    template <class v1, class v2>
    v1 ublas_elementwise_div(const v1& a, const v2& b)
    {
      typedef typename v1::value_type D;
      v1 c(a.size());
      std::transform(a.begin(),a.end(),b.begin(),c.begin(),std::divides<D>());
      return c;
    }

  } //namespace utils
} //namespace bayesopt

#endif


#include <boost/geometry/strategies/convex_hull.hpp>


#include "GenericPatch.h"
#include "dllmain.h"
#include "../external/IniReader/IniReader.h"
#include "Logging.h"
#include "boost/algorithm/string/replace.hpp"
#include "Memory.h"
#include "UserSettings.h"
#include <string>
#include "../external/tinyexpr/tinyexpr.h"
#include "../external/Hooking.Patterns/Hooking.Patterns.h"
#include <boost/lexical_cast.hpp>

void GenericPatch::Init()
{
	DBOUT("Initializing GenericPatch patches");
	PatchAll(GetConfigs());
}

std::vector<GenericPatch::Config> GenericPatch::GetConfigs()
{
	CIniReader ini(ININAME);

	std::vector<Config> configs;
	if (!UserSettings::config.enabled)
	{
		DBOUT("Patches disabled by UserSettings");
		return configs;
	}

	DBOUT("Searching for patches...");

	for (auto& entry : ini.data)
	{
		if (entry.first.find("Patch") == std::string::npos)
			continue;

		DBOUT("Found patch " << entry.first);

		Config config;

		for (auto& params : entry.second)
		{
			boost::erase_all(params.second, "\"");
			if (params.first == "Pattern")
			{
				DBOUT("Pattern=" << params.second);
				config.pattern = params.second;
				boost::replace_all(config.pattern, "??", "?");
			}
			else if (params.first == "Offset")
			{
				DBOUT("Offset=" << params.second);
				try
				{
					config.offset = std::stoi(params.second);
				}
				catch (std::exception const& e)
				{
					DBOUT("Offset is not an integer, skipping patch...");
					goto CONTINUE;
				}
				if (config.offset < 0)
				{
					DBOUT("Offset invalid, must be greater than or equal to 0. Skipping patch...");
					goto CONTINUE;
				}
			}
			else if (params.first == "Value")
			{
				DBOUT("Value=" << params.second);
				config.val = params.second;
			}
			else if (params.first == "ValueType")
			{
				DBOUT("ValueType=" << params.second);
				config.valType = params.second;
				if (config.valType != "float" && config.valType != "byte" && config.valType != "int" && config.valType != "double")
				{
					DBOUT("ValueType unsupported. Supported types are: float, byte...Skipping patch...");
					goto CONTINUE;
				}
			}
			else if (params.first == "Match")
			{
				DBOUT("Match=" << params.second);
				config.matches = params.second;
			}
			else if (params.first == "Enabled")
			{
				DBOUT("Enabled=" << params.second);
				if (params.second != "true")
				{
					DBOUT("Patch not enabled, skipping patch...");
					goto CONTINUE;
				}
			}
			else if (params.first == "Module")
			{
				DBOUT("Module=" << params.second);
				config.module = params.second;
			}
		}
		if (config.pattern.length() == 0)
		{
			DBOUT("No pattern found, skipping patch...");
			goto CONTINUE;
		}
		if (config.valType == "float" || config.valType == "int" || config.valType == "double")
		{
			try
			{
				boost::replace_all(config.val, "width", std::to_string(UserSettings::config.width));
				boost::replace_all(config.val, "height", std::to_string(UserSettings::config.height));
				boost::replace_all(config.val, "aspectratio", boost::lexical_cast<std::string>(UserSettings::config.aspectratio));
				int error = -1;
				double d = te_interp(config.val.c_str(), &error);

				if (error != 0)
				{
					DBOUT("Could not interpret value expression, skipping patch...");
					goto CONTINUE;
				}

				if (config.valType == "float")
				{
					float f = static_cast<float>(d);
					config.val = hexStr(reinterpret_cast<BYTE*>(&f), sizeof(float));
				}
				else if (config.valType == "int")
				{
					int i = static_cast<int>(d);
					config.val = hexStr(reinterpret_cast<BYTE*>(&i), sizeof(int));
				}
				else if (config.valType == "double")
				{
					config.val = hexStr(reinterpret_cast<BYTE*>(&d), sizeof(double));
				}
			}
			catch (std::exception const& e)
			{
				DBOUT("Value is not a float, skipping patch...");
				goto CONTINUE;
			}
		}
		else if (config.valType == "byte")
		{
			if (BytesFromString(config.val).size() == 0)
			{
				DBOUT("Value could not be read as bytes, skipping patch...");
				goto CONTINUE;
			}
		}
		configs.push_back(config);
	CONTINUE:
		continue;
	}
	DBOUT(configs.size() << " patches found");
	return configs;
}

void GenericPatch::PatchAll(std::vector<Config> configs)
{
	for (auto& config : configs)
	{
		DBOUT("Searching for bytes " << config.pattern);
		std::string moduleName;
		if (config.module == "auto")
		{
			moduleName = szFileName;
		}
		else
		{
			moduleName = config.module;
		}

		mInfo = GetModuleInfo(const_cast<char*>(moduleName.c_str()));

		if (mInfo.EntryPoint == nullptr)
		{
			DBOUT("Could not find module " << moduleName << ", skipping patch...");
			continue;
		}

		auto pattern = hook::pattern((uintptr_t)mInfo.lpBaseOfDll, (uintptr_t)mInfo.lpBaseOfDll + mInfo.SizeOfImage, config.pattern);
		DBOUT("Found " << pattern.size() << " matches");

		if (pattern.size() == 0)
		{
			continue;
		}

		auto value = BytesFromString(config.val);
		if (config.matches == "all")
		{
			for (int i = 0; i < pattern.size(); i++)
			{
				DBOUT("Writing " << hexStr(value.data(), value.size()) << "with length " << value.size() << " to result " << i + 1);
				Memory::Write(pattern.get(i).get<BYTE>(config.offset), value.data(), value.size());
			}
		}
		else
		{
			int match = -1;
			if (config.matches == "last" || config.matches == "end")
			{
				match = pattern.size();
			}
			else
			{
				try
				{
					match = std::stoi(config.matches);
				}
				catch (std::exception const& e)
				{
					DBOUT("Could not parse match number as integer, skipping...");
					continue;
				}
			}

			if (match < 1 || match > pattern.size())
			{
				DBOUT("Match setting invalid, must resolve to a value between 1 and " << pattern.size());
				continue;
			}

			DBOUT("Writing " << hexStr(value.data(), value.size()) << "with length " << value.size() << " to match " << match);
			Memory::Write(pattern.get(match - 1).get<BYTE>(config.offset), value.data(), value.size());
		}
	}
}


//
// For licensing see accompanying LICENSE.txt file.
// Copyright (C) 2020 Apple Inc. All Rights Reserved.
//

#include <assert.h>

#include <iostream>
#include <limits>
#include <string>

#include <args.hxx>

#include <armadillo>

#include <embree3/rtcore.h>
#include <embree3/rtcore_scene.h>
#include <embree3/rtcore_ray.h>

struct Vertex   { float x, y, z, a; };
struct Triangle { unsigned int v0, v1, v2; };

int main (int argc, const char** argv) {

    //
    // parse input arguments
    //

    args::ArgumentParser parser("generate_ray_intersections", "");

    args::HelpFlag               help                               (parser, "__DUMMY__",                      "Display this help menu",         {'h', "help"});
    args::ValueFlag<std::string> vertices_file_arg                  (parser, "VERTICES_FILE",                  "vertices_file",                  {"vertices_file"},                  args::Options::Required);
    args::ValueFlag<std::string> faces_file_arg                     (parser, "FACES_FILE",                     "faces_file",                     {"faces_file"},                     args::Options::Required);
    args::ValueFlag<std::string> ray_positions_file_arg             (parser, "RAY_POSITIONS_FILE",             "ray_positions_file",             {"ray_positions_file"},             args::Options::Required);
    args::ValueFlag<std::string> ray_directions_file_arg            (parser, "RAY_DIRECTIONS_FILE",            "ray_directions_file",            {"ray_directions_file"},            args::Options::Required);
    args::ValueFlag<std::string> output_ray_hit_data_float_file_arg (parser, "OUTPUT_RAY_HIT_DATA_FLOAT_FILE", "output_ray_hit_data_float_file", {"output_ray_hit_data_float_file"}, args::Options::Required);
    args::ValueFlag<std::string> output_ray_hit_data_int_file_arg   (parser, "OUTPUT_RAY_HIT_DATA_INT_FILE",   "output_ray_hit_data_int_file",   {"output_ray_hit_data_int_file"},   args::Options::Required);
    args::Flag                   silent_arg                         (parser, "__DUMMY__",                      "silent",                         {"silent"});

    try {
        parser.ParseCLI(argc, argv);
    } catch (args::Completion e) {
        std::cout << parser;
        return 1;
    } catch (args::Help e) {
        std::cout << parser;
        return 1;
    } catch (args::ParseError e) {
        std::cout << parser;
        std::cout << std::endl << std::endl << std::endl << e.what() << std::endl << std::endl << std::endl;
        return 1;
    } catch (args::RequiredError e) {
        std::cout << parser;
        std::cout << std::endl << std::endl << std::endl << e.what() << std::endl << std::endl << std::endl;
        return 1;
    }

    auto vertices_file                  = args::get(vertices_file_arg);
    auto faces_file                     = args::get(faces_file_arg);
    auto ray_positions_file             = args::get(ray_positions_file_arg);
    auto ray_directions_file            = args::get(ray_directions_file_arg);
    auto output_ray_hit_data_float_file = args::get(output_ray_hit_data_float_file_arg);
    auto output_ray_hit_data_int_file   = args::get(output_ray_hit_data_int_file_arg);
    auto silent                         = args::get(silent_arg);

    arma::mat vertices, faces, ray_positions, ray_directions;

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_RAY_INTERSECTIONS] Begin..." << std::endl;
    }

    //
    // load input data
    //

    vertices.load(vertices_file, arma::hdf5_binary_trans);
    faces.load(faces_file, arma::hdf5_binary_trans);
    ray_positions.load(ray_positions_file, arma::hdf5_binary_trans);
    ray_directions.load(ray_directions_file, arma::hdf5_binary_trans);

    assert(vertices.n_cols == 3);
    assert(faces.n_cols == 3);
    assert(ray_positions.n_cols == 3);
    assert(ray_directions.n_cols == 3);
    assert(ray_positions.n_rows == ray_directions.n_rows);

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_RAY_INTERSECTIONS] Tracing " << ray_positions.n_rows << " rays..." << std::endl;
    }

    //
    // construct Embree data
    //

    assert(RTC_MAX_INSTANCE_LEVEL_COUNT == 1);

    auto rtc_device = rtcNewDevice(nullptr);
    assert(rtcGetDeviceError(nullptr) == RTC_ERROR_NONE);

    auto rtc_scene = rtcNewScene(rtc_device);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    auto rtc_triangle_mesh = rtcNewGeometry(rtc_device, RTC_GEOMETRY_TYPE_TRIANGLE);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    // set vertices
    auto rtc_vertices = (Vertex*) rtcSetNewGeometryBuffer(rtc_triangle_mesh, RTC_BUFFER_TYPE_VERTEX, 0, RTC_FORMAT_FLOAT3, sizeof(Vertex), vertices.n_rows);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    for (int i = 0; i < vertices.n_rows; i++) {
        rtc_vertices[i].x = vertices(i,0);
        rtc_vertices[i].y = vertices(i,1);
        rtc_vertices[i].z = vertices(i,2);
    }

    // set triangles
    auto rtc_triangles = (Triangle*) rtcSetNewGeometryBuffer(rtc_triangle_mesh, RTC_BUFFER_TYPE_INDEX, 0, RTC_FORMAT_UINT3, sizeof(Triangle), faces.n_rows);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    for (int i = 0; i < faces.n_rows; i++) {
        rtc_triangles[i].v0 = faces(i,0);
        rtc_triangles[i].v1 = faces(i,1);
        rtc_triangles[i].v2 = faces(i,2);
    }

    rtcCommitGeometry(rtc_triangle_mesh);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    unsigned int geomID = rtcAttachGeometry(rtc_scene, rtc_triangle_mesh);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    rtcReleaseGeometry(rtc_triangle_mesh);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    rtcCommitScene(rtc_scene);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    RTCIntersectContext rtc_intersect_context;
    rtcInitIntersectContext(&rtc_intersect_context);
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    //
    // perform raycasting
    //

    arma::mat ray_hit_data_float = arma::ones<arma::mat>(ray_positions.n_rows,4) * std::numeric_limits<float>::infinity();
    arma::imat ray_hit_data_int  = arma::ones<arma::imat>(ray_positions.n_rows,1) * -1;

    for (int i = 0; i < ray_positions.n_rows; i++)  {

        // initialize ray
        arma::vec ray_position             = ray_positions.row(i).t();
        arma::vec ray_direction_normalized = arma::normalise(ray_directions.row(i).t());

        RTCRayHit rtc_ray_hit;

        rtc_ray_hit.ray.org_x = (float)ray_position(0);
        rtc_ray_hit.ray.org_y = (float)ray_position(1);
        rtc_ray_hit.ray.org_z = (float)ray_position(2);
        rtc_ray_hit.ray.dir_x = (float)ray_direction_normalized(0);
        rtc_ray_hit.ray.dir_y = (float)ray_direction_normalized(1);
        rtc_ray_hit.ray.dir_z = (float)ray_direction_normalized(2);
        rtc_ray_hit.ray.tnear = 0.0f;
        rtc_ray_hit.ray.tfar  = std::numeric_limits<float>::infinity();
        rtc_ray_hit.ray.time  = 0.0f;
        rtc_ray_hit.ray.mask  = 0;
        rtc_ray_hit.ray.id    = 0;
        rtc_ray_hit.ray.flags = 0;

        rtc_ray_hit.hit.Ng_x      = std::numeric_limits<float>::infinity();
        rtc_ray_hit.hit.Ng_y      = std::numeric_limits<float>::infinity();
        rtc_ray_hit.hit.Ng_z      = std::numeric_limits<float>::infinity();
        rtc_ray_hit.hit.u         = std::numeric_limits<float>::infinity();
        rtc_ray_hit.hit.v         = std::numeric_limits<float>::infinity();
        rtc_ray_hit.hit.primID    = RTC_INVALID_GEOMETRY_ID;
        rtc_ray_hit.hit.geomID    = RTC_INVALID_GEOMETRY_ID;
        rtc_ray_hit.hit.instID[0] = RTC_INVALID_GEOMETRY_ID;

        // intersect ray with scene
        rtcIntersect1(rtc_scene, &rtc_intersect_context, &rtc_ray_hit);

        if (rtc_ray_hit.ray.tfar < std::numeric_limits<float>::infinity()) {
            assert(rtc_ray_hit.hit.primID != RTC_INVALID_GEOMETRY_ID);
            assert(rtc_ray_hit.hit.geomID != RTC_INVALID_GEOMETRY_ID);
        } else {
            assert(rtc_ray_hit.hit.primID == RTC_INVALID_GEOMETRY_ID);
            assert(rtc_ray_hit.hit.geomID == RTC_INVALID_GEOMETRY_ID);
        }

        if (rtc_ray_hit.hit.geomID != RTC_INVALID_GEOMETRY_ID) {
            arma::vec ray_hit_normal_normalized = arma::normalise(arma::vec({rtc_ray_hit.hit.Ng_x, rtc_ray_hit.hit.Ng_y, rtc_ray_hit.hit.Ng_z}));

            ray_hit_data_float(i, 0) = rtc_ray_hit.ray.tfar;
            ray_hit_data_float(i, 1) = ray_hit_normal_normalized(0);
            ray_hit_data_float(i, 2) = ray_hit_normal_normalized(1);
            ray_hit_data_float(i, 3) = ray_hit_normal_normalized(2);

            ray_hit_data_int(i, 0) = rtc_ray_hit.hit.primID;
        }
    }

    //
    // saving intersection distances
    //

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_RAY_INTERSECTIONS] Saving intersection data..." << std::endl;
    }

    ray_hit_data_float.save(output_ray_hit_data_float_file, arma::hdf5_binary_trans);
    ray_hit_data_int.save(output_ray_hit_data_int_file, arma::hdf5_binary_trans);

    //
    // release Embree data
    //

    rtcReleaseScene(rtc_scene); rtc_scene = nullptr;
    assert(rtcGetDeviceError(rtc_device) == RTC_ERROR_NONE);

    rtcReleaseDevice(rtc_device); rtc_device = nullptr;
    assert(rtcGetDeviceError(nullptr) == RTC_ERROR_NONE);

    return 0;
}


/* based on test-suite\bermudanswaption.hpp */

/*
Copyright (C) 2005, 2007 StatPro Italia srl
Copyright (C) 2015 CompatibL

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef quantlib_test_ad_bermudan_swaption_hpp
#define quantlib_test_ad_bermudan_swaption_hpp
#pragma once

#include <boost/test/unit_test.hpp>

class AdjointBermudanSwaptionTest {
public:
    static bool testTreeSwaptionEngine();
    static bool testFdHullWhiteSwaptionEngine();
    static boost::unit_test_framework::test_suite* suite();
};

#endif


#include <iostream>
#include <fstream>

#include <string>
#include <vector>

//#include <boost/log/core.hpp>
#include "xpcf/xpcf.h"

#include <stdlib.h>
#include <stdio.h>
#include <iomanip>
#include <opengv/absolute_pose/methods.hpp>
#include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
#include <opengv/math/cayley.hpp>
#include <sstream>

#include "random_generators.hpp"
#include "experiment_helpers.hpp"
#include "time_measurement.hpp"

#include <boost/log/core.hpp>

using namespace Eigen;
using namespace opengv;

#include "core/Log.h"

// ADD COMPONENTS HEADERS HERE

#include "xpcf/xpcf.h"

#include "api/solver/pose/I3DTransformFinderFrom2D3D.h"
#include "api/solver/pose/I3DTransformSACFinderFrom2D3D.h"

using namespace SolAR;
using namespace SolAR::datastructure;
using namespace SolAR::api;
namespace xpcf  = org::bcom::xpcf;

void help()
{
    std::cout << "\n\n";
    std::cout << "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n";
    std::cout << "Something went wrong with input files \n";
    std::cout << "please refer to README.adoc in the project directory \n";
    std::cout << "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n\n";
    exit(-1);
}

int main()
{
#if NDEBUG
    boost::log::core::get()->set_logging_enabled(false);
#endif

    //initialize random seed
    initializeRandomSeed();

    //set experiment parameters
    double noise = 0.0;
    double outlierFraction = 0.0;
    size_t numberPoints = 100;

    //create a fake central camera
    translations_t camOffsets;
    rotations_t camRotations;
    generateCentralCameraSystem(camOffsets, camRotations);

    //create a random viewpoint pose
    translation_t position = generateRandomTranslation(2.0);
    rotation_t rotation = generateRandomRotation(0.5);

    //derive correspondences based on random point-cloud
    bearingVectors_t bearingVectors;
    points_t points;
    std::vector<int> camCorrespondences; //unused in the central case!
    Eigen::MatrixXd gt(3, numberPoints);
    generateRandom2D3DCorrespondences(
        position, rotation, camOffsets, camRotations, numberPoints, noise, outlierFraction,
        bearingVectors, points, camCorrespondences, gt);

    //print the experiment characteristics
    printExperimentCharacteristics(
        position, rotation, noise, outlierFraction);

    //create a central absolute adapter
    absolute_pose::CentralAbsoluteAdapter adapter(
        bearingVectors,
        points,
        rotation);

    try {

        SRef<xpcf::IComponentManager> xpcfComponentManager = xpcf::getComponentManagerInstance();

        if(xpcfComponentManager->load("SolARTest_ModuleOpenGV_PnP_conf.xml")!=org::bcom::xpcf::_SUCCESS)
        {
            LOG_ERROR("Failed to load the configuration file SolARTest_ModuleOpenGV_PnP_conf.xml")
            return -1;
        }

        CamCalibration  intrinsicParams;
        //set to identity Matrix
        intrinsicParams(0,0) = 1; intrinsicParams(0,1) = 0; intrinsicParams(0,2) = 0;
        intrinsicParams(1,0) = 0; intrinsicParams(1,1) = 1; intrinsicParams(1,2) = 0;
        intrinsicParams(2,0) = 0; intrinsicParams(2,1) = 0; intrinsicParams(2,2) = 1;

        CamDistortion   distortionParams;
        distortionParams(0,0) =0;
        distortionParams(1,0) =0;
        distortionParams(2,0) =0;
        distortionParams(3,0) =0;
        distortionParams(4,0) =0;

        auto poseEstimation_p3p_kneip = xpcfComponentManager->resolve<solver::pose::I3DTransformFinderFrom2D3D>("OpenGVP3PKNEIP");
        auto poseEstimation_p3p_epnp  = xpcfComponentManager->resolve<solver::pose::I3DTransformFinderFrom2D3D>("OpenGVEPNP");
        auto poseEstimation_p3p_gao   = xpcfComponentManager->resolve<solver::pose::I3DTransformFinderFrom2D3D>("OpenGVP3PGAO");
        auto poseEstimation_p3p_upnp  = xpcfComponentManager->resolve<solver::pose::I3DTransformFinderFrom2D3D>("OpenGVUPNP");

        auto poseEstimation_epnp_sac = xpcfComponentManager->resolve<solver::pose::I3DTransformSACFinderFrom2D3D>("OpenGVSACEPNP");
        auto poseEstimation_p3p_sac_gao = xpcfComponentManager->resolve<solver::pose::I3DTransformSACFinderFrom2D3D>("OpenGVSACP3PGAO");
        auto poseEstimation_p3p_sac_kneip  = xpcfComponentManager->resolve<solver::pose::I3DTransformSACFinderFrom2D3D>("OpenGVSACP3PKNEIP");

        // initialize pose estimation components with camera paremeters
        poseEstimation_p3p_kneip->setCameraParameters(  intrinsicParams, distortionParams);
        poseEstimation_p3p_gao->setCameraParameters(    intrinsicParams, distortionParams);
        poseEstimation_p3p_epnp->setCameraParameters(   intrinsicParams, distortionParams);
        poseEstimation_p3p_upnp->setCameraParameters(   intrinsicParams, distortionParams);
        poseEstimation_epnp_sac->setCameraParameters(   intrinsicParams, distortionParams);
        poseEstimation_p3p_sac_gao->setCameraParameters(   intrinsicParams, distortionParams);
        poseEstimation_p3p_sac_kneip->setCameraParameters(   intrinsicParams, distortionParams);

         //synthetize 2d points and 3d points to test the components.
         std::vector<Point2Df>  imagePoints;
         std::vector<Point3Df>  worldPoints;

         unsigned int Npoints = 1024;

         imagePoints.resize(Npoints);
         worldPoints.resize(Npoints);

         for(unsigned int kc =0; kc < Npoints; kc++){

             double minDepth = 4;
             double maxDepth = 8;

             Eigen::Vector3d  current_random_point = generateRandomPoint( maxDepth, minDepth );

             //get the camera transformation
             translation_t camOffset = camOffsets[0];
             rotation_t camRotation = camRotations[0];

             //project the point into the viewpoint frame
             point_t bodyPoint = rotation.transpose()*(current_random_point - position);

             imagePoints[kc] = Point2Df(bodyPoint(0,0), bodyPoint(1,0)) ;
             worldPoints[kc] = Point3Df(current_random_point(0,0),current_random_point(1,0),current_random_point(2,0));
         }

         Transform3Df pose_p3p_kneip;
         Transform3Df pose_p3p_gao;
         Transform3Df pose_p3p_epnp;
         Transform3Df pose_p3p_upnp;
         Transform3Df pose_p3p_epnp_sac;
         Transform3Df pose_p3p_gao_sac;
         Transform3Df pose_p3p_kneip_sac;

         std::vector<uint32_t> inlier_epnp_sac;
         std::vector<uint32_t> inlier_gao_sac;
         std::vector<uint32_t> inlier_kneip_sac;

         //timer
         struct timeval tic;
         struct timeval toc;
         size_t iterations = 50;

         std::cout<< "********************* pose_p3p_kneip **************************"<<std::endl;
         gettimeofday(&tic, 0);
         poseEstimation_p3p_kneip->estimate(imagePoints, worldPoints, pose_p3p_kneip);
         std::cout<<pose_p3p_kneip.matrix()<< std::endl;
         gettimeofday(&toc, 0);
         std::cout<<"Computed in "<< TIMETODOUBLE(timeval_minus(toc, tic)) <<"s"<<std::endl;

         std::cout<< "********************* pose_p3p_kneip_sac **************************"<<std::endl;
         gettimeofday(&tic, 0);
         poseEstimation_p3p_sac_kneip->estimate(imagePoints, worldPoints, inlier_kneip_sac, pose_p3p_kneip_sac);
         std::cout<<pose_p3p_kneip_sac.matrix()<< std::endl;
         gettimeofday(&toc, 0);
         std::cout<<"Computed in "<< TIMETODOUBLE(timeval_minus(toc, tic)) <<"s"<<std::endl;

         std::cout<< "********************* pose_p3p_gao **************************"<<std::endl;
         gettimeofday(&tic, 0);
         poseEstimation_p3p_gao->estimate(imagePoints, worldPoints, pose_p3p_gao);
         std::cout<<pose_p3p_kneip_sac.matrix()<< std::endl;
         gettimeofday(&toc, 0);
         std::cout<<"Computed in "<< TIMETODOUBLE(timeval_minus(toc, tic)) <<"s"<<std::endl;

         std::cout<< "********************* pose_p3p_gao_sac **************************"<<std::endl;
         gettimeofday(&tic, 0);
         poseEstimation_p3p_sac_gao->estimate(imagePoints, worldPoints, inlier_gao_sac, pose_p3p_gao_sac);
         std::cout<<pose_p3p_gao_sac.matrix()<< std::endl;
         gettimeofday(&toc, 0);
         std::cout<<"Computed in "<< TIMETODOUBLE(timeval_minus(toc, tic)) <<"s"<<std::endl;

         std::cout<< "********************* pose_p3p_epnp **************************"<<std::endl;
         gettimeofday(&tic, 0);
         poseEstimation_p3p_epnp->estimate(imagePoints, worldPoints, pose_p3p_epnp);
         std::cout<<pose_p3p_epnp.matrix()<< std::endl;
         gettimeofday(&toc, 0);
         std::cout<<"Computed in "<< TIMETODOUBLE(timeval_minus(toc, tic)) <<"s"<<std::endl;

         std::cout<< "********************* pose_p3p_epnp_sac **************************"<<std::endl;
         gettimeofday(&tic, 0);
         poseEstimation_epnp_sac->estimate(imagePoints, worldPoints, inlier_epnp_sac, pose_p3p_epnp_sac);
         std::cout<<pose_p3p_epnp_sac.matrix()<< std::endl;
         gettimeofday(&toc, 0);
         std::cout<<"Computed in "<< TIMETODOUBLE(timeval_minus(toc, tic)) <<"s"<<std::endl;

         std::cout<< "********************* pose_p3p_upnp **************************"<<std::endl;
         gettimeofday(&tic, 0);
         poseEstimation_p3p_upnp->estimate(imagePoints, worldPoints, pose_p3p_upnp);
         std::cout<<pose_p3p_upnp.matrix()<< std::endl;
         gettimeofday(&toc, 0);
         std::cout<<"Computed in "<< TIMETODOUBLE(timeval_minus(toc, tic)) <<"s"<<std::endl;
    }
    catch (xpcf::Exception e)
    {
        LOG_ERROR ("The following exception has been catched: {}", e.what());
        return -1;
    }

    return 0;
}