// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2018.
// Modifications copyright (c) 2018 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_GEOMETRIES_CONCEPTS_MATRIX_CONCEPT_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_GEOMETRIES_CONCEPTS_MATRIX_CONCEPT_HPP

#include <boost/concept_check.hpp>
#include <boost/core/ignore_unused.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/extensions/algebra/core/access.hpp>
#include <boost/geometry/extensions/algebra/core/coordinate_dimension.hpp>
#include <boost/geometry/extensions/algebra/core/coordinate_system.hpp>
#include <boost/geometry/extensions/algebra/core/coordinate_type.hpp>
#include <boost/mpl/assert.hpp>

namespace boost { namespace geometry { namespace concepts {

template <typename Geometry>
class Matrix
{
#ifndef DOXYGEN_NO_CONCEPT_MEMBERS

    typedef typename coordinate_type<Geometry>::type ctype;
    typedef typename coordinate_system<Geometry>::type csystem;

    enum { ccount = dimension<Geometry>::value };

    template <typename G, std::size_t I, std::size_t J, std::size_t N>
    struct dimension_checker_row
    {
        static void apply()
        {
            G* g = 0;
            geometry::set<I, J>(*g, geometry::get<I, J>(*g));
            dimension_checker_row<G, I, J+1, N>::apply();
        }
    };

    template <typename G, std::size_t I, std::size_t N>
    struct dimension_checker_row<G, I, N, N>
    {
        static void apply() {}
    };

    template <typename G, std::size_t I, std::size_t N>
    struct dimension_checker
    {
        static void apply()
        {
            dimension_checker_row<G, I, 0, N>::apply();
            dimension_checker<G, I+1, N>::apply();
        }
    };

    template <typename G, std::size_t N>
    struct dimension_checker<G, N, N>
    {
        static void apply() {}
    };

public:

    /// BCCL macro to apply the concept
    BOOST_CONCEPT_USAGE(Matrix)
    {
        static const bool cs_check = ::boost::is_same<csystem, cs::cartesian>::value;
        BOOST_MPL_ASSERT_MSG(cs_check, NOT_IMPLEMENTED_FOR_THIS_CS, (csystem));

        dimension_checker<Geometry, 0, ccount>::apply();
    }
#endif
};

template <typename Geometry>
class ConstMatrix
{
#ifndef DOXYGEN_NO_CONCEPT_MEMBERS

    //typedef typename coordinate_type<Geometry>::type ctype;
    //typedef typename coordinate_system<Geometry>::type csystem;

    //enum { ccount = dimension<Geometry>::value };

    template <typename G, std::size_t I, std::size_t J, std::size_t N>
    struct dimension_checker_row
    {
        static void apply()
        {
            const G* g = 0;
            typename coordinate_type<G>::type coord(geometry::get<I, J>(*g));
            boost::ignore_unused(coord);
            dimension_checker_row<G, I, J+1, N>::apply();
        }
    };

    template <typename G, std::size_t I, std::size_t N>
    struct dimension_checker_row<G, I, N, N>
    {
        static void apply() {}
    };

    template <typename G, std::size_t I, std::size_t N>
    struct dimension_checker
    {
        static void apply()
        {
            dimension_checker_row<G, I, 0, N>::apply();
            dimension_checker<G, I+1, N>::apply();
        }
    };

    template <typename G, std::size_t N>
    struct dimension_checker<G, N, N>
    {
        static void apply() {}
    };

public:

    /// BCCL macro to apply the concept
    BOOST_CONCEPT_USAGE(ConstMatrix)
    {
        //static const bool cs_check = ::boost::is_same<csystem, cs::cartesian>::value;
        //BOOST_MPL_ASSERT_MSG(cs_check, NOT_IMPLEMENTED_FOR_THIS_CS, (csystem));

        //dimension_checker<Geometry, 0, ccount>::apply();
    }
#endif
};

}}} // namespace boost::geometry::concepts

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_GEOMETRIES_CONCEPTS_MATRIX_CONCEPT_HPP


/*****************************************************************************
*   ExploringSfMWithOpenCV
******************************************************************************
*   by Roy Shilkrot, 5th Dec 2012
*   http://www.morethantechnical.com/
******************************************************************************
*   Ch4 of the book "Mastering OpenCV with Practical Computer Vision Projects"
*   Copyright Packt Publishing 2012.
*   http://www.packtpub.com/cool-projects-with-opencv/book
*****************************************************************************/

#include "Visualization.h"

#include <pcl/common/common.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/io.h>
#include <pcl/io/file_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>

#include <boost/thread.hpp>

#include <opencv2/core/core.hpp>

#include <Eigen/Eigen>

using namespace std;
using namespace Eigen;

void PopulatePCLPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& mycloud,
						   const vector<cv::Point3d>& pointcloud,
						   const std::vector<cv::Vec3b>& pointcloud_RGB,
						   bool write_to_file = false
						   );

#define pclp3(eigenv3f) pcl::PointXYZ(eigenv3f.x(),eigenv3f.y(),eigenv3f.z())

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,cloud1,cloud_no_floor,orig_cloud;
std::string cloud_to_show_name = "";
bool show_cloud = false;
bool sor_applied = false;
bool show_cloud_A = true;

////////////////////////////////// Show Camera ////////////////////////////////////
std::deque<std::pair<std::string,pcl::PolygonMesh> >					cam_meshes;
std::deque<std::pair<std::string,std::vector<Matrix<float,6,1> > > >	linesToShow;
//TODO define mutex
bool							bShowCam;
int								iCamCounter = 0;
int								iLineCounter = 0;
int								ipolygon[18] = {0,1,2,  0,3,1,  0,4,3,  0,2,4,  3,1,4,   2,4,1};

inline pcl::PointXYZ Eigen2PointXYZ(Eigen::Vector3f v) { return pcl::PointXYZ(v[0],v[1],v[2]); }
inline pcl::PointXYZRGB Eigen2PointXYZRGB(Eigen::Vector3f v, Eigen::Vector3f rgb) { pcl::PointXYZRGB p(rgb[0],rgb[1],rgb[2]); p.x = v[0]; p.y = v[1]; p.z = v[2]; return p; }
inline pcl::PointNormal Eigen2PointNormal(Eigen::Vector3f v, Eigen::Vector3f n) { pcl::PointNormal p; p.x=v[0];p.y=v[1];p.z=v[2];p.normal_x=n[0];p.normal_y=n[1];p.normal_z=n[2]; return p;}
inline float* Eigen2float6(Eigen::Vector3f v, Eigen::Vector3f rgb) { static float buf[6]; buf[0]=v[0];buf[1]=v[1];buf[2]=v[2];buf[3]=rgb[0];buf[4]=rgb[1];buf[5]=rgb[2]; return buf; }
inline Matrix<float,6,1> Eigen2Eigen(Vector3f v, Vector3f rgb) { return (Matrix<float,6,1>() << v[0],v[1],v[2],rgb[0],rgb[1],rgb[2]).finished(); }
inline std::vector<Matrix<float,6,1> > AsVector(const Matrix<float,6,1>& p1, const Matrix<float,6,1>& p2) { 	std::vector<Matrix<float,6,1> > v(2); v[0] = p1; v[1] = p2; return v; }

void visualizerShowCamera(const Matrix3f& R, const Vector3f& _t, float r, float g, float b, double s = 0.01 /*downscale factor*/, const std::string& name = "") {
	std::string name_ = name,line_name = name + "line";
	if (name.length() <= 0) {
		stringstream ss; ss<<"camera"<<iCamCounter++;
		name_ = ss.str();
		ss << "line";
		line_name = ss.str();
	}

	Vector3f t = -R.transpose() * _t;

	Vector3f vright = R.row(0).normalized() * s;
	Vector3f vup = -R.row(1).normalized() * s;
	Vector3f vforward = R.row(2).normalized() * s;

	Vector3f rgb(r,g,b);

	pcl::PointCloud<pcl::PointXYZRGB> mesh_cld;
	mesh_cld.push_back(Eigen2PointXYZRGB(t,rgb));
	mesh_cld.push_back(Eigen2PointXYZRGB(t + vforward + vright/2.0 + vup/2.0,rgb));
	mesh_cld.push_back(Eigen2PointXYZRGB(t + vforward + vright/2.0 - vup/2.0,rgb));
	mesh_cld.push_back(Eigen2PointXYZRGB(t + vforward - vright/2.0 + vup/2.0,rgb));
	mesh_cld.push_back(Eigen2PointXYZRGB(t + vforward - vright/2.0 - vup/2.0,rgb));

	//TODO Mutex acquire
	pcl::PolygonMesh pm;
	pm.polygons.resize(6);
	for(int i=0;i<6;i++)
		for(int _v=0;_v<3;_v++)
			pm.polygons[i].vertices.push_back(ipolygon[i*3 + _v]);
	pcl::toROSMsg(mesh_cld,pm.cloud);
	bShowCam = true;
	cam_meshes.push_back(std::make_pair(name_,pm));
	//TODO mutex release

	linesToShow.push_back(std::make_pair(line_name,
		AsVector(Eigen2Eigen(t,rgb),Eigen2Eigen(t + vforward*3.0,rgb))
		));
}
void visualizerShowCamera(const float R[9], const float t[3], float r, float g, float b) {
	visualizerShowCamera(Matrix3f(R).transpose(),Vector3f(t),r,g,b);
}
void visualizerShowCamera(const float R[9], const float t[3], float r, float g, float b, double s) {
	visualizerShowCamera(Matrix3f(R).transpose(),Vector3f(t),r,g,b,s);
}
void visualizerShowCamera(const cv::Matx33f& R, const cv::Vec3f& t, float r, float g, float b, double s, const std::string& name) {
	visualizerShowCamera(Matrix<float,3,3,RowMajor>(R.val),Vector3f(t.val),r,g,b,s,name);
}
/////////////////////////////////////////////////////////////////////////////////

