/*
 * ground_filter.cpp
 *
 * Created on	: May 19, 2017
 * Author	: Patiphon Narksri
 */
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <velodyne_pointcloud/point_types.h>
#include <opencv/cv.h>

#include <boost/lexical_cast.hpp> //U
#include <boost/chrono.hpp> //U
#include <iostream> //U

enum Label
{
        GROUND = 0,
        VERTICAL = 1,
        UNKNOWN = 3
};

class GroundFilter
{
public:

	GroundFilter();

private:

	ros::NodeHandle n;
        ros::Subscriber sub;
	ros::Publisher vertical_points_pub;
	ros::Publisher ground_points_pub;

        std::string     point_topic;
	int 		sensor_model;
	double 		sensor_height;
	double 		max_slope;
	double 		gap_thres;
        bool            floor_removal;
	double 		radius_coeff_close;
	double		radius_coeff_far;

	double 		point_distance;
	int 		min_point;

	int 		vertical_res;
	int 		horizontal_res;
	double 		limiting_ratio;
	cv::Mat 	index_map;
	Label 		class_label[64];
	double 		optimal_radius[64];

	//These will be deleted
	int		original_point;
	int		remaining_point;
	int		point_after_tf;

	boost::chrono::high_resolution_clock::time_point t1;
	boost::chrono::high_resolution_clock::time_point t2;
	boost::chrono::nanoseconds elap_time;

	void initLabelArray(int model);
	void initRadiusArray(double radius[], int model);
	void initDepthMap(int width);
	void publishPoint(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &msg,
				int index[], int &index_size,
				pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &topic);

	void velodyneCallback(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &msg);
	void groundSeparate(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &msg,
				pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &vertical_points,
				pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &ground_points);

};

GroundFilter::GroundFilter() : n("~")
{

	n.param<std::string>("point_topic", point_topic, "/points_raw");
	//If it set to False it will publish the original PointCloud
 	n.param("remove_floor",  floor_removal,  true);
	//Can be selected between 16, 32 and 64 (Have never tested on 16 though)
        n.param("sensor_model", sensor_model, 64);
	//This is the height of Velodyne measured from center of Velodyne to ground
        n.param("sensor_height", sensor_height, 1.80);
	//Maximum allowable slope i.e., any surface steeper than this angle[deg] will not be removed
        n.param("max_slope", max_slope, 15.0);

	//These parameters have been tested to be the optimal values for this algorithm
	//Shouldn't have to be changed for normal use
        n.param("min_point", min_point, 2);
	n.param("gap_thres", gap_thres, 0.15);
	n.param("radius_coeff_close", radius_coeff_close, 0.2);
	n.param("radius_coeff_far", radius_coeff_far, 0.7);

	//Number of laser rays in vertical direction
	vertical_res 	= 64;
	//Number of laser rays in horizontal direction (will be recalculated every rotation)
	horizontal_res 	= 2000;
	//Use tan instead of max_slope in degree for more efficient computation
	limiting_ratio 	= tan(20.0*M_PI/180);

	vertical_res = sensor_model;
	initLabelArray(sensor_model);
	limiting_ratio = tan(max_slope*M_PI/180);
	initRadiusArray(optimal_radius, sensor_model);

	sub = n.subscribe(point_topic, 10, &GroundFilter::velodyneCallback, this);
        vertical_points_pub = n.advertise<sensor_msgs::PointCloud2>("/points_lanes", 10);
        ground_points_pub = n.advertise<sensor_msgs::PointCloud2>("/points_ground", 10);

}
//This loop calculate the expected spaced between consecutive rings
void GroundFilter::initRadiusArray(double radius[], int model)
{
        if (model == 32)
        {
                double start_angle = 92.0/3;
                double angle_res = 4.0/3;
                for (int i = 0; i < model; i++)
                {
                        if (i == 0)
                        {
                                radius[i] = 999999;
                        } else {
                                double theta = start_angle - i*angle_res;
                                theta = theta*M_PI/180.0;
                                radius[i] = sensor_height*(1.0/tan(theta) - 1.0/tan(theta + angle_res*M_PI/180.0));
                        }

			if (i <= 12)
			{
				radius[i] = radius_coeff_close*radius[i];
			} else if (i <= 20) {
				radius[i] = radius_coeff_far*radius[i];
			} else {
				radius[i] = radius[20];
			}
                }
        } else {
                for (int i = 0; i < model; i++)
                {
                        if (i < 32)
                        {
                                double start_angle = 73.0/3;
                                double angle_res = 1.0/2;
                                if (i == 0)
                                {
                                        radius[i] = 999999;
                                } else {
                                        double theta = start_angle - i*angle_res;
                                        theta = theta*M_PI/180.0;
                                        radius[i] = sensor_height*(1.0/tan(theta) - 1.0/tan(theta + angle_res*M_PI/180.0));
                                }
                        } else {
                                double start_angle = 25.0/3;
                                double angle_res = 1.0/3;
                                if (i == 32)
                                {
                                        double theta = start_angle;
                                        theta = theta*M_PI/180.0;
                                        radius[i] = sensor_height*(1.0/tan(theta) - 1.0/tan(theta + 0.5*M_PI/180.0));
                                } else {
                                        double theta = start_angle - (i-32)*angle_res;
                                        theta = theta*M_PI/180.0;
                                        radius[i] = sensor_height*(1.0/tan(theta) - 1.0/tan(theta + angle_res*M_PI/180.0));
                                }
                        }

			if (i <= 15)
			{
				radius[i] = radius_coeff_close*radius[i];
			} else if (i <= 40) {
				radius[i] = radius_coeff_far*radius[i];
			} else {
				radius[i] = radius[40];
			}
                }
        }
}

//Create an enum array to store the current status of each point in the same bearing angle
void GroundFilter::initLabelArray(int model)
{
	for(int a = 0; a < vertical_res; a++)
	{
		class_label[a] = UNKNOWN;
	}
}

//Create a depth map that has a size of vertical_res x horizontal_res
void GroundFilter::initDepthMap(int width)
{
	const int mOne = -1;
	index_map = cv::Mat_<int>(vertical_res, width, mOne);
}

//Used for publish the separated PointCloud to a defined topic
void GroundFilter::publishPoint(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &msg,
				int index[], int &index_size,
				pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &topic)
{

	velodyne_pointcloud::PointXYZIR point;
	for (int i = 0; i < index_size; i++)
	{
		point.x = msg->points[index[i]].x;
		point.y = msg->points[index[i]].y;
		point.z = msg->points[index[i]].z;
		point.intensity = msg->points[index[i]].intensity;
		point.ring = msg->points[index[i]].ring;
		topic.push_back(point);

		remaining_point++;
	}
	index_size = 0;

}

//Main calculation is done in this function
void GroundFilter::groundSeparate(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &msg,
			pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &vertical_points,
			pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &ground_points)
{

        velodyne_pointcloud::PointXYZIR point;

        horizontal_res = int(msg->points.size()*1.8 / vertical_res);
        initDepthMap(horizontal_res);

	original_point = msg->points.size();
	remaining_point = 0;
	point_after_tf = 0;

	//This conversion has some losses
	//Convert coordinate of each point from Cartesian to Spherical (XYZ -> DepthMap)
        for (int i = 0; i < msg->points.size(); i++)
        {
                double u = atan2(msg->points[i].y,msg->points[i].x) * 180/M_PI;
                if (u < 0) u = 360 + u;
                int column = horizontal_res - (int)((double)horizontal_res * u / 360.0) - 1;
                int row = vertical_res - 1 - msg->points[i].ring;
		index_map.at<int>(row, column) = i;
        }

	//Iterate through each bearing angle (each horizontal angle)
	for (int i = 0; i < horizontal_res; i++)
        {
                Label point_class[vertical_res];
		int unknown_index[vertical_res];
		int point_index[vertical_res];
		int unknown_index_size = 0;
		int point_index_size = 0;
		double z_ref = 0;
		double r_ref = 0;
		//Initialize the enum array to be UKNOWN in every elements before calculation
		//Of each bearing angle
		std::copy(class_label, class_label + vertical_res, point_class);

		//Iterate through each vertical angle (each laser ray) starting from lowest ray
		for (int j = vertical_res - 1; j >= 0; j--)
                {
			//If the point has already been processed and already classified
			//It will not be processed again
                        if (index_map.at<int>(j,i) > -1 && point_class[j] == UNKNOWN)
                        {
				point_after_tf++;
				double x0 = msg->points[index_map.at<int>(j, i)].x;
				double y0 = msg->points[index_map.at<int>(j, i)].y;
				double z0 = msg->points[index_map.at<int>(j, i)].z;
				double r0 = sqrt(x0*x0 + y0*y0);
				double r_diff = r0 - r_ref;
				double z_diff = fabs(z0 - z_ref);
				double pair_angle = z_diff/r_diff;
				//Check if the angle between the current and the previous point is less than a defined maximum_slope
				//If the angle is less than maximum_slope, add the current point to "Candidate group"
				if (((pair_angle > 0 && pair_angle < limiting_ratio) && z_diff < gap_thres) || point_index_size == 0)
				{
					r_ref = r0;
					z_ref = z0;
					point_index[point_index_size] = j;
					point_index_size++;
				} else {
					//If the angle exceeds the maximum slope
					//Check number of point in "Candidate group", if exceeds the minimum_point threshold
					//Publish them as ground points
					if (point_index_size > min_point)
					{
						for (int m = 0; m < point_index_size; m++)
						{

								int index = index_map.at<int>(point_index[m],i);
								point.x = msg->points[index].x;
								point.y = msg->points[index].y;
								point.z = msg->points[index].z;
								point.intensity = msg->points[index].intensity;
								point.ring = msg->points[index].ring;
								ground_points.push_back(point);
								point_class[point_index[m]] = GROUND;

								remaining_point++;
						}
						point_index_size = 0;
					//If the number of point in "Candidate group" is less than the threshold, continue the calculation
					} else {
						for (int m = 0; m < point_index_size; m++)
						{
							int index = index_map.at<int>(point_index[m],i);
							point.x = msg->points[index].x;
							point.y = msg->points[index].y;
							point.z = msg->points[index].z;
							point.intensity = msg->points[index].intensity;
							point.ring = msg->points[index].ring;
							unknown_index[unknown_index_size] = index;
							unknown_index_size++;
						}
						point_index_size = 0;
					}

					r_ref = r0;
					z_ref = z0;
					point_index[point_index_size] = j;
					point_index_size++;
				}
  			}
			//If the highest ray is reached
                        if (j == 0)
                        {
				//First, check if the "Candidate group" contain any point, if so classify them using
				//the same criteria as above
				if (point_index_size != 0)
				{
					if (point_index_size > min_point)
					{
						for (int m = 0; m < point_index_size; m++)
						{

								int index = index_map.at<int>(point_index[m],i);
								point.x = msg->points[index].x;
								point.y = msg->points[index].y;
								point.z = msg->points[index].z;
								point.intensity = msg->points[index].intensity;
								point.ring = msg->points[index].ring;
								ground_points.push_back(point);
								point_class[point_index[m]] = GROUND;

								remaining_point++;
						}
						point_index_size = 0;
					} else {
						for (int m = 0; m < point_index_size; m++)
						{
							int index = index_map.at<int>(point_index[m],i);
							point.x = msg->points[index].x;
							point.y = msg->points[index].y;
							point.z = msg->points[index].z;
							point.intensity = msg->points[index].intensity;
							point.ring = msg->points[index].ring;
							unknown_index[unknown_index_size] = index;
							unknown_index_size++;
						}
						point_index_size = 0;
					}
				}
				//Lastly, the remaining unknown points are checked using different approach
				//Check if the radial distance between two consecutive points is less than
				//point_distance threshold, if so classify them as vertical
				double centroid = 0;
				int centroid_ring = 0;
				int cluster_index[vertical_res];
				int cluster_index_size = 0;
				for (int m = unknown_index_size - 1; m >= 0; m--)
				{
					double x0 = msg->points[unknown_index[m]].x;
					double y0 = msg->points[unknown_index[m]].y;
					double r0 = sqrt(x0*x0 + y0*y0);
					double r_diff = fabs(r0 - centroid);
					point_distance = optimal_radius[centroid_ring];
					if ((r_diff < point_distance) || cluster_index_size == 0)
					{
						cluster_index[cluster_index_size] = unknown_index[m];
						cluster_index_size++;
						centroid = r0;
						centroid_ring = msg->points[unknown_index[m]].ring;
					} else {
						if(cluster_index_size > 1)
						{
							publishPoint(msg, cluster_index	, cluster_index_size, vertical_points);
						} else {
							publishPoint(msg, cluster_index, cluster_index_size, ground_points);
						}

						cluster_index[cluster_index_size] = unknown_index[m];
						cluster_index_size++;
						centroid = r0;
						centroid_ring = msg->points[unknown_index[m]].ring;
					}
					if (m == 0)
					{
						if(cluster_index_size > 1)
						{
							publishPoint(msg, cluster_index, cluster_index_size, vertical_points);
						} else {
							publishPoint(msg, cluster_index, cluster_index_size, ground_points);
						}
					}
				}
                        }
                }
	}
}

