#include <rw/math/Q.hpp>
#include "ros/ros.h"
#include "ros/package.h"
#include "ros/this_node.h"
#include "Application.hpp"
#include <string>
#include <rw/math/MetricFactory.hpp>
#include <algorithm>
#include <rw/math/Math.hpp>
#include <time.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/algorithm/string.hpp>
#include <boost/chrono.hpp>
#include <stdlib.h>
#include <time.h>
#include <stdio.h>
#include <iomanip>

Application::Application(ros::NodeHandle h):

	_n(h)

{

	initWorkCell();
	path = std::list<rw::math::Q>(0);
	running_path = std::list<rw::math::Q>(0);
	test = std::vector<rw::math::Transform3D<double> >(0);
	testQ = std::vector<rw::math::Q>(0);
	srand (1234);

	ROS_INFO("%s", "Test_1");
	while(test.size() < 1000)
	{
		rw::math::Vector3D<double> newPos((rand()%1000)/1000.0 - 0.5, (rand()%1000)/1000.0 - 0.5, (rand()%2000)/1000.0 - 1);
		rw::math::Transform3D<double> newTrans(newPos);

		std::vector<rw::math::Q> qVec = _solver->solve(newTrans, _state);

		if(qVec.size() != 0 and qVec[0].size() >1)
		{
			//std::stringstream buffer;
			//buffer << "TestTrans:  " << newTrans << std::endl;
			//ROS_INFO("%s", buffer.str().c_str());
			test.push_back(newTrans);
			testQ.push_back(qVec[0]);

		}

	}
	ROS_INFO("%s", "Test_2");
	f = std::ofstream( ros::package::getPath("rovi2") + "/Roadmap/" + "Data.txt");

	rovi2::Plan srv;
	srv.request.goal = toRos(testQ.at(index));
	srv.request.init = toRos(_device->getQ(_state));
	index++;
	begin = std::chrono::steady_clock::now();
	path_call.call(srv);

};

Application::~Application()
{
	f.close();
	delete _workcell;
  	delete _device;
	delete _metric;
};

/************************************************************************
 * Q  -> lend from Caros
 ************************************************************************/
rw::math::Q Application::toRw(const rovi2::Q& q)
{
  rw::math::Q res(q.data.size());
  for (std::size_t i = 0; i < q.data.size(); ++i)
  {
    res(i) = q.data[i];
  }
  return res;
}

rovi2::Q Application::toRos(const rw::math::Q& q)
{
  rovi2::Q res;
  res.data.resize(q.size());
  for (std::size_t i = 0; i < q.size(); ++i)
  {
    res.data[i] = static_cast<double>(q(i));
  }
  return res;
}

void Application::initWorkCell()
{
	std::string path = ros::package::getPath("rovi2") + "/WorkStation_astar/WC3_Scene.wc.xml";
	_workcell = rw::loaders::WorkCellLoader::Factory::load(path);

	_device = _workcell->findDevice("UR1");

	_state =  _workcell->getDefaultState();

// Create metric weight, as motion weight in cartesian space.
	_metricWeights = rw::pathplanning::PlannerUtil::estimateMotionWeights(*_device, _device->getEnd(),_state,rw::pathplanning::PlannerUtil::WORSTCASE,1000);

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

	// Create the metric
	//_metric = rw::math::MetricFactory::makeWeightedEuclidean<rw::math::Q>(_metricWeights);
	_metric = rw::math::MetricFactory::makeEuclidean<rw::math::Q>();

	_BallErrorFrame = _workcell->findFrame("WSG50.BallError");
	_baseFrame = _workcell->findFrame("UR1.Base");

	_solver = new rw::invkin::JacobianIKSolver(_device, _BallErrorFrame, _state);

	_solver->setClampToBounds(true);

	//robot_controller = _n.subscribe("/rovi2/robot_node/Robot_state", 1, &Application::RobotCallback, this);
	//goal_controller = _n.subscribe("/ball_locator_3d/kalman_prediction", 1, &Application::GoalCallback, this);
	path_controller = _n.subscribe("/rovi2/Roadmap/Path" , 1, &Application::PathCallback, this);

	path_call = _n.serviceClient<rovi2::Plan>("/rovi2/Roadmap/StartPlan");
	//robot_call = _n.serviceClient<rovi2::MovePtp>("rovi2/robot_node/Move_nonlinear_ptp");

	moving_to = _device->getQ(_state);
	//moving_to = rw::math::Q(6,0.0,0.0,0.0,0.0,0.0,0.0);

};

void Application::RobotCallback(const rovi2::State &State)
{

	static int count = 0;
	/*if(!plan_incoming && !path.empty() && running_path.empty())
	{
		// TODO lazy collision detection
		running_path.push_back(path.front());
		path.pop_front();

	}
	_device->setQ(toRw(State.q), _state);

	if(!State.is_moving)
	{
		count++;
		if(count >= 20)
		{
			starting = true;
			count = 0;

		}

	}
	else
		count = 0;*/

	if(!path.empty())//!running_path.empty() && !State.e)
	{

		//double dist = _metric->distance(_device->getQ(_state), running_path.front());
		//std::stringstream diss;
		//diss << "Distance to next node:  " << dist << std::endl;
		//ROS_INFO("%s", diss.str().c_str());

		if(true)//dist < 0.05 || starting)
		{
			starting = false;
			rovi2::MovePtp srv;
			srv.request.target = toRos(path.front());//running_path.front());
			std::stringstream buffer;
			//buffer << "Robot moving to:  " << running_path.front() << std::endl;
			ROS_INFO("%s", buffer.str().c_str());
			//moving_to = running_path.front();
			path.pop_front();
			//running_path.pop_front();

			robot_call.call(srv);

		}

	}

}

void Application::GoalCallback(const rovi2::position3D &position)
{

}

void Application::PathCallback(const rovi2::path &path_in)
{

	path.clear();
	for(int i = 0; i< path_in.data.size(); i++)
	{

		path.push_back(toRw(path_in.data.at(i)));

	}

	end = std::chrono::steady_clock::now();
	if ( !f.fail())
	{

		if(path.size() > 0)
		{
			_device->setQ(path.back(), _state);
		}

		rw::math::Transform3D<double> goal = _baseFrame->fTf(_BallErrorFrame, _state);
		rw::math::Vector3D<double> error = test.at(index-1).P() - goal.P();
		f << error.norm2() << '\t';
		f << (std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin).count()) << '\t';
		f << path.size() << std::endl;

	}

	if(index < 1000)
	{
			rovi2::Plan srv;
			srv.request.goal = toRos(testQ.at(index));
			_state =  _workcell->getDefaultState();
			srv.request.init = toRos(_device->getQ(_state));
			index++;
			begin = std::chrono::steady_clock::now();
			ROS_INFO("%s%i", "Index is: ", index);
			path_call.call(srv);

	}
	else if(index == 1000)
		ROS_INFO("Done");

}

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

  ros::init(argc,argv,"Application_node");
  ros::NodeHandle n;

   Application Application_node(n);

  while(ros::ok())
  {
    ros::spinOnce();
  }
  return 0;
}


/*
Copyright 2018 Rui Miguel Horta Pimentel de Figueiredo

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

/*!
    \author Rui Figueiredo : ruipimentelfigueiredo
*/

#include <ctime>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
#include "cylinder_fitting_hough.h"
#include "cylinder_fitting_ransac.h"
#include "helpers.h"
#include <chrono>
using namespace std;
using namespace std::chrono;