void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
{
//	viewer.setBackgroundColor(255,255,255); //white background
//	viewer.removeCoordinateSystem();	//remove the axes
}

void SORFilter() {

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);

	std::cerr << "Cloud before SOR filtering: " << cloud->width * cloud->height << " data points" << std::endl;

	// Create the filtering object
	pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
	sor.setInputCloud (cloud);
	sor.setMeanK (50);
	sor.setStddevMulThresh (1.0);
	sor.filter (*cloud_filtered);

	std::cerr << "Cloud after SOR filtering: " << cloud_filtered->width * cloud_filtered->height << " data points " << std::endl;

	copyPointCloud(*cloud_filtered,*cloud);
	copyPointCloud(*cloud,*orig_cloud);

//	std::cerr << "PointCloud before VoxelGrid filtering: " << cloud->width * cloud->height << " data points (" << pcl::getFieldsList (*cloud) << ")."<<std::endl;
//
//	cloud_filtered.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
//
//	// Create the filtering object
//	pcl::VoxelGrid<pcl::PointXYZRGB> vgrid;
//	vgrid.setInputCloud (cloud);
//	vgrid.setLeafSize (0.1f, 0.1f, 0.1f);
//	vgrid.filter (*cloud_filtered);
//
//	std::cerr << "PointCloud after VoxelGrid filtering: " << cloud_filtered->width * cloud_filtered->height << " data points (" << pcl::getFieldsList (*cloud_filtered) << ")."<<std::endl;
//
//	copyPointCloud(*cloud_filtered,*cloud);
}

void keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event_,
                            void* viewer_void)
{
	pcl::visualization::CloudViewer* viewer = static_cast<pcl::visualization::CloudViewer *> (viewer_void);
//	cout << "event_.getKeySym () = " << event_.getKeySym () << " event_.keyDown () " << event_.keyDown () << endl;
	if ((event_.getKeySym () == "s" || event_.getKeySym () == "S") && event_.keyDown ())
	{
		cout << "s clicked" << endl;

		cloud->clear();
		copyPointCloud(*orig_cloud,*cloud);
		if (!sor_applied) {
			SORFilter();
			sor_applied = true;
		} else {
			sor_applied = false;
		}

		show_cloud = true;
	}
	if ((event_.getKeySym ().compare("1") == 0)
#ifndef WIN32
		&& event_.keyDown ()
#endif
		)
	{
		show_cloud_A = true;
		show_cloud = true;
	}
	if ((event_.getKeySym ().compare("2") == 0)
#ifndef WIN32
		&& event_.keyDown ()
#endif
		)
	{
		show_cloud_A = false;
		show_cloud = true;
	}
}

void RunVisualization(const vector<cv::Point3d>& pointcloud,
					  const vector<cv::Vec3b>& pointcloud_RGB,
					  const vector<cv::Point3d>& pointcloud1,
					  const vector<cv::Vec3b>& pointcloud1_RGB)
{
	ShowClouds(pointcloud,pointcloud_RGB,pointcloud1,pointcloud1_RGB);
	RunVisualizationOnly();
}

void ShowClouds(const vector<cv::Point3d>& pointcloud,
				const vector<cv::Vec3b>& pointcloud_RGB,
				const vector<cv::Point3d>& pointcloud1,
				const vector<cv::Vec3b>& pointcloud1_RGB)
{
	cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	cloud1.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	orig_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);

	PopulatePCLPointCloud(cloud,pointcloud,pointcloud_RGB);
	PopulatePCLPointCloud(cloud1,pointcloud1,pointcloud1_RGB);
	copyPointCloud(*cloud,*orig_cloud);
	cloud_to_show_name = "";
	show_cloud = true;
	show_cloud_A = true;
}

void ShowCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr _cloud, const std::string& name) {
	cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::copyPointCloud(*_cloud,*cloud);
	cloud_to_show_name = name;
	show_cloud = true;
	show_cloud_A = true;
}

void ShowCloud(const vector<cv::Point3d>& pointcloud,
				const vector<cv::Vec3b>& pointcloud_RGB,
				const std::string& name) {
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr newcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	PopulatePCLPointCloud(newcloud,pointcloud,pointcloud_RGB);
	ShowCloud(newcloud,name);
}

void RunVisualizationOnly() {
	pcl::visualization::PCLVisualizer viewer("SfMToyLibrary Viewe");

	viewer.registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer);

    while (!viewer.wasStopped ())
    {
		if (show_cloud) {
			cout << "Show cloud: ";
			if(cloud_to_show_name != "") {
				cout << "show named cloud " << cloud_to_show_name << endl;
				viewer.removePointCloud(cloud_to_show_name);
				viewer.addPointCloud(cloud,cloud_to_show_name);
			} else {
				if(show_cloud_A) {
					cout << "show cloud A" << endl;
					viewer.removePointCloud("orig");
					viewer.addPointCloud(cloud,"orig");
				} else {
					cout << "show cloud B" << endl;
					viewer.removePointCloud("orig");
					viewer.addPointCloud(cloud1,"orig");
				}
			}
			show_cloud = false;
		}
		if(cam_meshes.size() > 0) {
			int num_cams = cam_meshes.size();
			cout << "showing " << num_cams << " cameras" << endl;
			while(cam_meshes.size()>0) {
				viewer.removeShape(cam_meshes.front().first);
				viewer.addPolygonMesh(cam_meshes.front().second,cam_meshes.front().first);
				cam_meshes.pop_front();
			}
		}
		if(linesToShow.size() > 0) {
			cout << "showing " << linesToShow.size() << " lines" << endl;
			while(linesToShow.size()>0) {
				vector<Matrix<float,6,1> > oneline = linesToShow.front().second;
				pcl::PointXYZRGB	A(oneline[0][3],oneline[0][4],oneline[0][5]),
									B(oneline[1][3],oneline[1][4],oneline[1][5]);
				for(int j=0;j<3;j++) {A.data[j] = oneline[0][j]; B.data[j] = oneline[1][j];}
				viewer.removeShape(linesToShow.front().first);
				viewer.addLine<pcl::PointXYZRGB,pcl::PointXYZRGB>(A,B,linesToShow.front().first);
				linesToShow.pop_front();
			}
			linesToShow.clear();
		}
		viewer.spinOnce();
    }
}

boost::thread* _t = NULL;
void RunVisualizationThread() {
	_t = new boost::thread(RunVisualizationOnly);
}
void WaitForVisualizationThread() {
	_t->join();
}

void PopulatePCLPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& mycloud,
						   const vector<cv::Point3d>& pointcloud,
						   const std::vector<cv::Vec3b>& pointcloud_RGB,
						   bool write_to_file
						   )
	//Populate point cloud
{
	cout << "Creating point cloud...";
	double t = cv::getTickCount();

	for (unsigned int i=0; i<pointcloud.size(); i++) {
		// get the RGB color value for the point
		cv::Vec3b rgbv(255,255,255);
		if (pointcloud_RGB.size() > i) {
			rgbv = pointcloud_RGB[i];
		}

		// check for erroneous coordinates (NaN, Inf, etc.)
		if (pointcloud[i].x != pointcloud[i].x ||
			pointcloud[i].y != pointcloud[i].y ||
			pointcloud[i].z != pointcloud[i].z ||
#ifndef WIN32
			isnan(pointcloud[i].x) ||
			isnan(pointcloud[i].y) ||
			isnan(pointcloud[i].z) ||
#else
			_isnan(pointcloud[i].x) ||
			_isnan(pointcloud[i].y) ||
			_isnan(pointcloud[i].z) ||
#endif
			//fabsf(pointcloud[i].x) > 10.0 ||
			//fabsf(pointcloud[i].y) > 10.0 ||
			//fabsf(pointcloud[i].z) > 10.0
			false
			)
		{
			continue;
		}

		pcl::PointXYZRGB pclp;

		// 3D coordinates
		pclp.x = pointcloud[i].x;
		pclp.y = pointcloud[i].y;
		pclp.z = pointcloud[i].z;

		// RGB color, needs to be represented as an integer
		uint32_t rgb = ((uint32_t)rgbv[2] << 16 | (uint32_t)rgbv[1] << 8 | (uint32_t)rgbv[0]);
		pclp.rgb = *reinterpret_cast<float*>(&rgb);

		mycloud->push_back(pclp);
	}

	mycloud->width = (uint32_t) mycloud->points.size(); // number of points
	mycloud->height = 1;								// a list, one row of data

	t = ((double)cv::getTickCount() - t)/cv::getTickFrequency();
	cout << "Done. (" << t <<"s)"<< endl;

	// write to file
	if (write_to_file) {
		//pcl::PLYWriter pw;
		//pw.write("pointcloud.ply",*mycloud);
		pcl::PCDWriter pw;
		pw.write("pointcloud.pcd",*mycloud);
	}
}