void GroundFilter::velodyneCallback(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &msg)
{
	t1 = boost::chrono::high_resolution_clock::now();

	pcl::PointCloud<velodyne_pointcloud::PointXYZIR> vertical_points;
	pcl::PointCloud<velodyne_pointcloud::PointXYZIR> ground_points;
	vertical_points.header = msg->header;
        ground_points.header = msg->header;
        vertical_points.clear();
        ground_points.clear();

	groundSeparate(msg, vertical_points, ground_points);

	if (!floor_removal)
	{
		vertical_points = *msg;
	}

	vertical_points_pub.publish(vertical_points);
        ground_points_pub.publish(ground_points);

	t2 = boost::chrono::high_resolution_clock::now();
        elap_time = (boost::chrono::duration_cast<boost::chrono::nanoseconds>(t2-t1));
        std::cout << "Computational time for each frame is " << elap_time << " for total " << remaining_point << " points" << std::endl;
        //std::cout << "Original point is " << original_point << " The remaining point is " << remaining_point <<
	//" Lost point is " << original_point - remaining_point << " Point after transform " << point_after_tf<<
	//" Real missing point is " << point_after_tf - remaining_point  << std::endl;
}

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

        ros::init(argc, argv, "ground_filter");
	GroundFilter node;
        ros::spin();

	return 0;

}


#include <stan/math/prim/mat.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>

using Eigen::Dynamic;
using Eigen::Matrix;
using std::vector;

TEST(ProbDistributionsMultiNormal,NotVectorized) {
  Matrix<double,Dynamic,1> y(3,1);
  y << 2.0, -2.0, 11.0;
  Matrix<double,Dynamic,1> mu(3,1);
  mu << 1.0, -1.0, 3.0;
  Matrix<double,Dynamic,Dynamic> Sigma(3,3);
  Sigma << 9.0, -3.0, 0.0,
    -3.0,  4.0, 0.0,
    0.0, 0.0, 5.0;
  EXPECT_FLOAT_EQ(-11.73908, stan::math::multi_normal_log(y,mu,Sigma));
}