int main (int argc, char** argv)
{
	/* TODO - Process options */
	if (argc < 6)
	{
		std::cout << "invalid number of arguments: dataset_dir heights radii noise clutter"<< std::endl;
		exit(-1);
	}

	std::string dataset_dir=std::string(argv[1]);
	std::string ground_truth_dir=dataset_dir+"ground_truth/";
	std::string point_clouds_dir=dataset_dir+"point_clouds/";
	std::string output_dir=dataset_dir+"results/";

	// HOUGH PARAMETERS
	unsigned int angle_bins=atoi(argv[2]);
	std::cout << "angle_bins: " << angle_bins<< std::endl;

	unsigned int radius_bins=atoi(argv[3]);
	std::cout << "radius_bins: " << radius_bins<< std::endl;

	unsigned int position_bins=atoi(argv[4]);
	std::cout << "position_bins: " << position_bins<< std::endl;

	// OUR HOUGH IMPLEMENTATION PARAMETERS
	unsigned int orientation_accumulators_num=atoi(argv[5]);
	std::cout << "orientation_accumulators_num: " << orientation_accumulators_num<< std::endl;

	unsigned int gaussian_sphere_points_num=atoi(argv[6]);
	std::cout << "gaussian_sphere_points_num: " << gaussian_sphere_points_num<< std::endl;

	float accumulator_peak_threshold=atof(argv[7]);
	std::cout << "accumulator_peak_threshold: " << accumulator_peak_threshold<< std::endl;

	float min_radius=atof(argv[8]);
	std::cout << "min_radius: " << min_radius << std::endl;

	float max_radius=atof(argv[9]);
	std::cout << "max_radius: " << max_radius << std::endl;

	// RANSAC PARAMETERS
	float normal_distance_weight=atof(argv[10]);
	std::cout << "normal_distance_weight: " << normal_distance_weight<< std::endl;

	float distance_threshold=atof(argv[11]);
	std::cout << "distance_threshold: " << distance_threshold << std::endl;

	bool do_refine=atoi(argv[12]);
	std::cout << "do_refine: " << do_refine << std::endl;

	bool visualize=atoi(argv[13]);
	std::cout << "visualize: " << visualize << std::endl;

	boost::shared_ptr<VisualizeFittingData> visualizer;

	if(visualize)
		visualizer=boost::shared_ptr<VisualizeFittingData>(new VisualizeFittingData());

	// Voting Sphere Uniform Grid
	SphericalGrid spherical_grid(gaussian_sphere_points_num,orientation_accumulators_num);

	// Gaussian Sphere Uniform
	std::vector<double> weights;
	std::vector<Eigen::Matrix<double, 3 ,1> > means;
	std::vector<Eigen::Matrix<double, 3 ,1> > std_devs;
	weights.push_back(1.0);
	Eigen::Matrix<double, 3 ,1> mean_eigen(0,0,0);
	means.push_back(mean_eigen);
	Eigen::Matrix<double, 3 ,1> std_dev_eigen(0.5,0.5,0.5);
	std_devs.push_back(std_dev_eigen);

	GaussianMixtureModel gmm(weights,means,std_devs);
	GaussianSphere gaussian_sphere(gmm,gaussian_sphere_points_num,orientation_accumulators_num);

	// Gaussian Sphere Top Biased
	std::vector<double> weights_biased;
	std::vector<Eigen::Matrix<double, 3 ,1> > means_biased;
	std::vector<Eigen::Matrix<double, 3 ,1> > std_devs_biased;
	weights_biased.push_back(1.0);
	Eigen::Matrix<double, 3 ,1> mean_eigen_biased(0,0,1.0);
	means_biased.push_back(mean_eigen_biased);
	Eigen::Matrix<double, 3 ,1> std_dev_eigen_biased(0.5,0.5,0.5);
	std_devs_biased.push_back(std_dev_eigen_biased);

	GaussianMixtureModel gmm_biased(weights_biased,means_biased,std_devs_biased);
	GaussianSphere gaussian_sphere_biased(gmm_biased,gaussian_sphere_points_num,orientation_accumulators_num);

	// Gaussian Sphere Strong Top Biased
	std::vector<double> weights_super_biased;
	std::vector<Eigen::Matrix<double, 3 ,1> > means_super_biased;
	std::vector<Eigen::Matrix<double, 3 ,1> > std_devs_super_biased;
	weights_super_biased.push_back(1.0);
	Eigen::Matrix<double, 3 ,1> mean_eigen_super_biased(0,0,1.0);
	means_super_biased.push_back(mean_eigen_super_biased);
	Eigen::Matrix<double, 3 ,1> std_dev_eigen_super_biased(0.05,0.05,0.05);
	std_devs_super_biased.push_back(std_dev_eigen_super_biased);

	GaussianMixtureModel gmm_super_biased(weights_super_biased,means_super_biased,std_devs_super_biased);
	GaussianSphere gaussian_sphere_super_biased(gmm_super_biased,gaussian_sphere_points_num,orientation_accumulators_num);

	std::vector<boost::shared_ptr<CylinderFitting> > cylinder_segmentators;

	// HOUGH RABANI
	cylinder_segmentators.push_back(boost::shared_ptr<CylinderFittingHough> (new CylinderFittingHough(gaussian_sphere,(unsigned int)angle_bins,(unsigned int)radius_bins,(unsigned int)position_bins,(float)min_radius, (float)max_radius,(float)accumulator_peak_threshold,CylinderFittingHough::NORMAL, false, false)));

	// HOUGH HYBRID (soft-voting)
	cylinder_segmentators.push_back(boost::shared_ptr<CylinderFittingHough> (new CylinderFittingHough(gaussian_sphere,(unsigned int)angle_bins,(unsigned int)radius_bins,(unsigned int)position_bins,(float)min_radius, (float)max_radius,(float)accumulator_peak_threshold,CylinderFittingHough::HYBRID,false, true)));

	// HOUGH HYBRID BIASED (soft-voting)
	cylinder_segmentators.push_back(boost::shared_ptr<CylinderFittingHough> (new CylinderFittingHough(gaussian_sphere_biased,(unsigned int)angle_bins,(unsigned int)radius_bins,(unsigned int)position_bins,(float)min_radius, (float)max_radius,(float)accumulator_peak_threshold,CylinderFittingHough::HYBRID,false, true)));

	// HOUGH HYBRID SUPER BIASED (soft-voting)
	cylinder_segmentators.push_back(boost::shared_ptr<CylinderFittingHough> (new CylinderFittingHough(gaussian_sphere_super_biased,(unsigned int)angle_bins,(unsigned int)radius_bins,(unsigned int)position_bins,(float)min_radius, (float)max_radius,(float)accumulator_peak_threshold,CylinderFittingHough::HYBRID,false, true)));

	// HOUGH HYBRID (soft-voting ICARSC)
	cylinder_segmentators.push_back(boost::shared_ptr<CylinderFittingHough> (new CylinderFittingHough(gaussian_sphere,(unsigned int)angle_bins,(unsigned int)radius_bins,(unsigned int)position_bins,(float)min_radius, (float)max_radius,(float)accumulator_peak_threshold,2,false, true)));

	// Get ground truths
	GroundTruth ground_truths(ground_truth_dir);
	std::cout << "Number of ground truths:" << ground_truths.ground_truth.size() << std::endl;

	// Get point clouds
	PointClouds point_clouds(point_clouds_dir);
	std::cout << "Number of point_clouds:" << point_clouds.file_names.size() << std::endl;

	std::string detections_frame_id="world";
	std::string marker_detections_namespace_="detections";

	// Segment cylinder and store results
	std::vector<Eigen::VectorXf > detections;
	std::vector<float> position_errors;

	createDirectory(output_dir);

	pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne_org;
	ne_org.setNormalEstimationMethod (ne_org.AVERAGE_3D_GRADIENT);
	ne_org.setMaxDepthChangeFactor(0.02f);
	ne_org.setNormalSmoothingSize(10.0f);

	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
	pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
	ne.setKSearch (6);
	ne.setSearchMethod (tree);

	pcl::PointCloud<PointT>::Ptr point_cloud(new pcl::PointCloud<PointT>());
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>());

	for (unsigned int d=0;d < cylinder_segmentators.size();++d)
	{
		std::fstream fs_orientation;
		std::fstream fs_radius;
		std::fstream fs_position;
		std::fstream fs_time;

		fs_orientation.open (output_dir+"orientation_" + std::to_string(d)+".txt", std::fstream::in | std::fstream::out | std::fstream::trunc);
		fs_radius.open (output_dir+"radius_"           + std::to_string(d)+".txt", std::fstream::in | std::fstream::out | std::fstream::trunc);
		fs_position.open (output_dir+"position_"       + std::to_string(d)+".txt", std::fstream::in | std::fstream::out | std::fstream::trunc);
		fs_time.open (output_dir+"time_"               + std::to_string(d)+".txt", std::fstream::in | std::fstream::out | std::fstream::trunc);

		for(unsigned int i=0;i<point_clouds.file_names.size();++i)
		{
			// Ground truth iterations
			unsigned int iteration=i % ground_truths.ground_truth.size();

			Cylinder ground_truth=ground_truths.ground_truth[iteration];

			point_cloud=point_clouds.loadPointCloud(point_clouds_dir+point_clouds.file_names[i]);

			/* FIT */
			if(point_cloud->isOrganized())
			{
				ne_org.setInputCloud (point_cloud);
				ne_org.compute (*cloud_normals);
			}
			else
			{
				ne.setInputCloud (point_cloud);
				ne.compute (*cloud_normals);
			}

			for(unsigned int i=0; i<cloud_normals->size();++i)
			{
				double dot_product=cloud_normals->points[i].getNormalVector3fMap ().dot(Eigen::Vector3f(point_cloud->points[i].x,point_cloud->points[i].y,point_cloud->points[i].z));
				if(dot_product<0.0)
				{
					cloud_normals->at(i).normal_x=-cloud_normals->at(i).normal_x;
					cloud_normals->at(i).normal_y=-cloud_normals->at(i).normal_y;
					cloud_normals->at(i).normal_z=-cloud_normals->at(i).normal_z;
				}
			}

			high_resolution_clock::time_point t1 = high_resolution_clock::now();
			FittingData model_params=cylinder_segmentators[d]->fit(point_cloud, cloud_normals);
			high_resolution_clock::time_point t2 = high_resolution_clock::now();
			/* END FIT */

			/* STORE RESULTS */
			float orientation_error=acos(model_params.parameters.segment(3,3).dot(Eigen::Vector3f(ground_truth.direction[0],ground_truth.direction[1],ground_truth.direction[2])));
			if(orientation_error>M_PI/2.0)
				orientation_error=M_PI-orientation_error;
			fs_orientation << orientation_error << " " << "\n";

			float radius_error=fabs(model_params.parameters[6]-ground_truth.radius);
			fs_radius << radius_error << " " << "\n";

			float position_error=(model_params.parameters.head(2)-Eigen::Vector2f(ground_truth.position[0], ground_truth.position[1])).norm();
			fs_position << position_error << " " << "\n";

			auto duration = duration_cast<milliseconds>( t2 - t1 ).count();
			fs_time << duration << " " << "\n";
			/* END STORE RESULTS */

			/* VISUALIZE */
			if(visualize)
				model_params.visualize(point_cloud,visualizer);
			/* END VISUALIZE */

			std::cout << "detector:" << (d+1) << " of " << cylinder_segmentators.size() << "  -  iteration " << i << " of " << point_clouds.file_names.size() << "  -  fitting time: " << duration << " ms"<< std::endl;
		}

		fs_orientation.close();
		fs_radius.close();
		fs_position.close();
		fs_time.close();
	}

	return (0);
}


//
//  Copyright (c) 2009-2011 Artyom Beilis (Tonkikh)
//
//  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_LOCALE_SOURCE
#include <boost/config.hpp>
#ifdef BOOST_MSVC
#  pragma warning(disable : 4996)
#endif
#include <locale>
#include <string>
#include <ios>
#include <boost/locale/date_time_facet.hpp>
#include <boost/locale/date_time.hpp>
#include <boost/locale/hold_ptr.hpp>
#include <stdlib.h>
#include <ctime>
#include <memory>
#include <algorithm>
#include <limits>

#include "timezone.hpp"
#include "gregorian.hpp"

namespace boost {
namespace locale {
namespace util {
    namespace {

        int is_leap(int year)
        {
            if(year % 400 == 0)
                return 1;
            if(year % 100 == 0)
                return 0;
            if(year % 4 == 0)
                return 1;
            return 0;
        }

        int days_in_month(int year,int month)
        {
            static const int tbl[2][12] = {
                { 31,28,31,30,31,30,31,31,30,31,30,31 },
                { 31,29,31,30,31,30,31,31,30,31,30,31 }
            };
            return tbl[is_leap(year)][month - 1];
        }

        inline int days_from_0(int year)
        {
            year--;
            return 365 * year + (year / 400) - (year/100) + (year / 4);
        }

        int days_from_1970(int year)
        {
            static const int days_from_0_to_1970 = days_from_0(1970);
            return days_from_0(year) - days_from_0_to_1970;
        }

        int days_from_1jan(int year,int month,int day)
        {
            static const int days[2][12] = {
                { 0,31,59,90,120,151,181,212,243,273,304,334 },
                { 0,31,60,91,121,152,182,213,244,274,305,335 }
            };
            return days[is_leap(year)][month-1] + day - 1;
        }

        std::time_t internal_timegm(std::tm const *t)
        {
            int year = t->tm_year + 1900;
            int month = t->tm_mon;
            if(month > 11) {
                year += month/12;
                month %= 12;
            }
            else if(month < 0) {
                int years_diff = (-month + 11)/12;
                year -= years_diff;
                month+=12 * years_diff;
            }
            month++;
            int day = t->tm_mday;
            int day_of_year = days_from_1jan(year,month,day);
            int days_since_epoch = days_from_1970(year) + day_of_year;

            std::time_t seconds_in_day = 3600 * 24;
            std::time_t result =  seconds_in_day * days_since_epoch + 3600 * t->tm_hour + 60 * t->tm_min + t->tm_sec;

            return result;
        }

    } // anon

    namespace {

        // Locale dependent data

        bool comparator(char const *left,char const *right)
        {
            return strcmp(left,right) < 0;
        }

        //
        // Ref: CLDR 1.9 common/supplemental/supplementalData.xml
        //
        // monday - default
        // fri - MV
        // sat - AE AF BH DJ DZ EG ER ET IQ IR JO KE KW LY MA OM QA SA SD SO SY TN YE
        // sun - AR AS AZ BW CA CN FO GE GL GU HK IL IN JM JP KG KR LA MH MN MO MP MT NZ PH PK SG TH TT TW UM US UZ VI ZW
        //

        int first_day_of_week(char const *terr) {
            static char const * const sat[] = {
                "AE","AF","BH","DJ","DZ","EG","ER","ET","IQ","IR",
                "JO","KE","KW","LY","MA","OM","QA","SA","SD","SO",
                "SY","TN","YE"
            };
            static char const * const sunday[] = {
                "AR","AS","AZ","BW","CA","CN","FO","GE","GL","GU",
                "HK","IL","IN","JM","JP","KG","KR","LA","MH","MN",
                "MO","MP","MT","NZ","PH","PK","SG","TH","TT","TW",
                "UM","US","UZ","VI","ZW"
            };
            if(strcmp(terr,"MV") == 0)
                return 5; // fri
            if(std::binary_search<char const * const *>(sat,sat+sizeof(sat)/(sizeof(sat[0])),terr,comparator))
                return 6; // sat
            if(std::binary_search<char const * const *>(sunday,sunday+sizeof(sunday)/(sizeof(sunday[0])),terr,comparator))
                return 0; // sun
            // default
            return 1; // mon
        }
    }

    class gregorian_calendar : public abstract_calendar {
    public:

            gregorian_calendar(std::string const &terr)
            {
                first_day_of_week_ = first_day_of_week(terr.c_str());
                time_ = std::time(0);
                is_local_ = true;
                tzoff_ = 0;
                from_time(time_);
            }

            ///
            /// Make a polymorphic copy of the calendar
            ///
            virtual gregorian_calendar *clone() const
            {
                return new gregorian_calendar(*this);
            }