// Boost.Geometry
// Unit Test

// Copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/srs/epsg.hpp>
#include <boost/geometry/srs/esri.hpp>
#include <boost/geometry/srs/iau2000.hpp>
#include <boost/geometry/srs/projection.hpp>

#include "check_geometry.hpp"

int test_main(int, char*[])
{
    using namespace boost::geometry;
    using namespace boost::geometry::model;
    using namespace boost::geometry::srs;

    typedef point<double, 2, cs::geographic<degree> > point_ll;
    typedef point<double, 2, cs::cartesian> point_xy;

    {
        using namespace boost::geometry::srs::spar;

        point_ll pt_ll(1, 1);
        point_ll pt_ll2(0, 0);
        point_xy pt_xy(0, 0);

        projection<parameters<proj_tmerc, ellps_wgs84, units_m> > prj;

        prj.forward(pt_ll, pt_xy);
        test::check_geometry(pt_xy, "POINT(111308.33561309829 110591.34223734379)", 0.001);

        prj.inverse(pt_xy, pt_ll2);
        test::check_geometry(pt_ll2, "POINT(1 1)", 0.001);
    }

    {
        using namespace boost::geometry::srs::spar;

        projection<parameters<proj_tmerc> > prj1;
        projection<parameters<proj_tmerc, ellps_wgs84> > prj2;
        projection<parameters<proj_tmerc, ellps_wgs84, datum_wgs84> > prj3;
        projection<parameters<proj_tmerc, ellps_wgs84, x_0<>, y_0<> > > prj4
            = parameters<proj_tmerc, ellps_wgs84, x_0<>, y_0<> >(
                proj_tmerc(), ellps_wgs84(), x_0<>(0), y_0<>(0));

        typedef spheroid<double> sph;
        typedef ellps<sph> ell;
        typedef proj_tmerc prj;
        projection<parameters<ell, prj> > prj5
            = parameters<ell, prj>(ell(sph(1000, 999)));
    }

    {
        point_ll pt_ll(1, 1);
        point_ll pt_ll2(0, 0);
        point_xy pt_xy(0, 0);

        projection<static_epsg<2000> > prj;

        prj.forward(pt_ll, pt_xy);
        test::check_geometry(pt_xy, "POINT(9413505.3284665551 237337.74515944949)", 0.001);

        prj.inverse(pt_xy, pt_ll2);
        // TODO: investigate this wierd result
        test::check_geometry(pt_ll2, "POINT(-2.4463131191981073 1.5066638962045082)", 0.001);

        projection<static_esri<37001> > prj2;
        projection<static_iau2000<19900> > prj3;
    }

    // compile-time errors
    {
        point_ll pt_ll(1, 1);
        point_xy pt_xy(0, 0);

        //projection<spar::parameters<int> > prj1;
        //projection<int> prj2;

        projection<spar::parameters<spar::proj_bacon> > prj3;
        //prj3.inverse(pt_xy, pt_ll);
    }

    return 0;
}


//          Copyright W.P. McNeill 2010.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <boost/concept/assert.hpp>
#include <boost/graph/adjacency_iterator.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/properties.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <utility>

/*
This file defines a simple example of a read-only implicit weighted graph
built using the Boost Graph Library. It can be used as a starting point for
developers creating their own implicit graphs.

The graph models the following concepts:
  Graph
  IncidenceGraph
  BidirectionalGraph
  AdjacencyGraph
  VertexListGraph
  EdgeListGraph
  AdjacencyMatrix
  ReadablePropertyGraph

The graph defined here is a ring graph, a graph whose vertices are arranged in
a ring so that each vertex has exactly two neighbors. For example, here is a
ring graph with five nodes.

                                    0
                                  /   \
                                4      1
                                |      |
                                3 ---- 2

The edges of this graph are undirected and each has a weight that is a
function of its position in the graph.

The vertices indexed are by integer and arranged sequentially so that each
vertex i is adjacent to i-1 for i>0 and i+1 for i<n-1.  Vertex 0 is also
adjacent to vertex n-1.  Edges are indexed by pairs of vertex indices.

Various aspects of the graph are modeled by the following classes:

  ring_graph
    The graph class instantiated by a client. This defines types for the
    concepts that this graph models and keeps track of the number of vertices
    in the graph.

  ring_incident_edge_iterator
    This is an iterator that ranges over edges incident on a given vertex. The
    behavior of this iterator defines the ring topology. Other iterators that
    make reference to the graph structure are defined in terms of this one.

  edge_weight_map
    This defines a property map between edges and weights. Here edges have a
    weight equal to the average of their endpoint vertex indices, i.e. edge
    (2,3) has weight 2.5, edge (0,4) has weight 2, etc.

  boost::property_map<graph, boost::edge_weight_t>
    This tells Boost to associate the edges of the ring graph with the edge
    weight map.

Along with these classes, the graph concepts are modeled by various valid
expression functions defined below.  This example also defines a
get(boost::vertex_index_t, const ring_graph&) function which isn't part of a
graph concept, but is used for Dijkstra search.

Apart from graph, client code should not instantiate the model classes
directly. Instead it should access them and their properties via
graph_traits<...> and property_traits<...> lookups. For convenience,
this example defines short names for all these properties that client code can
use.
*/

// Forward declarations
class ring_graph;
class ring_incident_edge_iterator;
class ring_adjacency_iterator;
class ring_edge_iterator;
struct edge_weight_map;

// ReadablePropertyGraph associated types
namespace boost {
  template<>
  struct property_map< ring_graph, edge_weight_t > {
    typedef edge_weight_map type;
    typedef edge_weight_map const_type;
  };
}

// Tag values that specify the traversal type in graph::traversal_category.
struct ring_traversal_catetory:
  virtual public boost::bidirectional_graph_tag,
  virtual public boost::adjacency_graph_tag,
  virtual public boost::vertex_list_graph_tag,
  virtual public boost::edge_list_graph_tag
  {};

/*
Undirected graph of vertices arranged in a ring shape.

Vertices are indexed by integer, and edges connect vertices with consecutive
indices.  Vertex 0 is also adjacent to the vertex n-1.
*/
class ring_graph {
public:
  // Graph associated types
  typedef std::size_t vertex_descriptor;
  typedef boost::undirected_tag directed_category;
  typedef boost::disallow_parallel_edge_tag edge_parallel_category;
  typedef ring_traversal_catetory traversal_category;

  // IncidenceGraph associated types
  typedef std::pair<vertex_descriptor, vertex_descriptor> edge_descriptor;
  typedef ring_incident_edge_iterator out_edge_iterator;
  typedef std::size_t degree_size_type;

  // BidirectionalGraph associated types
  // Note that undirected graphs make no distinction between in- and out-
  // edges.
  typedef ring_incident_edge_iterator in_edge_iterator;

  // AdjacencyGraph associated types
  typedef ring_adjacency_iterator adjacency_iterator;

  // VertexListGraph associated types
  typedef boost::counting_iterator<vertex_descriptor> vertex_iterator;
  typedef std::size_t vertices_size_type;

  // EdgeListGraph associated types
  typedef ring_edge_iterator edge_iterator;
  typedef std::size_t edges_size_type;

  // This type is not part of a graph concept, but is used to return the
  // default vertex index map used by the Dijkstra search algorithm.
  typedef vertex_descriptor vertex_property_type;

  ring_graph(std::size_t n):m_n(n) {};
  std::size_t n() const {return m_n;}
private:
  // The number of vertices in the graph.
  std::size_t m_n;
};

// Use these graph_traits parameterizations to refer to the associated
// graph types.
typedef boost::graph_traits<ring_graph>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<ring_graph>::edge_descriptor edge_descriptor;
typedef boost::graph_traits<ring_graph>::out_edge_iterator out_edge_iterator;
typedef boost::graph_traits<ring_graph>::in_edge_iterator in_edge_iterator;
typedef boost::graph_traits<ring_graph>::adjacency_iterator adjacency_iterator;
typedef boost::graph_traits<ring_graph>::degree_size_type degree_size_type;
typedef boost::graph_traits<ring_graph>::vertex_iterator vertex_iterator;
typedef boost::graph_traits<ring_graph>::vertices_size_type vertices_size_type;
typedef boost::graph_traits<ring_graph>::edge_iterator edge_iterator;
typedef boost::graph_traits<ring_graph>::edges_size_type edges_size_type;

// Tag values passed to an iterator constructor to specify whether it should
// be created at the start or the end of its range.
struct iterator_position {};
struct iterator_start:virtual public iterator_position {};
struct iterator_end:virtual public iterator_position {};