TEST(ProbDistributionsMultiNormal,Vectorized) {
  vector< Matrix<double,Dynamic,1> > vec_y(2);
  vector< Matrix<double,1,Dynamic> > vec_y_t(2);
  Matrix<double,Dynamic,1> y(3);
  Matrix<double,1,Dynamic> y_t(3);
  y << 2.0, -2.0, 11.0;
  vec_y[0] = y;
  vec_y_t[0] = y;
  y << 4.0, -2.0, 1.0;
  vec_y[1] = y;
  vec_y_t[1] = y;
  y_t = y;

  vector< Matrix<double,Dynamic,1> > vec_mu(2);
  vector< Matrix<double,1,Dynamic> > vec_mu_t(2);
  Matrix<double,Dynamic,1> mu(3);
  Matrix<double,1,Dynamic> mu_t(3);
  mu << 1.0, -1.0, 3.0;
  vec_mu[0] = mu;
  vec_mu_t[0] = mu;
  mu << 2.0, -1.0, 4.0;
  vec_mu[1] = mu;
  vec_mu_t[1] = mu;
  mu_t = mu;

  Matrix<double,Dynamic,Dynamic> Sigma(3,3);
  Sigma << 10.0, -3.0, 0.0,
    -3.0,  5.0, 0.0,
    0.0, 0.0, 5.0;

  //y and mu vectorized
  EXPECT_FLOAT_EQ(-11.928077-6.5378327, stan::math::multi_normal_log(vec_y,vec_mu,Sigma));
  EXPECT_FLOAT_EQ(-11.928077-6.5378327, stan::math::multi_normal_log(vec_y_t,vec_mu,Sigma));
  EXPECT_FLOAT_EQ(-11.928077-6.5378327, stan::math::multi_normal_log(vec_y,vec_mu_t,Sigma));
  EXPECT_FLOAT_EQ(-11.928077-6.5378327, stan::math::multi_normal_log(vec_y_t,vec_mu_t,Sigma));

  //y vectorized
  EXPECT_FLOAT_EQ(-10.44027-6.537833, stan::math::multi_normal_log(vec_y,mu,Sigma));
  EXPECT_FLOAT_EQ(-10.44027-6.537833, stan::math::multi_normal_log(vec_y_t,mu,Sigma));
  EXPECT_FLOAT_EQ(-10.44027-6.537833, stan::math::multi_normal_log(vec_y,mu_t,Sigma));
  EXPECT_FLOAT_EQ(-10.44027-6.537833, stan::math::multi_normal_log(vec_y_t,mu_t,Sigma));

  //mu vectorized
  EXPECT_FLOAT_EQ(-6.26954-6.537833, stan::math::multi_normal_log(y,vec_mu,Sigma));
  EXPECT_FLOAT_EQ(-6.26954-6.537833, stan::math::multi_normal_log(y_t,vec_mu,Sigma));
  EXPECT_FLOAT_EQ(-6.26954-6.537833, stan::math::multi_normal_log(y,vec_mu_t,Sigma));
  EXPECT_FLOAT_EQ(-6.26954-6.537833, stan::math::multi_normal_log(y_t,vec_mu_t,Sigma));
}
TEST(ProbDistributionsMultiNormal,Sigma) {
  Matrix<double,Dynamic,1> y(2,1);
  y << 2.0, -2.0;
  Matrix<double,Dynamic,1> mu(2,1);
  mu << 1.0, -1.0;
  Matrix<double,Dynamic,Dynamic> Sigma(2,2);
  Sigma << 9.0, -3.0, -3.0, 4.0;
  EXPECT_NO_THROW (stan::math::multi_normal_log(y, mu, Sigma));

  // non-symmetric
  Sigma(0, 1) = -2.5;
  EXPECT_THROW (stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
}
TEST(ProbDistributionsMultiNormal,Mu) {
  Matrix<double,Dynamic,1> y(3,1);
  y << 2.0, -2.0, 11.0;
  Matrix<double,Dynamic,1> mu(3,1);
  mu << 1.0, -1.0, 3.0;
  Matrix<double,Dynamic,Dynamic> Sigma(3,3);
  Sigma << 9.0, -3.0, 0.0,
    -3.0,  4.0, 0.0,
    0.0, 0.0, 5.0;
  EXPECT_NO_THROW (stan::math::multi_normal_log(y, mu, Sigma));

  mu(0) = std::numeric_limits<double>::infinity();
  EXPECT_THROW (stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
  mu(0) = -std::numeric_limits<double>::infinity();
  EXPECT_THROW (stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
  mu(0) = std::numeric_limits<double>::quiet_NaN();
  EXPECT_THROW (stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
}
TEST(ProbDistributionsMultiNormal,MultiNormalOneRow) {
  Matrix<double,1,Dynamic> y(1,3);
  y << 2.0, -2.0, 11.0;
  Matrix<double,Dynamic,1> mu(3,1);
  mu << 1.0, -1.0, 3.0;
  Matrix<double,Dynamic,Dynamic> Sigma(3,3);
  Sigma << 9.0, -3.0, 0.0,
    -3.0,  4.0, 0.0,
    0.0, 0.0, 5.0;
  EXPECT_FLOAT_EQ(-11.73908, stan::math::multi_normal_log(y,mu,Sigma));
}

TEST(ProbDistributionsMultiNormal,SigmaMultiRow) {
  Matrix<double,1,Dynamic> y(1,2);
  y << 2.0, -2.0;
  Matrix<double,Dynamic,1> mu(2,1);
  mu << 1.0, -1.0;
  Matrix<double,Dynamic,Dynamic> Sigma(2,2);
  Sigma << 9.0, -3.0, -3.0, 4.0;
  EXPECT_NO_THROW (stan::math::multi_normal_log(y, mu, Sigma));

  // non-symmetric
  Sigma(0, 1) = -2.5;
  EXPECT_THROW (stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
  Matrix<double,Dynamic,1> z(2,1);

  // wrong dimensions
  z << 2.0, -2.0;
  EXPECT_THROW (stan::math::multi_normal_log(z, mu, Sigma), std::domain_error);
}
TEST(ProbDistributionsMultiNormal,MuMultiRow) {
  Matrix<double,1,Dynamic> y(1,3);
  y << 2.0, -2.0, 11.0;
  Matrix<double,Dynamic,1> mu(3,1);
  mu << 1.0, -1.0, 3.0;
  Matrix<double,Dynamic,Dynamic> Sigma(3,3);
  Sigma << 9.0, -3.0, 0.0,
    -3.0,  4.0, 0.0,
    0.0, 0.0, 5.0;
  EXPECT_NO_THROW (stan::math::multi_normal_log(y, mu, Sigma));

  mu(0) = std::numeric_limits<double>::infinity();
  EXPECT_THROW (stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
  mu(0) = -std::numeric_limits<double>::infinity();
  EXPECT_THROW (stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
  mu(0) = std::numeric_limits<double>::quiet_NaN();
  EXPECT_THROW (stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
}
TEST(ProbDistributionsMultiNormal,SizeMismatch) {
  Matrix<double,1,Dynamic> y(1,3);
  y << 2.0, -2.0, 11.0;
  Matrix<double,Dynamic,1> mu(2,1);
  mu << 1.0, -1.0;
  Matrix<double,Dynamic,Dynamic> Sigma(2,3);
  Sigma << 9.0, -3.0, 0.0,
    -3.0,  4.0, 0.0;
  EXPECT_THROW(stan::math::multi_normal_log(y, mu, Sigma), std::invalid_argument);
}

TEST(ProbDistributionsMultiNormal, error_check) {
  boost::random::mt19937 rng;
  Matrix<double,Dynamic,1> mu(3,1);
  mu << 2.0,
    -2.0,
    11.0;

  Matrix<double,Dynamic,Dynamic> sigma(3,3);
  sigma << 9.0, -3.0, 0.0,
    -3.0,  4.0, 1.0,
    0.0, 1.0, 3.0;
  EXPECT_NO_THROW(stan::math::multi_normal_rng(mu, sigma,rng));

  mu << stan::math::positive_infinity(),
    -2.0,
    11.0;
  EXPECT_THROW(stan::math::multi_normal_rng(mu,sigma,rng),std::domain_error);

  mu << 2.0,
    -2.0,
    11.0;
  sigma << 9.0, -3.0, 0.0,
    3.0,  4.0, 0.0,
    -2.0, 1.0, 3.0;
  EXPECT_THROW(stan::math::multi_normal_rng(mu,sigma,rng),std::domain_error);

}

TEST(ProbDistributionsMultiNormal, marginalOneChiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  Matrix<double,Dynamic,Dynamic> sigma(3,3);
  sigma << 9.0, -3.0, 0.0,
    -3.0,  4.0, 1.0,
    0.0, 1.0, 3.0;
  Matrix<double,Dynamic,1> mu(3,1);
  mu << 2.0,
    -2.0,
    11.0;
  int N = 10000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::normal_distribution<>dist (2.0,3.0);
  boost::math::chi_squared mydist(K-1);

  double loc[K - 1];
  for(int i = 1; i < K; i++)
    loc[i - 1] = quantile(dist, i * std::pow(K, -1.0));

  int count = 0;
  int bin [K];
  double expect [K];
  for(int i = 0 ; i < K; i++) {
    bin[i] = 0;
    expect[i] = N / K;
  }
  Eigen::VectorXd a(mu.rows());
  while (count < N) {
    a = stan::math::multi_normal_rng(mu,sigma,rng);
    int i = 0;
    while (i < K-1 && a(0) > loc[i])
      ++i;
    ++bin[i];
    count++;
   }

  double chi = 0;
  for(int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsMultiNormal, marginalTwoChiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  Matrix<double,Dynamic,Dynamic> sigma(3,3);
  sigma << 9.0, -3.0, 0.0,
    -3.0,  4.0, 1.0,
    0.0, 1.0, 3.0;
  Matrix<double,Dynamic,1> mu(3,1);
  mu << 2.0,
    -2.0,
    11.0;
  int N = 10000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::normal_distribution<>dist (-2.0,2.0);
  boost::math::chi_squared mydist(K-1);

  double loc[K - 1];
  for(int i = 1; i < K; i++)
    loc[i - 1] = quantile(dist, i * std::pow(K, -1.0));

  int count = 0;
  int bin [K];
  double expect [K];
  for(int i = 0 ; i < K; i++) {
    bin[i] = 0;
    expect[i] = N / K;
  }
  Eigen::VectorXd a(mu.rows());
  while (count < N) {
    a = stan::math::multi_normal_rng(mu,sigma,rng);
    int i = 0;
    while (i < K-1 && a(1) > loc[i])
      ++i;
    ++bin[i];
    count++;
   }

  double chi = 0;
  for(int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsMultiNormal, marginalThreeChiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  Matrix<double,Dynamic,Dynamic> sigma(3,3);
  sigma << 9.0, -3.0, 0.0,
    -3.0,  4.0, 1.0,
    0.0, 1.0, 16.0;
  Matrix<double,Dynamic,1> mu(3,1);
  mu << 2.0,
    -2.0,
    11.0;
  int N = 10000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::normal_distribution<>dist (11.0,4.0);
  boost::math::chi_squared mydist(K-1);

  double loc[K - 1];
  for(int i = 1; i < K; i++)
    loc[i - 1] = quantile(dist, i * std::pow(K, -1.0));

  int count = 0;
  int bin [K];
  double expect [K];
  for(int i = 0 ; i < K; i++) {
    bin[i] = 0;
    expect[i] = N / K;
  }
  Eigen::VectorXd a(mu.rows());
  while (count < N) {
    a = stan::math::multi_normal_rng(mu,sigma,rng);
    int i = 0;
    while (i < K-1 && a(2) > loc[i])
      ++i;
    ++bin[i];
    count++;
   }

  double chi = 0;
  for(int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(multiNormalRng, nonPosDefErrorTest) {
  using stan::math::multi_normal_rng;
  Eigen::MatrixXd S(2,2);
  S << 0, 1, 1, 0;  // not pos definite
  Eigen::VectorXd mu(2);
  mu << 1, 2;
  boost::random::mt19937 rng;
  EXPECT_THROW(multi_normal_rng(mu, S, rng), std::domain_error);
}


#include <Eigen/Dense>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include "principal_curvatures_can.hpp"

#define R 2
#define r 1
#define N 10000
#define THETA_SCALE (M_PI / 2)
#define THETA_OFFSET (M_PI / 2)
#define PHI_SCALE (M_PI / 4)
#define SEARCH_RADIUS 0.1

int main(void) {
  Eigen::VectorXf theta = THETA_SCALE * Eigen::VectorXf::Random(N) + Eigen::VectorXf::Constant(N, THETA_OFFSET);
  Eigen::VectorXf phi = PHI_SCALE * Eigen::VectorXf::Random(N);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>());
  for(int i=0;i<N;i++) {
    pcl::PointXYZ point;
    point.x = (R + r * cos(theta(i))) * cos(phi(i));
    point.y = (R + r * cos(theta(i))) * sin(phi(i));
    point.z = r * sin(theta(i));
    cloud->points.push_back(point);
  }
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
  normal_estimation.setInputCloud(cloud);
  normal_estimation.setSearchMethod(tree);
  normal_estimation.setRadiusSearch(SEARCH_RADIUS);
  normal_estimation.setViewPoint(0, 0, std::numeric_limits<float>::infinity());
  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>());
  normal_estimation.compute(*normals);
  PrincipalCurvaturesEstimationCAN curvature_estimation;
  curvature_estimation.setInputCloud(cloud);
  curvature_estimation.setInputNormals(normals);
  curvature_estimation.setSearchMethod(tree);
  curvature_estimation.setRadiusSearch(SEARCH_RADIUS);
  pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr curvatures (new pcl::PointCloud<pcl::PrincipalCurvatures>());
  curvature_estimation.compute(*curvatures);
  for(int i=0;i<N;i++) {
    pcl::PrincipalCurvatures curve_point = curvatures.get()->points[i];
    std::cout
      << theta(i) << " "
      << phi(i) << " "
      << curve_point.pc1 * curve_point.pc2 << " "
      << cos(theta(i)) / (r * (R + r * cos(theta(i))))
      << std::endl;
  }
}


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

// For IO
#include <stdlib.h>
#include <math.h>
#include <cmath>
#include <iostream>
#include <unistd.h>

// For measuring elapsed time
#include <chrono>

// For Random Float Generator
#include <time.h>

// For object under test
#include "nyquist-modulator.h"
#include "fftw3.h"

#define DIFFERENCE_THRESHOLD 0.0001

/**
* Test NYQUIST MODULATOR
*
*/
BOOST_AUTO_TEST_SUITE(NYQUIST_MODULATOR)

/**
* Generate random data (floats) Put it throguh IFFT.
* Copy the time domain samples into FFT input buffer.
* Execute and check the input and output are within a
* threshold value.
*
*/
BOOST_AUTO_TEST_CASE(ModToDemod)
{
    printf("\nTesting Modulation to Demodulation...\n");
    printf("\nMdoulator:\n");

    uint32_t nPoints = 512;
    uint32_t symbolSize = nPoints*2;
    //uint32_t prefixSize = 128;

    // Setup random float generator
    srand( (unsigned)time( NULL ) );

    DoubleVec ifftOutput;
    ifftOutput.resize(symbolSize);

    DoubleVec modulatorOutput;
    modulatorOutput.resize(symbolSize);

    DoubleVec rxSignal;
    rxSignal.resize(symbolSize*10);

    uint32_t symbolStart = rand() % ((nPoints*2)*9);
    printf("Random Symbol Start = %d\n",symbolStart);

    fftw_complex *demodulatorOutput;
    demodulatorOutput = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * nPoints);

    // Popilate ifft output double vec with random values
    for (size_t i = 0; i < symbolSize; i++)
    {
        ifftOutput[i] = (double) rand()/RAND_MAX;
    }

    std::copy(ifftOutput.begin(), ifftOutput.begin()+symbolSize, modulatorOutput.begin());

    NyquistModulator modulator(nPoints, (fftw_complex *) &modulatorOutput);
    NyquistModulator demodulator(nPoints, demodulatorOutput);

    // Measure wall time of the modulator execution.
    auto start = std::chrono::steady_clock::now();
    // Modulate data, set prefix size to 0 as it is not used in this test
    modulator.Modulate(modulatorOutput, 0);
    auto end = std::chrono::steady_clock::now();

    std::cout << "Digital quadrature modulator elapsed time: "
        << std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count()
        << " ns" << std::endl;

    printf("\nDemodulator:\n");

    // basically it works like this:
    //std::copy( src, src + size, dest );
    std::copy( modulatorOutput.begin(), modulatorOutput.end(), rxSignal.begin()+symbolStart);

    // Measure wall time of the fft execution.
    start = std::chrono::steady_clock::now();
    demodulator.Demodulate(rxSignal, symbolStart);
    end = std::chrono::steady_clock::now();

    std::cout << "Digital quadrature demodulator elapsed time: "
        << std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count()
        << " ns" << std::endl;

    // Print input and output buffers
    for (size_t i = 0; i < nPoints; i++)
    {

        //printf("Real Sample: %lu %+9.5f Input to Modulator vs. %+9.5f Output of Demodulator\n",
        //i, ifftOutput[(i*2)], demodulatorOutput[i][0]);

        // Check if real and complex element match within defined precision.
        BOOST_CHECK_MESSAGE(
        (std::abs( ifftOutput[(i*2)] -  demodulatorOutput[i][0] ) <= DIFFERENCE_THRESHOLD ),
        "Values vary more than threshold! - Occured at index: " << i );

        //printf("Imag Sample: %lu %+9.5f Input to Modulator vs. %+9.5f Output of Demodulator\n",
        //i, ifftOutput[(i*2)+1], demodulatorOutput[i][1]);

        // Check if real and complex element match within defined precision.
        BOOST_CHECK_MESSAGE(
        (std::abs( ifftOutput[(i*2)+1] - demodulatorOutput[i][1] ) <= DIFFERENCE_THRESHOLD ),
        "Values vary more than threshold! - Occured at index: " << i );

    }
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @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_COMMON_DETAIL_GENERIC_POW_EXPANDER_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_GENERIC_POW_EXPANDER_HPP_INCLUDED

#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/sqr.hpp>
namespace boost { namespace simd { namespace ext
{

  template<std::uintmax_t Exp, std::uintmax_t Odd = Exp%2>
  struct pow_expander;

  template<std::uintmax_t Exp>
  struct pow_expander<Exp, 0ULL>
  {
    template<class A0>
    static BOOST_FORCEINLINE A0 call( A0 const& a0) BOOST_NOEXCEPT
    {
      return pow_expander<Exp/2>::call(sqr(a0));
    }
  };

  template<std::uintmax_t Exp>
  struct pow_expander<Exp, 1ULL>
  {
    template<class A0>
    static BOOST_FORCEINLINE A0 call( A0 const& a0) BOOST_NOEXCEPT
    {
      return a0*pow_expander<Exp/2>::call(sqr(a0));
    }
  };

  template<>
  struct pow_expander<0ULL, 0ULL>
  {
    template<class A0>
    static BOOST_FORCEINLINE A0 call( A0 const&) BOOST_NOEXCEPT
    {
      return One<A0>();
    }
  };

  template<>
  struct pow_expander<0ULL, 1ULL>
  {
    template<class A0>
    static BOOST_FORCEINLINE A0 call( A0 const&) BOOST_NOEXCEPT
    {
      return One<A0>();
    }
  };
} } }

#endif


/*
 * dcdc.cc
 *
 *  created on: 18.04.2016
 *      author: rungger
 */

/*
 * demonstration of the boost ode solver usage
 *
 */

#include <array>
#include <iostream>

#include "cuddObj.hh"

#include "SymbolicSet.hh"
#include "SymbolicModelGrowthBound.hh"

#include "TicToc.hh"
#include <boost/numeric/odeint.hpp>

/* state space dim */
#define sDIM 2
#define iDIM 1

/* data types for the ode solver */
typedef std::array<double,2> state_type;
typedef std::array<double,1> input_type;

typedef boost::numeric::odeint::runge_kutta_dopri5< state_type > stepper_type;

/* we integrate the dcdc ode by 0.5 sec (the result is stored in x)  */
auto  dcdc_post = [](state_type &x, input_type &u) -> void {

  /* the ode describing the system */
  auto  system_ode = [&](const state_type &x, state_type &dxdt, const double) -> void {

    const double r0=1.0 ;
    const double vs = 1.0 ;
    const double rl = 0.05 ;
    const double rc = rl / 10 ;
    const double xl = 3.0 ;
    const double xc = 70.0 ;

    const double b[2]={vs/xl, 0};

    double a[2][2];
    if(u[0]==1) {
      a[0][0] = -rl / xl;
      a[0][1] = 0;
      a[1][0] = 0;
      a[1][1] = (-1 / xc) * (1 / (r0 + rc));
    } else {
      a[0][0] = (-1 / xl) * (rl + ((r0 * rc) / (r0 + rc))) ;
      a[0][1] =  ((-1 / xl) * (r0 / (r0 + rc))) / 5 ;
      a[1][0] = 5 * (r0 / (r0 + rc)) * (1 / xc);
      a[1][1] =(-1 / xc) * (1 / (r0 + rc)) ;
    }

    dxdt[0] = a[0][0]*x[0]+a[0][1]*x[1] + b[0];
    dxdt[1] = a[1][0]*x[0]+a[1][1]*x[1] + b[1];

  };
  boost::numeric::odeint::integrate_adaptive(make_controlled(1E-12 ,1E-12,stepper_type()),system_ode,x,0.0,0.5,0.5);
};

/* computation of the growth bound (the result is stored in r)  */
auto radius_post = [](state_type &r, input_type &u) -> void {

  /* the ode to determine the radius of the cell which over-approximates the
   * attainable set see: http://arxiv.org/abs/1503.03715v1 */
  auto growth_bound_ode = [&](const state_type &r, state_type &drdt, const double t) {
    /* for the dcdc boost converter the growth bound is simply given by the metzler matrix of the system matrices */
    const double r0=1.0 ;
    const double rl = 0.05 ;
    const double rc = rl / 10 ;
    const double xl = 3.0 ;
    const double xc = 70.0 ;

    double a[2][2];
    if(u[0]==1) {
      a[0][0] = -rl / xl;
      a[0][1] = 0;
      a[1][0] = 0;
      a[1][1] = (-1 / xc) * (1 / (r0 + rc));
    } else {
      a[0][0] = (-1 / xl) * (rl + ((r0 * rc) / (r0 + rc))) ;
      a[0][1] =  ((1 / xl) * (r0 / (r0 + rc))) / 5 ;
      a[1][0] = 5 * (r0 / (r0 + rc)) * (1 / xc);
      a[1][1] =(-1 / xc) * (1 / (r0 + rc)) ;
    }

    drdt[0] = a[0][0]*r[0]+a[0][1]*r[1];
    drdt[1] = a[1][0]*r[0]+a[1][1]*r[1];
  };

  boost::numeric::odeint::integrate_adaptive(make_controlled(1E-12,1E-12,stepper_type()),growth_bound_ode,r,0.0,0.5,0.5);
};

int main() {
  /* to measure time */
  TicToc tt;
  /* there is one unique manager to organize the bdd variables */
  Cudd mgr;

  /****************************************************************************/
  /* construct SymbolicSet for the state space */
  /****************************************************************************/
  /* setup the workspace of the synthesis problem and the uniform grid */
  /* lower bounds of the hyper rectangle */
  double lb[sDIM]={1.15,5.45};
  /* upper bounds of the hyper rectangle */
  double ub[sDIM]={1.55,5.85};
  /* grid node distance diameter */
  double eta[sDIM]={2/4e3,2/4e3};
  scots::SymbolicSet ss(mgr,sDIM,lb,ub,eta);
  ss.addGridPoints();

  /****************************************************************************/
  /* construct SymbolicSet for the input space */
  /****************************************************************************/
  double ilb[iDIM]={1};
  double iub[iDIM]={2};
  double ieta[iDIM]={1};
  scots::SymbolicSet is(mgr,iDIM,ilb,iub,ieta);
  is.addGridPoints();

  /****************************************************************************/
  /* setup class for symbolic model computation */
  /****************************************************************************/
  /* create SymbolicSet for the post domain postX in preX x U x postX
   * by coping preX and assigning new BDD IDs */
  scots::SymbolicSet sspost(ss,1);
  scots::SymbolicModelGrowthBound<state_type,input_type> abs(&ss, &is, &sspost);
  /* compute the transition relation */
  tt.tic();
  abs.computeTransitionRelation(dcdc_post, radius_post);
  std::cout << std::endl;
  tt.toc();
  /* get the number of elements in the transition relation */
  std::cout << std::endl << "Number of elements in the transition relation: " << abs.getSize() << std::endl;

  /* get SymbolicSet containing the transition relation with domain X x U x X */
  scots::SymbolicSet tr=abs.getTransitionRelation();
  /* write SymbolicSet to file */
  tr.writeToFile("dcdc_abs.bdd");

  /* read SymbolicSet containing the transition relation from file */
  scots::SymbolicSet trset(mgr,"dcdc_abs.bdd");

  return 1;
}


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

// Equations and logic parts of this code was based on ;
// https://github.com/MPC-Berkeley/genesis_path_follower
// Also refer to;
// https://github.com/MPC-Berkeley/barc/wiki/Car-Model

#ifndef VOX_NAV_CONTROL__MPC_CONTROLLER__MPC_CONTROLLER_CORE_HPP_
#define VOX_NAV_CONTROL__MPC_CONTROLLER__MPC_CONTROLLER_CORE_HPP_

#include <casadi/casadi.hpp>

#include <vector>
#include <memory>
#include <chrono>
#include <Eigen/Eigen>
#include <vox_nav_control/common.hpp>

namespace vox_nav_control
{
  namespace mpc_controller_casadi
  {

/**
 * @brief CASADI based MOdel Predcitive Control for Ackermann Vehicle
 *
 */
    class MPCControllerCasadiCore
    {
    public:
      /**
       * @brief structure for slack variables, these are used to relax input rate constraints
       *
       */
      struct SlackVars
      {
        double sl_acc;
        double sl_df;
        SlackVars()
        : sl_acc(0.0),
          sl_df(0.0) {}
      };

      /**
       * @brief result is passed as this struct
       *
       */
      struct SolutionResult
      {
        // whether the found solution was optimal
        bool is_optimal;
        // Execution ime took by solver in milliseconds
        int solve_time_ms;
        // computed control input as result of optimal control
        vox_nav_control::common::ControlInput control_input;
        // actual states that are going to be achieved in the time horizon
        std::vector<vox_nav_control::common::States> actual_computed_states;
        SolutionResult()
        : is_optimal(false),
          solve_time_ms(0)
        {}
      };

      /**
       * @brief Construct a new MPCControllerCasadiCore object
       *
       * @param params
       */
      MPCControllerCasadiCore(vox_nav_control::common::Parameters params);

      /**
       * @brief Destroy the MPCControllerCasadiCore object
       *
       */
      ~MPCControllerCasadiCore();

      /**
       * @brief state dynamcs constraints, control input
       * constraints and boundries are defined through this function
       *
       */
      void addConstraints();

      /**
       * @brief objective function and cost is defined with this function
       *
       */
      void addCost();

      /**
       * @brief update the current states
       *
       * @param curr_states
       */
      void updateCurrentStates(vox_nav_control::common::States curr_states);

      /**
       * @brief update current refernce states in horizon
       *
       * @param reference_states
       */
      void updateReferences(std::vector<vox_nav_control::common::States> reference_states);

      /**
       * @brief update previous control input
       *
       * @param previous_control_input
       */
      void updatePreviousControlInput(vox_nav_control::common::ControlInput previous_control_input);

      /**
       * @brief initialize slack variables, mostly just equalizing them to zero
       *
       * @param initial_slack_vars
       */
      void initializeSlackVars(std::vector<SlackVars> initial_slack_vars);

      /**
       * @brief  initialize actual states, mostly just equalizing them to zero.
       * These cannot be updated
       * they are variables of optimal control problem itself
       *
       * @param initial_actual_states
       */
      void initializeActualStates(
        std::vector<vox_nav_control::common::States> initial_actual_states);

      /**
     * @brief  initialize control inputs, mostly just equalizing them to zero. These cannot be updated
     *  they are variables of optimal control problem itself
     *
     * @param initial_actual_control_inputs
     */
      void initializeActualControlInputs(
        std::vector<vox_nav_control::common::ControlInput> initial_actual_control_inputs);

      /**
       * @brief solve the actual optiml control problem
       *
       * @return SolutionResult, includes resulting control inputs
       * as well as optimal control variables; computed actual states
       */
      SolutionResult solve(const std::vector<vox_nav_control::common::Ellipsoid> & obstacles);

    private:
      std::shared_ptr<casadi::Opti> opti_;
      // used to slice casadi matrixes
      casadi::Slice slice_all_;
      casadi::Matrix<double> Q;
      casadi::Matrix<double> R;

      casadi::MX u_prev_; // previous input: [u_{acc, -1}, u_{df, -1}]
      casadi::MX z_curr_; // current state:  [x_0, y_0, psi_0, v_0]

      // reference traj that we would like to follow
      // First index corresponds to our desired state at timestep k+1:
      // i.e. z_ref_[0,:] = z_{desired, 1}.
      // second index selects the state element from [x_k, y_k, psi_k, v_k].
      casadi::MX z_ref_;
      casadi::MX x_ref_;
      casadi::MX y_ref_;
      casadi::MX psi_ref_;
      casadi::MX v_ref_;

      // Actual trajectory we will follow given the optimal solution.
      // First index is the timestep k, i.e. self.z_dv[0,:] is z_0.
      // It has N+1 timesteps since we go from z_0, ..., z_N.
      // Second index is the state element, as detailed below.
      casadi::MX z_dv_;
      casadi::MX x_dv_;
      casadi::MX y_dv_;
      casadi::MX psi_dv_;
      casadi::MX v_dv_;

      // Control inputs used to achieve z_dv according to dynamics.
      // First index is the timestep k, i.e. self.u_dv[0,:] is u_0.
      // Second index is the input element as detailed below.
      casadi::MX u_dv_;
      casadi::MX acc_dv_;
      casadi::MX df_dv_;

      // Slack variables used to relax input rate constraints.
      casadi::MX sl_dv_;
      casadi::MX sl_acc_dv_;
      casadi::MX sl_df_dv_;

      // this will be updated through the constructor.
      vox_nav_control::common::Parameters params_;

      casadi::MX z_static_obs_;
      casadi::MX x_static_obs_;
      casadi::MX y_static_obs_;
      casadi::MX a_static_obs_;
      casadi::MX b_static_obs_;

      casadi::MX z_dynamic_obs_;
      casadi::MX x_dynamic_obs_;
      casadi::MX y_dynamic_obs_;
      casadi::MX a_dynamic_obs_;
      casadi::MX b_dynamic_obs_;
      casadi::MX yaw_dynamic_obs_;

    };
  } // namespace mpc_controller_casadi
}  // namespace vox_nav_control

#endif  // VOX_NAV_CONTROL__MPC_CONTROLLER__MPC_CONTROLLER_CORE_HPP_


#include "incidencematrices.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <array>
#include <memory>

namespace IncidenceMatrices {

/** @brief Create the mesh consisting of a triangle and quadrilateral
 *         from the exercise sheet.
 * @return Shared pointer to the hybrid2d mesh.
 */
std::shared_ptr<lf::mesh::Mesh> createDemoMesh() {
  // builder for a hybrid mesh in a world of dimension 2
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // Add points
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 0});    // (0)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 0});    // (1)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 1});    // (2)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 1});    // (3)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0.5, 1});  // (4)

  // Add the triangle
  // First set the coordinates of its nodes:
  Eigen::MatrixXd nodesOfTria(2, 3);
  nodesOfTria << 1, 1, 0.5, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kTria(),  // we want a triangle
      std::array<lf::mesh::Mesh::size_type, 3>{
          {1, 2, 4}},  // indices of the nodes
      std::make_unique<lf::geometry::TriaO1>(nodesOfTria));  // node coords

  // Add the quadrilateral
  Eigen::MatrixXd nodesOfQuad(2, 4);
  nodesOfQuad << 0, 1, 0.5, 0, 0, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kQuad(),
      std::array<lf::mesh::Mesh::size_type, 4>{{0, 1, 4, 3}},
      std::make_unique<lf::geometry::QuadO1>(nodesOfQuad));

  std::shared_ptr<lf::mesh::Mesh> demoMesh_p = mesh_factory_ptr->Build();

  return demoMesh_p;
}

/** @brief Compute the edge-vertex incidence matrix G for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The edge-vertex incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<int> computeEdgeVertexIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store edge-vertex incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> G;

  //====================

  // Get number of edges: codim = 1
  // Get number of nodes: codim = 0
  const lf::mesh::Mesh::size_type numEdges = mesh.NumEntities(1),
                                  numNodes = mesh.NumEntities(2);

  // We know, G has exactly 2 non-zero entries per row.
  G = Eigen::SparseMatrix<int, Eigen::RowMajor> (numEdges, numNodes);
  G.reserve(Eigen::VectorXi::Constant(numEdges, 2)); //size=numEdges, value=2

  // 1. Iterate over alledges
  // 2. Check index of nodes which are endpoints of the edges
  // ! We cannot iterate ver vertices. LehrFem++ does not allow to visit !
  // ! edges adjacent to a vertex.                                       !

  for ( const lf::mesh::Entity *edge : mesh.Entities(1) ) {
    // Get index of this edge
    lf::mesh::Mesh::size_type edgeIdx = mesh.Index(*edge);

    // Get nodes and their indices.
    // ! Now codim(nodes)=1 - because it's a relative codim !
    // ! Seen from the edge, a node has codim 1 !

    auto nodes = edge->SubEntities(1); // ! Relativ Codim !
    lf::mesh::Mesh::size_type firstNodeIdx = mesh.Index(*nodes[0]);
    lf::mesh::Mesh::size_type lastNodeIdx = mesh.Index(*nodes[1]);

    // Add matrix entries to G
    G.coeffRef(edgeIdx, firstNodeIdx) = 1.0;
    G.coeffRef(edgeIdx, lastNodeIdx) = -1.0;
  }

  //====================

  return G;
}
/* SAM_LISTING_END_1 */

/** @brief Compute the cell-edge incidence matrix D for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The cell-edge incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::SparseMatrix<int> computeCellEdgeIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store cell-edge incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> D;

  //====================

  // Get number of cells and edges
  const lf::mesh::Mesh::size_type numCells = mesh.NumEntities(0),
                                  numEdges = mesh.NumEntities(1);

  // Sparse init. of D. D has at most 4 nnz-entries per row.
  D = Eigen::SparseMatrix<int, Eigen::RowMajor> (numCells, numEdges);
  D.reserve( Eigen::VectorXi::Constant(numCells, 4) );

  // Loop over all cells.
  for ( const lf::mesh::Entity *cell : mesh.Entities(0) ) {
    // Get cell index
    lf::mesh::Mesh::size_type cellIdx = mesh.Index(*cell);

    // Get edges of current cell
    auto edges = cell->SubEntities(1);

    // Check orientation of all cells
    auto edgeOrientations = cell->RelativeOrientations();

    // Iterate over both and add to D
    auto edgeIt = edges.begin();
    auto orntIt = edgeOrientations.begin();

    // Fill D by comparing the orientation of the edges of the current cell
    for(; edgeIt != edges.end() && orntIt != edgeOrientations.end();
        ++edgeIt, ++orntIt) {
      lf::mesh::Mesh::size_type edgeIdx = mesh.Index(**edgeIt);
      D.coeffRef(cellIdx, edgeIdx) += lf::mesh::to_sign(*orntIt);
    }
  }

  //====================

  return D;
}
/* SAM_LISTING_END_2 */

/** @brief For a given mesh test if the product of cell-edge and edge-vertex
 *        incidence matrix is zero: D*G == 0?
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *             such as lf::mesh::hybrid2d::Mesh)
 * @return true, if the product is zero and false otherwise
 */
/* SAM_LISTING_BEGIN_3 */
bool testZeroIncidenceMatrixProduct(const lf::mesh::Mesh &mesh) {
  bool isZero = false;

  //====================
  Eigen::SparseMatrix<int> G = computeEdgeVertexIncidenceMatrix(mesh);
  Eigen::SparseMatrix<int> D = computeCellEdgeIncidenceMatrix(mesh);

  isZero = ( (D*G).norm() == 0);

  //====================
  return isZero;
}
/* SAM_LISTING_END_3 */

}  // namespace IncidenceMatrices


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

// Copyright (c) 2015, Oracle and/or its affiliates

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

//[is_valid_failure
//` Checks whether a geometry is valid and, if not valid, checks if it could be fixed by bg::correct; if so bg::correct is called on the geometry

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
/*<-*/ #include "create_svg_one.hpp" /*->*/

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point_type;
    typedef boost::geometry::model::polygon<point_type> polygon_type;

    polygon_type poly;
    boost::geometry::read_wkt("POLYGON((0 0,0 10,10 10,10 0),(0 0,9 2,9 1,0 0),(0 0,2 9,1 9,0 0))", poly);

    std::cout << "original geometry: " << boost::geometry::dsv(poly) << std::endl;
    boost::geometry::validity_failure_type failure;
    bool valid = boost::geometry::is_valid(poly, failure);

    // if the invalidity is only due to lack of closing points and/or wrongly oriented rings, then bg::correct can fix it
    bool could_be_fixed = (failure == boost::geometry::failure_not_closed
                           || boost::geometry::failure_wrong_orientation);
    std::cout << "is valid? " << (valid ? "yes" : "no") << std::endl;
    if (! valid)
    {
        std::cout << "can boost::geometry::correct remedy invalidity? " << (could_be_fixed ? "possibly yes" : "no") << std::endl;
        if (could_be_fixed)
        {
            boost::geometry::correct(poly);
            std::cout << "after correction: " << (boost::geometry::is_valid(poly) ? "valid" : "still invalid") << std::endl;
            std::cout << "corrected geometry: " << boost::geometry::dsv(poly) << std::endl;
        }
    }
    /*<-*/ create_svg("is_valid_failure_example.svg", poly); /*->*/
    return 0;
}

//]

//[is_valid_failure_output
/*`
Output:
[pre
original geometry: (((0, 0), (0, 10), (10, 10), (10, 0)), ((0, 0), (9, 2), (9, 1), (0, 0)), ((0, 0), (2, 9), (1, 9), (0, 0)))
is valid? no
can boost::geometry::correct remedy invalidity? possibly yes
after correction: valid
corrected geometry: (((0, 0), (0, 10), (10, 10), (10, 0), (0, 0)), ((0, 0), (9, 1), (9, 2), (0, 0)), ((0, 0), (2, 9), (1, 9), (0, 0)))

[$img/algorithms/is_valid_failure_example.png]

]

*/
//]


// BLAS level 2

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <boost/numeric/bindings/atlas/cblas1.hpp>
#include <boost/numeric/bindings/atlas/cblas2.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;

using std::cout;
using std::endl;

typedef ublas::vector<double> vct_t;
typedef ublas::matrix<double, ublas::row_major> rm_t;
typedef ublas::matrix<double, ublas::column_major> cm_t;

int main() {

  cout << endl;

  vct_t vx (2);
  vct_t vy (4);

  // row major matrix
  rm_t rm (4, 2);
  init_m (rm, kpp (1));
  print_m (rm, "row major matrix m");
  cout << endl;

  atlas::set (1., vx);
  print_v (vx, "vx");
  cout << endl;

  // vy = m vx
  atlas::gemv (CblasNoTrans, 1.0, rm, vx, 0.0, vy);
  print_v (vy, "vy = m vx");
  cout << endl;

  atlas::set (1., vy);
  print_v (vy, "vy");
  cout << endl;

  // vx = m^T vy
  atlas::gemv (CblasTrans, 1.0, rm, vy, 0.0, vx);
  print_v (vx, "vx = m^T vy");
  cout << endl;

  cout << endl;

  // column major matrix
  cm_t cm (4, 2);
  init_m (cm, kpp (1));
  print_m (cm, "column major matrix m");
  cout << endl;

  atlas::set (1., vx);
  print_v (vx, "vx");
  cout << endl;

  // vy = m vx
  atlas::gemv (CblasNoTrans, 1.0, cm, vx, 0.0, vy);
  print_v (vy, "vy = m vx");
  cout << endl;

  atlas::set (1., vy);
  print_v (vy, "vy");
  cout << endl;

  // vx = m^T vy
  atlas::gemv (CblasTrans, 1.0, cm, vy, 0.0, vx);
  print_v (vx, "vx = m^T vy");
  cout << endl;

}


// Copyright John Maddock 2014

// Use, modification and distribution are 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)

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp> // Boost.Test
#include <boost/test/results_collector.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/fusion/include/tuple.hpp>
#include <boost/fusion/include/std_pair.hpp>
#ifndef BOOST_NO_CXX11_HDR_TUPLE
#include <tuple>
#endif

#include <iostream>
#include <iomanip>
   using std::cout;
   using std::endl;
   using std::setprecision;

#include <boost/math/tools/roots.hpp>

//
// We'll use cbrt as an example:
//
struct cbtr_functor_1
{
   cbtr_functor_1(double x) : m_target(x) {}
   double operator()(double x)
   {
      return x * x * x - m_target;
   }
private:
   double m_target;
};

struct cbtr_functor_2a
{
   cbtr_functor_2a(double x) : m_target(x) {}
   std::pair<double, double> operator()(double x)
   {
      return std::make_pair(x * x * x - m_target, 3 * x * x);
   }
private:
   double m_target;
};
#ifndef BOOST_NO_CXX11_HDR_TUPLE
struct cbtr_functor_2b
{
   cbtr_functor_2b(double x) : m_target(x) {}
   std::tuple<double, double> operator()(double x)
   {
      return std::tuple<double, double>(x * x * x - m_target, 3 * x * x);
   }
private:
   double m_target;
};
#endif
struct cbtr_functor_2c
{
   cbtr_functor_2c(double x) : m_target(x) {}
   boost::tuple<double, double> operator()(double x)
   {
      return boost::tuple<double, double>(x * x * x - m_target, 3 * x * x);
   }
private:
   double m_target;
};
struct cbtr_functor_2d
{
   cbtr_functor_2d(double x) : m_target(x) {}
   boost::fusion::tuple<double, double> operator()(double x)
   {
      return boost::fusion::tuple<double, double>(x * x * x - m_target, 3 * x * x);
   }
private:
   double m_target;
};

#ifndef BOOST_NO_CXX11_HDR_TUPLE
struct cbtr_functor_3b
{
   cbtr_functor_3b(double x) : m_target(x) {}
   std::tuple<double, double, double> operator()(double x)
   {
      return std::tuple<double, double, double>(x * x * x - m_target, 3 * x * x, 6 * x);
   }
private:
   double m_target;
};
#endif
struct cbtr_functor_3c
{
   cbtr_functor_3c(double x) : m_target(x) {}
   boost::tuple<double, double, double> operator()(double x)
   {
      return boost::tuple<double, double, double>(x * x * x - m_target, 3 * x * x, 6 * x);
   }
private:
   double m_target;
};
struct cbtr_functor_3d
{
   cbtr_functor_3d(double x) : m_target(x) {}
   boost::fusion::tuple<double, double, double> operator()(double x)
   {
      return boost::fusion::tuple<double, double, double>(x * x * x - m_target, 3 * x * x, 6 * x);
   }
private:
   double m_target;
};

BOOST_AUTO_TEST_CASE( test_main )
{
   double x = 27;
   double expected = 3;
   double result;
   double tolerance = std::numeric_limits<double>::epsilon() * 5;
   std::pair<double, double> p;
   //
   // Start by trying the unary functors, bisect first:
   //
   cbtr_functor_1 f1(x);
   boost::math::tools::eps_tolerance<double> t(std::numeric_limits<double>::digits - 1);
   p = boost::math::tools::bisect(f1, 0.0, x, t);
   result = (p.first + p.second) / 2;
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
   //
   // bracket_and_solve_root:
   //
   boost::uintmax_t max_iter = boost::math::policies::get_max_root_iterations<boost::math::policies::policy<> >();
   p = boost::math::tools::bracket_and_solve_root(f1, x, 2.0, true, t, max_iter);
   result = (p.first + p.second) / 2;
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
   //
   // toms748_solve:
   //
   max_iter = boost::math::policies::get_max_root_iterations<boost::math::policies::policy<> >();
   p = boost::math::tools::toms748_solve(f1, 0.0, x, t, max_iter);
   result = (p.first + p.second) / 2;
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);

#ifndef BOOST_NO_CXX11_LAMBDAS
   //
   // Now try again with C++11 lambda's
   //
   p = boost::math::tools::bisect([x](double z){ return z * z * z - x; }, 0.0, x, t);
   result = (p.first + p.second) / 2;
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
   //
   // bracket_and_solve_root:
   //
   max_iter = boost::math::policies::get_max_root_iterations<boost::math::policies::policy<> >();
   p = boost::math::tools::bracket_and_solve_root([x](double z){ return z * z * z - x; }, x, 2.0, true, t, max_iter);
   result = (p.first + p.second) / 2;
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
   //
   // toms748_solve:
   //
   max_iter = boost::math::policies::get_max_root_iterations<boost::math::policies::policy<> >();
   p = boost::math::tools::toms748_solve([x](double z){ return z * z * z - x; }, 0.0, x, t, max_iter);
   result = (p.first + p.second) / 2;
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
#endif

   cbtr_functor_2a f2(x);
#ifndef BOOST_NO_CXX11_HDR_TUPLE
   cbtr_functor_2b f3(x);
#endif
   cbtr_functor_2c f4(x);
   cbtr_functor_2d f5(x);

   //
   // Binary Functors - newton_raphson_iterate - test each possible tuple type:
   //
#ifndef BOOST_NO_CXX11_HDR_TUPLE
   result = boost::math::tools::newton_raphson_iterate(f2, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
   result = boost::math::tools::newton_raphson_iterate(f3, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
#endif
   result = boost::math::tools::newton_raphson_iterate(f4, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
   result = boost::math::tools::newton_raphson_iterate(f5, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
   //
   // And again but with lambdas:
   //
#ifndef BOOST_NO_CXX11_LAMBDAS
   result = boost::math::tools::newton_raphson_iterate([x](double z){ return std::make_pair(z * z * z - x, 3 * z * z); }, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
#ifndef BOOST_NO_CXX11_HDR_TUPLE
   result = boost::math::tools::newton_raphson_iterate([x](double z){ return std::make_tuple(z * z * z - x, 3 * z * z); }, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
#endif
   result = boost::math::tools::newton_raphson_iterate([x](double z){ return boost::tuple<double, double>(z * z * z - x, 3 * z * z); }, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
   result = boost::math::tools::newton_raphson_iterate([x](double z){ return boost::fusion::tuple<double, double>(z * z * z - x, 3 * z * z); }, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
#endif

#ifndef BOOST_NO_CXX11_HDR_TUPLE
   cbtr_functor_3b f6(x);
#endif
   cbtr_functor_3c f7(x);
   cbtr_functor_3d f8(x);

   //
   // Ternary functors:
   //
#ifndef BOOST_NO_CXX11_HDR_TUPLE
   result = boost::math::tools::halley_iterate(f6, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
#endif
   result = boost::math::tools::halley_iterate(f7, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
   result = boost::math::tools::halley_iterate(f8, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
#ifndef BOOST_NO_CXX11_LAMBDAS
#ifndef BOOST_NO_CXX11_HDR_TUPLE
   result = boost::math::tools::halley_iterate([x](double z){ return std::make_tuple(z * z * z - x, 3 * z * z, 6 * z); }, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
#endif
   result = boost::math::tools::halley_iterate([x](double z){ return boost::tuple<double, double, double>(z * z * z - x, 3 * z * z, 6 * z); }, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
   result = boost::math::tools::halley_iterate([x](double z){ return boost::fusion::tuple<double, double, double>(z * z * z - x, 3 * z * z, 6 * z); }, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
#endif
#ifndef BOOST_NO_CXX11_HDR_TUPLE
   result = boost::math::tools::schroder_iterate(f6, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
#endif
   result = boost::math::tools::schroder_iterate(f7, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
   result = boost::math::tools::schroder_iterate(f8, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
#ifndef BOOST_NO_CXX11_LAMBDAS
#ifndef BOOST_NO_CXX11_HDR_TUPLE
   result = boost::math::tools::schroder_iterate([x](double z){ return std::make_tuple(z * z * z - x, 3 * z * z, 6 * z); }, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
#endif
   result = boost::math::tools::schroder_iterate([x](double z){ return boost::tuple<double, double, double>(z * z * z - x, 3 * z * z, 6 * z); }, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
   result = boost::math::tools::schroder_iterate([x](double z){ return boost::fusion::tuple<double, double, double>(z * z * z - x, 3 * z * z, 6 * z); }, x, 0.0, x, std::numeric_limits<double>::digits - 1);
   BOOST_CHECK_CLOSE_FRACTION(expected, result, tolerance);
#endif
} // BOOST_AUTO_TEST_CASE( test_main )


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010,2012 Jitse Niesen <jitse@maths.leeds.ac.uk>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <limits>
#include <Eigen/Eigenvalues>

template<typename MatrixType> void verifyIsQuasiTriangular(const MatrixType& T)
{
  typedef typename MatrixType::Index Index;

  const Index size = T.cols();
  typedef typename MatrixType::Scalar Scalar;

  // Check T is lower Hessenberg
  for(int row = 2; row < size; ++row) {
    for(int col = 0; col < row - 1; ++col) {
      VERIFY(T(row,col) == Scalar(0));
    }
  }

  // Check that any non-zero on the subdiagonal is followed by a zero and is
  // part of a 2x2 diagonal block with imaginary eigenvalues.
  for(int row = 1; row < size; ++row) {
    if (T(row,row-1) != Scalar(0)) {
      VERIFY(row == size-1 || T(row+1,row) == 0);
      Scalar tr = T(row-1,row-1) + T(row,row);
      Scalar det = T(row-1,row-1) * T(row,row) - T(row-1,row) * T(row,row-1);
      VERIFY(4 * det > tr * tr);
    }
  }
}

template<typename MatrixType> void schur(int size = MatrixType::ColsAtCompileTime)
{
  // Test basic functionality: T is quasi-triangular and A = U T U*
  for(int counter = 0; counter < g_repeat; ++counter) {
    MatrixType A = MatrixType::Random(size, size);
    RealSchur<MatrixType> schurOfA(A);
    VERIFY_IS_EQUAL(schurOfA.info(), MySuccess);
    MatrixType U = schurOfA.matrixU();
    MatrixType T = schurOfA.matrixT();
    verifyIsQuasiTriangular(T);
    VERIFY_IS_APPROX(A, U * T * U.transpose());
  }

  // Test asserts when not initialized
  RealSchur<MatrixType> rsUninitialized;
  VERIFY_RAISES_ASSERT(rsUninitialized.matrixT());
  VERIFY_RAISES_ASSERT(rsUninitialized.matrixU());
  VERIFY_RAISES_ASSERT(rsUninitialized.info());

  // Test whether compute() and constructor returns same result
  MatrixType A = MatrixType::Random(size, size);
  RealSchur<MatrixType> rs1;
  rs1.compute(A);
  RealSchur<MatrixType> rs2(A);
  VERIFY_IS_EQUAL(rs1.info(), MySuccess);
  VERIFY_IS_EQUAL(rs2.info(), MySuccess);
  VERIFY_IS_EQUAL(rs1.matrixT(), rs2.matrixT());
  VERIFY_IS_EQUAL(rs1.matrixU(), rs2.matrixU());

  // Test maximum number of iterations
  RealSchur<MatrixType> rs3;
  rs3.setMaxIterations(RealSchur<MatrixType>::m_maxIterationsPerRow * size).compute(A);
  VERIFY_IS_EQUAL(rs3.info(), MySuccess);
  VERIFY_IS_EQUAL(rs3.matrixT(), rs1.matrixT());
  VERIFY_IS_EQUAL(rs3.matrixU(), rs1.matrixU());
  if (size > 2) {
    rs3.setMaxIterations(1).compute(A);
    VERIFY_IS_EQUAL(rs3.info(), NoConvergence);
    VERIFY_IS_EQUAL(rs3.getMaxIterations(), 1);
  }

  MatrixType Atriangular = A;
  Atriangular.template triangularView<StrictlyLower>().setZero();
  rs3.setMaxIterations(1).compute(Atriangular); // triangular matrices do not need any iterations
  VERIFY_IS_EQUAL(rs3.info(), MySuccess);
  VERIFY_IS_APPROX(rs3.matrixT(), Atriangular); // approx because of scaling...
  VERIFY_IS_EQUAL(rs3.matrixU(), MatrixType::Identity(size, size));

  // Test computation of only T, not U
  RealSchur<MatrixType> rsOnlyT(A, false);
  VERIFY_IS_EQUAL(rsOnlyT.info(), MySuccess);
  VERIFY_IS_EQUAL(rs1.matrixT(), rsOnlyT.matrixT());
  VERIFY_RAISES_ASSERT(rsOnlyT.matrixU());

  if (size > 2 && size < 20)
  {
    // Test matrix with NaN
    A(0,0) = std::numeric_limits<typename MatrixType::Scalar>::quiet_NaN();
    RealSchur<MatrixType> rsNaN(A);
    VERIFY_IS_EQUAL(rsNaN.info(), NoConvergence);
  }
}

void test_schur_real()
{
  CALL_SUBTEST_1(( schur<Matrix4f>() ));
  CALL_SUBTEST_2(( schur<MatrixXd>(internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4)) ));
  CALL_SUBTEST_3(( schur<Matrix<float, 1, 1> >() ));
  CALL_SUBTEST_4(( schur<Matrix<double, 3, 3, Eigen::RowMajor> >() ));

  // Test problem size constructors
  CALL_SUBTEST_5(RealSchur<MatrixXf>(10));
}


#include "MoonMotionSimulator.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/math/common_factor_rt.hpp>

#include <numeric>
__END_LIBRARIES_DISABLE_WARNINGS

namespace AdventOfCode
{
namespace Year2019
{
namespace Day12
{

Moon::Moon(Vector3D pos)
    : pos{std::move(pos)}
    , vel{0, 0, 0}
{

}

bool Moon::isAxisMatching(const Moon& other, size_t axisIndex) const
{
    return pos[axisIndex] == other.pos[axisIndex] && vel[axisIndex] == other.vel[axisIndex];
}

MoonMotionSimulator::MoonMotionSimulator(std::vector<Moon> moons)
    : m_initialMoons{std::move(moons)}
{
    reset();
}

void MoonMotionSimulator::simulate(unsigned numSteps)
{
    for (size_t i = 0; i < numSteps; ++i)
    {
        step();
    }
}

void MoonMotionSimulator::simulateUntilRepetition()
{
    std::vector<unsigned long long> individualAxisResults;

    for (int axisIndex = 0; axisIndex <= 2; ++axisIndex)
    {
        reset();

        simulateAxisUntilRepetition(axisIndex);

        individualAxisResults.push_back(getNumSteps());
    }

    m_numSteps = std::accumulate(individualAxisResults.cbegin(), individualAxisResults.cend(), 1ull, [](auto total, auto current)
                                    {
                                        return boost::math::lcm(total, current);
                                    });
}

int MoonMotionSimulator::getTotalEnergy() const
{
    return std::accumulate(m_moons.cbegin(), m_moons.cend(), 0, [](int total, const Moon& moon)
                            {
                                return total + (moon.pos.cwiseAbs().sum() * moon.vel.cwiseAbs().sum());
                            });
}

unsigned long long MoonMotionSimulator::getNumSteps() const
{
    return m_numSteps;
}

void MoonMotionSimulator::step()
{
    ++m_numSteps;
    applyAllGravity();
    applyAllVelocity();
}

void MoonMotionSimulator::applyAllGravity()
{
    for (auto firstMoonIter = m_moons.begin(); firstMoonIter != m_moons.end(); ++firstMoonIter)
    {
        for (auto secondMoonIter = std::next(firstMoonIter); secondMoonIter != m_moons.end(); ++secondMoonIter)
        {
            applyGravity(*firstMoonIter, *secondMoonIter);
        }
    }
}

void MoonMotionSimulator::applyAllVelocity()
{
    for (auto& moon : m_moons)
    {
        moon.pos += moon.vel;
    }
}

void MoonMotionSimulator::simulateAxisUntilRepetition(size_t axisIndex)
{
    while (true)
    {
        step();

        bool isRepeat = std::equal(m_moons.cbegin(), m_moons.cend(), m_initialMoons.cbegin(), [axisIndex](const auto& lhs, const auto& rhs)
                                    {
                                        return lhs.isAxisMatching(rhs, axisIndex);
                                    });

        if (isRepeat)
        {
            break;
        }
    }
}

void MoonMotionSimulator::reset()
{
    m_numSteps = 0;
    m_moons = m_initialMoons;
}

void MoonMotionSimulator::applyGravity(Moon& firstMoon, Moon& secondMoon)
{
    for (int axisIndex = 0; axisIndex <= 2; ++axisIndex)
    {
        int& firstMoonPositionCoordinate = firstMoon.pos[axisIndex];
        int& secondMoonPositionCoordinate = secondMoon.pos[axisIndex];

        int& firstMoonVelocityCoordinate = firstMoon.vel[axisIndex];
        int& secondMoonVelocityCoordinate = secondMoon.vel[axisIndex];

        pullCloser(firstMoonPositionCoordinate, secondMoonPositionCoordinate, firstMoonVelocityCoordinate, secondMoonVelocityCoordinate);
    }
}

void MoonMotionSimulator::pullCloser(int p1, int p2, int& v1, int& v2)
{
    if (p1 < p2)
    {
        ++v1;
        --v2;
    }
    else if (p1 > p2)
    {
        --v1;
        ++v2;
    }
}

}
}
}


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

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

// 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_TRIANGULATION_STRATEGIES_CARTESIAN_SIDE_ROBUST_HPP
#define BOOST_GEOMETRY_EXTENSIONS_TRIANGULATION_STRATEGIES_CARTESIAN_SIDE_ROBUST_HPP

#include <boost/geometry/util/select_most_precise.hpp>
#include <boost/geometry/extensions/triangulation/strategies/cartesian/detail/precise_math.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Adaptive precision predicate to check at which side of a segment a point lies:
    left of segment (>0), right of segment (< 0), on segment (0).
\ingroup strategies
\tparam CalculationType \tparam_calculation (numeric_limits<ct>::epsilon() and numeric_limits<ct>::digits must be supported for calculation type ct)
\tparam robustness Number that determines maximum precision. Values from 0 to 2 may make the calculation terminate faster for inputs that may require higher precision to ensure correctness.
\details This predicate determines at which side of a segment a point lies using an algorithm that is adapted from orient2d as described in "Adaptive Precision Floating-Point Arithmetic and Fast Robust Geometric Predicates" by Jonathan Richard Shewchuk ( https://dl.acm.org/citation.cfm?doid=237218.237337 ). More information and copies of the paper can also be found at https://www.cs.cmu.edu/~quake/robust.html . It is designed to be adaptive in the sense that it should be fast for inputs that lead to correct results with plain float operations but robust for inputs that require higher precision arithmetics.
 */
template
<
    typename CalculationType = void,
    int robustness = 3
>
struct side_robust
{
public:
    //! \brief Computes double the signed area of the CCW triangle p1, p2, p
    template
    <
        typename CoordinateType,
        typename PromotedType,
        typename P1,
        typename P2,
        typename P
    >
    static inline PromotedType side_value(P1 const& p1, P2 const& p2,
        P const& p)
    {
        std::array<PromotedType, 2> pa {{ get<0>(p1), get<1>(p1) }};
        std::array<PromotedType, 2> pb {{ get<0>(p2), get<1>(p2) }};
        std::array<PromotedType, 2> pc {{ get<0>(p), get<1>(p) }};
        return ::boost::geometry::detail::precise_math::orient2d
            <PromotedType, robustness>(pa, pb, pc);
    }

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    template
    <
        typename P1,
        typename P2,
        typename P
    >
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename coordinate_type<P1>::type coordinate_type1;
        typedef typename coordinate_type<P2>::type coordinate_type2;
        typedef typename coordinate_type<P>::type coordinate_type3;

        typedef typename boost::mpl::if_c
            <
                boost::is_void<CalculationType>::type::value,
                typename select_most_precise
                    <
                        typename select_most_precise
                            <
                                coordinate_type1, coordinate_type2
                            >::type,
                        coordinate_type3
                    >::type,
                CalculationType
            >::type coordinate_type;
        typedef typename select_most_precise
            <
                coordinate_type,
                double
            >::type promoted_type;

        promoted_type sv =
            side_value<coordinate_type, promoted_type>(p1, p2, p);
        return sv > 0 ? 1
            : sv < 0 ? -1
            : 0;
    }
#endif

};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_TRIANGULATION_STRATEGIES_CARTESIAN_SIDE_ROBUST_HPP


// This file is part of the dune-xt-common project:
//   https://github.com/dune-community/dune-xt-common
// Copyright 2009-2018 dune-xt-common developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   René Fritze    (2018)
//   Tobias Leibner (2018)

#ifndef DUNE_XT_COMMON_COORDINATES_HH
#define DUNE_XT_COMMON_COORDINATES_HH

#include <dune/xt/common/disable_warnings.hh>
#include <boost/geometry.hpp>
#include <dune/xt/common/reenable_warnings.hh>

#include <dune/xt/common/fvector.hh>

namespace Dune {
namespace XT {
namespace Common {

/** Converts from (x, y, z) to (theta, phi) on the unit sphere s.t.
 * (x, y, z) = (sin(theta) cos(phi), sin(theta) sin(phi), cos(theta))
 * with 0 \leq \theta \leq \pi and 0 \leq \varphi < 2\pi. **/
template <class DomainFieldType>
class CoordinateConverter
{
  typedef typename boost::geometry::model::point<DomainFieldType, 3, typename boost::geometry::cs::cartesian>
      BoostCartesianCoordType;
  typedef typename boost::geometry::model::
      point<DomainFieldType, 2, typename boost::geometry::cs::spherical<boost::geometry::radian>>
          BoostSphericalCoordType;

public:
  typedef FieldVector<DomainFieldType, 3> CartesianCoordType;
  typedef FieldVector<DomainFieldType, 2> SphericalCoordType;

  static SphericalCoordType to_spherical(const CartesianCoordType& x)
  {
    BoostCartesianCoordType x_boost(x[0], x[1], x[2]);
    BoostSphericalCoordType x_spherical_boost;
    boost::geometry::transform(x_boost, x_spherical_boost);
    return SphericalCoordType{boost::geometry::get<1>(x_spherical_boost), boost::geometry::get<0>(x_spherical_boost)};
  }

  static CartesianCoordType to_cartesian(const SphericalCoordType& x_spherical, bool first_is_cosine = false)
  {
    // if first_is_cosine, the first coordinate is not theta but rather cos(theta)
    if (first_is_cosine) {
      const auto& mu = x_spherical[0];
      const auto& phi = x_spherical[1];
      return CartesianCoordType{
          std::sqrt(1 - std::pow(mu, 2)) * std::cos(phi), std::sqrt(1 - std::pow(mu, 2)) * std::sin(phi), mu};
    } else {
      BoostSphericalCoordType x_spherical_boost(x_spherical[1], x_spherical[0]);
      BoostCartesianCoordType x_boost;
      boost::geometry::transform(x_spherical_boost, x_boost);
      return CartesianCoordType{
          boost::geometry::get<0>(x_boost), boost::geometry::get<1>(x_boost), boost::geometry::get<2>(x_boost)};
    }
  }
};

} // namespace Common
} // namespace XT
} // namespace Dune

#endif // DUNE_XT_COMMON_COORDINATES_HH


#pragma once

#include <Eigen/Sparse>
#include <vector>

#include "mtao/eigen/shape_checks.hpp"
#include "mtao/quadrature/simpsons.hpp"

//#include "mtao/geometry/mesh/dual_volumes.hpp"

namespace mtao::simulation::hexahedral {
template <typename Scalar, int D>
Eigen::Matrix<Scalar, 1 << D, 1 << D> laplacian_stencil() {
    using Vec = mtao::Vector<Scalar, D>;
    Eigen::Matrix<Scalar, 1 << D, 1 << D> S;
    S.setZero();
    auto eval = [](int elem, const std::array<Scalar, D>& vec) {
        Vec coeffs;
        Vec grad;
        for (size_t idx = 0; idx < D; ++idx) {
            if (elem & (1 << idx)) {
                grad(idx) = -1;
                coeffs(idx) = (Scalar(1) - vec[idx]);
            } else {
                grad(idx) = 1;
                coeffs(idx) = (vec[idx]);
            }
        }
        for (size_t idx = 0; idx < D; ++idx) {
            for (size_t j = 0; j < D; ++j) {
                if (j != idx) {
                    grad(idx) *= coeffs(j);
                }
            }
        }

        return grad;
    };

    auto quad = [&](int i, int j) {
        const int num_samples = 256;
        Scalar val = Scalar(0);

        val = quadrature::multidim_simpsons_rule<D, Scalar>(
            [&](const std::array<Scalar, D>& p) -> Scalar {
                return eval(i, p).dot(eval(j, p));
            },
            0., 1., num_samples);
        return val;
    };

    /*
    std::array<Scalar, D + 1> memo;
    for (int j = 0; j <= D; ++j) {
        memo[j] = quad(0, (1 << (j)) - 1);
        std::cout << memo[j] << std::endl;
    }
    */
    for (int i = 0; i < S.rows(); ++i) {
        S(i, i) = quad(i, i);
        // S(i, i) = memo[0];
        for (int j = i + 1; j < S.cols(); ++j) {
            /*
            int v = i ^ j;
            int count = 0;
            for (int u = 0; u < D; ++u) {
                if (v & (1 << u)) {
                    count++;
                }
            }
            */
            // S(j, i) = S(i, j) = memo[count];
            S(j, i) = S(i, j) = quad(i, j);
        }
    }
    return S;
}

}  // namespace mtao::simulation::hexahedral


/**
 * @file dynkin.hpp
 * @author freol35241
 * @brief A toolkit for 3D dynamics and kinematics of rigid bodies using
 * the YPR euler angle convention.
 * @version 0.3.0
 * @date 2021-01-30
 *
 * @copyright Copyright (c) 2021
 *
 */
#include <cmath>
#include <memory>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <unsupported/Eigen/EulerAngles>

namespace Eigen {
    typedef Eigen::Matrix<double,6,1> Vector6d;
    typedef Eigen::Matrix<double,6,6> Matrix6d;
}

/**
 * @brief `dynkin` namespace
 *
 */
namespace dynkin {

    struct _Frame;
    using Frame = std::shared_ptr<_Frame>;
    struct Transform;
    Transform transform(Frame zeroth, Frame end);

    /**
     * @brief Convert a 3x3 rotation matrix to euler angles using
     * the YPR euler angle convention
     *
     * @param rotation A 3x3 rotation matrix
     * @return Calculated euler angles [roll, pitch, yaw]
     */
    inline Eigen::Vector3d rotation_to_euler(const Eigen::Matrix3d& rotation){
        return rotation.eulerAngles(2,1,0).reverse();
    }

    /**
     * @brief Convert a vector of euler angles to a rotation matrix
     * using the YPR uler angle convention
     *
     * @param attitude A vector of euler angles [roll, pitch, yaw]
     * @return Calculated 3x3 rotation matrix
     */
    inline Eigen::Matrix3d euler_to_rotation(const Eigen::Vector3d& attitude){
        return (
            Eigen::AngleAxisd(attitude(2), Eigen::Vector3d::UnitZ())
            * Eigen::AngleAxisd(attitude(1), Eigen::Vector3d::UnitY())
            * Eigen::AngleAxisd(attitude(0), Eigen::Vector3d::UnitX())
            ).toRotationMatrix();
    }

    /**
     * @brief Representing a transform between two `Frames`
     *
     */
    struct Transform{
        Eigen::Isometry3d HTM;

        Transform(const Eigen::Isometry3d& HTM):HTM(HTM){}

        /**
         * @brief Apply this transformation to a vector
         *
         * @param vector Vector to be transformed
         * @return Transformed vector
         */
        Eigen::Vector3d apply_vector(const Eigen::Vector3d& vector){
            return this->HTM.linear()*vector;
        }

        /**
         * @brief Apply this transformation to a position vector
         *
         * @param position Position vector to be transformed
         * @return Transformed position vector
         */
        Eigen::Vector3d apply_position(const Eigen::Vector3d& position){
            return this->HTM*position;
        }

        /**
         * @brief Apply this transformation to a wrench
         *
         * @param wrench Wrench to be transformed
         * @return Transformed wrench
         */
        Eigen::Vector6d apply_wrench(const Eigen::Vector6d& wrench){
            Eigen::Vector6d out;
            out.head(3) = this->apply_vector(wrench.head(3));
            out.tail(3) = this->apply_vector(wrench.tail(3)) + this->HTM.translation().cross(out.head<3>());
            return out;
        }

        /**
         * @brief Create a Transform which is the inverse of this
         *
         * @return Inverted transform
         */
        Transform inverse(){
            return Transform(this->HTM.inverse());
        }

    };

    /**
     * @brief Create a Frame object
     *
     * @param parent The parent frame of the new frame
     * @return A new Frame
     */
    Frame create_frame(Frame parent = nullptr){
      return std::make_shared<_Frame>(parent);
    }

    /**
     * @brief Representing a coordinate frame
     *
     */
    struct _Frame: public std::enable_shared_from_this<_Frame>{
        Eigen::Isometry3d HTM = Eigen::Isometry3d::Identity();
        Eigen::Vector3d _linear_velocity = Eigen::Vector3d::Zero();
        Eigen::Vector3d _angular_velocity = Eigen::Vector3d::Zero();
        Frame parent;

        _Frame(Frame parent):parent(parent){};

        /**
         * @brief Create a new Frame with this Frame as parent
         *
         * @return Child Frame
         */
        Frame create_child(){
          return create_frame(this->shared_from_this());
        }

        /**
         * @brief Position of this Frame in relation to parent Frame
         *
         * @return Position (3x1)
         */
        auto position(){
            return this->HTM.translation();
        }

        /**
         * @brief Rotation of this Frame in relation to parent Frame
         *
         * @return Rotation matrix 3x3
         */
        auto rotation(){
            return this->HTM.linear();
        }

        /**
         * @brief Linear velocity of this Frame in relation to parent Frame,
         * decomposed in this Frame
         *
         * @return Linear velocity (3x1)
         */
        Eigen::Vector3d& linear_velocity(){
            return this->_linear_velocity;
        }

        /**
         * @brief Angular velocity of this Frame in relation to parent Frame,
         * decomposed in this Frame
         *
         * @return Angular velocity (3x1)
         */
        Eigen::Vector3d& angular_velocity(){
            return this->_angular_velocity;
        }

        /**
         * @brief Get attitude of this Frame in relation to parent Frame
         *
         * @return Attitude (euler angles)(3x1)
         */
        Eigen::Vector3d get_attitude(){
            return rotation_to_euler(this->rotation());
        }

        /**
         * @brief Set attitude of this Frame in relation to parent Frame
         *
         * @param attitude Attitude (euler angles)(3x1)
         */
        void set_attitude(const Eigen::Vector3d& attitude){
            this->rotation() = euler_to_rotation(attitude);
        }

        /**
         * @brief Get pose of this Frame in relation to the inertial Frame
         *
         * @return Pose (6x1)
         */
        Eigen::Vector6d get_pose(){
            Eigen::Vector6d out;
            Transform t = transform(nullptr, this->shared_from_this());
            out.head(3) = t.HTM.translation();
            out.tail(3) = rotation_to_euler(t.HTM.linear());
            return out;
        }

        /**
         * @brief Get twist of this Frame in relation to the inertial Frame
         *
         * @return Twist (6x1)
         */
        Eigen::Vector6d get_twist(){
            Eigen::Vector6d out;

            Eigen::Vector3d v = this->linear_velocity();
            Eigen::Vector3d w = this->angular_velocity();

            if (this->parent != nullptr){
                Eigen::Vector6d twist_p = this->parent->get_twist();
                Transform t = transform(this->shared_from_this(), this->parent);
                v += t.apply_vector(twist_p.head(3)) + twist_p.tail<3>().cross(this->position());
                w += t.apply_vector(twist_p.tail(3));
            }
            out.head(3) = v;
            out.tail(3) = w;
            return out;
        }

    };

    /**
     * @brief Find transform between two frames
     *
     * @param zeroth Zeroth frame of transform
     * @param end End frame of transform
     * @return Transform from zeroth to end
     */
    inline Transform transform(Frame zeroth, Frame end){

        Eigen::Isometry3d HTM = Eigen::Isometry3d::Identity();
        Frame f = end;

        while (f != nullptr){
            if (f == zeroth){
                return Transform(HTM);
            }

            HTM = f->HTM*HTM;
            f = f->parent;

        }

        // If we get to here, the least common base frame is the inertial frame (nullptr)
        Transform T = Transform(HTM);

        if (zeroth == nullptr){
            return T;
        }

        Transform T_ = transform(nullptr, zeroth).inverse();

        return Transform(T_.HTM*T.HTM);
    }

    /**
     * @brief `rigidbody`namespace
     *
     */
    namespace rigidbody{

        /**
         * @brief Create a skew matrix (3x3) from a (3x1) vector
         *
         * @param v Vector to create skew matrix from
         * @return Skew matrix
         */
        inline Eigen::Matrix3d skew(const Eigen::Vector3d& v){
            Eigen::Matrix3d out;
            out <<  0,  -v(2),  v(1),
                    v(2),   0,  -v(0),
                    -v(1),  v(0),   0;
            return out;
        }

        /**
         * @brief Create a Motion Transformation Matrix (6x6)
         *
         * @param position Position vector defining the transformation
         * @return Motion Transformation Matrix
         */
        inline Eigen::Matrix6d motion_transformation_matrix(const Eigen::Vector3d& position){
            Eigen::Matrix6d out = Eigen::Matrix6d::Identity();
            out.block<3,3>(0,3) = skew(position).transpose();
            return out;
        }

        /**
         * @brief Create Generalized Inertia Matrix (6x6)
         *
         * @param mass Rigid body mass
         * @param gyradii Rigid body gyradii (3x1)
         * @return Generalized Inertia Matrix (6x6)
         */
        inline Eigen::Matrix6d generalized_inertia_matrix(const double& mass, const Eigen::Vector3d& gyradii){
            if (mass <= 0.0){
                throw std::invalid_argument("mass must be greater than zero!");
            }
            if ((gyradii.array() <= 0.0).any()){
                throw std::invalid_argument("All gyradii must be greater than zero!");
            }

            Eigen::Matrix6d H = Eigen::Matrix6d::Zero();
            H.block<3,3>(0,0) = Eigen::Matrix3d::Identity() * mass;
            H.block<3,3>(3,3) = (mass*gyradii.array().square()).matrix().asDiagonal();
            return H;
        }

        /**
         * @brief Create an Eulerian Matrix (3x3) relating angular velocity to euler angle derivatives
         *
         * @param attitude Attitude (3x1)
         * @return Eulerian Matrix (3x3)
         */
        inline Eigen::Matrix3d eulerian(const Eigen::Vector3d& attitude){
            double fi = attitude(0);
            double theta = attitude(1);
            Eigen::Matrix3d out;
            out <<  1,      sin(fi)*cos(theta),     cos(fi)*tan(theta),
                    0,      cos(fi),                -sin(fi),
                    0,      sin(fi)/cos(theta),     cos(fi)/cos(theta);
            return out;
        }

        /**
         * @brief Convert angular velocities to euler angle derivatives
         *
         * @param attitude Attitude of rigid body (3x1)
         * @param angular_velocity Angular velocity of rigid body (3x1)
         * @return Euler angle derivatives of rigid body (3x1)
         */
        inline Eigen::Vector3d angular_velocity_to_deuler(
            const Eigen::Vector3d& attitude,
            const Eigen::Vector3d& angular_velocity
        ){
            return eulerian(attitude)*angular_velocity;
        }

        /**
         * @brief Representing an ideal rigid body
         *
         */
        struct RigidBody {
            Eigen::Matrix6d inertia = Eigen::Matrix6d::Identity();
            const Frame origin = create_frame();
            const Frame CoG = create_frame(origin);

            /**
             * @brief Construct a new Rigid Body object
             *
             * @param inertia Generalized Inertia of the rigid body (6x6)
             * @param cog Position vector relating the origin and the CoG of the (3x1)
             * rigid body
             */
            RigidBody(
                const Eigen::Matrix6d inertia,
                const Eigen::Vector3d& cog = Eigen::Vector3d::Zero()
                ): inertia(inertia)
                {
                    this->CoG->position() = cog;
                };

            /**
             * @brief Returns the mass of this rigid body
             *
             * @return Mass
             */
            double mass(){
                return this->inertia(0,0);
            }

            /**
             * @brief Assemble Coriolis-Centripetal Matrix
             *
             * @param inertia Generalized Inertia Matrix of rigid body (6x6)
             * @param twist Twist of rigid body (3x1)
             * @return Coriolis-Centripetal Matrix (6x6)
             */
            Eigen::Matrix6d coriolis_centripetal_matrix(
                const Eigen::Matrix6d inertia,
                const Eigen::Vector6d twist
            ){
                Eigen::Matrix6d C = Eigen::Matrix6d::Zero();
                C.block<3,3>(0,0) = this->mass()*skew(twist.tail(3));
                C.block<3,3>(3,3) = -skew(inertia.block<3,3>(3,3)*twist.tail(3));
                return C;
            }

            /**
             * @brief Generalized coordinates of this rigid body
             * [x, y, z, fi, theta, psi]
             *
             * @return Generalized coordinates (6x1)
             */
            Eigen::Vector6d generalized_coordinates(){
                return this->origin->get_pose();
            }

            /**
             * @brief Generalized velocities of this rigid body
             * [dx, dy, dz, dfi, dtheta, dpsi]
             *
             * @return Generalized velocities (6x1)
             */
            Eigen::Vector6d generalized_velocities(){
                Eigen::Vector6d twist = this->origin->get_twist();
                Transform t = transform(nullptr, this->origin);

                Eigen::Vector6d out = Eigen::Vector6d::Zero();
                out.head(3) = t.apply_vector(twist.head(3));
                out.tail(3) = angular_velocity_to_deuler(
                    this->origin->get_attitude(), twist.tail(3)
                );

                return out;
            }

            /**
             * @brief Returns the resulting acceleration from the given wrench
             *
             * @param wrench Wrench acting on the rigid body (6x1)
             * @param additional_inertia Inertia in addition to the inertia of this
             * rigid body that should be accelerated (6x6)
             * @return Acceleration (6x1)
             */
            Eigen::Vector6d acceleration(
                const Eigen::Vector6d& wrench,
                const Eigen::Matrix6d& additional_inertia = Eigen::Matrix6d::Zero()
            ){
                Eigen::Vector6d twist, f_cc, f;
                Eigen::Matrix6d H, I_cg, C_cg, I_b, C_b;

                f = wrench;
                twist = this->origin->get_twist();

                H = motion_transformation_matrix(this->CoG->position());
                I_cg = (this->inertia.array() + additional_inertia.array()).matrix();
                C_cg = this->coriolis_centripetal_matrix(I_cg, twist);
                I_b = H.transpose() * I_cg * H;
                C_b = H.transpose() * C_cg * H;

                f_cc = C_b * twist;
                f -= f_cc;

                return I_b.lu().solve(f);
            }

            /**
             * @brief Returns the required wrench to obtain the given acceleration
             *
             * @param acceleration Given acceleration (6x1)
             * @param additional_inertia Inertia in addition to the inertia of this
             * rigid body that should be accelerated (6x6)
             * @return Wrench (6x1)
             */
            Eigen::Vector6d wrench(
                const Eigen::Vector6d& acceleration,
                const Eigen::Matrix6d& additional_inertia = Eigen::Matrix6d::Zero()
            ){
                Eigen::Vector6d twist, f_cc, f;
                Eigen::Matrix6d H, I_cg, C_cg, I_b, C_b;

                twist = this->origin->get_twist();

                H = motion_transformation_matrix(this->CoG->position());
                I_cg = (this->inertia.array() + additional_inertia.array()).matrix();
                C_cg = this->coriolis_centripetal_matrix(I_cg, twist);
                I_b = H.transpose() * I_cg * H;
                C_b = H.transpose() * C_cg * H;

                f = I_b * acceleration;
                f_cc = C_b * twist;
                f += f_cc;

                return f;
            }

        };

    }

}