            ///
            /// Set specific \a value for period \a p, note not all values are settable.
            ///
            virtual void set_value(period::marks::period_mark p,int value)
            {
                using namespace period::marks;
                switch(p) {
                case era:                        ///< Era i.e. AC, BC in Gregorian and Julian calendar, range [0,1]
                    return;
                case year:                       ///< Year, it is calendar specific
                case extended_year:              ///< Extended year for Gregorian/Julian calendars, where 1 BC == 0, 2 BC == -1.
                    tm_updated_.tm_year = value - 1900;
                    break;
                case month:
                    tm_updated_.tm_mon = value;
                    break;
                case day:
                    tm_updated_.tm_mday = value;
                    break;
                case hour:                       ///< 24 clock hour [0..23]
                    tm_updated_.tm_hour = value;
                    break;
                case hour_12:                    ///< 12 clock hour [0..11]
                    tm_updated_.tm_hour = tm_updated_.tm_hour / 12 * 12 + value;
                    break;
                case am_pm:                      ///< am or pm marker, [0..1]
                    tm_updated_.tm_hour = 12 * value + tm_updated_.tm_hour % 12;
                    break;
                case minute:                     ///< minute [0..59]
                    tm_updated_.tm_min = value;
                    break;
                case second:
                    tm_updated_.tm_sec = value;
                    break;
                case day_of_year:
                    normalize();
                    tm_updated_.tm_mday += (value - (tm_updated_.tm_yday + 1));
                    break;
                case day_of_week:           ///< Day of week, starting from Sunday, [1..7]
                    if(value < 1) // make sure it is positive
                        value += (-value / 7) * 7 + 7;
                    // convert to local DOW
                    value = (value - 1 - first_day_of_week_ + 14) % 7 + 1;
                    // fall throght
                case day_of_week_local:     ///< Local day of week, for example in France Monday is 1, in US Sunday is 1, [1..7]
                    normalize();
                    tm_updated_.tm_mday += (value - 1) - (tm_updated_.tm_wday - first_day_of_week_ + 7) % 7;
                    break;
                case day_of_week_in_month:  ///< Original number of the day of the week in month. (1st sunday, 2nd sunday etc)
                case week_of_year:          ///< The week number in the year, 4 is the minimal number of days to be in month
                case week_of_month:         ///< The week number withing current month
                    {
                        normalize();
                        int current_week = get_value(p,current);
                        int diff = 7 * (value - current_week);
                        tm_updated_.tm_mday += diff;
                    }
                    break;
                case period::marks::first_day_of_week:          ///< For example Sunday in US, Monday in France
                default:
                    return;
                }
                normalized_ = false;
            }

            void normalize()
            {
                if(!normalized_) {
                    std::tm val = tm_updated_;
                    val.tm_isdst = -1;
                    val.tm_wday = -1; // indecator of error
                    std::time_t point = -1;
                    if(is_local_) {
                        point = std::mktime(&val);
                        if(point == static_cast<std::time_t>(-1)){
                            #ifndef BOOST_WINDOWS
                            // windows does not handle negative time_t, under other plaforms
                            // it may be actually valid value in  1969-12-31 23:59:59
                            // so we check that a filed was updated - does not happen in case of error
                            if(val.tm_wday == -1)
                            #endif
                            {
                                throw date_time_error("boost::locale::gregorian_calendar: invalid time");
                            }
                        }
                    }
                    else {
                        point = internal_timegm(&val);
                        #ifdef BOOST_WINDOWS
                        // Windows uses TLS, thread safe
                        std::tm *revert_point = 0;
                        if(point < 0  || (revert_point = gmtime(&point)) == 0)
                            throw date_time_error("boost::locale::gregorian_calendar time is out of range");
                        val = *revert_point;
                        #else
                        if(!gmtime_r(&point,&val))
                            throw date_time_error("boost::locale::gregorian_calendar invalid time");
                        #endif

                    }

                    time_ = point - tzoff_;
                    tm_ = val;
                    tm_updated_ = val;
                    normalized_ = true;
                }
            }

            int get_week_number(int day,int wday) const
            {
                ///
                /// This is the number of days that are considered withing
                /// period such that the week belongs there
                ///
                static const int days_in_full_week = 4;

                // Alaways use local week start
                int current_dow = (wday - first_day_of_week_ + 7) % 7;
                // Calculate local week day of Jan 1st.
                int first_week_day = (current_dow + 700 - day) % 7;
                    // adding something big devidable by 7

                int start_of_period_in_weeks;
                if(first_week_day < days_in_full_week) {
                    start_of_period_in_weeks = - first_week_day;
                }
                else {
                    start_of_period_in_weeks = 7 - first_week_day;
                }
                int week_number_in_days = day - start_of_period_in_weeks;
                if(week_number_in_days < 0)
                    return -1;
                return week_number_in_days / 7 + 1;
            }

            ///
            /// Get specific value for period \a p according to a value_type \a v
            ///
            virtual int get_value(period::marks::period_mark p,value_type v) const
            {
                using namespace period::marks;
                switch(p) {
                case era:
                    return 1;
                case year:
                case extended_year:
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        #ifdef BOOST_WINDOWS
                        return 1970; // Unix epoch windows can't handle negative time_t
                        #else
                        if(sizeof(std::time_t) == 4)
                            return 1901; // minimal year with 32 bit time_t
                        else
                            return 1;
                        #endif
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        if(sizeof(std::time_t) == 4)
                            return 2038; // Y2K38 - maximal with 32 bit time_t
                        else
                            return std::numeric_limits<int>::max();
                    case current:
                        return tm_.tm_year + 1900;
                    };
                    break;
                case month:
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 0;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 11;
                    case current:
                        return tm_.tm_mon;
                    };
                    break;
                case day:
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                        return 31;
                    case least_maximum:
                        return 28;
                    case actual_maximum:
                        return days_in_month(tm_.tm_year + 1900,tm_.tm_mon + 1);
                    case current:
                        return tm_.tm_mday;
                    };
                    break;
                case day_of_year:                ///< The number of day in year, starting from 1
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                        return 366;
                    case least_maximum:
                        return 365;
                    case actual_maximum:
                        return is_leap(tm_.tm_year + 1900) ? 366 : 365;
                    case current:
                        return tm_.tm_yday + 1;
                    }
                    break;
                case day_of_week:                ///< Day of week, starting from Sunday, [1..7]
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 7;
                    case current:
                        return tm_.tm_wday + 1;
                    }
                    break;
                case day_of_week_local:     ///< Local day of week, for example in France Monday is 1, in US Sunday is 1, [1..7]
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 7;
                    case current:
                        return (tm_.tm_wday - first_day_of_week_ + 7) % 7 + 1;
                    }
                    break;
                case hour:                       ///< 24 clock hour [0..23]
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 0;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 23;
                    case current:
                        return tm_.tm_hour;
                    }
                    break;
                case hour_12:                    ///< 12 clock hour [0..11]
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 0;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 11;
                    case current:
                        return tm_.tm_hour % 12;
                    }
                    break;
                case am_pm:                      ///< am or pm marker, [0..1]
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 0;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 1;
                    case current:
                        return tm_.tm_hour >= 12 ? 1 : 0;
                    }
                    break;
                case minute:                     ///< minute [0..59]
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 0;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 59;
                    case current:
                        return tm_.tm_min;
                    }
                    break;
                case second:                     ///< second [0..59]
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 0;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 59;
                    case current:
                        return tm_.tm_sec;
                    }
                    break;
                case period::marks::first_day_of_week:          ///< For example Sunday in US, Monday in France
                    return first_day_of_week_ + 1;

                case week_of_year:               ///< The week number in the year
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                        return 53;
                    case least_maximum:
                        return 52;
                    case actual_maximum:
                        {
                            int year = tm_.tm_year + 1900;
                            int end_of_year_days = (is_leap(year) ? 366 : 365) - 1;
                            int dow_of_end_of_year = (end_of_year_days - tm_.tm_yday + tm_.tm_wday) % 7;
                            return get_week_number(end_of_year_days,dow_of_end_of_year);
                        }
                    case current:
                        {
                            int val = get_week_number(tm_.tm_yday,tm_.tm_wday);
                            if(val < 0)
                                return 53;
                            return val;
                        }
                    }
                case week_of_month:              ///< The week number withing current month
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                        return 5;
                    case least_maximum:
                        return 4;
                    case actual_maximum:
                        {
                            int end_of_month_days = days_in_month(tm_.tm_year + 1900,tm_.tm_mon + 1);
                            int dow_of_end_of_month = (end_of_month_days - tm_.tm_mday + tm_.tm_wday) % 7;
                            return get_week_number(end_of_month_days,dow_of_end_of_month);
                        }
                    case current:
                        {
                            int val = get_week_number(tm_.tm_mday,tm_.tm_wday);
                            if(val < 0)
                                return 5;
                            return val;
                        }
                    }

                case day_of_week_in_month:       ///< Original number of the day of the week in month.
                    switch(v) {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                        return 5;
                    case least_maximum:
                        return 4;
                    case actual_maximum:
                        if(tm_.tm_mon == 1 && !is_leap(tm_.tm_year + 1900)) {
                            // only in february in non leap year is 28 days, the rest
                            // conver more then 4 weeks
                            return 4;
                        }
                        return 5;
                    case current:
                        return (tm_.tm_mday - 1) / 7 + 1;
                    default:
                        ;
                    }
                default:
                    ;
                }
                return 0;

            }

            ///
            /// Set current time point
            ///
            virtual void set_time(posix_time const &p)
            {
                from_time(static_cast<std::time_t>(p.seconds));
            }
            virtual posix_time get_time() const
            {
                posix_time pt = { time_, 0};
                return pt;
            }

            ///
            /// Set option for calendar, for future use
            ///
            virtual void set_option(calendar_option_type opt,int /*v*/)
            {
                switch(opt) {
                case is_gregorian:
                    throw date_time_error("is_gregorian is not settable options for calendar");
                case is_dst:
                    throw date_time_error("is_dst is not settable options for calendar");
                default:
                    ;
                }
            }
            ///
            /// Get option for calendar, currently only check if it is Gregorian calendar
            ///
            virtual int get_option(calendar_option_type opt) const
            {
                switch(opt) {
                case is_gregorian:
                    return 1;
                case is_dst:
                    return tm_.tm_isdst == 1;
                default:
                    return 0;
                };
            }

            ///
            /// Adjust period's \a p value by \a difference items using a update_type \a u.
            /// Note: not all values are adjustable
            ///
            virtual void adjust_value(period::marks::period_mark p,update_type u,int difference)
            {
                switch(u) {
                case move:
                    {
                        using namespace period::marks;
                        switch(p) {
                        case year:                       ///< Year, it is calendar specific
                        case extended_year:              ///< Extended year for Gregorian/Julian calendars, where 1 BC == 0, 2 BC == -1.
                            tm_updated_.tm_year +=difference;
                            break;
                        case month:
                            tm_updated_.tm_mon +=difference;
                            break;
                        case day:
                        case day_of_year:
                        case day_of_week:                ///< Day of week, starting from Sunday, [1..7]
                        case day_of_week_local: ///< Local day of week, for example in France Monday is 1, in US Sunday is 1, [1..7]
                            tm_updated_.tm_mday +=difference;
                            break;
                        case hour:                       ///< 24 clock hour [0..23]
                        case hour_12:                    ///< 12 clock hour [0..11]
                            tm_updated_.tm_hour += difference;
                            break;
                        case am_pm:                      ///< am or pm marker, [0..1]
                            tm_updated_.tm_hour += 12 * difference;
                            break;
                        case minute:                     ///< minute [0..59]
                            tm_updated_.tm_min += difference;
                            break;
                        case second:
                            tm_updated_.tm_sec += difference;
                            break;
                        case week_of_year:               ///< The week number in the year
                        case week_of_month:              ///< The week number withing current month
                        case day_of_week_in_month:       ///< Original number of the day of the week in month.
                            tm_updated_.tm_mday +=difference * 7;
                            break;
                        default:
                            ; // Not all values are adjustable
                        }
                        normalized_ = false;
                        normalize();
                    }
                    break;
                case roll:
                    { // roll
                        int cur_min = get_value(p,actual_minimum);
                        int cur_max = get_value(p,actual_maximum);
                        int max_diff = cur_max - cur_min + 1;
                        if(max_diff > 0) {
                            int value = get_value(p,current);
                            int addon = 0;
                            if(difference < 0)
                                addon = ((-difference/max_diff) + 1) * max_diff;
                            value = (value - cur_min + difference + addon) % max_diff + cur_min;
                            set_value(p,value);
                            normalize();
                        }
                    }
                default:
                    ;
                }
            }

            int get_diff(period::marks::period_mark p,int diff,gregorian_calendar const *other) const
            {
                if(diff == 0)
                    return 0;
                hold_ptr<gregorian_calendar> self(clone());
                self->adjust_value(p,move,diff);
                if(diff > 0){
                    if(self->time_ > other->time_)
                        return diff - 1;
                    else
                        return diff;
                }
                else {
                    if(self->time_ < other->time_)
                        return diff + 1;
                    else
                        return diff;
                }
             }

            ///
            /// Calculate the difference between this calendar  and \a other in \a p units
            ///
            virtual int difference(abstract_calendar const *other_cal,period::marks::period_mark p) const
            {
                hold_ptr<gregorian_calendar> keeper;
                gregorian_calendar const *other = dynamic_cast<gregorian_calendar const *>(other_cal);
                if(!other) {
                    keeper.reset(clone());
                    keeper->set_time(other_cal->get_time());
                    other = keeper.get();
                }

                int factor = 1; // for weeks vs days handling

                using namespace period::marks;
                switch(p) {
                case era:
                    return 0;
                case year:
                case extended_year:
                    {
                        int diff = other->tm_.tm_year - tm_.tm_year;
                        return get_diff(period::marks::year,diff,other);
                    }
                case month:
                    {
                        int diff = 12 * (other->tm_.tm_year - tm_.tm_year)
                                    + other->tm_.tm_mon - tm_.tm_mon;
                        return get_diff(period::marks::month,diff,other);
                    }
                case day_of_week_in_month:
                case week_of_month:
                case week_of_year:
                    factor = 7;
                    // fall
                case day:
                case day_of_year:
                case day_of_week:
                case day_of_week_local:
                    {
                        int diff = other->tm_.tm_yday - tm_.tm_yday;
                        if(other->tm_.tm_year != tm_.tm_year) {
                            diff += days_from_0(other->tm_.tm_year + 1900) -
                                    days_from_0(tm_.tm_year + 1900);
                        }
                        return get_diff(period::marks::day,diff,other) / factor;
                    }
                case am_pm:
                    return static_cast<int>( (other->time_ - time_) / (3600*12) );
                case hour:
                case hour_12:
                    return static_cast<int>( (other->time_ - time_) / 3600 );
                case minute:
                    return static_cast<int>( (other->time_ - time_) / 60 );
                case second:
                    return static_cast<int>( other->time_ - time_  );
                default:
                    return 0;
                };
            }

            ///
            /// Set time zone, empty - use system
            ///
            virtual void set_timezone(std::string const &tz)
            {
                if(tz.empty()) {
                    is_local_ = true;
                    tzoff_ = 0;
                }
                else {
                    is_local_ = false;
                    tzoff_ = parse_tz(tz);
                }
                from_time(time_);
                time_zone_name_ = tz;
            }
            virtual std::string get_timezone() const
            {
                return time_zone_name_;
            }

            virtual bool same(abstract_calendar const *other) const
            {
                gregorian_calendar const *gcal = dynamic_cast<gregorian_calendar const *>(other);
                if(!gcal)
                    return false;
                return
                    gcal->tzoff_ == tzoff_
                    && gcal->is_local_ == is_local_
                    && gcal->first_day_of_week_  == first_day_of_week_;
            }

            virtual ~gregorian_calendar()
            {
            }

    private:

        void from_time(std::time_t point)
        {
            std::time_t real_point = point + tzoff_;
            std::tm *t = 0;
            #ifdef BOOST_WINDOWS
            // Windows uses TLS, thread safe
            t = is_local_ ? localtime(&real_point) : gmtime(&real_point);
            #else
            std::tm tmp_tm;
            t = is_local_ ? localtime_r(&real_point,&tmp_tm) : gmtime_r(&real_point,&tmp_tm);
            #endif
            if(!t) {
                throw date_time_error("boost::locale::gregorian_calendar: invalid time point");
            }
            tm_ = *t;
            tm_updated_ = *t;
            normalized_ = true;
            time_ = point;
        }
        int first_day_of_week_;
        std::time_t time_;
        std::tm tm_;
        std::tm tm_updated_;
        bool normalized_;
        bool is_local_;
        int tzoff_;
        std::string time_zone_name_;

    };

    abstract_calendar *create_gregorian_calendar(std::string const &terr)
    {
        return new gregorian_calendar(terr);
    }

    class gregorian_facet : public calendar_facet {
    public:
        gregorian_facet(std::string const &terr,size_t refs = 0) :
            calendar_facet(refs),
            terr_(terr)
        {
        }
        virtual abstract_calendar *create_calendar() const
        {
            return create_gregorian_calendar(terr_);
        }
    private:
        std::string terr_;
    };

    std::locale install_gregorian_calendar(std::locale const &in,std::string const &terr)
    {
        return std::locale(in,new gregorian_facet(terr));
    }

} // util
} // locale
} //boost