/*
Iterator over edges incident on a vertex in a ring graph.

For vertex i, this returns edge (i, i+1) and then edge (i, i-1), wrapping
around the end of the ring as needed.

It is implemented with the boost::iterator_adaptor class, adapting an
offset into the dereference::ring_offset array.
*/
class ring_incident_edge_iterator:public boost::iterator_adaptor <
    ring_incident_edge_iterator,
    boost::counting_iterator<std::size_t>,
    edge_descriptor,
    boost::use_default,
    edge_descriptor > {
public:
  ring_incident_edge_iterator():
    ring_incident_edge_iterator::iterator_adaptor_(0),m_n(0),m_u(0) {};
  explicit ring_incident_edge_iterator(const ring_graph& g,
                                       vertex_descriptor u,
                                       iterator_start):
    ring_incident_edge_iterator::iterator_adaptor_(0),
    m_n(g.n()),m_u(u) {};
  explicit ring_incident_edge_iterator(const ring_graph& g,
                                       vertex_descriptor u,
                                       iterator_end):
    // A graph with one vertex only has a single self-loop.  A graph with
    // two vertices has a single edge between them.  All other graphs have
    // two edges per vertex.
    ring_incident_edge_iterator::iterator_adaptor_(g.n() > 2 ? 2:1),
    m_n(g.n()),m_u(u) {};

private:
  friend class boost::iterator_core_access;

  edge_descriptor dereference() const {
    static const int ring_offset[] = {1, -1};
    vertex_descriptor v;

    std::size_t p = *this->base_reference();
    if (m_u == 0 && p == 1)
      v = m_n-1; // Vertex n-1 precedes vertex 0.
    else
      v = (m_u+ring_offset[p]) % m_n;
    return edge_descriptor(m_u, v);
  }

  std::size_t m_n; // Size of the graph
  vertex_descriptor m_u; // Vertex whose out edges are iterated
};

// IncidenceGraph valid expressions
vertex_descriptor source(edge_descriptor e, const ring_graph&) {
  // The first vertex in the edge pair is the source.
  return e.first;
}

vertex_descriptor target(edge_descriptor e, const ring_graph&) {
 // The second vertex in the edge pair is the target.
 return e.second;
}

std::pair<out_edge_iterator, out_edge_iterator>
out_edges(vertex_descriptor u, const ring_graph& g) {
  return std::pair<out_edge_iterator, out_edge_iterator>(
    out_edge_iterator(g, u, iterator_start()),
    out_edge_iterator(g, u, iterator_end()) );
}

degree_size_type out_degree(vertex_descriptor, const ring_graph&) {
  // All vertices in a ring graph have two neighbors.
  return 2;
}

// BidirectionalGraph valid expressions
std::pair<in_edge_iterator, in_edge_iterator>
in_edges(vertex_descriptor u, const ring_graph& g) {
  // The in-edges and out-edges are the same in an undirected graph.
  return out_edges(u, g);
}

degree_size_type in_degree(vertex_descriptor u, const ring_graph& g) {
  // The in-degree and out-degree are both equal to the number of incident
  // edges in an undirected graph.
  return out_degree(u, g);
}

degree_size_type degree(vertex_descriptor u, const ring_graph& g) {
  // The in-degree and out-degree are both equal to the number of incident
  // edges in an undirected graph.
  return out_degree(u, g);
}

/*
Iterator over vertices adjacent to a given vertex.

This iterates over the target vertices of all the incident edges.
*/
class ring_adjacency_iterator:public boost::adjacency_iterator_generator<
  ring_graph,
  vertex_descriptor,
  out_edge_iterator>::type {
  // The parent class is an iterator_adpator that turns an iterator over
  // out edges into an iterator over adjacent vertices.
  typedef boost::adjacency_iterator_generator<
    ring_graph,
    vertex_descriptor,
    out_edge_iterator>::type parent_class;
public:
  ring_adjacency_iterator() {};
  ring_adjacency_iterator(vertex_descriptor u,
                          const ring_graph& g,
                          iterator_start):
    parent_class(out_edge_iterator(g, u, iterator_start()), &g) {};
  ring_adjacency_iterator(vertex_descriptor u,
                          const ring_graph& g,
                          iterator_end):
    parent_class(out_edge_iterator(g, u, iterator_end()), &g) {};
};

// AdjacencyGraph valid expressions
std::pair<adjacency_iterator, adjacency_iterator>
adjacent_vertices(vertex_descriptor u, const ring_graph& g) {
  return std::pair<adjacency_iterator, adjacency_iterator>(
    adjacency_iterator(u, g, iterator_start()),
    adjacency_iterator(u, g, iterator_end()));
}

// VertexListGraph valid expressions
vertices_size_type num_vertices(const ring_graph& g) {
  return g.n();
};

std::pair<vertex_iterator, vertex_iterator> vertices(const ring_graph& g) {
  return std::pair<vertex_iterator, vertex_iterator>(
    vertex_iterator(0),                 // The first iterator position
    vertex_iterator(num_vertices(g)) ); // The last iterator position
}

/*
Iterator over edges in a ring graph.

This object iterates over all the vertices in the graph, then for each
vertex returns its first outgoing edge.

It is implemented with the boost::iterator_adaptor class, because it is
essentially a vertex_iterator with a customized deference operation.
*/
class ring_edge_iterator:public boost::iterator_adaptor<
  ring_edge_iterator,
  vertex_iterator,
  edge_descriptor,
  boost::use_default,
  edge_descriptor > {
public:
  ring_edge_iterator():
    ring_edge_iterator::iterator_adaptor_(0),m_g(NULL) {};
  explicit ring_edge_iterator(const ring_graph& g, iterator_start):
    ring_edge_iterator::iterator_adaptor_(vertices(g).first),m_g(&g) {};
  explicit ring_edge_iterator(const ring_graph& g, iterator_end):
    ring_edge_iterator::iterator_adaptor_(
      // Size 2 graphs have a single edge connecting the two vertices.
      g.n() == 2 ? ++(vertices(g).first) : vertices(g).second ),
    m_g(&g) {};

private:
  friend class boost::iterator_core_access;

  edge_descriptor dereference() const {
    // The first element in the incident edge list of the current vertex.
    return *(out_edges(*this->base_reference(), *m_g).first);
  }

  // The graph being iterated over
  const ring_graph *m_g;
};

// EdgeListGraph valid expressions
std::pair<edge_iterator, edge_iterator> edges(const ring_graph& g) {
  return std::pair<edge_iterator, edge_iterator>(
    ring_edge_iterator(g, iterator_start()),
    ring_edge_iterator(g, iterator_end()) );
}

edges_size_type num_edges(const ring_graph& g) {
  // There are as many edges as there are vertices, except for size 2
  // graphs, which have a single edge connecting the two vertices.
  return g.n() == 2 ? 1:g.n();
}

// AdjacencyMatrix valid expressions
std::pair<edge_descriptor, bool>
edge(vertex_descriptor u, vertex_descriptor v, const ring_graph& g) {
  if (abs(u-v) == 1 &&
      u >= 0 && u < num_vertices(g) && v >= 0 && v < num_vertices(g))
    return std::pair<edge_descriptor, bool>(edge_descriptor(u, v), true);
  else
    return std::pair<edge_descriptor, bool>(edge_descriptor(), false);
}

/*
Map from edges to weight values
*/
struct edge_weight_map {
  typedef double value_type;
  typedef value_type reference;
  typedef edge_descriptor key_type;
  typedef boost::readable_property_map_tag category;

  // Edges have a weight equal to the average of their endpoint indexes.
  reference operator[](key_type e) const {
    return (e.first + e.second)/2.0;
  }
};

// Use these propety_map and property_traits parameterizations to refer to
// the associated property map types.
typedef boost::property_map<ring_graph,
                            boost::edge_weight_t>::const_type
        const_edge_weight_map;
typedef boost::property_traits<const_edge_weight_map>::reference
        edge_weight_map_value_type;
typedef boost::property_traits<const_edge_weight_map>::key_type
        edge_weight_map_key;

// PropertyMap valid expressions
edge_weight_map_value_type
get(const_edge_weight_map pmap, edge_weight_map_key e) {
  return pmap[e];
}

// ReadablePropertyGraph valid expressions
const_edge_weight_map get(boost::edge_weight_t, const ring_graph&) {
  return const_edge_weight_map();
}

edge_weight_map_value_type get(boost::edge_weight_t tag,
                               const ring_graph& g,
                               edge_weight_map_key e) {
  return get(tag, g)[e];
}

// This expression is not part of a graph concept, but is used to return the
// default vertex index map used by the Dijkstra search algorithm.
boost::identity_property_map get(boost::vertex_index_t, const ring_graph&) {
  // The vertex descriptors are already unsigned integer indices, so just
  // return an identity map.
  return boost::identity_property_map();
}

// Print edges as (x, y)
std::ostream& operator<<(std::ostream& output, const edge_descriptor& e) {
  return output << "(" << e.first << ", " << e.second << ")";
}

int main (int argc, char const *argv[]) {
  using namespace boost;
  // Check the concepts that graph models.  This is included to demonstrate
  // how concept checking works, but is not required for a working program
  // since Boost algorithms do their own concept checking.
  BOOST_CONCEPT_ASSERT(( BidirectionalGraphConcept<ring_graph> ));
  BOOST_CONCEPT_ASSERT(( AdjacencyGraphConcept<ring_graph> ));
  BOOST_CONCEPT_ASSERT(( VertexListGraphConcept<ring_graph> ));
  BOOST_CONCEPT_ASSERT(( EdgeListGraphConcept<ring_graph> ));
  BOOST_CONCEPT_ASSERT(( AdjacencyMatrixConcept<ring_graph> ));
  BOOST_CONCEPT_ASSERT((
    ReadablePropertyMapConcept<const_edge_weight_map, edge_descriptor> ));
  BOOST_CONCEPT_ASSERT((
    ReadablePropertyGraphConcept<ring_graph, edge_descriptor, edge_weight_t> ));

  // Specify the size of the graph on the command line, or use a default size
  // of 5.
  std::size_t n = argc == 2 ? boost::lexical_cast<std::size_t>(argv[1]) : 5;

  // Create a small ring graph.
  ring_graph g(n);
  const_edge_weight_map m = get(edge_weight, g);

  // Print the outgoing edges of all the vertices.  For n=5 this will print:
  //
  // Vertices, outgoing edges, and adjacent vertices
  // Vertex 0: (0, 1)  (0, 4)   Adjacent vertices 1 4
  // Vertex 1: (1, 2)  (1, 0)   Adjacent vertices 2 0
  // Vertex 2: (2, 3)  (2, 1)   Adjacent vertices 3 1
  // Vertex 3: (3, 4)  (3, 2)   Adjacent vertices 4 2
  // Vertex 4: (4, 0)  (4, 3)   Adjacent vertices 0 3
  // 5 vertices
  std::cout << "Vertices, outgoing edges, and adjacent vertices" << std::endl;
  vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; vi++) {
    vertex_descriptor u = *vi;
    std::cout << "Vertex " << u << ": ";
    // Adjacenct edges
    out_edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = out_edges(u, g); ei != ei_end; ei++)
      std::cout << *ei << "  ";
    std::cout << " Adjacent vertices ";
    // Adjacent vertices
    // Here we want our adjacency_iterator and not boost::adjacency_iterator.
    ::adjacency_iterator ai, ai_end;
    for (boost::tie(ai, ai_end) = adjacent_vertices(u, g); ai != ai_end; ai++) {
      std::cout << *ai << " ";
    }
    std::cout << std::endl;
  }
  std::cout << num_vertices(g) << " vertices" << std::endl << std::endl;

  // Print all the edges in the graph along with their weights.  For n=5 this
  // will print:
  //
  // Edges and weights
  // (0, 1) weight 0.5
  // (1, 2) weight 1.5
  // (2, 3) weight 2.5
  // (3, 4) weight 3.5
  // (4, 0) weight 2
  // 5 edges
  std::cout << "Edges and weights" << std::endl;
  edge_iterator ei, ei_end;
  for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ei++) {
    edge_descriptor e = *ei;
    std::cout << e << " weight " << get(edge_weight, g, e) << std::endl;
  }
  std::cout << num_edges(g) << " edges"  << std::endl;

  if (n>0) {
    std::cout << std::endl;
    // Do a Dijkstra search from vertex 0.  For n=5 this will print:
    //
    // Dijkstra search from vertex 0
    // Vertex 0: parent 0, distance 0
    // Vertex 1: parent 0, distance 0.5
    // Vertex 2: parent 1, distance 2
    // Vertex 3: parent 2, distance 4.5
    // Vertex 4: parent 0, distance 2
    vertex_descriptor source = 0;
    std::vector<vertex_descriptor> pred(num_vertices(g));
    std::vector<edge_weight_map_value_type> dist(num_vertices(g));

    dijkstra_shortest_paths(g, source,
                            predecessor_map(&pred[0]).
                            distance_map(&dist[0]) );

    std::cout << "Dijkstra search from vertex " << source << std::endl;
    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
      vertex_descriptor u = *vi;
      std::cout << "Vertex " << u << ": "
                << "parent "<< pred[*vi] << ", "
                << "distance " << dist[u]
                << std::endl;
    }
  }

  return 0;
}


/** Copyright 2017-2020 CNRS-AIST JRL and CNRS-UM LIRMM */

#include <tvm/Variable.h>
#include <tvm/function/BasicLinearFunction.h>
#include <tvm/utils/memoryChecks.h>

#include <Eigen/Core>

#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN
#define DOCTEST_CONFIG_SUPER_FAST_ASSERTS
#include "doctest/doctest.h"

using namespace Eigen;
using namespace tvm;

TEST_CASE("Test subvariable jacobians")
{
  VariablePtr x = Space(7).createVariable("x");
  VariablePtr y = Space(8).createVariable("y");
  VariablePtr z = Space(6).createVariable("z");
  VariablePtr w = Space(4).createVariable("w");

  VariablePtr x1 = x->subvariable(3, "x1", 1);
  VariablePtr y1 = y->subvariable(6, "y1", 1);
  VariablePtr y2 = y->subvariable(3, "y2", 2);

  MatrixXd A = MatrixXd::Random(3, 7);
  MatrixXd B = MatrixXd::Random(3, 6);
  MatrixXd C = MatrixXd::Random(3, 6);

  function::BasicLinearFunction f(A * x + B * y1 + C * z);

  tvm::utils::set_is_malloc_allowed(false);
  FAST_CHECK_UNARY(f.jacobian(*x).isApprox(A));
  FAST_CHECK_UNARY(f.jacobian(*x1).isApprox(A.middleCols(1, 3)));
  FAST_CHECK_UNARY(f.jacobian(*y1).isApprox(B));
  FAST_CHECK_UNARY(f.jacobian(*y2).isApprox(B.middleCols(1, 3)));
  CHECK_THROWS(f.jacobian(*y));
  CHECK_THROWS(f.jacobian(*w));
  tvm::utils::set_is_malloc_allowed(true);
}

TEST_CASE("Test subvariable JDot")
{
  VariablePtr x = Space(7).createVariable("x");
  VariablePtr y = Space(8).createVariable("y");
  VariablePtr z = Space(6).createVariable("z");
  VariablePtr w = Space(4).createVariable("w");

  VariablePtr x1 = x->subvariable(3, "x1", 1);
  VariablePtr y1 = y->subvariable(6, "y1", 1);
  VariablePtr y2 = y->subvariable(3, "y2", 2);

  MatrixXd A = MatrixXd::Random(3, 7);
  MatrixXd B = MatrixXd::Random(3, 6);
  MatrixXd C = MatrixXd::Random(3, 6);

  function::BasicLinearFunction f(A * x + B * y1 + C * z);

  tvm::utils::set_is_malloc_allowed(false);
  FAST_CHECK_EQ(f.JDot(*x).cols(), 7);
  FAST_CHECK_EQ(f.JDot(*x1).cols(), 3);
  FAST_CHECK_EQ(f.JDot(*y1).cols(), 6);
  FAST_CHECK_EQ(f.JDot(*y2).cols(), 3);
  CHECK_THROWS(f.JDot(*y));
  CHECK_THROWS(f.JDot(*w));
  tvm::utils::set_is_malloc_allowed(true);
}


/*
 * Copyright (c) Anton Liaukevich 2009-2010 <leva.dev@gmail.com>
 *
 * 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.
 */

#include <boost/test/unit_test.hpp>
#include <polishboolfunctions/exceptions.h>
#include <polishboolfunctions/Constant.h>
#include <polishboolfunctions/BinaryOperation.h>
#include <polishboolfunctions/NotOperation.h>

BOOST_AUTO_TEST_SUITE(ConstructSuite)

using namespace PolishBoolFunctions;

BOOST_AUTO_TEST_CASE(Constants)
{
    Constant constant1(true);
    Constant constant2 = constant1,
             constant3(constant2);
    Constant constant4(false);
    constant4 = constant1;

    BOOST_CHECK( constant1.getValue() == true &&
                 constant2.getValue() == true &&
                 constant3.getValue() == true &&
                 constant4.getValue() == true );
}

AbstractOperation::Vector alone(Type x0)
{
    return AbstractOperation::Vector(1, x0);
}

AbstractOperation::Vector pair(Type x0, Type x1)
{
    AbstractOperation::Vector result(2);
    result[0] = x0;
    result[1] = x1;
    return result;
}