// vim: tabstop=4 expandtab shiftwidth=4 softtabstop=4


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS

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

#include <boost/simd/function/accurate.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

   BOOST_DISPATCH_OVERLOAD ( acos_
                          , (typename A0)
                          , bs::sse2_
                          , bs::accurate_tag
                          , bs::pack_< bd::double_<A0>, bs::sse_>
                          )
  {

    BOOST_FORCEINLINE A0 operator() (const accurate_tag &,  A0 const& a0) const BOOST_NOEXCEPT
    {
      return A0(bs::accurate_(bs::acos)(a0[0]), bs::accurate_(bs::acos)(a0[1]));
    }
  };
} } }

#endif


#include <Eigen/Dense>
#include <iostream>

using namespace std;

MatrixXd arrowmatvec ( VectorXd d, VectorXd a, VectorXd x){
	A=d.diagonal(1:(d.size()-1)
}

Nummerical Methods for CSE/arrowmatvec.cpp


#include <boost/test/unit_test.hpp>

#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include "PeriodVal.h"
#include "PatternScannerEngine.h"
#include "SegmentConstraint.h"
#include "SegmentValsCloseToLinearEq.h"
#include "SegmentListConstraint.h"
#include "SlopeIncreasesConstraint.h"
#include "PatternMatchValidator.h"
#include "EndWithinPercentOfStart.h"
#include "PeriodValSegment.h"
#include "CupScanner.h"
#include "CupWithHandleScanner.h"
#include "ORPatternMatchValidator.h"
#include "PatternMatchValidator.h"
#include "TestHelper.h"
#include "VScanner.h"
#include "BreakoutAboveFirstHighValidatorFactory.h"
#include "MultiPatternScanner.h"

using namespace boost::posix_time;
using namespace boost::gregorian;
using namespace testHelper;

BOOST_AUTO_TEST_CASE( MultiSegmentPatternScannerEngine_SAVE_20130722 )
{

	// "typical price" data for SAVE, starting on 2013-07-22,
	// where typical price is (high + low + close)/3.0

	PeriodValSegmentPtr chartData = PeriodValSegment::readFromFile("./patternScan/SAVE_Cup_Weekly_20130722_20131028.csv");

	BOOST_TEST_MESSAGE("CupWithHandle_SAVE_20130722");

	SegmentConstraintPtr segConstraint(new SegmentValsCloseToLinearEq(3.0));
	SegmentListConstraintPtr segListConstraint(new SlopeIncreasesConstraint());
	PatternMatchValidatorPtr patternMatchValidator(new EndWithinPercentOfStart(8.0));

	PatternScannerEngine scanner(segConstraint,segListConstraint,patternMatchValidator);

	genPeriodValSegmentInfo("Cup data",*chartData);

	PatternMatchListPtr patternMatches = scanner.scanPatternMatches(chartData);

    verifyMatchList("MultiSegmentPatternScannerEngine_SAVE_20130722",patternMatches,2);

	PatternMatchPtr thePatternMatch = patternMatches->front();
	BOOST_CHECK(segListConstraint->validSegments(thePatternMatch->segments()) == true);

	verifyPatternMatch("Cup Match",
            ptime(date(2013,7,22)),ptime(date(2013,10,7)),4,thePatternMatch);

}

BOOST_AUTO_TEST_CASE( CupWithHandle_SAVE_20130722_CupWithHandleScanner )
{
	PeriodValSegmentPtr chartData = PeriodValSegment::readFromFile("./patternScan/SAVE_Cup_Weekly_20130722_20131028.csv");

    CupWithHandleScanner scanner;
	PatternMatchListPtr patternMatches = scanner.scanPatternMatches(chartData);

    // Except for the last candle plunging below 50% of the depth of the cup,  there would be at least one
    // match with a 3 period handle, right after the cup, but CupWithHandleScanner isn't picking this up.
    // This could be because trend line slope is too shallow.
    verifyMatchList("CupWithHandle_SAVE_20130722_CupScanner",patternMatches,0);
}

BOOST_AUTO_TEST_CASE( CupWithHandle_SAVE_20130722_CupWithHandle_V_Shaped_Handle )
{
    PeriodValSegmentPtr chartData = PeriodValSegment::readFromFile("./patternScan/SAVE_Cup_Weekly_20130722_20131028.csv");
    BOOST_TEST_MESSAGE("Testing V sub-patterns for RHS of cup with handle");
    PeriodValSegmentPair splitData = chartData->split(8);
    PeriodValSegmentPtr rhsSegData = splitData.second;

    genPeriodValSegmentInfo("V RHS segment data",*rhsSegData);

    // The VScanner below uses the same parameters as in CupWithHandleScanner.cpp. However it doesn't have
    // the validator to ensure the handle doesn't go below 50% of the depth of the cup. The last bar in the
    // pattern plunges below 50%, which causes the cup-with-handle scanning to fail.

    // With the ValuesCloseToTrendlineValidator() added to the downtrend and and uptrend of the V, this
    // test has started returning 0 matches.
    // TODO - For short pattern segments, is ValuesCloseToTrendlineValidator the best validator to use?
    VScanner scanner(3.0,2);
    scanner.overallValidatorFactory().addFactory(PatternMatchValidatorFactoryPtr(new BreakoutAboveFirstHighValidatorFactory()));

    PatternMatchListPtr patternMatches = scanner.scanPatternMatches(rhsSegData);

    verifyMatchList("CupWithHandle_SAVE_20130722_CupWithHandle_V_Shaped_Handle",patternMatches,0);
}

BOOST_AUTO_TEST_CASE( CupWithHandle_CELG_CupWithHandleScanner )
{
    PeriodValSegmentPtr chartData = PeriodValSegment::readFromFile("./patternScan/CELG_20140501_20140814_Daily.csv");
    PeriodValSegmentPair splitSeg = chartData->split(9);
    PeriodValSegmentPtr cupWithHandleData = splitSeg.second;
    genPeriodValSegmentInfo("cup data",*cupWithHandleData);

    CupWithHandleScanner scanner;
    PatternMatchListPtr patternMatches = scanner.scanPatternMatches(cupWithHandleData);

    // This pattern previously matched a cup with handle, but no longer does due to the
    // depth restrictions (i.e., pattern must be > 8% deep from the high of the LHS cup.
    verifyMatchList("CupWithHandle_CELG_CupWithHandleScanner",patternMatches,0);
}

BOOST_AUTO_TEST_CASE( CupWithHandle_CMG )
{
    PeriodValSegmentPtr chartData = PeriodValSegment::readFromFile("./patternScan/CMG_Daily.csv");

    PatternScannerPtr cupWithHandleScanner(new CupWithHandleScanner());
    MultiPatternScanner multiCupWithHandleScanner(cupWithHandleScanner);

    PatternMatchListPtr cupWithHandleMatches = multiCupWithHandleScanner.scanUniquePatternMatches(chartData);

    // Without the pattern matching constraints, several mal-formed cup with handles
    // are matched in the CMG data. None of these looked very well-formed, and were
    // used to add rules to cup and handle scanning to rule them out.
    verifyMatchList("CupWithHandle_CMG (cup with handle matches)",cupWithHandleMatches,0);
}


#include <tradelayer/rpcrequirements.h>

#include <tradelayer/dex.h>
#include <tradelayer/mdex.h>
#include <tradelayer/rules.h>
#include <tradelayer/sp.h>
#include <tradelayer/tradelayer.h>
#include <tradelayer/tradelayer_matrices.h>
#include <tradelayer/uint256_extensions.h>
#include <tradelayer/utilsbitcoin.h>

#include <amount.h>
#include <arith_uint256.h>
#include <rpc/protocol.h>
#include <sync.h>
#include <tinyformat.h>
#include <validation.h>

#include <stdint.h>
#include <string>

#include <boost/algorithm/string.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/rational.hpp>

using namespace mastercore;

using boost::algorithm::token_compress_on;
typedef boost::rational<boost::multiprecision::checked_int128_t> rational_t;

void RequireBalance(const std::string& address, uint32_t propertyId, int64_t amount)
{
    int64_t balance = getMPbalance(address, propertyId, BALANCE);
    if (balance < amount) {
        throw JSONRPCError(RPC_TYPE_ERROR, "Sender has insufficient property balance");
    }
    int64_t balanceUnconfirmed = getUserAvailableMPbalance(address, propertyId);
    if (balanceUnconfirmed < amount) {
        throw JSONRPCError(RPC_TYPE_ERROR, "Sender has insufficient balance (due to pending transactions)");
    }
}

void RequireCollateral(const std::string& address, std::string name_traded, int64_t amount, uint64_t leverage)
{
    //NOTE: add changes to inverse quoted
    int64_t uPrice = COIN;
    uint32_t propertyId = 0;
    CMPSPInfo::Entry sp;
    std::pair<int64_t, int64_t> factor;

    getEntryFromName(name_traded, propertyId, sp);

    // max = 2.5 basis point in oracles, max = 1.0 basis point in natives
    (sp.isOracle()) ? (factor.first = 100025, factor.second = 100000) : (sp.isNative()) ? (factor.first = 10001, factor.second = 10000) : (factor.first = 1, factor.second = 1);

    arith_uint256 amountTR = (ConvertTo256(factor.first) * ConvertTo256(COIN) * ConvertTo256(amount) * ConvertTo256(sp.margin_requirement)) / (ConvertTo256(leverage) * ConvertTo256(uPrice) * ConvertTo256(factor.second));
    int64_t amountToReserve = ConvertTo64(amountTR);

    int64_t nBalance = getMPbalance(address, sp.collateral_currency, BALANCE);

    if (nBalance < amountToReserve || nBalance == 0)
        throw JSONRPCError(RPC_TYPE_ERROR, "Sender has insufficient balance for collateral");

    int64_t balanceUnconfirmed = getUserAvailableMPbalance(address, sp.collateral_currency);
    if (balanceUnconfirmed == 0)
        throw JSONRPCError(RPC_TYPE_ERROR, "Sender has insufficient balance (due to pending transactions)");

}

void RequirePosition(const std::string& address, uint32_t contractId)
{
    const int64_t position = getMPbalance(address, contractId, CONTRACT_BALANCE);
    if (position == 0 ) {
        throw JSONRPCError(RPC_TYPE_ERROR, "Sender has not position in this contract");
    }
}

void RequirePrimaryToken(uint32_t propertyId)
{
    if (propertyId < 1 || 2 < propertyId) {
        throw JSONRPCError(RPC_INVALID_PARAMETER, "Property identifier must be 1 (TL) or 2 (TTL)");
    }
}

/* checking there's no active orders */
void RequireNoOrders(std::string sender, uint32_t propertyId)
{
    if(ContractDex_CHECK_ORDERS(sender, propertyId)) {
        throw JSONRPCError(RPC_INVALID_PARAMETER, "Cancel orders before close position\n");
    }

}

void RequirePropertyName(const std::string& name)
{
    if (name.empty()) {
        throw JSONRPCError(RPC_TYPE_ERROR, "Property name must not be empty");
    }
}

void RequireDifferentAddrs(const std::string& oracleAddress,const std::string& backupAddress)
{
  if (oracleAddress == backupAddress) {
      throw JSONRPCError(RPC_TYPE_ERROR, "oracle and backup addresses must be differents");
  }
}

void RequireExistingProperty(uint32_t propertyId)
{
  LOCK(cs_tally);
  if (!IsPropertyIdValid(propertyId)) {
    throw JSONRPCError(RPC_INVALID_PARAMETER, "Property identifier does not exist");
  }
}

void RequireDifferentIds(uint32_t propertyId, uint32_t otherId)
{
    if (propertyId == otherId) {
        throw JSONRPCError(RPC_INVALID_PARAMETER, "Property identifiers must not be the same");
    }
}

void RequireManagedProperty(uint32_t propertyId)
{
    LOCK(cs_tally);
    CMPSPInfo::Entry sp;
    if (!_my_sps->getSP(propertyId, sp)) {
        throw JSONRPCError(RPC_DATABASE_ERROR, "Failed to retrieve property");
    }
    if (sp.fixed || !sp.manual) {
        throw JSONRPCError(RPC_INVALID_PARAMETER, "Property identifier does not refer to a managed property");
    }
}

void RequireAssociation(uint32_t propertyId,uint32_t contractId)
{
  LOCK(cs_tally);
  CMPSPInfo::Entry sp;
  if (!_my_sps->getSP(propertyId, sp)) {
      throw JSONRPCError(RPC_DATABASE_ERROR, "Failed to retrieve property");
  }
  if (sp.contract_associated != contractId) {
      throw JSONRPCError(RPC_INVALID_PARAMETER, "Pegged does not comes from this contract");
  }
}

void RequireTokenIssuer(const std::string& address, uint32_t propertyId)
{
    LOCK(cs_tally);
    CMPSPInfo::Entry sp;
    if (!_my_sps->getSP(propertyId, sp)) {
        throw JSONRPCError(RPC_DATABASE_ERROR, "Failed to retrieve property");
    }
    if (address != sp.issuer) {
        throw JSONRPCError(RPC_TYPE_ERROR, "Sender is not authorized to manage the property");
    }
}

void RequireSaneReferenceAmount(int64_t amount)
{
    if ((0.01 * COIN) < amount) {
        throw JSONRPCError(RPC_TYPE_ERROR, "Reference amount higher is than 0.01 BTC");
    }
}

void RequireHeightInChain(int blockHeight)
{
    if (blockHeight < 0 || GetHeight() < blockHeight) {
        throw JSONRPCError(RPC_INVALID_PARAMETER, "Block height is out of range");
    }
}

void RequireNotVesting(uint32_t propertyId)
{
  LOCK(cs_tally);
  CMPSPInfo::Entry sp;
  if (!_my_sps->getSP(propertyId, sp)) {
    throw JSONRPCError(RPC_DATABASE_ERROR, "Failed to retrieve property");
  }

  const CConsensusParams &params = ConsensusParams();
  if (sp.attribute_type == ALL_PROPERTY_TYPE_VESTING && lastBlockg < params.MSC_VESTING_BLOCK + 210240) {
    throw JSONRPCError(RPC_INVALID_PARAMETER, "Vesting Tokens can not be traded at DEx before one year\n");
  }
}

void RequireNotContract(uint32_t propertyId)
{
    LOCK(cs_tally);
    CMPSPInfo::Entry sp;
    if (!_my_sps->getSP(propertyId, sp)) {
        throw JSONRPCError(RPC_DATABASE_ERROR, "Failed to retrieve property");
    }
    if (sp.isContract()) {
        throw JSONRPCError(RPC_INVALID_PARAMETER, "Property must not be future contract\n");
    }
}

void RequireContract(uint32_t propertyId)
{
    LOCK(cs_tally);
    CMPSPInfo::Entry sp;
    if (!_my_sps->getSP(propertyId, sp)) {
        throw JSONRPCError(RPC_DATABASE_ERROR, "Failed to retrieve property");
    }
    if (!sp.isContract()) {
        throw JSONRPCError(RPC_INVALID_PARAMETER, "contractId must be future contract\n");
    }
}

// NOTE: improve this function
void RequireContract(std::string name_contract)
{
    struct FutureContractObject *pfuture = getFutureContractObject(name_contract);
    uint32_t propertyId = (pfuture) ? pfuture->fco_propertyId : 0;

    LOCK(cs_tally);
    CMPSPInfo::Entry sp;
    if (!_my_sps->getSP(propertyId, sp)) {
        throw JSONRPCError(RPC_DATABASE_ERROR, "Failed to retrieve property");
    }
    if (!sp.isContract()) {
        throw JSONRPCError(RPC_INVALID_PARAMETER, "contractId must be future contract\n");
    }
}

void RequireOracleContract(uint32_t propertyId)
{
    LOCK(cs_tally);
    CMPSPInfo::Entry sp;
    if (!_my_sps->getSP(propertyId, sp)) {
        throw JSONRPCError(RPC_DATABASE_ERROR, "Failed to retrieve property");
    }

    if (!sp.isOracle()) {
        throw JSONRPCError(RPC_INVALID_PARAMETER, "contractId must be oracle future contract\n");
    }
}

void RequirePeggedCurrency(uint32_t propertyId)
{
    LOCK(cs_tally);
    CMPSPInfo::Entry sp;
    if (!_my_sps->getSP(propertyId, sp)) {
      throw JSONRPCError(RPC_DATABASE_ERROR, "Failed to retrieve property");
    }

    if (!sp.isPegged()) {
      throw JSONRPCError(RPC_INVALID_PARAMETER, "propertyId must be a pegged currency\n");
    }
}

void RequireForPegged(const std::string& address, uint32_t propertyId, uint32_t contractId, uint64_t amount)
{
    uint64_t balance = static_cast<uint64_t>(getMPbalance(address, propertyId, BALANCE));
    int64_t position = getMPbalance(address, contractId, CONTRACT_BALANCE);
    if (balance < amount) {
        throw JSONRPCError(RPC_TYPE_ERROR, "Sender has not enough amount of collateral currency");
    }

    if (position == 0) {
        throw JSONRPCError(RPC_TYPE_ERROR, "Sender has not the short position required on this contract");
    }
    // int64_t balanceUnconfirmed = getUserAvailableMPbalance(address, propertyId); // check the pending amounts
    // if (balanceUnconfirmed > 0) {
    //     throw JSONRPCError(RPC_TYPE_ERROR, "Sender has insufficient balance (due to pending transactions)");
    // }

}

void RequireMatchingDExOffer(const std::string& address, uint32_t propertyId)
{
    LOCK(cs_tally);
    if (!DEx_offerExists(address, propertyId)) {
        throw JSONRPCError(RPC_TYPE_ERROR, "No matching sell offer on the distributed exchange");
    }
}

void RequireNoOtherDExOffer(const std::string& address, uint32_t propertyId)
{
    LOCK(cs_tally);
    if (DEx_offerExists(address, propertyId)) {
        throw JSONRPCError(RPC_TYPE_ERROR, "Another active sell offer from the given address already exists on the distributed exchange");
    }
}

void RequireSaneDExFee(const std::string& address, uint32_t propertyId)
{
    LOCK(cs_tally);
    const CMPOffer* poffer = DEx_getOffer(address, propertyId);
    if (poffer == nullptr) {
        throw JSONRPCError(RPC_DATABASE_ERROR, "Unable to load sell offer from the distributed exchange");
    }

    if (poffer->getMinFee() > 1000000) {
        throw JSONRPCError(RPC_TYPE_ERROR, "Minimum accept fee is higher than 0.01 LTC (use override = true to continue)");
    }
}

void RequireSaneDExPaymentWindow(const std::string& address, uint32_t propertyId)
{
    LOCK(cs_tally);
    const CMPOffer* poffer = DEx_getOffer(address, propertyId);
    if (poffer == nullptr) {
        throw JSONRPCError(RPC_DATABASE_ERROR, "Unable to load sell offer from the distributed exchange");
    }
    if (poffer->getBlockTimeLimit() < 10) {
        throw JSONRPCError(RPC_TYPE_ERROR, "Payment window is less than 10 blocks (use override = true to continue)");
    }
}

void RequireShort(std::string& fromAddress, uint32_t contractId, uint64_t amount)
{
    LOCK(cs_tally);
    int64_t contractsNeeded = 0;
    // int index = static_cast<int>(contractId);

    CMPSPInfo::Entry sp;
    if (!_my_sps->getSP(contractId, sp)) {
        throw JSONRPCError(RPC_DATABASE_ERROR, "Failed to retrieve property");
    }

    if (sp.isContract()) {
        int64_t notionalSize = static_cast<int64_t>(sp.notional_size);
        int64_t position = getMPbalance(fromAddress, contractId, CONTRACT_BALANCE);
        // rational_t conv = notionalChange(contractId);
        rational_t conv = rational_t(1,1);
        int64_t num = conv.numerator().convert_to<int64_t>();
        int64_t denom = conv.denominator().convert_to<int64_t>();
        arith_uint256 Amount = ConvertTo256(num) * ConvertTo256(amount) / ConvertTo256(denom); // Alls needed
        arith_uint256 contracts = DivideAndRoundUp(Amount * ConvertTo256(notionalSize), ConvertTo256(1)) ;
        contractsNeeded = ConvertTo64(contracts);
        if (contractsNeeded > position) {
            throw JSONRPCError(RPC_INVALID_PARAMETER, "Not enough short position\n");
        }

    } else  {
        throw JSONRPCError(RPC_INVALID_PARAMETER, "contractId must be future contract\n");
    }
}

void RequireContractTxId(std::string& txid)
{
    std::string result;
    uint256 tx;
    tx.SetHex(txid);
    if (!p_txlistdb->getTX(tx, result)) {
          throw JSONRPCError(RPC_INVALID_PARAMETER, "TxId doesn't exist\n");
    }

    std::vector<std::string> vstr;
    boost::split(vstr, result, boost::is_any_of(":"), token_compress_on);
    unsigned int type = atoi(vstr[2]);
    if (type != 29) {
        throw JSONRPCError(RPC_INVALID_PARAMETER, "TxId isn't a future contract trade\n");
    }

}

void RequireSaneName(std::string& name)
{
    LOCK(cs_tally);
    uint32_t nextSPID = _my_sps->peekNextSPID();
    for (uint32_t propertyId = 1; propertyId < nextSPID; propertyId++) {
        CMPSPInfo::Entry sp;
        if (_my_sps->getSP(propertyId, sp)) {
            if (sp.name == name){
                throw JSONRPCError(RPC_INVALID_PARAMETER,"We have another property with the same name\n");
            }
        }
    }

}

void RequirePeggedSaneName(std::string& name)
{
    if (name.empty()) { throw JSONRPCError(RPC_INVALID_PARAMETER,"Pegged need a name!\n"); }
}

void RequireContractOrder(std::string& fromAddress, uint32_t contractId)
{

  if (!ContractDex_CHECK_ORDERS(fromAddress, contractId)) {
        throw JSONRPCError(RPC_INVALID_PARAMETER,"There's no order in this future contract\n");
  }

}

void RequireFeatureActivated(const uint16_t feature)
{
    if (!IsFeatureActivated(feature, GetHeight())) {
       throw JSONRPCError(RPC_TYPE_ERROR, "Feature is not Activated");
    }
}

void RequireAmountForFee(const std::string& address, uint32_t propertyId, int64_t amount)
{
    const arith_uint256 am = ConvertTo256(amount) + DivideAndRoundUp(ConvertTo256(amount) * ConvertTo256(5), ConvertTo256(BASISPOINT) * ConvertTo256(BASISPOINT));
    int64_t amountToReserve = ConvertTo64(am);

    int64_t nBalance = getMPbalance(address, propertyId, BALANCE);

    if (nBalance < amountToReserve || nBalance == 0)
        throw JSONRPCError(RPC_TYPE_ERROR, "Sender has insufficient balance");

    int64_t balanceUnconfirmed = getUserAvailableMPbalance(address, propertyId);
    if (balanceUnconfirmed == 0)
        throw JSONRPCError(RPC_TYPE_ERROR, "Sender has insufficient balance (due to pending transactions)");

}

// input block must be greater or equal to blockheight
void RequireBlockHeight(const int& block)
{
    const int height = GetHeight();
    if (block < height)
        throw JSONRPCError(RPC_TYPE_ERROR, "Block height needed not reached");

}


#pragma once

#include <iostream>
#include <cmath>
#include <vector>
#include <set>
#include <map>
#include <array>
#include <boost/dynamic_bitset.hpp>
#include "Chaperone/constexpr_math.hpp"
#include "Chaperone/NamedParameter.hpp"
#include "Chaperone/Vector3D.hpp"
#include "ReadFile.hpp"
#include <stack>

NP_MAKE_NAMED_PARAMETER(theta);//[0:pi]
NP_MAKE_NAMED_PARAMETER(phi);  //[0:2pi)
NP_MAKE_NAMED_PARAMETER(u);//amplitude
NP_MAKE_NAMED_PARAMETER(k);//index of element of P
NP_MAKE_NAMED_PARAMETER(d);//distance between two points

typedef std::tuple<double,double,double> Beta;

constexpr size_t N_GRID_POINTS    = 50;
#include "grid.hpp"

bool operator<(const k_<size_t>& a, const k_<size_t>& b)
{
   return a.value()<b.value();
}

std::vector<std::tuple<theta_<double>,phi_<double> > > grid_points = generate_random_theta_phi();

class Voronoi_cell
{
   public:
   k_<size_t> k;
   std::vector<Vector3D> const * P; //pointer of P (ps)
   private:
   std::vector< u_<double> >     u; //amplitude(theta,phi)
   std::set<k_<size_t> >         K; //indexies of neighboring subset P in X
   public:
   Voronoi_cell(const k_<size_t>& i, std::vector<Vector3D> const * ps);
   double get_volume()const;
   std::set<k_<size_t> > get_neighbor()const; //return indexes of neighboring voronoi cells (K)
   void change_pointer(std::vector<Vector3D> const * ps);
   private:
   void boundary_fitting();//to determine the cell, fit u to the boundary.
};

class Voronoi_diagram
{
   private:
   std::vector<Vector3D> const * P; //point to P in X
   std::vector<Voronoi_cell>     R; //voronoi cells
   public:
   Voronoi_diagram(std::vector<Vector3D>* const ps);
   void generate(); //for all voronoi cells
   std::vector<Voronoi_cell> get_vertual_cells(k_<size_t> k, const Vector3D& p)const;//for MC
   void change_pointer(std::vector<Vector3D> const * ps);
   std::map<k_<size_t>,std::list<k_<size_t> > > get_Delaunay_diagram()const;
   std::list<k_<size_t> > get_partial_Delaunay_diagram(const k_<size_t>& center)const;
};

std::set<k_<size_t> > Voronoi_cell::get_neighbor()const
{//return indexes of neighboring voronoi cells (K)
   return K;
}

void Voronoi_diagram::generate()
{
   R.reserve(P->size());
   for(size_t s=0, size=P->size(); s<size; ++s)
   {
      R.push_back(Voronoi_cell(k_<size_t>(s), P));
   }
   return ;
}

Voronoi_cell::Voronoi_cell(const k_<size_t>& i, std::vector<Vector3D> const * ps)
{
   k=i;
   change_pointer(ps);
   u.resize(N_GRID_POINTS);
   boundary_fitting();
   for(size_t i_=0,size=u.size();i_<size;++i_)
   {
      std::cout<<i_<<" u: "<<u.at(i_).value()<<std::endl;
   }
   exit(0);
}

void Voronoi_cell::change_pointer(std::vector<Vector3D> const * ps)
{
   P=ps;
}

double Voronoi_cell::get_volume()const
{
   constexpr size_t N = N_GRID_POINTS;
   constexpr double V0 = (4.0*M_PI/(3*N))*cexpr_math::sqrt(1.0-4.0/N);
   //here V0 is a volume of unit-cone (bottom area is Area_of_unit_sphere/N, length of cone is unit length(1)).
   double sum=0.0;
   for(size_t i=0,size=u.size();i<size;++i)
   {
      sum+=u.at(i).value();
   }
   return sum*V0;
}

void Voronoi_cell::boundary_fitting()
{
   #warning //書いてね
}

Voronoi_diagram::Voronoi_diagram(std::vector<Vector3D>* const ps)
{
   P=ps;
   generate();
}

void Voronoi_diagram::change_pointer(std::vector<Vector3D> const * ps)
{
   P=ps;
   std::for_each(R.begin(),R.end(),[this](auto& r){r.change_pointer(P);});
}

std::list<k_<size_t> > Voronoi_diagram::get_partial_Delaunay_diagram(const k_<size_t>& center)const
{
   std::list<k_<size_t> > res;
   const Voronoi_cell& r = R.at(center.value());
   const std::set<k_<size_t> >& set = r.get_neighbor();
   for(auto& a: set)
   {
      res.push_back(a);
   }
   return res;
}

std::map<k_<size_t>,std::list<k_<size_t> > > Voronoi_diagram::get_Delaunay_diagram()const
{
   std::map<k_<size_t>,std::list<k_<size_t> > > res;
   for(size_t r=0,size=R.size();r<size;++r)
   {
      const k_<size_t> k_(r);
      res.insert(std::make_pair(k_,get_partial_Delaunay_diagram(k_)));
   }
   return res;
}


/*
 * Copyright 2018-, Olivier Stasse 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/>.
 */

#include <Eigen/Dense>
#include <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/ddp-actuator-solver.hh>

#include <ddp-actuator-solver/examples/dctemp.hh>
#include <ddp-actuator-solver/examples/costtemp.hh>

#if DEBUG
#define ODEBUG(x) std::cout << x << std::endl
#else
#define ODEBUG(x)
#endif
#define ODEBUG3(x) std::cout << x << std::endl

#define DBGFILE "/tmp/debug-ddp_actuator_solver.dat"

#define RESETDEBUG5() { std::ofstream DebugFile;	\
    DebugFile.open(DBGFILE,std::ofstream::out);		\
    DebugFile.close();}