BOOST_AUTO_TEST_CASE(SomeReadyOperators)
{
    using namespace UnaryOperators;
    using namespace BinaryOperators;

    BOOST_CHECK_EQUAL(Not.getArity(), 1);
    BOOST_CHECK_EQUAL(And.getArity(), 2);

    BOOST_CHECK_THROW(And.evaluate(AbstractOperation::Vector(0)), ArgumentDoesntAgreeWithArity);
    BOOST_CHECK_THROW(And.evaluate(AbstractOperation::Vector(10)), ArgumentDoesntAgreeWithArity);
    BOOST_CHECK_THROW(And.evaluate(alone(false)), ArgumentDoesntAgreeWithArity);
    BOOST_CHECK_THROW(Not.evaluate(pair(false, true)), ArgumentDoesntAgreeWithArity);

    BOOST_CHECK_EQUAL(And.evaluate(pair(false, false)), false);
    BOOST_CHECK_EQUAL(And.evaluate(pair(true,  false)), false);
    BOOST_CHECK_EQUAL(And.evaluate(pair(false, true)),  false);
    BOOST_CHECK_EQUAL(And.evaluate(pair(true,  true)),  true);

    BOOST_CHECK_EQUAL(Or.evaluate(pair(false, false)), false);
    BOOST_CHECK_EQUAL(Or.evaluate(pair(true,  false)), true);
    BOOST_CHECK_EQUAL(Or.evaluate(pair(false, true)),  true);
    BOOST_CHECK_EQUAL(Or.evaluate(pair(true,  true)),  true);

    BOOST_CHECK_EQUAL(Xor.evaluate(pair(false, false)), false);
    BOOST_CHECK_EQUAL(Xor.evaluate(pair(true,  false)), true);
    BOOST_CHECK_EQUAL(Xor.evaluate(pair(false, true)),  true);
    BOOST_CHECK_EQUAL(Xor.evaluate(pair(true,  true)),  false);

    BOOST_CHECK_EQUAL(Not.evaluate(alone(false)), true);
    BOOST_CHECK_EQUAL(Not.evaluate(alone(true)),  false);

    BOOST_CHECK_EQUAL(AndNot.evaluate(pair(false, false)), false);
    BOOST_CHECK_EQUAL(AndNot.evaluate(pair(true,  false)), true);
    BOOST_CHECK_EQUAL(AndNot.evaluate(pair(false, true)),  false);
    BOOST_CHECK_EQUAL(AndNot.evaluate(pair(true,  true)),  false);

    BOOST_CHECK( AndNot == Greater );
    BOOST_CHECK( And != Xor );

    BOOST_CHECK( Not.isEquivalentTo(Not) );
    BOOST_CHECK( AndNot.isEquivalentTo(AndNot) );
    BOOST_CHECK( AndNot.isEquivalentTo(Greater) );
    BOOST_CHECK( !And.isEquivalentTo(Xor) );
    BOOST_CHECK( !And.isEquivalentTo(Not) );
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * adaptive_iterator.cpp
 *
 * Copyright 2012-2013 Karsten Ahnert
 * Copyright 2012 Mario Mulansky
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>
#include <iterator>
#include <utility>
#include <algorithm>
#include <cassert>

#include <boost/array.hpp>

#include <boost/range/algorithm.hpp>
#include <boost/range/adaptor/filtered.hpp>
#include <boost/range/numeric.hpp>

#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/generation.hpp>

#include <boost/numeric/odeint/iterator/adaptive_iterator.hpp>
#include <boost/numeric/odeint/iterator/adaptive_time_iterator.hpp>

#define tab "\t"

using namespace std;
using namespace boost::numeric::odeint;

const double sigma = 10.0;
const double R = 28.0;
const double b = 8.0 / 3.0;

struct lorenz
{
    template< class State , class Deriv >
    void operator()( const State &x , Deriv &dxdt , double t ) const
    {
        dxdt[0] = sigma * ( x[1] - x[0] );
        dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
        dxdt[2] = -b * x[2] + x[0] * x[1];
    }
};

#include <typeinfo>

int main( int argc , char **argv )
{
    typedef boost::array< double , 3 > state_type;

    /*
     * Controlled steppers with time iterator
     */

    // std::for_each
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        std::for_each( make_adaptive_time_iterator_begin( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
                       make_adaptive_time_iterator_end( stepper , lorenz() , x ) ,
                       []( const std::pair< const state_type&, double > &x ) {
                           std::cout << x.second << tab << x.first[0] << tab << x.first[1] << tab << x.first[2] << "\n"; } );
    }

    // std::copy_if
    {
        std::vector< pair< state_type , double > > res;
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        std::copy_if( make_adaptive_time_iterator_begin( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
                      make_adaptive_time_iterator_end( stepper , lorenz() , x ) ,
                      std::back_inserter( res ) ,
                      []( const pair< const state_type& , double > &x ) {
                          return ( x.first[0] > 0.0 ) ? true : false; } );
        for( size_t i=0 ; i<res.size() ; ++i )
            cout << res[i].first[0] << tab << res[i].first[1] << tab << res[i].first[2] << "\n";
    }

    // std::accumulate
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        double res = std::accumulate( make_adaptive_time_iterator_begin( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
                                      make_adaptive_time_iterator_end( stepper , lorenz() , x ) ,
                                      0.0 ,
                                      []( double sum , const pair< const state_type& , double > &x ) {
                                          return sum + x.first[0]; } );
        cout << res << endl;
    }

    // std::transform
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        vector< double > weights;
        std::transform( make_adaptive_time_iterator_begin( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
                        make_adaptive_time_iterator_end( stepper , lorenz() , x ) ,
                        back_inserter( weights ) ,
                        []( const pair< const state_type& , double > &x ) {
                            return sqrt( x.first[0] * x.first[0] + x.first[1] * x.first[1] + x.first[2] * x.first[2] ); } );
        for( size_t i=0 ; i<weights.size() ; ++i )
            cout << weights[i] << "\n";
    }

    /*
     * Boost.Range versions of controlled stepper with time iterator
     */

    // boost::range::for_each
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        boost::range::for_each( make_adaptive_time_range( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
                                []( const std::pair< const state_type& , double > &x ) {
                                    std::cout << x.second << tab << x.first[0] << tab << x.first[1] << tab << x.first[2] << "\n"; } );
    }

    // boost::range::copy with filtered adaptor (simulating std::copy_if)
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        std::vector< std::pair< state_type , double > > res;
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        boost::range::copy( make_adaptive_time_range( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) |
                            boost::adaptors::filtered( [] ( const pair< const state_type& , double > &x ) { return ( x.first[0] > 0.0 ); } ) ,
                            std::back_inserter( res ) );
        for( size_t i=0 ; i<res.size() ; ++i )
            cout << res[i].first[0] << tab << res[i].first[1] << tab << res[i].first[2] << "\n";
    }

    // boost::range::accumulate
    {
        //[adaptive_time_iterator_accumulate_range
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        double res = boost::accumulate( make_adaptive_time_range( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) , 0.0 ,
                                        []( double sum , const pair< const state_type& , double > &x ) {
                                            return sum + x.first[0]; } );
        cout << res << endl;
        //]
    }

    //  boost::range::transform
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        vector< double > weights;
        boost::transform( make_adaptive_time_range( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) , back_inserter( weights ) ,
                          []( const pair< const state_type& , double > &x ) {
                              return sqrt( x.first[0] * x.first[0] + x.first[1] * x.first[1] + x.first[2] * x.first[2] ); } );
        for( size_t i=0 ; i<weights.size() ; ++i )
            cout << weights[i] << "\n";
    }

    // boost::range::find with time iterator
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        auto iter = boost::find_if( make_adaptive_time_range( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
                                    []( const std::pair< const state_type & , double > &x ) {
                                        return ( x.first[0] < 0.0 ); } );
        cout << iter->second << "\t" << iter->first[0] << "\t" << iter->first[1] << "\t" << iter->first[2] << "\n";
    }

    // /*
    //  * Boost.Range versions for dense output steppers
    //  */

    // // boost::range::for_each
    // {
    //     runge_kutta_dopri5< state_type > stepper;
    //     state_type x = {{ 10.0 , 10.0 , 10.0 }};
    //     boost::range::for_each( make_adaptive_range( make_dense_output( 1.0e-6 , 1.0e-6 , stepper ) , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
    //                             []( const state_type &x ) {
    //                                 std::cout << x[0] << tab << x[1] << tab << x[2] << "\n"; } );
    // }

    // // boost::range::for_each with time iterator
    // {
    //     runge_kutta_dopri5< state_type > stepper;
    //     state_type x = {{ 10.0 , 10.0 , 10.0 }};
    //     boost::range::for_each( make_adaptive_time_range( make_dense_output( 1.0e-6 , 1.0e-6 , stepper ) , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
    //                             []( const std::pair< state_type& , double > &x ) {
    //                                 std::cout << x.second << tab << x.first[0] << tab << x.first[1] << tab << x.first[2] << "\n"; } );

    // }

    /*
     * Pure iterators for controlled stepper without time iterator
     */

    // std::for_each
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        std::for_each( make_adaptive_iterator_begin( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
                       make_adaptive_iterator_end( stepper , lorenz() , x ) ,
                       []( const state_type& x ) {
                           std::cout << x[0] << tab << x[1] << tab << x[2] << "\n"; } );
    }

    // std::copy_if
    {
        std::vector< state_type > res;
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        std::copy_if( make_adaptive_iterator_begin( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
                      make_adaptive_iterator_end( stepper , lorenz() , x ) ,
                      std::back_inserter( res ) ,
                      []( const state_type& x ) {
                          return ( x[0] > 0.0 ) ? true : false; } );
        for( size_t i=0 ; i<res.size() ; ++i )
            cout << res[i][0] << tab << res[i][1] << tab << res[i][2] << "\n";
    }

    // std::accumulate
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        double res = std::accumulate( make_adaptive_iterator_begin( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
                                      make_adaptive_iterator_end( stepper , lorenz() , x ) ,
                                      0.0 ,
                                      []( double sum , const state_type& x ) {
                                          return sum + x[0]; } );
        cout << res << endl;
    }

    // std::transform
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        vector< double > weights;
        std::transform( make_adaptive_iterator_begin( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
                        make_adaptive_iterator_end( stepper , lorenz() , x ) ,
                        back_inserter( weights ) ,
                        []( const state_type& x ) {
                            return sqrt( x[0] * x[0] + x[1] * x[1] + x[2] * x[2] ); } );
        for( size_t i=0 ; i<weights.size() ; ++i )
            cout << weights[i] << "\n";
    }

    /*
     * Boost.Range versions of controlled stepper WITHOUT time iterator
     */

    // boost::range::for_each
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        boost::range::for_each( make_adaptive_range( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
                                []( const state_type &x ) {
                                    std::cout << x[0] << tab << x[1] << tab << x[2] << "\n"; } );
    }

    // boost::range::copy with filtered adaptor (simulating std::copy_if)
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        std::vector< state_type > res;
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        boost::range::copy( make_adaptive_range( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) |
                            boost::adaptors::filtered( [] ( const state_type& x ) { return ( x[0] > 0.0 ); } ) ,
                            std::back_inserter( res ) );
        for( size_t i=0 ; i<res.size() ; ++i )
            cout << res[i][0] << tab << res[i][1] << tab << res[i][2] << "\n";
    }

    // boost::range::accumulate
    {
        //[adaptive_iterator_accumulate_range
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        double res = boost::accumulate( make_adaptive_range( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) , 0.0 ,
                                        []( double sum , const state_type& x ) {
                                            return sum + x[0]; } );
        cout << res << endl;
        //]
    }

    //  boost::range::transform
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        vector< double > weights;
        boost::transform( make_adaptive_range( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) , back_inserter( weights ) ,
                          []( const state_type& x ) {
                              return sqrt( x[0] * x[0] + x[1] * x[1] + x[2] * x[2] ); } );
        for( size_t i=0 ; i<weights.size() ; ++i )
            cout << weights[i] << "\n";
    }

    // boost::range::find
    {
        auto stepper = make_controlled( 1.0e-6 , 1.0e-6 , runge_kutta_cash_karp54< state_type >() );
        state_type x = {{ 10.0 , 10.0 , 10.0 }};
        auto iter = boost::find_if( make_adaptive_range( stepper , lorenz() , x , 0.0 , 1.0 , 0.01 ) ,
                                    []( const state_type &x ) {
                                        return ( x[0] < 0.0 ); } );
        cout << (*iter)[0] << "\t" << (*iter)[1] << "\t" << (*iter)[2] << "\n";
    }

    return 0;
}


#ifndef BOOST_SAFE_FLOAT_POLICY_CHECK_SUBTRACTION_UNDERFLOW_HPP
#define BOOST_SAFE_FLOAT_POLICY_CHECK_SUBTRACTION_UNDERFLOW_HPP

#include <boost/safe_float/policy/check_base_policy.hpp>

#ifdef FENV_AVAILABLE
#pragma STDC FENV_ACCESS ON
#include <fenv.h>
#endif

namespace boost {
namespace safe_float{
namespace policy{

template<class FP>
class check_subtraction_underflow : public check_policy<FP> {
public:
    bool pre_subtraction_check(const FP& lhs, const FP& rhs){
#ifndef FENV_AVAILABLE
        return true;
#else
        return ! std::feclearexcept(FE_UNDERFLOW);
#endif
    }

    bool post_subtraction_check(const FP& rhs){
#ifndef FENV_AVAILABLE
        return std::fpclassify( rhs ) != FP_SUBNORMAL;
#else
        return ! std::fetestexcept(FE_UNDERFLOW);
#endif
    }

    std::string subtraction_failure_message(){
        return std::string("Underflow from operation");
    }
};

}
}
}

#endif // BOOST_SAFE_FLOAT_POLICY_CHECK_SUBTRACTION_UNDERFLOW_HPP


// MIT License
//
// Copyright (c) 2019 Jelle Spijker
//
// 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.
//

#include <gtest/gtest.h>

#ifndef STAT_TEST // Don't perform statistical tests if boost is not found
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#endif

#include "chrono/motion_functions/ChFunction_Ramp.h"

#include "chrono_sensor/ChFunction_SensorNoise.h"
#include "chrono_sensor/ChFunction_SensorBias.h"
#include "chrono_sensor/ChFunction_SensorDigitize.h"

using namespace chrono;
using namespace chrono::vehicle::sensor;

#ifndef STAT_TEST // Don't perform statistical tests if boost is not found
using namespace boost::accumulators;

TEST(Function_Noise, stddev_mean_single_value) {
  ChFunction_Ramp f_ramp;
  f_ramp.Set_ang(0.1);
  f_ramp.Set_y0(0.4);

  double mean_val(0.5);
  double stddev_val(0.2);

  ChFunction_SensorNoise<> f_noise(mean_val, stddev_val);

  accumulator_set<double, stats<tag::mean, tag::variance>> acc;

  for (int i = 0; i < 1000; i++) {
    double x = (double) i / 50.0;
    double y = f_ramp.Get_y(x);
    double y_noise = f_noise.Get_y(y);
    double noise = y_noise - y;
    acc(noise);
  }
  auto m = mean(acc);
  auto st = sqrt(variance(acc));
  ASSERT_NEAR(mean(acc), mean_val, mean_val / 30.);
  ASSERT_NEAR(sqrt(variance(acc)), stddev_val, stddev_val / 30.);
}

TEST(Function_Noise, stddev_mean_vector) {
  ChFunction_Ramp f_ramp;
  f_ramp.Set_ang(0.1);
  f_ramp.Set_y0(0.4);

  ChVector<> mean_val(0.5, 0.25, 0.);
  ChVector<> stddev_val(0.2, 0.4, 0.);

  ChFunction_SensorNoise<ChVector<>> f_noise(mean_val, stddev_val);

  accumulator_set<double, stats<tag::mean, tag::variance>> acc_x;
  accumulator_set<double, stats<tag::mean, tag::variance>> acc_y;
  accumulator_set<double, stats<tag::mean, tag::variance>> acc_z;

  for (int i = 0; i < 10000; i++) {
    double id = static_cast<double>(i) / 50.;
    ChVector<> x = ChVector<>(id);
    ChVector<> y = ChVector<>(f_ramp.Get_y(x.x()), f_ramp.Get_y(x.y()), f_ramp.Get_y(x.z()));
    ChVector<> y_noise = f_noise.Get_y(y);
    ChVector<> noise = y_noise - y;
    acc_x(noise.x());
    acc_y(noise.y());
    acc_z(noise.z());
  }

  ASSERT_NEAR(mean(acc_x), mean_val.x(), mean_val.x() / 30.);
  ASSERT_NEAR(sqrt(variance(acc_x)), stddev_val.x(), stddev_val.x() / 30.);
  ASSERT_NEAR(mean(acc_y), mean_val.y(), mean_val.y() / 30.);
  ASSERT_NEAR(sqrt(variance(acc_y)), stddev_val.y(), stddev_val.y() / 30.);
  ASSERT_NEAR(mean(acc_z), mean_val.z(), mean_val.z() / 30.);
  ASSERT_NEAR(sqrt(variance(acc_z)), stddev_val.z(), stddev_val.z() / 30.);
}
#endif

TEST(Function_Noise, clone) {
  double mean_val = 0.5;
  double stddev_val = 0.2;

  ChFunction_SensorNoise<> f_noise(mean_val, stddev_val);
  auto f_noise_clone = f_noise.Clone();
  ASSERT_EQ(f_noise, *f_noise_clone);
  delete f_noise_clone;
}

TEST(Function_Bias, single_value) {
  double bias = 5.;
  double x = 1.;
  ChFunction_SensorBias<> f_bias(bias);
  ASSERT_EQ(x + bias, f_bias.Get_y(x));
}

TEST(Function_Bias, vector) {
  ChVector<> bias(5., 4., 3.);
  ChVector<> x(2., 4., 8.);
  ChFunction_SensorBias<ChVector<>> f_bias(bias);
  ASSERT_EQ(x + bias, f_bias.Get_y(x));
}

TEST(Function_Bias, quaternion) {
  ChQuaternion<> initial_rot = Q_from_AngAxis(20 * CH_C_DEG_TO_RAD, ChVector<>(0., 0., 1.));
  ChQuaternion<> bias = Q_from_AngAxis(25 * CH_C_DEG_TO_RAD, ChVector<>(0., 0., 1.));
  ChFunction_SensorBias<ChQuaternion<>> f_bias(bias);
  auto ret = f_bias.Get_y(initial_rot);
  ChVector<> va(5., 0., 0.);
  ChVector<> vb = ret.Rotate(va);
  ASSERT_NEAR(vb.x(), 3.53553390593, 1e-6);
  ASSERT_NEAR(vb.y(), 3.53553390593, 1e-6);
}