#define ODEBUG5FULL(x) { std::ofstream DebugFile;	\
    DebugFile.open(DBGFILE,std::ofstream::app);		\
    DebugFile << __FILE__ << ":"			\
              << __FUNCTION__ << "(#"			\
              << __LINE__ << "):" << x << std::endl;	\
    DebugFile.close();}
#define ODEBUG5(x) { std::ofstream DebugFile;	\
    DebugFile.open(DBGFILE,std::ofstream::app); \
    DebugFile << x << std::endl;		\
    DebugFile.close();}

#define RESETDEBUG4()
#define ODEBUG4FULL(x)
#define ODEBUG4(x)

#include <ddp-actuator-solver/examples/dctemp.hh>
#include <ddp-actuator-solver/examples/costtemp.hh>

#if DEBUG
#define ODEBUG(x) std::cout << x << std::endl
#else
#define ODEBUG(x)
#endif
#define ODEBUG3(x) std::cout << x << std::endl

#define DBGFILE "/tmp/debug-ddp_actuator_solver.dat"

#define RESETDEBUG5() { std::ofstream DebugFile;	\
    DebugFile.open(DBGFILE,std::ofstream::out);		\
    DebugFile.close();}
#define ODEBUG5FULL(x) { std::ofstream DebugFile;	\
    DebugFile.open(DBGFILE,std::ofstream::app);		\
    DebugFile << __FILE__ << ":"			\
              << __FUNCTION__ << "(#"			\
              << __LINE__ << "):" << x << std::endl;	\
    DebugFile.close();}
#define ODEBUG5(x) { std::ofstream DebugFile;	\
    DebugFile.open(DBGFILE,std::ofstream::app); \
    DebugFile << x << std::endl;		\
    DebugFile.close();}

#define RESETDEBUG4()
#define ODEBUG4FULL(x)
#define ODEBUG4(x)

namespace dynamicgraph
{
  namespace sot
  {
    namespace torque_control
    {

      namespace dynamicgraph = ::dynamicgraph;
      using namespace dynamicgraph;
      using namespace dynamicgraph::command;
      using namespace Eigen;

      /// Define EntityClassName here rather than in the header file
      /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
      typedef DdpActuatorSolver EntityClassName;

      /* --- DG FACTORY ------------------------------------------------------- */
      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DdpActuatorSolver,"DdpActuatorSolver");

      DdpActuatorSolver::
      DdpActuatorSolver(const std::string &name)
	: Entity(name),
	  CONSTRUCT_SIGNAL_IN (pos_des,           dynamicgraph::Vector),
	  CONSTRUCT_SIGNAL_IN (pos_motor_measure, dynamicgraph::Vector),
	  CONSTRUCT_SIGNAL_IN (pos_joint_measure, dynamicgraph::Vector),
	  CONSTRUCT_SIGNAL_IN (dx_measure,        dynamicgraph::Vector),
	  CONSTRUCT_SIGNAL_IN (tau_measure,       dynamicgraph::Vector),
          CONSTRUCT_SIGNAL_IN (tau_des,           dynamicgraph::Vector),
	  CONSTRUCT_SIGNAL_IN (temp_measure,      dynamicgraph::Vector),
	  CONSTRUCT_SIGNAL_OUT(tau,               dynamicgraph::Vector, m_pos_desSIN),
	  m_dt(1e-3),
	  m_solver(m_model,m_cost,
		   DISABLE_FULLDDP,
		   DISABLE_QPBOX),
	  m_T(3000),
	  m_stopCrit(1e-5),
	  m_iterMax(100)
      {
        RESETDEBUG5();
	Entity::signalRegistration( ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS );

      m_zeroState.setZero();

      /* Commands. */
	addCommand("init",
		   makeCommandVoid4(*this, &DdpActuatorSolver::param_init,
				    docCommandVoid4("Initialize the DDP solver.",
						    "Control timestep [s].",
						    "Size of the preview window (in nb of samples)",
						    "Max. nb. of iterations",
						    "Stopping criteria")));

      }