TEST(Function_Bias, clone) {
  double bias = 5.;
  ChFunction_SensorBias<> f_bias(bias);

  auto f_bias_clone = f_bias.Clone();
  ASSERT_EQ(f_bias, *f_bias_clone);
  delete f_bias_clone;
}

TEST(Function_Digitize, single_value) {
  double bits = 4;
  double min = 0.0;
  double max = 50.;
  double res = (max - min) / pow(2, bits);
  ChFunction_SensorDigitize<> f_dig(bits, max - min);
  auto test = f_dig.Get_y(3.45);
  ASSERT_EQ(f_dig.Get_y(3.45), res * round(3.45 / res));
}

TEST(Function_Digitize, vector) {
  double bits(12.);
  ChVector<> min(0.0);
  ChVector<> max(50.);
  ChVector<> res;
  ChFunction_SensorDigitize<ChVector<>> f_dig(bits, max - min);
  for (int i = 0; i < 3; ++i) {
    res[i] = (max[i] - min[i]) / pow(2., bits);
    ASSERT_EQ(f_dig.Get_y(ChVector<>(3.45))[i], res[i] * round(3.45 / res[i]));
  }
}

TEST(Function_Digitize, quaternion) {
  double bits(12.);
  ChVector<> min(0.);
  ChVector<> max(50.);
  ChQuaternion<> res;
  ChFunction_SensorDigitize<ChQuaternion<>> f_dig(bits, max - min);
  auto t = f_dig.Get_y(ChQuaternion<>(3.45, ChVector<>(3.45)));
  auto r = ChQuaternion<>(0.49950151852068797, ChVector<>(0.50016605009254234));
  ASSERT_TRUE(t.Equals(r));
}


/**
 * math lib test
 * @author Tobias Weber <tweber@ill.fr>
 * @date mar-19
 * @license GPLv3, see 'LICENSE' file
 *
 * g++ -std=c++20 -o mat1 mat1.cpp
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE Mat1
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include <iostream>
#include <vector>

#include "libs/maths.h"
using namespace tl2_ops;

using t_types = std::tuple<double, float>;
BOOST_AUTO_TEST_CASE_TEMPLATE(test_mat1, t_real, t_types)
{
	//using t_cplx = std::complex<t_real>;
	using t_vec = std::vector<t_real>;
	//using t_mat = tl2::mat<t_real, std::vector>;
	//using t_vec_cplx = std::vector<t_cplx>;
	//using t_mat_cplx = tl2::mat<t_cplx, std::vector>;

	std::cout << tl2::stoval<unsigned int>("123") << std::endl;

	std::vector vec1{{
		tl2::create<t_vec>({1, 2, 3, 4, 5, 6, 7, 8, 9, 10}),
		tl2::create<t_vec>({5, 5, 7, 9, 9.5, 10.5, 10.5, 12, 13.5, 14})
	}};
	std::vector vec2{{
		tl2::create<t_vec>({1, 2, 3, 4, 5, 6, 7, 8, 9, 10.5}),
		tl2::create<t_vec>({5, 5, 7, 9, 9.5, 10.5, 10.5, 12, 13.5, 14})
	}};
	std::vector vec3{{
		tl2::create<t_vec>({1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 10}),
		tl2::create<t_vec>({5, 5, 7, 9, 9.5, 10.5, 10.5, 12, 13.5, 14, 14})
	}};

	BOOST_TEST(tl2::equals_all(vec1, vec1, 1e-5));
	BOOST_TEST(tl2::equals_all(vec3, vec3, 1e-5));
	BOOST_TEST(!tl2::equals_all(vec1, vec2, 1e-5));
	BOOST_TEST(!tl2::equals_all(vec1, vec3, 1e-5));
}


#ifndef STAN_MATH_TORSTEN_DSOLVE_ODEINT_INTEGRATOR_HPP
#define STAN_MATH_TORSTEN_DSOLVE_ODEINT_INTEGRATOR_HPP

#include <stan/math/torsten/dsolve/pmx_ode_system.hpp>
#include <stan/math/torsten/mpi/precomputed_gradients.hpp>
#include <stan/math/torsten/dsolve/ode_observer.hpp>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/eigen/eigen.hpp>
#include <type_traits>

namespace torsten {
namespace dsolve {

  using odeint_scheme_rk45 = boost::numeric::odeint::runge_kutta_dopri5<std::vector<double>, double, std::vector<double>, double>;
  using odeint_scheme_ckrk = boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double>, double, std::vector<double>, double>;

  // FIXME: vector_space_algebra for Eigen::VectorXd is not efficient
  // compared to std::vector
  // using odeint_scheme_rk45 = boost::numeric::odeint::runge_kutta_dopri5<Eigen::VectorXd, double, Eigen::VectorXd, double, boost::numeric::odeint::vector_space_algebra>;
  // using odeint_scheme_ckrk = boost::numeric::odeint::runge_kutta_cash_karp54<Eigen::VectorXd, double, Eigen::VectorXd, double, boost::numeric::odeint::vector_space_algebra>;

/**
 * @c boost::odeint ODE integrator.
 */
  template<typename scheme_t>
  struct PMXOdeintIntegrator {
    static constexpr int lmm_t = -1;

    const double rtol_;
    const double atol_;
    const int64_t max_num_steps_;

  public:
    /**
     * constructor
     * @param[in] rtol relative tolerance
     * @param[in] atol absolute tolerance
     * @param[in] max_num_steps max nb. of times steps
     */
    PMXOdeintIntegrator(const double rtol, const double atol,
                        const int64_t max_num_steps)
      : rtol_(rtol), atol_(atol), max_num_steps_(max_num_steps)
    {}

    template <typename Ode, typename Observer>
    inline void integrate(Ode& ode, Observer& observer) {
      std::vector<double> ts_vec(ode.ts_.size() + 1);
      ts_vec[0] = ode.t0_;
      for (size_t i = 0; i < ode.ts_.size(); ++i) {
        ts_vec[i + 1] = stan::math::value_of(ode.ts_[i]);
      }

      const double init_dt = 0.1;
      // integrate_times(make_dense_output(atol_, rtol_, scheme_t()),
      integrate_times(make_stepper(scheme_t()),
                      boost::ref(ode), ode.y0_fwd_system,
                      ts_vec.begin(), ts_vec.end(),
                      init_dt, boost::ref(observer),
                      boost::numeric::odeint::max_step_checker(max_num_steps_));

      // integrate_times(make_controlled(absolute_tolerance, relative_tolerance,
      //                                 runge_kutta_cash_karp54<std::vector<double>, double,
      //                                 std::vector<double>, double>()),
      //                 std::ref(coupled_system), initial_coupled_state, std::begin(ts_vec),
      //                 std::end(ts_vec), step_size, filtered_observer,
      //                 max_step_checker(max_num_steps));
    }

    template<typename... Ts>
    auto make_stepper(boost::numeric::odeint::runge_kutta_dopri5<Ts...> const& stepper)
    {
      return make_dense_output(atol_, rtol_, stepper);
    }

    template<typename... Ts>
    auto make_stepper(boost::numeric::odeint::runge_kutta_cash_karp54<Ts...> const& stepper)
    {
      return make_controlled(atol_, rtol_, stepper);
    }
  };
}
}
#endif


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/tenpower.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/mtwo.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/ratio.hpp>

STF_CASE_TPL (" tenpower unsigned_int", STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::tenpower;
  using r_t = decltype(tenpower(T()));

  STF_TYPE_IS(r_t, bd::as_floating_t<T>);

  // specific values tests
  STF_ULP_EQUAL(tenpower(bs::One<T>()), (bs::Ratio<T, 10>()), 0.5);
  STF_ULP_EQUAL(tenpower(bs::Two<T>()), (bs::Ratio<T, 100>()), 0.5);
  STF_ULP_EQUAL(tenpower(bs::Zero<T>()), bs::One<r_t>(), 0.5);
} // end of test for unsigned_int_

STF_CASE_TPL (" tenpower signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::tenpower;
  using r_t = decltype(tenpower(T()));

  STF_TYPE_IS(r_t, bd::as_floating_t<T>);

  // specific values tests
  STF_ULP_EQUAL(tenpower(bs::Mone<T>()), (bs::Ratio<r_t, 1, 10>()), 0.5);
  STF_ULP_EQUAL(tenpower(bs::One<T>()), bs::Ten<r_t>(), 0.5);
  STF_ULP_EQUAL(tenpower(bs::Two<T>()), (bs::Ratio<T, 100>()), 0.5);
  STF_ULP_EQUAL(tenpower(bs::Mtwo<T>()), (bs::Ratio<r_t, 1, 100>()), 0.5);
  STF_ULP_EQUAL(tenpower(bs::Zero<T>()), bs::One<r_t>(), 0.5);
} // end of test for signed_int_