      /* --- COMMANDS ---------------------------------------------------------- */
      DEFINE_SIGNAL_OUT_FUNCTION(tau, dynamicgraph::Vector)
      {
	/// ---- Get the information -----
	/// Desired position
	const dynamicgraph::Vector &
          pos_des = m_pos_desSIN(iter);
	/// Measured position
	const dynamicgraph::Vector &
	  pos_joint_measure = m_pos_joint_measureSIN(iter);
	/// Measured speed
	const dynamicgraph::Vector &
	  dx_measure = m_dx_measureSIN(iter);
	/// Measured temperature
	const dynamicgraph::Vector &
	  temp_measure = m_temp_measureSIN(iter);
	/// Measured torque
	const dynamicgraph::Vector &
          tau_measure = m_tau_measureSIN(iter);
        /// Desired torque
        const dynamicgraph::Vector &
          tau_des = m_tau_desSIN(iter);

	DDPSolver<double,5,1>::stateVec_t xinit,xDes;

	/// --- Initialize solver ---
	xinit << pos_joint_measure(0),
	  dx_measure(0),
	  temp_measure(0),
	  tau_measure(0),
          //m_ambiant_temperature;
          25.0;

        xDes << pos_des, 0.0, 25.0, tau_des, 25.0;
        ODEBUG5(xinit);
        ODEBUG5("");
        ODEBUG5(xDes);

        DCTemp model;
        CostTemp cost;
        DDPSolver<double,5,1> m_solver(model,cost,0,0);

        m_solver.FirstInitSolver(xinit,xDes,m_T,m_dt,m_iterMax, m_stopCrit);
        ODEBUG5("FirstInitSolver");

	/// --- Solve the DDP ---
        m_solver.solveTrajectory();
        ODEBUG5("Trajectory solved");

        /// --- Get the command ---
        DDPSolver<double,5,1>::traj lastTraj;
        lastTraj = m_solver.getLastSolvedTrajectory();
        ODEBUG5("getLastSolvedTrajectory");

        DDPSolver<double,5,1>::commandVecTab_t uList;
        uList = lastTraj.uList;
        ODEBUG5("uList");

        //s = uList[0];
        s.resize(32);
        s << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, uList[0], 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        //s.setZero(32);
        ODEBUG5(s);
        return s;
      }

      void DdpActuatorSolver::
      param_init(const double &timestep,
		 const int &T,
		 const int &nbItMax,
		 const double &stopCriteria)
      {
	m_T = T;
	m_dt = timestep;
	m_iterMax = nbItMax;
	m_stopCrit = stopCriteria;
      m_solver.FirstInitSolver( m_zeroState, m_zeroState,
                                m_T ,m_dt,m_iterMax,m_stopCrit);
      }

      void DdpActuatorSolver::
      display(std::ostream &os) const
      {
	os << " T: " << m_T
	   << " timestep: " << m_dt
	   << " nbItMax: " << m_iterMax
	   << " stopCriteria: "<< m_stopCrit << std::endl;
      }
    } // namespace torque_control
  } // namespace sot
} // namespace dynamicgraph


/*******************************************************************************
*
*  Filename    : KeysErrPlot.cc
*  Description : Keys PDF constructed from different nuisance parameters
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "TstarAnalysis/Common/interface/PlotStyle.hpp"
#include "TstarAnalysis/CompareDataMC/interface/SampleErrHistMgr.hpp"
#include "TstarAnalysis/MassRecoCompare/interface/Common.hpp"
#include "TstarAnalysis/MassRecoCompare/interface/KeysErrMgr.hpp"

#include "ManagerUtils/PlotUtils/interface/Common.hpp"
#include "ManagerUtils/PlotUtils/interface/RooFitUtils.hpp"

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

using namespace std;

void
MakeKeysErrPlots( KeysErrMgr* mgr )
{

  for( const auto histerr : histerrlist ){
    const string& tag = histerr.tag;

    TCanvas* c   = mgr::NewCanvas();
    TPad* toppad = mgr::NewTopPad();
    TPad* botpad = mgr::NewBottomPad();

    RooPlot* frame    = KeysErrMgr::x().frame();
    const double xmin = KeysErrMgr::x().getMin();
    const double xmax = KeysErrMgr::x().getMax();

    RooAbsPdf* central = mgr->Pdf( "" );
    RooAbsPdf* uppdf   = mgr->Pdf( tag+"Up" );
    RooAbsPdf* downpdf = mgr->Pdf( tag+"Down" );

    /*******************************************************************************
    *   Drawing top pad
    *******************************************************************************/
    toppad->Draw();
    toppad->cd();

    const double centralnorm = mgr->DataSet( "" )->sumEntries();
    const double upnorm      = ( tag == "pdf" || tag == "scale" ) ? centralnorm : mgr->DataSet( tag+"Up" )->sumEntries();
    const double downnorm    = ( tag == "pdf" || tag == "scale" ) ? centralnorm : mgr->DataSet( tag+"Down" )->sumEntries();

    TGraph* cenplot  = mgr::PlotOn( frame, central, RooFit::Normalization( centralnorm, RooAbsReal::NumEvent ) );
    TGraph* upplot   = mgr::PlotOn( frame, uppdf,   RooFit::Normalization( upnorm, RooAbsReal::NumEvent ) );
    TGraph* downplot = mgr::PlotOn( frame, downpdf, RooFit::Normalization( downnorm, RooAbsReal::NumEvent ) );
    frame->Draw();

    const double pltmax = mgr::GetYmax( cenplot, upplot, downplot );

    c->cd();
    /*******************************************************************************
    *   Drawing bottom plot
    *******************************************************************************/
    botpad->Draw();
    botpad->cd();
    TGraph* uprel   = mgr::DividedGraphSimple( upplot, cenplot );
    TGraph* downrel = mgr::DividedGraphSimple( downplot, cenplot );

    uprel->Draw( "AL" );
    downrel->Draw( "L" );

    TLine cenline( xmin, 1, xmax, 1  );
    TLine upline( xmin, 2, xmax, 2 );
    TLine downline( xmin, 0, xmax, 0 );

    cenline.Draw();
    upline.Draw();
    downline.Draw();

    c->cd();

    /*******************************************************************************
    *   Plot objects styling
    *******************************************************************************/
    mgr::SetTopPlotAxis( frame );
    frame->GetYaxis()->SetTitle( "Arbitrary unit" );
    frame->SetTitle( "" );
    frame->SetMinimum( 0.3 );
    frame->SetMaximum( pltmax * 1.8 );

    cenplot->SetLineColor( kBlack );
    upplot->SetLineColor( KBLUE );
    downplot->SetLineColor( KRED );
    uprel->SetLineColor( KBLUE );
    downrel->SetLineColor( KRED );

    mgr::SetBottomPlotAxis( uprel );
    uprel->GetXaxis()->SetTitle( frame->GetXaxis()->GetTitle() );
    uprel->GetXaxis()->SetRangeUser( xmin, xmax );
    uprel->GetYaxis()->SetTitle( "#frac{up,down}{central}" );
    uprel->SetMaximum( 2.2 );
    uprel->SetMinimum( -0.2 );

    upline.SetLineStyle( 3 );
    upline.SetLineColor( kBlack );
    downline.SetLineStyle( 3 );
    downline.SetLineColor( kBlack );

    /*******************************************************************************
    *   Drawing additional objects
    *******************************************************************************/
    mgr::DrawCMSLabel( SIMULATION );

    TLegend* legend    = mgr::NewLegend( 0.5, 0.75 );
    const string label = ( histerr.label != "" ) ? histerr.label : "1#sigma";
    legend->AddEntry( cenplot,  "Central Value",                                   "l" );
    legend->AddEntry( upplot,   ( histerr.rootname + "(+" + label + ")" ).c_str(), "l" );
    legend->AddEntry( downplot, ( histerr.rootname + "(-" + label + ")" ).c_str(), "l" );
    legend->Draw();

    mgr::LatexMgr latex;
    latex.SetOrigin( PLOT_X_TEXT_MAX, 0.75-TEXT_MARGIN, TOP_RIGHT )
    .WriteLine( mgr->LatexName() )
    .WriteLine( boost::str( boost::format( "M_{t*}=%dGeV/c^{2}" )%reconamer.GetInput<int>( "mass" ) ) );

    mgr::SaveToPDF( c, reconamer.PlotFileName( "errcomp", tag ) );

    delete uprel;
    delete downrel;
    delete toppad;
    delete botpad;
    delete frame;
    delete c;
  }
}


//
// Created by Hamza El-Kebir on 6/13/21.
//

#ifndef LODESTAR_NANOPBWRAPPER_HPP
#define LODESTAR_NANOPBWRAPPER_HPP

#include <Eigen/Dense>
#include "Vector.pb.h"
#include "TypeEnum.pb.h"

template<typename TType, bool isStatic = true>
struct NanopbWrapper {
};

template<>
struct NanopbWrapper<npb_Vector2d, true> {
    using npbType = npb_Vector2d;
    using npbTypeEnum = npb_Vector2d;
    using cppType = Eigen::Matrix<double, 2, 1>;

    static cppType unwrap(const npbType &obj)
    {
        cppType ret;
        ret << obj.coeff[0], obj.coeff[1];
        // ret = Eigen::Map<Eigen::Matrix<double, 2, 1>>(obj.coeff, 2, 1);

        return ret;
    }

    static void unwrap(const npbType &obj, cppType &ret)
    {
        ret.setZero();
        ret << obj.coeff[0], obj.coeff[1];
        // ret = Eigen::Map<cppType>(obj.coeff, 2, 1);
    }

    static npbType wrap(const cppType &obj)
    {
        npbType ret;
        ret.coeff[0] = obj(0);
        ret.coeff[1] = obj(1);
        // Eigen::Map<cppType>(ret.coeff, 2, 1) = obj;

        ret.size = 2;
        ret.has_size = true;

        return ret;
    }

    static npbType wrap(const cppType &obj, npbType &ret)
    {
        ret.coeff[0] = obj(0);
        ret.coeff[1] = obj(1);
        // Eigen::Map<cppType>(ret.coeff, 2, 1) = obj;

        ret.size = 2;
        ret.has_size = true;

        return ret;
    }
};

template<>
struct NanopbWrapper<Eigen::Matrix<double, 2, 1>, true> : NanopbWrapper<npb_Vector2d, true> {};

template<>
struct NanopbWrapper<npb_Vector2d, false> {
    using npbType = npb_Vector2d;
    using npbTypeEnum = npb_Vector2d;
    using cppType = Eigen::VectorXd;

    static cppType unwrap(const npbType &obj)
    {
        int32_t size = (obj.size > 2 ? 2 : obj.size);
        cppType ret(size);
        for (int i = 0; i < size; i++)
            ret(i) = obj.coeff[i];
        // ret = Eigen::Map<Eigen::Matrix<double, 2, 1>>(obj.coeff, 2, 1);

        return ret;
    }

    static void unwrap(const npbType &obj, cppType &ret)
    {
        int32_t size = (obj.size > 2 ? 2 : obj.size);
        ret.conservativeResize(size, 1);
        for (int i = 0; i < size; i++)
            ret(i) = obj.coeff[i];

        // ret = Eigen::Map<cppType>(obj.coeff, size, 1);
    }

    static npbType wrap(const cppType &obj)
    {
        npbType ret;
        int32_t size = (obj.rows() > 2 ? 2 : obj.rows());
        for (int i = 0; i < size; i++)
            ret.coeff[i] = obj(i);
        // Eigen::Map<cppType>(ret.coeff, size, 1) = obj;

        ret.size = size;
        ret.has_size = true;

        return ret;
    }

    static npbType wrap(const cppType &obj, npbType &ret)
    {
        int32_t size = (obj.rows() > 2 ? 2 : obj.rows());
        for (int i = 0; i < size; i++)
            ret.coeff[i] = obj(i);
        // Eigen::Map<cppType>(ret.coeff, size, 1) = obj;

        ret.size = size;
        ret.has_size = true;

        return ret;
    }
};

// ============ npb_Vector3d ========================================

template<>
struct NanopbWrapper<npb_Vector3d, true> {
    using npbType = npb_Vector3d;
    using npbTypeEnum = npb_Vector3d;
    using cppType = Eigen::Matrix<double, 3, 1>;

    static cppType unwrap(const npbType &obj)
    {
        cppType ret;
        ret << obj.coeff[0], obj.coeff[1], obj.coeff[2];
        // ret = Eigen::Map<Eigen::Matrix<double, 2, 1>>(obj.coeff, 2, 1);

        return ret;
    }

    static void unwrap(const npbType &obj, cppType &ret)
    {
        ret.setZero();
        ret << obj.coeff[0], obj.coeff[1], obj.coeff[2];
        // ret = Eigen::Map<cppType>(obj.coeff, 2, 1);
    }

    static npbType wrap(const cppType &obj)
    {
        npbType ret;
        ret.coeff[0] = obj(0);
        ret.coeff[1] = obj(1);
        ret.coeff[2] = obj(2);
        // Eigen::Map<cppType>(ret.coeff, 2, 1) = obj;

        ret.size = 3;
        ret.has_size = true;

        return ret;
    }

    static npbType wrap(const cppType &obj, npbType &ret)
    {
        ret.coeff[0] = obj(0);
        ret.coeff[1] = obj(1);
        ret.coeff[2] = obj(2);
        // Eigen::Map<cppType>(ret.coeff, 2, 1) = obj;

        ret.size = 3;
        ret.has_size = true;

        return ret;
    }
};

template<>
struct NanopbWrapper<Eigen::Matrix<double, 3, 1>, true> : NanopbWrapper<npb_Vector3d, true> {};

template<>
struct NanopbWrapper<npb_Vector3d, false> {
    using npbType = npb_Vector3d;
    using npbTypeEnum = npb_Vector3d;
    using cppType = Eigen::VectorXd;

    static cppType unwrap(const npbType &obj)
    {
        int32_t size = (obj.size > 3 ? 3 : obj.size);
        cppType ret(size);
        for (int i = 0; i < size; i++)
            ret(i) = obj.coeff[i];
        // ret = Eigen::Map<Eigen::Matrix<double, 2, 1>>(obj.coeff, 2, 1);

        return ret;
    }

    static void unwrap(const npbType &obj, cppType &ret)
    {
        int32_t size = (obj.size > 3 ? 3 : obj.size);
        ret.conservativeResize(size, 1);
        for (int i = 0; i < size; i++)
            ret(i) = obj.coeff[i];

        // ret = Eigen::Map<cppType>(obj.coeff, size, 1);
    }

    static npbType wrap(const cppType &obj)
    {
        npbType ret;
        int32_t size = (obj.rows() > 3 ? 3 : obj.rows());
        for (int i = 0; i < size; i++)
            ret.coeff[i] = obj(i);
        // Eigen::Map<cppType>(ret.coeff, size, 1) = obj;

        ret.size = size;
        ret.has_size = true;

        return ret;
    }

    static npbType wrap(const cppType &obj, npbType &ret)
    {
        int32_t size = (obj.rows() > 3 ? 3 : obj.rows());
        for (int i = 0; i < size; i++)
            ret.coeff[i] = obj(i);
        // Eigen::Map<cppType>(ret.coeff, size, 1) = obj;

        ret.size = size;
        ret.has_size = true;

        return ret;
    }
};

#endif //LODESTAR_NANOPBWRAPPER_HPP


/* -------------------------------------------------------------------------
 *  A repertory of multi primitive-to-primitive (MP2) ICP algorithms in C++
 * Copyright (C) 2018-2019 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
/**
 * @file   ICP_GaussNewton.cpp
 * @brief  ICP registration for points and planes
 * @author Jose Luis Blanco Claraco
 * @date   May 11, 2019
 */

#include <mp2p_icp/ICP_GaussNewton.h>
#include <mp2p_icp/optimal_tf_gauss_newton.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/math/CVectorDynamic.h>
#include <mrpt/poses/Lie/SE.h>
#include <Eigen/Dense>

IMPLEMENTS_MRPT_OBJECT_NS_PREFIX(ICP_GaussNewton, mp2p_icp::ICP_Base, mp2p_icp);

using namespace mp2p_icp;

void ICP_GaussNewton::impl_ICP_iteration(
    ICP_State& s, const Parameters& p, ICP_iteration_result& out)
{
    using namespace std::string_literals;

    MRPT_START

    // the global list of pairings:
    Pairings_GaussNewton pairings;

    // Correspondences for each point layer:
    // ---------------------------------------
    // Find correspondences for each point cloud "layer":
    for (const auto& kv1 : s.pc1.point_layers)
    {
        const auto &m1 = kv1.second, &m2 = s.pc2.point_layers.at(kv1.first);
        ASSERT_(m1);
        ASSERT_(m2);

        // Don't try to match the layer of plane centroids
        if (kv1.first == pointcloud_t::PT_LAYER_PLANE_CENTROIDS) continue;

        // Ignore this layer?
        if (p.weight_pt2pt_layers.count(kv1.first) == 0) continue;

        auto& mp = s.mps.at(kv1.first);

        // Measure angle distances from the current estimate:
        mp.angularDistPivotPoint =
            mrpt::math::TPoint3D(s.current_solution.asTPose());

        // Find closest pairings
        mrpt::tfest::TMatchingPairList mpl;
        m1->determineMatching3D(
            m2.get(), out.new_solution, mpl, mp, s.mres[kv1.first]);

        // Shuffle decimated points for next iter:
        if (++mp.offset_other_map_points >= mp.decimation_other_map_points)
            mp.offset_other_map_points = 0;

        // merge lists:
        // layer weight:
        const double lyWeight = p.weight_pt2pt_layers.at(kv1.first);

        // A standard layer: point-to-point correspondences:
        pairings.paired_points.insert(
            pairings.paired_points.end(), mpl.begin(), mpl.end());

        // and their weights:
        pairings.point_weights.emplace_back(mpl.size(), lyWeight);
    }  // end for each layer

    // point-to-planes
    if (!p.pt2pl_layer.empty())
    {
        const auto &m1 = s.pc1.point_layers.at("plane_centroids"),
                   &m2 = s.pc2.point_layers.at(p.pt2pl_layer);
        ASSERT_(m1);
        ASSERT_(m2);

        auto& mp = s.mps.at(p.pt2pl_layer);
        // Measure angle distances from the current estimate:
        mp.angularDistPivotPoint =
            mrpt::math::TPoint3D(s.current_solution.asTPose());

        // Find closest pairings
        mrpt::tfest::TMatchingPairList mpl;
        m1->determineMatching3D(
            m2.get(), s.current_solution, mpl, mp, s.mres[p.pt2pl_layer]);
        // Plane-to-plane correspondence:

        // We have pairs of planes whose centroids are quite close.
        // Check their normals too:
        for (const auto& pair : mpl)
        {
            // 1) Check fo pairing sanity:
            ASSERTDEB_(pair.this_idx < pcs1.planes.size());
            ASSERTDEB_(pair.other_idx < m2->size());

            const auto&           pl_this = s.pc1.planes[pair.this_idx];
            mrpt::math::TPoint3Df pt_other;
            m2->getPoint(pair.other_idx, pt_other.x, pt_other.y, pt_other.z);

            // 2) append to list of plane pairs:
            // Accept pairing:
            pairings.paired_pt2pl.emplace_back(pl_this, pt_other);
        }
    }

    if (pairings.empty() || pairings.paired_points.size() < 3)
    {
        // Skip ill-defined problems if the no. of points is too small.
        // There's no check for this inside olae_match() because it also
        // handled lines, planes, etc. but we don't want to rely on that for
        // this application.

        // Nothing we can do !!
        out.success = false;
        return;
    }

    // Compute the optimal pose, using the OLAE method
    // (Optimal linear attitude estimator)
    // ------------------------------------------------
    OptimalTF_Result res;

    pairings.use_robust_kernel = p.use_kernel;
    MRPT_TODO("make param");
    // pairings.robust_kernel_param = mrpt::DEG2RAD(0.05);
    // pairings.robust_kernel_scale = 1500.0;

    optimal_tf_gauss_newton(pairings, res);
    out.success      = true;
    out.new_solution = mrpt::poses::CPose3D(res.optimal_pose);

    MRPT_END
}