#include <vector>

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/qi_parse.hpp>
#include <boost/spirit/include/phoenix_function.hpp>
#include <boost/spirit/include/phoenix_stl.hpp>
#include <boost/fusion/include/std_pair.hpp>

#include "parser.h"

Parser::Parser() {}

namespace qi = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;
namespace phx = boost::phoenix;

struct rexp_var_impl
{
        template <typename T>
        struct result { typedef CommutativeRExp type; }; // this tells Boost the return type

        const CommutativeRExp operator()(std::string& s) const
        {
                // create an element with the given var
                return CommutativeRExp(Var::GetVarId(s));
        }
};
const phx::function<rexp_var_impl> rexp_var;

struct free_var_impl
{
        template <typename T>
        struct result { typedef FreeSemiring type; }; // this tells Boost the return type

        const FreeSemiring operator()(std::string& s) const
        {
                // create an element with the given var
                return FreeSemiring(Var::GetVarId(s));
        }
};
const phx::function<free_var_impl> free_var;

struct prefix_impl
{
	template <typename T, typename U>
	struct result { typedef PrefixSemiring type; }; // this tells Boost the return type

	const PrefixSemiring operator()(std::vector<VarId>& p, unsigned int length) const
	{
		// create a prefix with the given vars
		return PrefixSemiring(p, length);
	}
};
const phx::function<prefix_impl> prefix;

#ifdef USE_GENEPI
struct slsetndd_var_impl
{
	template <typename T, typename U>
	struct result { typedef SemilinSetNdd type; }; // this tells Boost the return type

	const SemilinSetNdd operator()(std::string& s, unsigned int& cnt) const
	{
		// create an element with the given var
		return SemilinSetNdd(Var::GetVarId(s), cnt);
	}
};
const phx::function<slsetndd_var_impl> slsetndd_var;
#endif

struct var_impl
{
	template <typename T>
	struct result { typedef VarId type; }; // this tells Boost the return type

	const VarId operator()(std::string& var) const
	{
		return Var::GetVarId(var);
	}
};
const phx::function<var_impl> variable;

// some boost::qi methods for pushing values through our parser
using qi::_val;
using qi::_1;
using qi::_2;
using qi::_a;
using qi::lit;
using qi::lexeme;
using qi::eps;
using phx::ref; // must be used as phx::ref, otherwise g++ is confused with std::ref...
using phx::insert;
using phx::end;
using phx::begin;
using phx::ref;

typedef std::string::const_iterator iterator_type;

// parser for a commutative regular expression semiring element
struct rexp_elem_parser : qi::grammar<iterator_type, CommutativeRExp()>
{
	rexp_elem_parser() : rexp_elem_parser::base_type(elem)
	{
		elem = '"' >> qi::as_string[lexeme[+(ascii::char_ -'"')]] [_val = rexp_var(_1)] >> '"';
	}
	qi::rule<iterator_type, CommutativeRExp()> elem;
};

#ifdef USE_GENEPI
// parser for a semilinear set expression semiring element
// e.g.: "<a:2, b:5, c:7>" or "<a:1, b:2>" or "<>" (for the one-element)
struct slsetndd_elem_parser : qi::grammar<iterator_type, SemilinSetNdd(), qi::space_type>
{
	slsetndd_elem_parser() : slsetndd_elem_parser::base_type(elem)
	{
		// create new sl-set elements for each entry e.g. "a:2" and aggregate them by using multiplication!
		elem = qi::lit("\"<") >> eps [_val = SemilinSetNdd::one()] >>
				   var_cnt [_val = _val * _1] >>
				   *( (',' >> var_cnt) [_val = _val * _1] ) >> qi::lit(">\"")
		    |  qi::lit("\"<") >> eps [_val = SemilinSetNdd::one()] >> qi::lit(">\"");
		var_cnt = (varidentifier >> ':' >> qi::uint_) [_val = slsetndd_var(_1, _2)];
		varidentifier = qi::as_string[+(qi::alnum)];
	}
	qi::rule<iterator_type, SemilinSetNdd(), qi::space_type> elem;
	qi::rule<iterator_type, SemilinSetNdd(), qi::space_type> var_cnt;
	qi::rule<iterator_type, std::string()> varidentifier;
};
#endif

// parser for a free semiring element
struct free_elem_parser : qi::grammar<iterator_type, FreeSemiring()>
{
        free_elem_parser() : free_elem_parser::base_type(elem)
        {
          //quotes are optional, but if they are present they must surround the constant
          elem = (qi::as_string[lexeme[+(ascii::char_ - '"' - '|' - '<' - ';' - ' ')]] [_val = free_var(_1)])
               | ('"' >> qi::as_string[lexeme[+(ascii::char_ - '"')]] [_val = free_var(_1)] >> '"');

        }
        qi::rule<iterator_type, FreeSemiring()> elem;
};

// parser for a prefix semiring element
struct prefix_elem_parser : qi::grammar<iterator_type, PrefixSemiring()>
{
  unsigned int max_length;
        prefix_elem_parser() : prefix_elem_parser::base_type(elem)
        {
                elem = '"' >> (*var)[_val = prefix(_1,phx::ref(max_length))] >> '"';
                var = qi::as_string[lexeme[+(ascii::char_ - '"' - ',')]] [_val = variable(_1)] >> (',');
        }
        qi::rule<iterator_type, PrefixSemiring()> elem;
        qi::rule<iterator_type, VarId()> var;
};

// use the given parser and return a list of equations of SR polynomials
// the second parameter is either Polynomial<SR> or NonCommutativePolynomial<SR>
template <typename SR_Parser, typename PSR>
struct equation_parser : qi::grammar<iterator_type, std::vector<std::pair<VarId, PSR>>(), qi::space_type>
{
	equation_parser() : equation_parser::base_type(equations)
	{
		equations %= *(equation);
		equation %= (var >> lexeme["::="] >> polynomial >> ';');
		polynomial = summand [_val = _1] >> *('|' >> summand [_val = _val + _1]); // addition
		summand = eps [_val = PSR::one()] >> // set _val to one-element of the semiring
				*(
					('(' >> polynomial >> ')') [_val = _val * _1] | // group term
					var [_val = _val * _1] | // variable factor
					sr_elem [_val = _val * _1] // sr-elem factor
				 );
		var = varidentifier[_val = variable(_1)];
		varidentifier = qi::as_string[lexeme['<' >> +(ascii::char_ - '>') >> '>']];
	}
	qi::rule<iterator_type, std::vector<std::pair<VarId, PSR>>(), qi::space_type> equations;
	qi::rule<iterator_type, std::pair<VarId, PSR>(), qi::space_type> equation;
	qi::rule<iterator_type, PSR(), qi::space_type> polynomial;
	qi::rule<iterator_type, PSR(), qi::space_type> summand;
	qi::rule<iterator_type, std::string()> varidentifier;
	qi::rule<iterator_type, VarId()> var;
	SR_Parser sr_elem;
};

// non commutative function for using the parser with one int and the prefix semiring
std::vector<std::pair<VarId, NonCommutativePolynomial<PrefixSemiring>>> non_commutative_prefix_parser(std::string input, unsigned int length)
{
        typedef equation_parser<prefix_elem_parser, NonCommutativePolynomial<PrefixSemiring>> equation_parser;
        equation_parser equation;
        equation.sr_elem.max_length = length;

        iterator_type iter = input.begin();
        iterator_type end = input.end();

        std::vector<std::pair<VarId, NonCommutativePolynomial<PrefixSemiring>>> result;
        if(!(qi::phrase_parse(iter, end, equation, qi::space, result) && iter == end))
                std::cout << "bad input, failed at: " << std::string(iter, end) << std::endl;

        return result;
}

// non commutative generic function for using the parser
template <typename SR_Parser, typename SR>
std::vector<std::pair<VarId, NonCommutativePolynomial<SR>>> non_commutative_parser(std::string input)
{
        typedef equation_parser<SR_Parser, NonCommutativePolynomial<SR>> equation_parser;
        equation_parser equation;

        iterator_type iter = input.begin();
        iterator_type end = input.end();

        std::vector<std::pair<VarId, NonCommutativePolynomial<SR>>> result;
        if(!(qi::phrase_parse(iter, end, equation, qi::space, result) && iter == end))
                std::cout << "bad input, failed at: " << std::string(iter, end) << std::endl;
        return result;
}

// commutative generic function for using the parser
template <typename SR_Parser, typename SR>
std::vector<std::pair<VarId, CommutativePolynomial<SR>>> commutative_parser(std::string input)
{
	typedef equation_parser<SR_Parser, CommutativePolynomial<SR>> equation_parser;
	equation_parser equation;

	iterator_type iter = input.begin();
	iterator_type end = input.end();

	std::vector<std::pair<VarId, CommutativePolynomial<SR>>> result;
	if(!(qi::phrase_parse(iter, end, equation, qi::space, result) && iter == end))
		std::cout << "bad input, failed at: " << std::string(iter, end) << std::endl;

	return result;
}

// wrapper function for regular expression equations
std::vector<std::pair<VarId, CommutativePolynomial<CommutativeRExp>>> Parser::rexp_parser(std::string input)
{
	return commutative_parser<rexp_elem_parser, CommutativeRExp>(input);
}

#ifdef USE_GENEPI
// wrapper function for ndd semilinear set equations
std::vector<std::pair<VarId, CommutativePolynomial<SemilinSetNdd>>> Parser::slsetndd_parser(std::string input)
{
        return commutative_parser<slsetndd_elem_parser, SemilinSetNdd>(input);
}
#endif

// wrapper function for free semiring equations
std::vector<std::pair<VarId, NonCommutativePolynomial<FreeSemiring>>> Parser::free_parser(std::string input)
{
        return non_commutative_parser<free_elem_parser, FreeSemiring>(input);
}

// wrapper function for prefix semiring equations
std::vector<std::pair<VarId, NonCommutativePolynomial<PrefixSemiring>>> Parser::prefix_parser(std::string input, unsigned int length)
{
	return non_commutative_prefix_parser(input, length);
}


// #include <boost/graph/adjacency_list.hpp>
// #include <boost/graph/connected_components.hpp>
// #include <boost/lexical_cast.hpp>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Sparse>
#include <iomanip>
#include <iostream>
#include <numeric>
#include <vector>

#include "io.hh"
#include "io_visitor.hh"
#include "utils/eigen_util.hh"
#include "utils/std_util.hh"
#include "utils/math.hh"
#include "utils/util.hh"
#include "utils/check.hh"

#ifndef MMUTIL_HH_
#define MMUTIL_HH_

using Scalar = float;
using SpMat = Eigen::SparseMatrix<Scalar, Eigen::RowMajor, std::ptrdiff_t>;
using Index = SpMat::Index;

using Mat = typename Eigen::
    Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
using Vec = typename Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
using IntMat = typename Eigen::
    Matrix<std::ptrdiff_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
using IntVec = typename Eigen::Matrix<std::ptrdiff_t, Eigen::Dynamic, 1>;

inline std::tuple<Index, Index, Scalar>
parse_triplet(const std::tuple<Index, Index, Scalar> &tt)
{
    return tt;
}

inline std::tuple<Index, Index, Scalar>
parse_triplet(const Eigen::Triplet<Scalar> &tt)
{
    return std::make_tuple(tt.row(), tt.col(), tt.value());
}

#endif


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2022, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 * Created on November 10, 2020, 10:34 AM
 */

#include <gtest/gtest.h>

#include <Eigen/Eigen>
#include <boost/math/constants/constants.hpp>

#include "MaterialLib/MPL/Medium.h"
#include "MaterialLib/MPL/Properties/CreateSaturationDependentThermalConductivity.h"
#include "MaterialLib/MPL/Properties/SaturationDependentThermalConductivity.h"
#include "MaterialLib/MPL/Utils/FormEigenTensor.h"
#include "ParameterLib/ConstantParameter.h"
#include "TestMPL.h"
#include "Tests/TestTools.h"

TEST(MaterialPropertyLib, SaturationDependentThermalConductivity)
{
    double const k_dry = 0.2;
    double const k_wet = 1.5;

    auto const k_model_eff = MPL::SaturationDependentThermalConductivity(
        "thermal_conductivity", k_dry, k_wet);

    ParameterLib::SpatialPosition const pos;
    double const t = std::numeric_limits<double>::quiet_NaN();
    double const dt = std::numeric_limits<double>::quiet_NaN();
    MPL::VariableArray vars;

    /// Just test for an arbitrary set of values
    {
        double const sat = 0.345;

        vars[static_cast<int>(MPL::Variable::liquid_saturation)] = sat;

        auto const k = std::get<double>(k_model_eff.value(vars, pos, t, dt));

        double const k_expected = 0.6485;

        ASSERT_LE(std::fabs(k_expected - k) / k_expected, 1e-10)
            << "for expected effective thermal conductivity " << k_expected
            << " and for computed thermal conductivity " << k;
    }
}


#ifndef CANNON_CONTROL_AFFINE_CONTROLLER_H
#define CANNON_CONTROL_AFFINE_CONTROLLER_H

/*!
 * \file cannon/control/affine_controller.hpp
 * \brief File containing AffineController class definition.
 */

/*!
 * \namespace cannon::control
 * \brief Namespace containing control-relevant classes and functions.
 */

#include <Eigen/Dense>

#include <cannon/ml/adam.hpp>

using namespace Eigen;

using namespace cannon::ml;

namespace cannon {
  namespace control {

    /*!
     * \brief Class representing an affine-controller.
     *
     * An affine controller is one with a term linear in the state and a
     * constant offset; this controller is explicitly multidimensional and
     * designed to be updated by gradient descent.
     *
     * \sa cannon::ml::AdamOptimizer
     */
    class AffineController {
      public:
        AffineController() = delete;

        /*!
         * Constructor taking the state dimension and action (control) dimension.
         *
         * \param state_dim Dimensionality of state space
         * \param action_dim Dimensionality of action (control) space
         * \param learning_rate Learning rate for gradient-based updating of the controller
         * \param use_adam Whether to use the Adam optimizer. If false,
         * stochastic gradient descent is used
         *
         */
        AffineController(unsigned int state_dim, unsigned int action_dim,
            double learning_rate=1e-4, bool use_adam=true) :
        state_dim_(state_dim), action_dim_(action_dim),
        learning_rate_(learning_rate), initial_learning_rate_(learning_rate),
        use_adam_(use_adam), K_opt_(action_dim, state_dim,learning_rate),
        k_opt_(action_dim, 1, learning_rate) {
          K_ = MatrixXd::Zero(action_dim_, state_dim_);
          k_ = VectorXd::Zero(action_dim_);
        }

        /*!
         * Computes the action (control) applied by this controller for the input state.
         *
         * \param state The state to compute an action for
         *
         * \returns The action applied by this controller
         */
        VectorXd get_action(const VectorXd& state) const;

        /*!
         * Returns the vector offset and linear terms defining this controller.
         * These are the k and K vector and matrix, respectively, in control
         * literature.
         *
         * \returns k and K, the constant and linear terms for this controller.
         */
        std::pair<VectorXd, MatrixXd> get_mats() const;

        /*!
         * Applies the input gradient to this controller, updating the k and K parameters.
         *
         * \param k_gradient The gradient to apply to the vector offset for this controller.
         * \param K_gradient The gradient to apply to the linear term of this controller.
         */
        void apply_gradient(const VectorXd& k_gradient, const MatrixXd& K_gradient);

        /*!
         * Set the control offset vector.
         *
         * \param k New control offset vector, which should be of the correct
         * dimension.
         */
        void set_k(const VectorXd& k);

        /*!
         * Set the control gain matrix.
         *
         * \param K New control gain matrix, which should be of the correct
         * dimension.
         */
        void set_K(const MatrixXd& K);

      private:
        // Parameters
        unsigned int state_dim_; //!< Dimensionality of state space
        unsigned int action_dim_; //!< Dimensionality of action space
        double learning_rate_; //!< Current learning rate for updates
        double initial_learning_rate_; //!< Initial learning rate
        bool use_adam_; //!< Whether to use AdamOptimizer for updating

        /*!
         * \rst
         * .. todo:: Make learning rate decay parameter for AffineController
         * \endrst
         */
        double decay_ = 0.1; //!< Learning rate decay

        unsigned int overall_t_ = 0; //!< Number of updates applied to this controller

        // Matrices
        MatrixXd K_; //!< Linear term for this controller
        VectorXd k_; //!< Constant offset for this controller

        // Optimizers
        AdamOptimizer K_opt_; //!< Optimizer for the K matrix of this controller
        AdamOptimizer k_opt_; //!< Optimizer for the k matrix of this controller
    };

  } // namespace control
} // namespace cannon

#endif /* ifndef CANNON_CONTROL_AFFINE_CONTROLLER_H */


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

/**
 * @file odometry.cpp
 * @author Cyril Jourdan
 * @date Nov 26, 2018
 * @version 0.1.0
 * @brief File for the odometry of the holonomic base with 3 wheels
 * With help of website https://bharat-robotics.github.io/blog/kinematic-analysis-of-holonomic-robot
 *
 * Contact: cyril.jourdan@therobotstudio.com
 * Created on : Nov 26, 2018
 */

/*** Includes ***/
#include "ros/ros.h"
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <osa_msgs/MotorDataMultiArray.h>
//#include <Eigen/Dense>

using namespace std;

/*** Variables ***/
osa_msgs::MotorDataMultiArray motor_data_array;

//to wait for the msg from both posture and anglesArmDescription topics
bool motor_data_array_arrived = false;

/*** Callback functions ***/
void motorDataArrayCallback(const osa_msgs::MotorDataMultiArrayConstPtr& data)
{
	motor_data_array = *data;
	motor_data_array_arrived = true;
}

/*** Main ***/
int main (int argc, char** argv)
{
	// Initialize ROS
	ros::init(argc, argv, "osa_r2p1_odometry_node");
	ros::NodeHandle nh("~");

	//Subscribers
	ros::Subscriber sub_motor_data_array = nh.subscribe("/foldy_base/motor_data_array", 10, motorDataArrayCallback);

	//Publishers
	ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);

	// initial position
	double x = 0.0;
	double y = 0.0;
	double th = 0.0;

	// velocity
	double vx = 0.0;
	double vy = 0.0;
	double vth = 0.0;

	// motor position
	int curr_mot_enc_pos[3] = {0};
	int prev_mot_enc_pos[3] = {0};
	int diff_mot_enc_pos[3] = {0};

	double wheel_lin_vel[3] = {0}; //V1, V2, V3

	const double gear_ratio = 28/1;
	const double enc_tic_per_motor_turn = 1000*4; //multiply by 4 for the quadrate encoder
	const double enc_tic_per_shaft_turn = enc_tic_per_motor_turn*gear_ratio;
	const double robot_base_radius = 0.29; //in meters
	const double swedish_wheel_radius = 0.0625;

	ros::Time curr_time;
	ros::Time prev_time;
	curr_time = ros::Time::now();
	prev_time = ros::Time::now();
	double dt = 0.0;

	double delta_x = 0.0;
	double delta_y = 0.0;
	double delta_th = 0.0;

	tf::TransformBroadcaster broadcaster;
	ros::Rate loop_rate(20);

	//const double degree = M_PI/180;

	// message declarations
	geometry_msgs::TransformStamped odom_trans;
	odom_trans.header.frame_id = "odom";
	odom_trans.child_frame_id = "dummy_link";

	//grasp first position before starting the loop
	if(ros::ok())
	{
		ros::spinOnce();

		if(motor_data_array_arrived)
		{
			prev_time = ros::Time::now();

			for(int i=0; i<3; i++) prev_mot_enc_pos[i] = motor_data_array.motor_data.at(i).position; //fill the prev values

			motor_data_array_arrived = false;
		}
	}

	while(ros::ok())
	{
		ros::spinOnce();

		if(motor_data_array_arrived)
		{
			// time calculation
			curr_time = ros::Time::now();
			dt = (curr_time - prev_time).toSec();

			// encoder position
			for(int i=0; i<3; i++)
			{
				curr_mot_enc_pos[i] = motor_data_array.motor_data.at(i).position;
				diff_mot_enc_pos[i] = curr_mot_enc_pos[i] - prev_mot_enc_pos[i];

				//Explaination : enc_tic_per_shaft_turn is done in 2*Pi. So an angle theta is done in 2*Pi*diff_enc/enc_tic_per_shaft_turn
				// v = d/t so d = theta*r = 2*Pi*r*diff_enc/enc_tic_per_shaft_turn
				// then just divide by dt to get the linear velocity of the wheel
				wheel_lin_vel[i] = (2*M_PI*swedish_wheel_radius*diff_mot_enc_pos[i])/(enc_tic_per_shaft_turn*dt);

				//ROS_INFO("Motor %d: enc=%d, diff_enc=%d, lin_vel=%f", i, curr_mot_enc_pos[i], diff_mot_enc_pos[i], curr_mot_enc_pos[2]);
			}

			//ROS_INFO("enc1=%d, enc2=%d, enc3=%d", curr_mot_enc_pos[0], curr_mot_enc_pos[1], curr_mot_enc_pos[2]);

			//calculation of theta_dot which is the variable vth here
			vth = -(wheel_lin_vel[0]+wheel_lin_vel[1]+wheel_lin_vel[2])/(3*robot_base_radius);
			delta_th = vth*dt;

			//ROS_INFO("delta_th=%f", delta_th);

			vx = -2*(-cos(delta_th)*wheel_lin_vel[0] + cos(M_PI/3-delta_th)*wheel_lin_vel[1]  + cos(M_PI/3+delta_th)*wheel_lin_vel[2])/3; //multiply by -1 to change X direction
			vy = 2*(-sin(delta_th)*wheel_lin_vel[0] - sin(M_PI/3-delta_th)*wheel_lin_vel[1]  + sin(M_PI/3+delta_th)*wheel_lin_vel[2])/3;

			delta_x = vx*dt;
			delta_y = vy*dt;

			//accumulated position rotated by delta_th and final angle, this will drift over time with the accumulated errors
			//The angle of Pi/6 is because of the convention of having a wheel on the X axis as opposed to the Y axis
			x += cos(th-M_PI/6)*delta_x - sin(th-M_PI/6)*delta_y;
			y += sin(th-M_PI/6)*delta_x + cos(th-M_PI/6)*delta_y;
			th += delta_th;

			ROS_DEBUG("vth=%f, delta_th=%f, vx=%f, vy=%f, delta_x=%f, delta_y=%f, x=%f, y=%f, th=%f", vth, delta_th, vx, vy, delta_x, delta_y, x, y, th);
			//ROS_INFO("vx=%f, delta_x=%f, x=%f", vx, delta_x, x);
			//ROS_INFO("dx=%f", delta_x);

			geometry_msgs::Quaternion odom_quat;
			odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,th);

			// update transform
			odom_trans.header.stamp = curr_time;
			odom_trans.transform.translation.x = x;
			odom_trans.transform.translation.y = y;
			odom_trans.transform.translation.z = 0.0;
			odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(th);

			// filling the odometry
			nav_msgs::Odometry odom;
			odom.header.stamp = curr_time;
			odom.header.frame_id = "odom";
			odom.child_frame_id = "dummy_link";

			// position
			odom.pose.pose.position.x = x;
			odom.pose.pose.position.y = y;
			odom.pose.pose.position.z = 0.0;
			odom.pose.pose.orientation = odom_quat;

			// velocity
			odom.twist.twist.linear.x = vx;
			odom.twist.twist.linear.y = vy;
			odom.twist.twist.linear.z = 0.0;
			odom.twist.twist.angular.x = 0.0;
			odom.twist.twist.angular.y = 0.0;
			odom.twist.twist.angular.z = vth;

			// publishing the odometry and the new tf
			broadcaster.sendTransform(odom_trans);
			odom_pub.publish(odom);

			motor_data_array_arrived = false;

			prev_time = curr_time;
			prev_mot_enc_pos[0] = curr_mot_enc_pos[0];
			prev_mot_enc_pos[1] = curr_mot_enc_pos[1];
			prev_mot_enc_pos[2] = curr_mot_enc_pos[2];

			loop_rate.sleep();
		}
	}

	return 0;
}


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_READVTK_HPP
#define MCL_READVTK_HPP 1

#include <Eigen/Core>
#include <vector>

namespace mcl
{

// Reads VTK tet mesh
bool readVTK(const std::string &vtk, Eigen::MatrixXd &V, Eigen::MatrixXi &T)
{
	using namespace Eigen;

	auto ret_err = [](const std::string &s)
	{
		printf("**mcl::readVTK Error: %s\n", s.c_str());
		return false;
	};

	std::vector<Vector3d> verts;
	std::vector<Vector4i> tets;

    auto file_exists = [](const std::string& n)
	{
        std::ifstream f(n.c_str());
        return f.good();
    };

	if (!file_exists(vtk)) { return ret_err("No vtk file"); }

	// Read all lines into buffer
	std::vector<std::string> lines;
	std::ifstream vtk_file(vtk.c_str());
	if (vtk_file.is_open())
	{
		std::string line;
		while( std::getline(vtk_file, line) ){ lines.emplace_back(line); }
		vtk_file.close();
	}

	if (lines.size()==0) { return ret_err("No data"); }
	int num_lines = lines.size();
	int min_idx = 999;

	// Get pts and tets
	{
		std::string buff;
		int n_pts = 0;
		std::stringstream lV(lines[4]);
		lV >> buff >> n_pts;
		if (n_pts==0) { return ret_err("No points"); }
		for (int i=5; i<n_pts+5; ++i)
		{
			std::stringstream l(lines[i]);
			Vector3d v(0,0,0);
			l >> v[0] >> v[1] >> v[2];
			verts.emplace_back(v);
		}

		// loop until we find tets
		// then add them as needed.
		for (int i=n_pts+5; i<num_lines; ++i)
		{
			std::stringstream lT(lines[i]);

			// Empty string?
			if (!lT.good())
				continue;

			// nvar should be 4
			int nvar = 0;
			lT >> nvar;
			if (nvar != 4)
				continue;

			Vector4i t(0,0,0,0);
			lT >> t[0] >> t[1] >> t[2] >> t[3];
			min_idx = std::min(min_idx, t.minCoeff());
			tets.emplace_back(t);
		}
/*
		std::stringstream lT(lines[n_pts+5]);
		lT >> buff >> n_tets;
		if (n_tets==0) { return ret_err("No tets"); }
		for (int i=n_pts+6; i<(n_pts+6)+n_tets; ++i)
		{
			std::stringstream l(lines[i]);
			Vector4i t(0,0,0,0);
			int dim = 0;
			l >> dim;
			if( dim != 4 ){
                return ret_err("Not a tet mesh");
			}
			l >> t[0] >> t[1] >> t[2] >> t[3];
			tets.emplace_back(t);
		}
*/
	}

	if( verts.size()==0 || tets.size()==0 ){
        return ret_err("No verts/tets");
	}

	{ // Copy to V, T, start T at zero
		V.resize( verts.size(), 3 );
		T.resize( tets.size(), 4 );
		for(int i=0; i<(int)verts.size(); ++i){ V.row(i)=verts[i]; }
		for(int i=0; i<(int)tets.size(); ++i){
			T.row(i)=tets[i];
			T.row(i)-=RowVector4i::Ones()*min_idx;
		}
	}

	return verts.size()>0 && tets.size()>0;

} // end read vtk

} // ns mcl

#endif


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/config.hpp>

#include <algorithm>
#include <vector>
#include <utility>
#include <iostream>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/graph/graph_utility.hpp>

int
main()
{
  using namespace boost;
  typedef adjacency_list < vecS, vecS, bidirectionalS > Graph;

  Graph G(5);
  add_edge(0, 2, G);
  add_edge(1, 1, G);
  add_edge(1, 3, G);
  add_edge(1, 4, G);
  add_edge(2, 1, G);
  add_edge(2, 3, G);
  add_edge(2, 4, G);
  add_edge(3, 1, G);
  add_edge(3, 4, G);
  add_edge(4, 0, G);
  add_edge(4, 1, G);

  std::cout << "original graph:" << std::endl;
  print_graph(G, get(vertex_index, G));

  std::cout << std::endl << "reversed graph:" << std::endl;
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300  // avoid VC++ bug...
  reverse_graph<Graph> R(G);
  print_graph(R, get(vertex_index, G));
#else
  print_graph(make_reverse_graph(G), get(vertex_index, G));
#endif

  return EXIT_SUCCESS;
}


//
// Created by Amir Masoud Abdol on 2019-04-30
//

#define BOOST_TEST_DYN_LINK

#define BOOST_TEST_MODULE ExperimentSetup Tests

#include "nlohmann/json.hpp"
#include <boost/test/unit_test.hpp>
namespace tt = boost::test_tools;
namespace utf = boost::unit_test;

#include "ExperimentSetup.h"

#include "sample_experiment_setup.h"

using json = nlohmann::ordered_json;

using namespace arma;
using namespace sam;
using namespace std;

BOOST_AUTO_TEST_SUITE(constructor)

  BOOST_AUTO_TEST_CASE( default_constructor )
  {
      ExperimentSetup setup;

      BOOST_TEST(setup.nc() == 0);
  }

  BOOST_FIXTURE_TEST_SUITE( json_constructor, ExperimentSetupSampleConfigs )

    BOOST_AUTO_TEST_CASE( json_constructor )
    {
      ExperimentSetup setup{sample_experiment_setup["experiment_parameters"]};

      BOOST_TEST(setup.nc() == 2);
      BOOST_TEST(setup.nd() == 2);
      BOOST_TEST(setup.ng() == 4);
      BOOST_TEST(setup.nreps() == 5);

      arma::Row<int> obs{10, 10, 10, 10};
      BOOST_TEST(arma::approx_equal(setup.nobs(), obs,
                                    "absdiff", 0.0001));

    }

  BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()

// Setter's Tests
// --------------

BOOST_FIXTURE_TEST_SUITE( setter_tests, ExperimentSetupSampleConfigs )

  BOOST_AUTO_TEST_CASE( independent_parameters )
  {
    ExperimentSetup setup{sample_experiment_setup["experiment_parameters"]};

    setup.setNR(1);
    BOOST_TEST(setup.nreps() == 1);
  }

  BOOST_AUTO_TEST_CASE( dependent_parameters )
  {
    ExperimentSetup setup{sample_experiment_setup["experiment_parameters"]};

    setup.setNC(5);
    BOOST_TEST(setup.nc() == 5);
    BOOST_TEST(setup.ng() == 10);

    setup.setND(4);
    BOOST_TEST(setup.ng() == 5 * 4);
  }

  BOOST_AUTO_TEST_CASE( n_obs_parameter )
  {
    ExperimentSetup setup{sample_experiment_setup["experiment_parameters"]};

    arma::Row<int> obs{10, 10, 10, 10};
    BOOST_TEST(arma::approx_equal(setup.nobs(), obs,
                                  "absdiff", 0.0001));

    json normal_dist = R"(
      {
      "param": {
        "dist": "normal_distribution",
        "mean": 0,
        "stddev": 1
        }
      }
    )"_json;

    setup.setObs(normal_dist["param"], setup.nc(), setup.nd());
    BOOST_TEST(arma::accu(setup.nobs()) < 40);

    obs = {10, 20, 30, 40};
    setup.setObs(obs, 2, 2);
    BOOST_TEST(arma::accu(setup.nobs()) == 100);

  }

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <fstream>
#include <sstream>
#include <algorithm>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Polyhedron_3.h>

//for obj export
#include <CGAL/IO/print_wavefront.h>

// Simplification function
#include <CGAL/Surface_mesh_simplification/edge_collapse.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/property_map.h>

// Stop-condition policy
#include <CGAL/Surface_mesh_simplification/Policies/Edge_collapse/Count_stop_predicate.h>
#include <CGAL/Surface_mesh_simplification/Policies/Edge_collapse/Edge_length_cost.h>
#include <CGAL/Surface_mesh_simplification/Policies/Edge_collapse/Midpoint_placement.h>

#include<CGAL/Polyhedron_incremental_builder_3.h>

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/trim.hpp>

#include <scm/core.h>
#include <scm/core/math.h>

typedef CGAL::Simple_cartesian<double> Kernel;
typedef CGAL::Polyhedron_3<Kernel> Surface_mesh;
typedef Surface_mesh::HalfedgeDS HalfedgeDS;

typedef boost::graph_traits<Surface_mesh>::vertex_descriptor vertex_descriptor;

typedef CGAL::Surface_mesh<Kernel::Point_3> GMesh;
typedef GMesh::Vertex_index vertex_index;

namespace SMS = CGAL::Surface_mesh_simplification ;

struct GVertex
{
  Kernel::Point_3 v_pos;
  Kernel::Point_2 t_coord;
  Kernel::Point_3 normal;
};

char* getCmdOption(char ** begin, char ** end, const std::string & option) {
  char ** itr = std::find(begin, end, option);
  if (itr != end && ++itr != end) {
      return *itr;
  }
  return 0;
}

bool cmdOptionExists(char** begin, char** end, const std::string& option) {
  return std::find(begin, end, option) != end;
}

// // A modifier creating a triangle with the incremental builder.
// template<class HDS>
// class polyhedron_builder : public CGAL::Modifier_base<HDS> {

// public:
//   std::vector<double> &coords;
//   std::vector<int>    &tris;

//   polyhedron_builder( std::vector<double> &_coords, std::vector<int> &_tris ) : coords(_coords), tris(_tris) {}

//   void operator()( HDS& hds) {
//     typedef typename HDS::Vertex   Vertex;
//     typedef typename Vertex::Point Point;

//     // create a cgal incremental builder
//     CGAL::Polyhedron_incremental_builder_3<HDS> B( hds, true);
//     B.begin_surface( coords.size()/3, tris.size()/3 );

//     // add the polyhedron vertices
//     for( int i=0; i<(int)coords.size(); i+=3 ){
//       B.add_vertex( Point( coords[i+0], coords[i+1], coords[i+2] ) );
//     }

//     // add the polyhedron triangles
//     for( int i=0; i<(int)tris.size(); i+=3 ){
//       B.begin_facet();
//       B.add_vertex_to_facet( tris[i+0] );
//       B.add_vertex_to_facet( tris[i+1] );
//       B.add_vertex_to_facet( tris[i+2] );
//       B.end_facet();
//     }

//     // finish up the surface
//     B.end_surface();
//     }
// };

//custom function to build a mesh of type Surface Mesh
void build_surface_mesh (GMesh &_gmesh,
              std::vector<GVertex> &_vertices){

  //to store refs to vertices while the mesh is being built
  std::vector<vertex_index> v_indices;

  //add vertex positions, record indexes
  for (int i = 0; i < (int)_vertices.size(); i+=3){
    vertex_index v1 = _gmesh.add_vertex(_vertices[i].v_pos);
    vertex_index v2 = _gmesh.add_vertex(_vertices[i+1].v_pos);
    vertex_index v3 = _gmesh.add_vertex(_vertices[i+2].v_pos);

    //add face
    _gmesh.add_face(v1, v2, v3);

    // v_indices.push_back(v);
    v_indices.insert(v_indices.end(), {v1, v2, v3});
  }

  //add faces
  // for (int i = 0; i < (int)_vertices.size(); i+=3){
  //   _gmesh.add_face(v_indices[_tris[i+0]],v_indices[_tris[i+1]], v_indices[_tris[i+2]]);
  // }

  //create a property map for adding normals
  //ref: https://doc.cgal.org/4.7/Surface_mesh/index.html#sectionSurfaceMesh_properties
  GMesh::Property_map<vertex_index, Kernel::Point_3> normal;
  bool created;
  boost::tie(normal, created) =
    _gmesh.add_property_map<vertex_index, Kernel::Point_3>("v:normal", Kernel::Point_3(0,0,0));
  assert(created);

  //add normals
  for (int i = 0; i < (int)v_indices.size(); ++i)
  {
    normal[v_indices[i]] = _vertices[i].normal;
    std::cout << normal[v_indices[i]] << std::endl;
  }

  // uint32_t num_normals = 0;
  // BOOST_FOREACH( vertex_index vi, _gmesh.vertices()) {
  //   normal[vi] = Kernel::Point_3(_normals[num_normals+0], _normals[num_normals+1], _normals[num_normals+2]);
  //   std::cout << normal[vi] << std::endl;
  //   num_normals+=3;
  // }

}

//parses a face string like "f  2//1  8//1  4//1 " into 3 given arrays
void parse_face_string (std::string face_string, uint32_t (&index)[3], uint32_t (&coord)[3], uint32_t (&normal)[3]){

  //split by space into faces
  std::vector<std::string> faces;
  boost::algorithm::trim(face_string);
  boost::algorithm::split(faces, face_string, boost::algorithm::is_any_of(" "), boost::algorithm::token_compress_on);

  for (int i = 0; i < 3; ++i)
  {
    //split by / for indices
    std::vector<std::string> inds;
    boost::algorithm::split(inds, faces[i], [](char c){return c == '/';}, boost::algorithm::token_compress_off);

    for (int j = 0; j < (int)inds.size(); ++j)
    {
      uint32_t idx = 0;
      //parse value from string
      if (inds[j] != ""){
        idx = (uint32_t)stoi(inds[j]);
      }
      if (j == 0){index[i] = idx;}
      else if (j == 1){coord[i] = idx;}
      else if (j == 2){normal[i] = idx;}

    }
  }
}

// load obj function from vt_obj_loader/Utils.h
void load_obj(const std::string& filename,
              std::vector<GVertex>& vertices){

  std::vector<Kernel::Point_3> v;
  std::vector<uint32_t> vindices;
  std::vector<Kernel::Point_3> n;
  std::vector<uint32_t> nindices;
  std::vector<Kernel::Point_2> t;
  std::vector<uint32_t> tindices;

  FILE *file = fopen(filename.c_str(), "r");

  if (0 != file) {

    while (true) {
      char line[128];
      int32_t l = fscanf(file, "%s", line);

      if (l == EOF) break;
      if (strcmp(line, "v") == 0) {
        double vx, vy, vz;
        fscanf(file, "%lf %lf %lf\n", &vx, &vy, &vz);
        v.push_back(Kernel::Point_3(vx,vy,vz));
      }
      else if (strcmp(line, "vn") == 0) {
        float nx, ny, nz;
        fscanf(file, "%f %f %f\n", &nx, &ny, &nz);
        n.push_back(Kernel::Point_3(nx,ny,nz));
      }
      else if (strcmp(line, "vt") == 0) {
        float tx, ty;
        fscanf(file, "%f %f\n", &tx, &ty);
        t.push_back(Kernel::Point_2(tx,ty));
      }
      else if (strcmp(line, "f") == 0) {
        fgets(line, 128, file);
        std::string face_string = line;
        uint32_t index[3];
        uint32_t coord[3];
        uint32_t normal[3];

        parse_face_string(face_string, index, coord, normal);

        vindices.insert(vindices.end(), {index[0], index[1], index[2]});
        tindices.insert(tindices.end(), {coord[0], coord[1], coord[2]});
        nindices.insert(nindices.end(), {normal[0], normal[1], normal[2]});
      }
    }

    fclose(file);

    std::cout << "positions: " << v.size() << std::endl;
    std::cout << "normals: " << n.size() << std::endl;
    std::cout << "coords: " << t.size() << std::endl;
    std::cout << "faces: " << vindices.size() << std::endl;

  }

  //read from indices arrays to construct list of vertices in open GL style
  //http://www.opengl-tutorial.org/beginners-tutorials/tutorial-7-model-loading/
  for (int i = 0; i < (int)vindices.size(); ++i)
  {
    Kernel::Point_3 newv;
    Kernel::Point_2 newt;
    Kernel::Point_3 newn;

    //guard against empty arrays
    if (v.size() == 0) {newv = Kernel::Point_3(0,0,0);}
    else {newv = v[vindices[i]-1];}

    if (t.size() == 0) {newt = Kernel::Point_2(0,0,0);}
    else {newt = t[tindices[i]-1];}

    if (n.size() == 0) {newn = Kernel::Point_3(0,0,0);}
    else {newn = n[nindices[i]-1];}

    GVertex new_Vertex = {newv, newt, newn};
    vertices.push_back(new_Vertex);
  }

}

// void output_obj (GMesh &gmesh, const std::string& filename, std::vector<double>& v, std::vector<int>& vindices) {
//   // https://doc.cgal.org/4.9/Surface_mesh/index.html

//     typedef typename HDS::Vertex   Vertex;
//     typedef typename Vertex::Point Point;

//   std::cout << "all vertices " << std::endl;
//   // The vertex iterator type is a nested type of the Vertex_range
//   GMesh::Vertex_range::iterator  vb, ve;
//   GMesh::Vertex_range r = gmesh.vertices();
//   // The iterators can be accessed through the C++ range API
//   vb = r.begin();
//   ve = r.end();
//   // or the boost Range API
//   vb = boost::begin(r);
//   ve = boost::end(r);
//   // or with boost::tie, as the CGAL range derives from std::pair
//   for(boost::tie(vb, ve) = gmesh.vertices(); vb != ve; ++vb){
//     Point p = gmesh.point(*vb);
//     std::cout << p << std::endl;
//           // std::cout << *vb << std::endl;
//   }
// }

int main( int argc, char** argv )
{
  std::string obj_filename = "dino.obj";
  if (cmdOptionExists(argv, argv+argc, "-f")) {
    obj_filename = std::string(getCmdOption(argv, argv + argc, "-f"));
  }
  else {
    std::cout << "Please provide a obj filename using -f <filename.obj>" << std::endl;
    return 1;
  }
  std::string out_filename = "data/simplified_mesh.obj";
  if (cmdOptionExists(argv, argv+argc, "-o")) {
    out_filename = std::string(getCmdOption(argv, argv + argc, "-o"));
  }

  //load OBJ into vertex array
  std::vector<GVertex> vertices;
  load_obj( obj_filename, vertices);

  if (vertices.size() == 0 ) {
    std::cout << "didnt find any vertices" << std::endl;
    return 1;
  }
  std::cout << "Mesh loaded (" << vertices.size() << " vertices)" << std::endl;

  // print loaded obj info
  // std::cout << "points" << std::endl;
  // for (int i = 0; i < points.size(); i+=3)
  // {
  //   std::cout << points[i] << " "  << points[i+1] << " "  << points[i+2] << std::endl;
  // }
  // std::cout << "faces" << std::endl;
  // for (int i = 0; i < tris.size(); i+=3)
  // {
  //   std::cout << tris[i] << " "  << tris[i+1] << " "  << tris[i+2] << std::endl;
  // }
  // std::cout << "normals" << std::endl;
  // for (int i = 0; i < normals.size(); i+=3)
  // {
  //   std::cout << normals[i] << " "  << normals[i+1] << " "  << normals[i+2] << std::endl;
  // }
  // std::cout << "norm indexes" << std::endl;
  // for (int i = 0; i < n_inds.size(); i+=3)
  // {
  //   std::cout << n_inds[i] << " "  << n_inds[i+1] << " "  << n_inds[i+2] << std::endl;
  // }

  //create a mesh from vectors
  GMesh gmesh;
  build_surface_mesh(gmesh, vertices);

  if (gmesh.is_valid(true)){
    std::cout << "mesh valid\n";
  }

  if (!CGAL::is_triangle_mesh(gmesh)){
    std::cerr << "Input geometry is not triangulated." << std::endl;
    return EXIT_FAILURE;
  }
  // This is a stop predicate (defines when the algorithm terminates).
  // In this example, the simplification stops when the number of undirected edges
  // left in the surface mesh drops below the specified number (1000)
  SMS::Count_stop_predicate<Surface_mesh> stop(300);

  std::cout << "Starting simplification" << std::endl;

  // This the actual call to the simplification algorithm.
  // The surface mesh and stop conditions are mandatory arguments.
  // The index maps are needed because the vertices and edges
  // of this surface mesh lack an "id()" field.
  // int r = SMS::edge_collapse
  //           (gmesh
  //           ,stop
  //            ,CGAL::parameters::halfedge_index_map  (get(CGAL::halfedge_external_index  ,gmesh))
  //            // ,CGAL::parameters::vertex_index_map(get(CGAL::vertex_external_index,gmesh))
  //                              // .halfedge_index_map  (get(CGAL::halfedge_external_index  ,gmesh))
  //            //                   .get_cost (SMS::Edge_length_cost <Surface_mesh>())
  //            //                   .get_placement(SMS::Midpoint_placement<Surface_mesh>())
  //           );

  // std::cout << "\nFinished...\n" << (gmesh.size_of_halfedges()/2) << " final edges.\n" ;

  // output_obj(gmesh,"bla", points, tris);

  //write to file
  std::ofstream ofs(out_filename);
  ofs << gmesh;
  // CGAL::print_polyhedron_wavefront(ofs, gmesh);
  ofs.close();
  std::cout << "Simplified mesh was written to " << out_filename << std::endl;

  return EXIT_SUCCESS ;
}


#ifndef LIVENESS_ANALYSIS_HPP
#define LIVENESS_ANALYSIS_HPP

/* Liveness analysis */

#include <crab/common/stats.hpp>
#include <crab/common/debug.hpp>
#include <crab/common/types.hpp>
#include <boost/noncopyable.hpp>

#if 1

#include <crab/iterators/killgen_fixpoint_iterator.hpp>

namespace crab {

  namespace analyzer {

   template<typename V>
   using liveness_domain = crab::domains::flat_killgen_domain<V>;

   template<class CFG>
   class liveness_ops:
      public crab::iterators::
      killgen_operations_api<CFG, liveness_domain<typename CFG::varname_t> > {

   public:

     typedef liveness_domain<typename CFG::varname_t> liveness_domain_t;
     typedef typename CFG::basic_block_label_t basic_block_label_t;

   private:

     typedef crab::iterators::killgen_operations_api<CFG, liveness_domain_t> this_type;
     typedef std::pair<liveness_domain_t, liveness_domain_t> binding_t;
     typedef boost::unordered_map<basic_block_label_t, binding_t> liveness_map_t;

     liveness_map_t _liveness_map;

    public:

     liveness_ops (CFG cfg): this_type (cfg) { }

     virtual bool is_forward () { return false; }

     virtual liveness_domain_t entry()
     { return liveness_domain_t::bottom();}

     virtual liveness_domain_t merge(liveness_domain_t d1,  liveness_domain_t d2)
     { return d1 | d2; }

     virtual void init_fixpoint() {
        for (auto &b: boost::make_iterator_range(this->_cfg.begin(),this->_cfg.end())) {
          liveness_domain_t kill, gen;
          for (auto &s: boost::make_iterator_range(b.rbegin(),b.rend())) {
             auto live = s.get_live();
             for (auto d: boost::make_iterator_range(live.defs_begin(),
                                                     live.defs_end())) {
               kill += d;
               gen -= d;
             }
             for (auto u: boost::make_iterator_range(live.uses_begin(),
                                                     live.uses_end())) {
               gen  += u;
             }
          }
          _liveness_map.insert(std::make_pair(b.label(), binding_t(kill, gen)));
        }
      }

      virtual liveness_domain_t analyze (basic_block_label_t bb_id, liveness_domain_t in) {
        auto it = _liveness_map.find (bb_id);
        assert(it != _liveness_map.end ());
        in -= it->second.first;
        in += it->second.second;
        return in;
      }

      virtual std::string name () { return "liveness";}
   };

   //! Live variable analysis
   template<typename CFG>
   class liveness:
      boost::noncopyable,
      public crab::iterators::killgen_fixpoint_iterator <CFG, liveness_ops<CFG> >{

    public:

     typedef typename CFG::basic_block_label_t basic_block_label_t;
     typedef typename CFG::statement_t statement_t;
     typedef typename CFG::varname_t varname_t;

    private:

     typedef liveness_ops<CFG> liveness_ops_t;
     typedef typename liveness_ops_t::liveness_domain_t liveness_domain_t;
     typedef crab::iterators::killgen_fixpoint_iterator<CFG,liveness_ops_t>
             killgen_fixpoint_iterator_t;

    public:

     //for backward compatibility
     //XXX: maybe unused already
     typedef liveness_domain_t set_t;

    private:

     // output of the analysis: map basic blocks to set of dead
     // variables at the end of the blocks
     boost::unordered_map<basic_block_label_t, liveness_domain_t> _dead_map;

     // statistics
     unsigned _max_live;
     unsigned _total_live;
     unsigned _total_blks;

     void process_post (basic_block_label_t bb,
                        liveness_domain_t live_out) {
       // --- Collect dead variables at the exit of bb
       if (!live_out.is_bottom ()) {
         liveness_domain_t dead_set = this->_cfg.get_node(bb).live();
         dead_set -= live_out;
         CRAB_LOG("liveness",
                  crab::outs() << cfg_impl::get_label_str(bb)
                               << " dead variables=" << dead_set <<"\n";);
         _dead_map.insert (std::make_pair(bb, dead_set));
         // update statistics
         _total_live += live_out.size ();
         _max_live = std::max (_max_live, live_out.size ());
         _total_blks ++;
       } else {
         CRAB_LOG("liveness",
                  crab::outs() << cfg_impl::get_label_str(bb)
                               << " dead variables=" << live_out <<"\n";);
       }
     }

    public:

     liveness (CFG cfg)
         : killgen_fixpoint_iterator_t(cfg),
           _max_live (0), _total_live (0), _total_blks (0) { }

     void exec() {
       this->run();
       for (auto p : boost::make_iterator_range(this->out_begin(), this->out_end()))
       { process_post (p.first, p.second); }
       this->release_memory();
     }

     //! return the set of dead variables at the exit of block bb
     liveness_domain_t dead_exit (basic_block_label_t bb) const {
       auto it = _dead_map.find(bb);
       if (it == _dead_map.end())
         return liveness_domain_t();
       else
         return it->second;
     }

     void get_stats (unsigned& total_live,
                     unsigned& max_live_per_blk,
                     unsigned& avg_live_per_blk)  const {
       total_live = _total_live;
       max_live_per_blk = _max_live;
       avg_live_per_blk = (_total_blks == 0 ? 0 : (int) _total_live / _total_blks);
     }

     // TODO
     void write (crab_os &o) const { }
   };

   template <typename CFG>
   crab_os& operator << (crab_os& o, const liveness<CFG> &l) {
     l.write (o);
     return o;
   }

  } // end namespace analyzer
} // end namespace crab
#else

#include <crab/domains/discrete_domains.hpp>
#include <crab/cfg/cfg_bgl.hpp>
#include <crab/analysis/graphs/sccg.hpp>
#include <crab/analysis/graphs/topo_order.hpp>

namespace crab {

  namespace analyzer {

    using namespace cfg;

    namespace liveness_discrete_impl{

       template< typename Element>
       class liveness_domain: public ikos::writeable {

        private:
         typedef liveness_domain< Element > liveness_domain_t;
         typedef discrete_domain< Element > discrete_domain_t;

        public:
         typedef typename discrete_domain_t::iterator iterator;

        private:
         discrete_domain_t _inv;

        public:
         // hook to speedup check_post in liveness class
         liveness_domain(discrete_domain_t inv):
             ikos::writeable(), _inv(inv){ }

        public:
         static liveness_domain_t top() {
           return liveness_domain(discrete_domain_t::top());
         }

         static liveness_domain_t bottom() {
           return liveness_domain(discrete_domain_t::bottom());
         }

        public:

         liveness_domain(): _inv(discrete_domain_t::bottom()){ }

         liveness_domain(Element e): ikos::writeable(), _inv(e) { }

         liveness_domain(const liveness_domain_t &other):
             ikos::writeable(), _inv(other._inv) { }

         liveness_domain_t& operator=(const liveness_domain_t &other) {
           if (this != &other)
             this->_inv = other._inv;
           return *this;
         }

         iterator begin() {
           return this->_inv.begin();
         }

         iterator end() {
           return this->_inv.end();
         }

         unsigned size() {
           return this->_inv.size();
         }

         bool is_bottom() {
           return this->_inv.is_bottom();
         }

         bool is_top() {
           return this->_inv.is_top();
         }

         bool operator<=(liveness_domain_t other) {
           if (is_bottom ())
             return true;
           else if (other.is_top ())
             return true;
           else
             return (this->_inv <= other._inv);
         }

         void operator-=(Element x) {
           if (is_bottom ())
             return;

           this->_inv -= x;
         }

         void operator-=(liveness_domain_t other) {
           if (is_bottom () || other.is_bottom ())
             return;

           if (!other._inv.is_top()) {
             for ( auto v : other)
               this->_inv -= v;
           }
         }

         void operator+=(Element x) {
           if (is_top ())
             return;

           this->_inv += x;
         }

         void operator+=(liveness_domain_t other) {
           if (is_top () || other.is_bottom ()) {
             return;
           }
           else if (other.is_top ()) {
             _inv = discrete_domain_t::top();
           }
           else {
             this->_inv = (this->_inv | other._inv);
           }
         }

         liveness_domain_t operator|(liveness_domain_t other) {
           return (this->_inv | other._inv);
         }

         liveness_domain_t operator&(liveness_domain_t other) {
           return (this->_inv & other._inv);
         }

         liveness_domain_t operator||(liveness_domain_t other) {
           // liveness domain is finite so no infinite AC
           return other;
         }

         liveness_domain_t operator&&(liveness_domain_t other) {
           // liveness domain is finite so no infinite DC
           return other;
         }

         void write(crab_os& o) {
           this->_inv.write(o);
         }

       };

    } // end namespace liveness_discrete_impl

    #if 0
    // The implementation using discrete_domain is significantly faster
    // than the one using std::set
    namespace liveness_set_impl {

      template< typename Element>
      class liveness_domain {

        typedef liveness_domain< Element > liveness_domain_t;
        typedef std::set<Element> ElemSet;

       public:

        typedef typename ElemSet::iterator       iterator;
        typedef typename ElemSet::const_iterator const_iterator;

       private:

        bool m_is_top;
        ElemSet m_inv;

        liveness_domain (ElemSet inv, bool is_top):
            m_is_top(is_top), m_inv(inv) {
          if (m_is_top)
            m_inv.clear ();
        }

       public:

        static liveness_domain_t top() {
          return liveness_domain (ElemSet(), true);
        }

        static liveness_domain_t bottom() {
          return liveness_domain (ElemSet(), false);
        }

        liveness_domain(): m_is_top (false) { }

        liveness_domain(Element e): m_is_top(false)  {
          m_inv.insert(e);
        }

        ~liveness_domain() {
          //m_inv.clear();
        }

        liveness_domain (const liveness_domain_t &other):
            m_is_top(other.m_is_top), m_inv (other.m_inv) {
          if (is_top ())
            m_inv.clear ();
        }

        liveness_domain_t& operator=(const liveness_domain_t &other) {
          if (this != &other) {
            m_is_top = other.m_is_top;
            m_inv = other.m_inv;
          }
          return *this;
        }

        iterator begin()             { return m_inv.begin(); }
        iterator end()               { return m_inv.end();   }
        const_iterator begin() const { return m_inv.begin(); }
        const_iterator end()   const { return m_inv.end();   }

        unsigned size() const { return m_inv.size (); }

        bool is_top() const { return m_is_top; }

        bool is_bottom () const { return (!is_top() && (size() == 0)); }

        bool operator<=(const liveness_domain_t &other) {
          if (is_bottom() || other.is_top())    return true;
          if (is_top()    || other.is_bottom()) return false;

          return std::includes(m_inv.begin (), m_inv.end (),
                               other.m_inv.begin (), other.m_inv.end ());
        }

        void operator-=(const liveness_domain_t &other) {
          if (is_bottom () || is_bottom ())
            return;
          else if (is_top ())
            CRAB_ERROR ("not defined set difference if first operand is top");
          else if (other.is_top ())  {
            *this = liveness_domain_t::bottom ();
          }
          else {
            for (auto v : other)
              m_inv.erase (v);
            //m_inv.erase(other.m_inv.begin (), other.m_inv.end ());
          }
        }

        void operator+=(const liveness_domain_t &other) {
          if (is_top())
            return;
          else if (is_bottom () || other.is_top())
            *this = other;
          else if (other.is_bottom ())
            return;
          else {
          m_inv.insert( other.m_inv.begin (), other.m_inv.end ());
          }
        }

        // Apply kill-gen dataflow equation by composing (inv -= kill ; inv += get)
        void apply (const liveness_domain_t &kill, const liveness_domain_t &gen){
          if (is_bottom () || kill.is_bottom ()) {
            this->operator+= (gen);
          }
          else if (is_top ())
            CRAB_ERROR ("not defined set difference if first operand is top");
          else if (kill.is_top ()) {
            *this = gen;
          }
          else if (gen.is_top ()) {
            *this = liveness_domain_t::top ();
          }
          else if (gen.is_bottom ()) {
            this->operator-= (kill);
          }
          else {
            // minimize the number of deletions
            ElemSet s;
            std::set_difference(kill.m_inv.begin (), kill.m_inv.end (),
                                gen.m_inv.begin (), gen.m_inv.end (),
                                std::inserter (s, s.end()));
            for (auto &v : s)
              m_inv.erase (v);

            m_inv.insert( gen.m_inv.begin (), gen.m_inv.end ());
          }
        }

        liveness_domain_t operator|(const liveness_domain_t &other) {

          if (is_top() || other.is_top())
            return liveness_domain_t::top();
          else if (is_bottom())
            return other;
          else if (other.is_bottom())
            return *this;
          else {
            ElemSet s(m_inv);
            s.insert(other.m_inv.begin (), other.m_inv.end ());
            return liveness_domain_t (s, false);
          }
        }

        liveness_domain_t operator&(const liveness_domain_t &other) {
          if (is_bottom() || other.is_bottom())
            return liveness_domain_t::bottom();
          else if (is_top()) return other;
          else if (other.is_top()) return *this;
          else {
            ElemSet s;
            std::set_intersection(m_inv.begin (), m_inv.end (),
                                  other.m_inv.begin (), other.m_inv.end (),
                                  std::inserter (s, s.end()));
            return liveness_domain_t (s, false);
          }
        }

        liveness_domain_t operator|| (const liveness_domain_t &other) {
          return this->operator|(other);
        }

        liveness_domain_t operator&& (const liveness_domain_t &other) {
          return this->operator&(other);
        }

        void write(crab_os& o) {
          if (is_top ())  o << "{...}";
          else if (is_bottom ())   o << "_|_";
          else
          {
            o << "{";
            for (iterator it = begin(); it != end(); )
            {
              o << *it;  ++it;
              if (it != end())
                o << "; ";
            }
            o << "}";
          }
        }
      };
    } // end namespace liveness_set_impl
   #endif

   //! Live variable analysis
   template<typename CFG>
   class liveness: boost::noncopyable {

    public:
     typedef typename CFG::basic_block_label_t basic_block_label_t;
     typedef typename CFG::varname_t varname_t;
     typedef liveness_discrete_impl::liveness_domain<varname_t> liveness_domain_t;
     typedef liveness_domain_t set_t;

    private:
     typedef boost::shared_ptr <set_t> set_ptr;
     typedef boost::unordered_map< basic_block_label_t, set_ptr > liveness_map_t;
     typedef std::pair< liveness_domain_t, liveness_domain_t >   kill_gen_t;
     typedef boost::unordered_map< basic_block_label_t, kill_gen_t>  kill_gen_map_t;
     typedef typename liveness_map_t::value_type l_binding_t;
     typedef typename kill_gen_map_t::value_type kg_binding_t;

     CFG m_cfg;
     boost::unordered_map< basic_block_label_t, set_t> m_in_map;
     boost::unordered_map< basic_block_label_t, set_t> m_out_map;

     // for internal use
     kill_gen_map_t m_kill_gen_map;
     // for external queries
     liveness_map_t m_dead_map;

     // avoid run twice
     bool m_has_exec;

     // statistics
     unsigned m_max_live;
     unsigned m_total_live;
     unsigned m_total_blks;

    private:

     // precompute use/def sets
     void init() {
       for (auto &b: boost::make_iterator_range(m_cfg.begin(),m_cfg.end())) {

         liveness_domain_t kill, gen;
         for (auto &s: boost::make_iterator_range(b.rbegin(),b.rend())) {

           auto live = s.get_live();
           for (auto d: boost::make_iterator_range(live.defs_begin(),
                                                   live.defs_end())) {
             kill += d;
             gen -= d;
           }
           for (auto u: boost::make_iterator_range(live.uses_begin(),
                                                   live.uses_end())) {
             gen  += u;
           }
         }
         m_kill_gen_map.insert ( kg_binding_t (b.label (),
                                               kill_gen_t (kill, gen)));
       }
     }

    public:

     liveness (CFG cfg)
         : m_cfg (cfg), m_has_exec (false),
           m_max_live (0), m_total_live (0), m_total_blks (0) {
       crab::ScopedCrabStats __st__("liveness");
       init();
     }

     void exec() {
        crab::ScopedCrabStats __st__("liveness");

       if (m_has_exec) {
         CRAB_WARN ("Trying to execute liveness twice!");
         return;
       }

        std::vector<typename CFG::node_t> order =
            crab::analyzer::graph_algo::weak_rev_topo_sort(m_cfg);

       assert (order.size () == std::distance(m_cfg.begin(), m_cfg.end()));

       CRAB_LOG("liveness",
                crab::outs()  << "liveness fixpoint ordering={";
                for (auto &v : order)
                  crab::outs() << cfg_impl::get_label_str(v) << " -- ";
                crab::outs() << "}\n";);

       bool change = true;
       unsigned iterations = 0;
       while (change) {
         change = false;
         ++iterations;
         for (auto &n : order) {
           liveness_domain_t out = liveness_domain_t::bottom();
           for (auto p: m_cfg.next_nodes (n))
             out = out | m_in_map [p];
           liveness_domain_t in = analyze (n, out);
           liveness_domain_t old_in = m_in_map [n];
           if (!(in <= old_in)) {
             m_in_map [n] = in | old_in;
             change = true;
           }
           else
             m_out_map [n] = out;
         }
       }

       for (auto &n : order) {
         process_post (n, m_out_map [n]);
       }

      CRAB_LOG("liveness",
               crab::outs() << "liveness fixpoint reached in "
                         << iterations << " iterations \n");
       m_has_exec = true;

      CRAB_LOG("liveness",
               crab::outs() << "liveness sets: \n";
               for (auto n: boost::make_iterator_range (m_cfg.label_begin (),
                                                        m_cfg.label_end ())) {
                 crab::outs() << cfg_impl::get_label_str(n) << " "
                              << "OUT=" << m_out_map [n]  << " "
                              << "IN=" << m_in_map [n] << "\n";
               }
               crab::outs() << "\n";);

       // --- Keep a small memory footprint in client analyses
       m_in_map.clear ();
       m_out_map.clear ();
     }

     //! return the set of dead variables at the exit of block bb
     set_t dead_exit (basic_block_label_t bb) const {
       auto it = m_dead_map.find(bb);
       if (it == m_dead_map.end())
         return set_t ();
       else
         return *(it->second);
     }

     void get_stats (unsigned& total_live,
                     unsigned& max_live_per_blk,
                     unsigned& avg_live_per_blk)  const {
       total_live = m_total_live;
       max_live_per_blk = m_max_live;
       avg_live_per_blk = (m_total_blks == 0 ? 0 : (int) m_total_live / m_total_blks);
     }

     void write (crab_os &o) const {
     }

    private:

     liveness_domain_t analyze (basic_block_label_t bb_id,
                                liveness_domain_t live_out) {
       auto it = m_kill_gen_map.find (bb_id);
       assert(it != m_kill_gen_map.end ());

       // --- compute live_in
       live_out -= it->second.first;
       live_out += it->second.second;
       return live_out;
     }

     void process_post (basic_block_label_t bb,
                        liveness_domain_t live_out) {

       // --- Collect dead variables at the exit of bb
       if (!live_out.is_bottom ()) {
         auto dead_set = boost::make_shared<set_t>(m_cfg.get_node (bb).live ());
         *dead_set -= live_out;
         CRAB_LOG("liveness",
                  crab::outs() << cfg_impl::get_label_str(bb)
                               << " dead variables=" << *dead_set <<"\n";);
         m_dead_map.insert (l_binding_t (bb, dead_set));
         // update statistics
         m_total_live += live_out.size ();
         m_max_live = std::max (m_max_live, live_out.size ());
         m_total_blks ++;
       } else {
         CRAB_LOG("liveness",
                  crab::outs() << cfg_impl::get_label_str(bb)
                               << " dead variables=" << live_out <<"\n";);
       }
     }

   };

   template <typename CFG>
   crab_os& operator << (crab_os& o, const liveness<CFG> &l) {
     l.write (o);
     return o;
   }
  } // end namespace analyzer
} // end namespace crab
#endif

#endif


#include <qlo/objects/obj_latentmodels.hpp>
#include <boost/make_shared.hpp>
#include <ql/experimental/credit/defaultprobabilitylatentmodel.hpp>

QuantLibAddin::GaussianDefProbLM::GaussianDefProbLM(
    const boost::shared_ptr<reposit::ValueObject>& properties,
            // BEGIN typemap rp_tm_default
            boost::shared_ptr< QuantLib::Basket > const &Basket,
            std::vector< std::vector< QuantLib::Real > > const &Factors,
            // END   typemap rp_tm_default
    bool permanent)
: reposit::LibraryObject<QuantLib::GaussianDefProbLM>(properties, permanent) {
        libraryObject_ = boost::make_shared<QuantLib::GaussianDefProbLM>(
            //basket,
            Factors,
            QuantLib::LatentModelIntegrationType::GaussianQuadrature);
        libraryObject_->resetBasket(Basket);
}

QuantLibAddin::TDefProbLM::TDefProbLM(
    const boost::shared_ptr<reposit::ValueObject>& properties,
            // BEGIN typemap rp_tm_default
            std::vector< QuantLib::Integer > const &Torders,
            boost::shared_ptr< QuantLib::Basket > const &Basket,
            std::vector< std::vector< QuantLib::Real > > const &Factors,
            // END   typemap rp_tm_default
    bool permanent)
: reposit::LibraryObject<QuantLib::TDefProbLM>(properties, permanent) {
        QuantLib::TCopulaPolicy::initTraits initsT;
        initsT.tOrders = Torders;
        libraryObject_ = boost::make_shared<QuantLib::TDefProbLM>(
            //basket,
            Factors,
            QuantLib::LatentModelIntegrationType::GaussianQuadrature,
            initsT);
        libraryObject_->resetBasket(Basket);
}


#include <frovedis.hpp>
#include <frovedis/matrix/sliced_matrix.hpp>
#include <frovedis/matrix/pblas_wrapper.hpp>

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

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    auto bm = make_blockcyclic_matrix_load<float> ("./sample_4x4");

    // creating slices from above blockcyclic matrix
    auto sm1 = make_sliced_blockcyclic_matrix<float>(bm,0,0,2,2);
    auto sm2 = make_sliced_blockcyclic_matrix<float>(bm,2,2,2,2);
    auto sm3 = make_sliced_blockcyclic_matrix<float>(bm,0,2,2,2);

    // <bm> top-right = top-left * bottom-right
    gemm<float>(sm1,sm2,sm3);
    bm.save("./out_4x4");

    // checking whether slices have been created properly and
    // top-right sliced square is correctly modified with result
    BOOST_CHECK (system("diff ./out_4x4 ./ref_4x4") == 0);
    system("rm -f ./out_4x4");
}


#ifndef EDGE_PLANE_IDENTITY_HPP
#define EDGE_PLANE_IDENTITY_HPP

#include <Eigen/Dense>
#include <g2o/core/base_binary_edge.h>
#include <g2o/types/slam3d_addons/vertex_plane.h>

namespace g2o {

/**
 * @brief A modified version of g2o::EdgePlane. This class takes care of flipped plane normals.
 *
 */
class EdgePlaneIdentity : public BaseBinaryEdge<4, Eigen::Vector4d, VertexPlane, VertexPlane> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  EdgePlaneIdentity() : BaseBinaryEdge<4, Eigen::Vector4d, VertexPlane, VertexPlane>() {
    _information.setIdentity();
    _error.setZero();
  }
  void computeError() {
    const VertexPlane* v1 = static_cast<const VertexPlane*>(_vertices[0]);
    const VertexPlane* v2 = static_cast<const VertexPlane*>(_vertices[1]);

    Eigen::Vector4d p1 = v1->estimate().toVector();
    Eigen::Vector4d p2 = v2->estimate().toVector();

    if(p1.dot(p2) < 0.0) {
      p2 = -p2;
    }

    _error = (p2 - p1) - _measurement;
  }
  virtual bool read(std::istream& is) override {
    Eigen::Vector4d v;
    for(int i = 0; i < 4; ++i) {
      is >> v[i];
    }

    setMeasurement(v);
    for(int i = 0; i < information().rows(); ++i) {
      for(int j = i; j < information().cols(); ++j) {
        is >> information()(i, j);
        if(i != j) {
          information()(j, i) = information()(i, j);
        }
      }
    }

    return true;
  }

  virtual bool write(std::ostream& os) const override {
    for(int i = 0; i < 4; ++i) {
      os << _measurement[i] << " ";
    }

    for(int i = 0; i < information().rows(); ++i) {
      for(int j = i; j < information().cols(); ++j) {
        os << " " << information()(i, j);
      };
    }
    return os.good();
  }

  virtual void setMeasurement(const Eigen::Vector4d& m) override {
    _measurement = m;
  }

  virtual int measurementDimension() const override {
    return 4;
  }
};

}  // namespace g2o

#endif  // EDGE_PLANE_PARALLEL_HPP


#pragma once
#include <vector>
#include <optional>
#include <iostream>
#include <Eigen/Dense>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace Openwarp{
    class OpenwarpApplication;
    class OpenwarpUtils;

    typedef struct pose_t {
        Eigen::Vector3f position;
        Eigen::Vector3f relative_pos;
        Eigen::Quaternionf orientation;
    } pose_t;
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/bessel.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

EVE_TEST_TYPES( "Check return types of cyl_bessel_i0"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::cyl_bessel_i0(T(0)), T);
  TTS_EXPR_IS(eve::cyl_bessel_i0(v_t(0)), v_t);
};

 EVE_TEST( "Check behavior of cyl_bessel_i0 on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 5.5),
                              eve::test::randoms(5.5, 9.5),
                              eve::test::randoms(9.5, 60.0))
         )
   <typename T>(T const& a0, T const& a1, T const& a2)
{
  using v_t = eve::element_type_t<T>;
  auto eve__cyl_bessel_i0 =  [](auto x) { return eve::cyl_bessel_i0(x); };
#if defined(__cpp_lib_math_special_functions)
  auto std__cyl_bessel_i0 =  [](auto x)->v_t { return std::cyl_bessel_i(v_t(0), x); };
#else
  auto std__cyl_bessel_i0 =  [](auto x)->v_t { return boost::math::cyl_bessel_i(double(0), double(x)); };
#endif

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_i0(eve::inf(eve::as<v_t>())), eve::inf(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_i0(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_i0(eve::inf(eve::as< T>())),  eve::inf(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_i0(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_IEEE_EQUAL(eve__cyl_bessel_i0(v_t(-1)), eve::nan(eve::as<v_t>()));
  TTS_ULP_EQUAL(eve__cyl_bessel_i0(v_t(500)), std__cyl_bessel_i0(v_t(500)), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0(v_t(10)), std__cyl_bessel_i0(v_t(10))  , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0(v_t(5)),  std__cyl_bessel_i0(v_t(5))   , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0(v_t(2)),  std__cyl_bessel_i0(v_t(2))   , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0(v_t(1.5)),std__cyl_bessel_i0(v_t(1.5)) , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0(v_t(0.5)),std__cyl_bessel_i0(v_t(0.5)) , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0(v_t(1)),  std__cyl_bessel_i0(v_t(1))   , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0(v_t(0)),  eve::zero(eve::as<v_t>()), 0.0);

  TTS_IEEE_EQUAL(eve__cyl_bessel_i0(T(-1)), eve::nan(eve::as<T>()));
  TTS_ULP_EQUAL(eve__cyl_bessel_i0( T(500)),  T(std__cyl_bessel_i0(v_t(500)) ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0( T(10)) ,  T(std__cyl_bessel_i0( v_t(10)) ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0( T(5))  ,  T(std__cyl_bessel_i0( v_t(5))  ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0( T(2))  ,  T(std__cyl_bessel_i0( v_t(2))  ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0( T(1.5)),  T(std__cyl_bessel_i0( v_t(1.5))), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0( T(0.5)),  T(std__cyl_bessel_i0( v_t(0.5))), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0( T(1))  ,  T(std__cyl_bessel_i0( v_t(1))  ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0( T(0))  , eve::zero(eve::as< T>()), 0.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_i0(a0), map(std__cyl_bessel_i0, a0), 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0(a1), map(std__cyl_bessel_i0, a1), 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i0(a2), map(std__cyl_bessel_i0, a2), 10.0);

};

EVE_TEST( "Check behavior of cyl_bessel_i0 on wide with negative non integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 60.0))
        )
  <typename T>(T a0 )
{
  using v_t = eve::element_type_t<T>;
  auto eve__diff_bessel_i0 =  [](auto x) { return eve::diff(eve::cyl_bessel_i0)(x); };
  auto std__diff_bessel_i0 =  [](auto x)->v_t { return boost::math::cyl_bessel_i_prime(double(0), double(x)); };
  TTS_RELATIVE_EQUAL(eve__diff_bessel_i0(a0),   map(std__diff_bessel_i0, a0)   , 1.0e-3);

};


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      111215    T. Secretin       First creation of the code.
 *      111221    T. Secretin       Removed memory leaks. Added test for circular and
 *                                  near-parabolic orbits, as well as for negative eccentricities.
 *      120326    D. Dirkx          Changed raw pointers to shared pointers.
 *      120421    K. Kumar          Updated test fixtures and cases to use updated conversion
 *                                  object.
 *      120813    P. Musegaas       Updated unit test to new root finding structure.
 *      120822    P. Musegaas       Added near-parabolic functionality, with accompanying unit
 *                                  tests. Added two big random unit tests, to test behaviour that
 *                                  tends to be chaotic for some values with different implemen-
 *                                  tations of the function. Added error file writing function.
 *                                  Added small test for specifying other initial guess. Updated
 *                                  tests for invalid input.
 *      120903    P. Musegaas       Improved random test (does not fail on mean anomaly of 0.0).
 *                                  Decreased number of random values for random tests.
 *      121205    P. Musegaas       Updated code to final version of rootfinders.
 *      130123    K. Kumar          Updated test tolerance for near-parabolic cases in Test 5 to
 *                                  deal with conversion failure on some systems.
 *      150417    D. Dirkx          Made modifications for templated element conversions.
 *
 *    References
 *      GTOP, http://www.esa.int/gsp/ACT/doc/INF/Code/globopt/GTOPtoolbox.rar
 *      ODTBX tolbox: http://sourceforge.net/projects/odtbx/
 *
 *    Notes
 *      Note that for near-parabolic cases, the tolerance used for the to-and-fro conversions
 *      (Tests 5 & 6) is several order of magnitudes higher than used for the regular cases. This
 *      should be investigated further in the future to fully characterize the nature of the
 *      conversions in the near-parabolic cases.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>

#include <fstream>

#include "Tudat/Astrodynamics/BasicAstrodynamics/convertMeanToEccentricAnomalies.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

using namespace mathematical_constants;
using namespace unit_conversions;
using namespace orbital_element_conversions;

BOOST_AUTO_TEST_SUITE( test_mean_to_eccentric_anomaly_conversion )

//! Error writing function.
/*!
 * This function writes the input values that led to errors to a unique file, if any errors occured
 * during the random tests. To make the file unique, the date and time of execution is added. An
 * error message also shows the location of the file.
 * \param eccentricities A vector containing the eccentricities that caused a failure.
 * \param meanAnomalies A vector containing the mean anomalies that caused a failure.
 * \param testName A string specifying the name of the test that failed.
 */
void writeErrorsToFile( std::vector< double > eccentricities, std::vector< double > meanAnomalies,
                        std::string testName )
{
    // Obtain the current time.
    const boost::posix_time::ptime now = boost::posix_time::second_clock::local_time( );

    // Make a string containing the output file name. This output file is tagged with the date and
    // time at which the code was executed. The default date format is: YYYYMMDDTHHMMSS, in which T
    // separates date and time.
    const std::string outputFileName = input_output::getTudatRootPath( ) +
            "Astrodynamics/BasicAstrodynamics/UnitTests/" +
            "ErrorReportConversionMeanToEccentricAnomaly" +
            testName + "RunAt" + boost::posix_time::to_iso_string( now )
            + ".txt";

    // Make a stream to a file.
    std::ofstream errorFile( outputFileName.c_str( ) );

    // Write an introduction in the file explaining what happened. 70 lines long.
    errorFile << "This error report was generated because the unit test for the" << std::endl
              << "conversion of mean to eccentric anomaly has failed in one of the" << std::endl
              << "random tests. To ensure the data for which it failed is not lost," << std::endl
              << "the corresponding input variables for these cases are listed below." << std::endl
              << "Please report a bug on the Tudat website (tudat.tudelft.nl), with"<< std::endl
              << "these values, so that someone will look into it and the code can be"<< std::endl
              << "improved." << std::endl << std::endl
              << "Eccentricities:           Mean anomalies:" << std::endl;

    // Set the precision for the output of the variables to 16 digits.
    errorFile.precision( 16 );

    // Add the corresponding eccentricities and mean anomalies at neatly arranged positions.
    for ( unsigned int counter = 0; counter < eccentricities.size( ); counter++ )
    {
        errorFile << std::setw( 25 ) << eccentricities[ counter ]
                  << std::setw( 25 ) << meanAnomalies[ counter ] << std::endl;
    }
    errorFile.close( );

    // Add an error message specifying the file that the values have been written to.
    std::cerr << "One or multiple errors occurred during random sampling. " << std::endl
              << "The values leading to these errors have been written to the following file: "
              << std::endl << outputFileName;
}

//! Test 1: Test conversion for circular orbits.
BOOST_AUTO_TEST_CASE( test_convertMeanAnomalyToEccentricAnomaly_circular )
{
    // Set test value for eccentricity.
    const double testEccentricity = 0.0;

    // Set test value for mean anomaly.
    const double testMeanAnomaly = 1.0472;

    // Set reference value for eccentric anomaly;
    const double referenceEccentricAnomaly = 1.0472;

    // Compute eccentric anomaly.
    const double eccentricAnomaly = convertMeanAnomalyToEccentricAnomaly( testEccentricity,
                                                                          testMeanAnomaly );

    // Check if computed eccentric anomaly is less than error tolerance.
    BOOST_CHECK_CLOSE_FRACTION( eccentricAnomaly, referenceEccentricAnomaly,
                                1.0E-13 );
}

//! Test 2: Test conversion for valid range of eccentricities.
BOOST_AUTO_TEST_CASE( test_convertMeanAnomalyToEccentricAnomaly_range )
{
    // Set test values for eccentricity.
    const double arrayOfTestEccentricities [4] = { 0.01671, 0.43582, 0.78514, 0.91525 };

    // Set test values for mean anomaly.
    const double arrayOfTestMeanAnomalies [4] = {
        convertDegreesToRadians( 60.0 ),
        convertDegreesToRadians( 90.0 ),
        convertDegreesToRadians( 120.0 ),
        convertDegreesToRadians( 220.0 ) };

    // Set reference values for eccentric anomaly. These were obtained using GTOP.
    const double arrayOfReferenceEccentricAnomalies [4] = { 1.06178920406832,
                                                            1.97200731113253,
                                                            2.5392410896466,
                                                            3.51006218528448 };

    // Loop over sets of data.
    for ( int i = 0; i < 4; i++ )
    {
        // Compute eccentric anomaly.
        const double eccentricAnomaly = convertMeanAnomalyToEccentricAnomaly(
                    arrayOfTestEccentricities[ i ], arrayOfTestMeanAnomalies[ i ] );

        // Check if computed eccentric anomaly is less than error tolerance.
        BOOST_CHECK_CLOSE_FRACTION( eccentricAnomaly, arrayOfReferenceEccentricAnomalies[ i ],
                                    1.0E-13 );
    }
}

//! Test 3: Test conversion for negative eccentricities.
BOOST_AUTO_TEST_CASE( test_convertMeanAnomalyToEccentricAnomaly_negative )
{
    // Set test value for eccentricity.
    const double testEccentricity = -0.5;

    // Set test value for mean anomaly.
    const double testMeanAnomaly = 1.0472;

    // Check if a runtime error is thrown if the anomaly is converted for this eccentricity.
    BOOST_CHECK_THROW( convertMeanAnomalyToEccentricAnomaly( testEccentricity, testMeanAnomaly ),
                       std::runtime_error );
}

//! Test 4: Test conversion for eccentricity above 1.0.
BOOST_AUTO_TEST_CASE( test_convertMeanAnomalyToEccentricAnomaly_TooHigh )
{
    // Set test value for eccentricity.
    const double testEccentricity = 2.0;

    // Set test value for mean anomaly.
    const double testMeanAnomaly = 1.0472;

    // Check if a runtime error is thrown if the anomaly is converted for this eccentricity.
    BOOST_CHECK_THROW( convertMeanAnomalyToEccentricAnomaly( testEccentricity, testMeanAnomaly ),
                       std::runtime_error );
}

//! Test 5: Test conversion for near-parabolic orbits.
BOOST_AUTO_TEST_CASE( test_convertMeanAnomalyToEccentricAnomaly_nearParabolic )
{
    // Set test value for eccentricity, which is just below  1.0.
    const double testEccentricity = 1.0 - 1.0e-15;

    // Set test values for mean anomaly, to verify that a wide range can be handled. The set was
    // selected such that it contains all possible limit cases, and a wide enough variety of random
    // values.
    const double arrayOfTestMeanAnomalies [ 17 ] = { 0.0,
                                                     1.0e-10,
                                                     0.5,
                                                     PI / 2 - 1.0e-10,
                                                     PI / 2,
                                                     PI / 2 + 1.0e-10,
                                                     2.5,
                                                     PI - 1.0e-10,
                                                     PI,
                                                     PI + 1.0e-10,
                                                     4.0,
                                                     3.0 / 2.0 * PI - 1.0e-10,
                                                     3.0 / 2.0 * PI,
                                                     3.0 / 2.0 * PI + 1.0e-10,
                                                     5.5,
                                                     2.0 * PI - 1.0e-10,
                                                     2.0 * PI };

    // Set the expected eccentric anomalies corresponding to the corresponding test mean anomaly
    // array. These values were obtained using the convertMeanAnomalyToEccentricAnomaly method,
    // and subsequently verified using the eccentric -> mean anomaly method, by running GTOP code
    // with the same values and by running ODTBX code with the same values. The iteration scheme
    // converges very rapidly except for values close to zero or 2*n*pi. Still Newton Raphson
    // converges in 20-40 iterations for these cases.
    const double arrayOfExpectedEccentricAnomalies [ 17 ] = { 0.0,
                                                              0.000843432672832182,
                                                              1.49730038909589,
                                                              2.30988145995031,
                                                              2.30988146001006,
                                                              2.30988146006981,
                                                              2.81798706288006,
                                                              3.14159265353979,
                                                              3.14159265358979,
                                                              3.14159265363979,
                                                              3.57764001198758,
                                                              3.97330384710978,
                                                              3.97330384722928,
                                                              3.97330384722972,
                                                              4.51869928040234,
                                                              6.28234187379524,
                                                              0.0 };

    // Test the values which are supposed to be 0.0.
    double eccentricAnomaly = convertMeanAnomalyToEccentricAnomaly(
                testEccentricity, arrayOfTestMeanAnomalies[ 0 ] );
    BOOST_CHECK_SMALL( eccentricAnomaly, 1.0E-9 );
    eccentricAnomaly = convertMeanAnomalyToEccentricAnomaly(
                testEccentricity, arrayOfTestMeanAnomalies[ 16 ] );
    BOOST_CHECK_SMALL( eccentricAnomaly, 1.0E-9 );

    // Test the values that are supposed to be equal to certain other values.
    for ( int counter = 1; counter < 16; counter++ )
    {
        // Compute eccentric anomaly.
        eccentricAnomaly = convertMeanAnomalyToEccentricAnomaly(
                    testEccentricity, arrayOfTestMeanAnomalies[ counter ] );

        // Check whether the expected value is set to the eccentric anomaly.
        BOOST_CHECK_CLOSE_FRACTION( eccentricAnomaly, arrayOfExpectedEccentricAnomalies[ counter ],
                                    1.0E-9 );
    }
}

//! Generalized function to test many to and fro mean to eccentric anomalies.
/*!
 *  Generalized function to test many to and fro mean to eccentric anomalies.
 *  The function allows random variations of both the mean anomlay and eccentricity. The
 *  eccentricity may be fixed, for instance to test near-parabolic orbits for many mean
 *  anomalies.
 *  \param caseId Name of test case, to be used for error output purposes.
 *  \param testTolerance Absolute acceptance tolerance to be used for difference between original
 *  and reconverted mean anomaly.
 *  \param useConstantEccentricity Boolean determining if a constant eccentricity is used.
 *  \param minimumEccentricity Minimum value to be used for random eccentricities (only used if
 *  useConstantEccentricity is false).
 *  \param maximumEccentricity Maximum value to be used for random eccentricities (only used if
 *  useConstantEccentricity is false).
 *  \param constantEccentricity Constant value to be used for eccentricity (only used if
 *  useConstantEccentricity is true).
 *  \param numberOfSamples Number of random cases that are to be tested.
 */
template< typename ScalarType >
void testMeanToEccentricAnomalyConversions(
        const std::string& caseId,
        const ScalarType testTolerance,
        const bool useConstantEccentricity = 0,
        const ScalarType minimumEccentricity = TUDAT_NAN,
        const ScalarType maximumEccentricity = TUDAT_NAN,
        const ScalarType constantEccentricity = TUDAT_NAN,
        const int numberOfSamples = 1E5 )
{
    // Create vectors that will store the input variables of a test that resulted in an error, such
    // that the error scenario can be reproduced.
    std::vector< double > failedMeanAnomalies, failedEccentricities;

    // Boolean that will be set true if a runtime error occurred.
    bool aRuntimeErrorOccurred = false;

    // Set test value for eccentricity.
    ScalarType testEccentricity = constantEccentricity;

    // Initialize both test and reverse calculated mean anomaly and the eccentric anomaly.
    ScalarType testMeanAnomaly, reverseCalculatedMeanAnomaly, eccentricAnomaly = 0.0;

    // Instantiate random number generator.
    boost::mt19937 randomNumbergenerator( time( 0 ) );

    // Create generator for eccentricity (only used if useConstantEccentricity is false).
    boost::random::uniform_real_distribution< > eccentricityDistribution;
    if( !useConstantEccentricity )
    {
                eccentricityDistribution =
                        boost::random::uniform_real_distribution< >(
                            minimumEccentricity, maximumEccentricity );
    }

    boost::variate_generator< boost::mt19937&, boost::random::uniform_real_distribution < > >
            eccentricityGenerator(
                randomNumbergenerator, eccentricityDistribution );

    // Create generator for mean anomaly.
    boost::random::uniform_real_distribution< ScalarType > distribution0To2Pi(
                getFloatingInteger< ScalarType >( 0 ), getFloatingInteger< ScalarType >( 2 ) *
                getPi< ScalarType >( ) );
    boost::variate_generator<
            boost::mt19937&, boost::random::uniform_real_distribution < ScalarType > >
            generateRandomNumber0To2Pi( randomNumbergenerator, distribution0To2Pi );

    // Perform the conversion for the specified number of samples and test whether the values that
    // are subsequently converted back match the initial values.
    for ( int counter = 0; counter < numberOfSamples; counter++ )
    {
        // Set random value in test mean anomaly.
        testMeanAnomaly = generateRandomNumber0To2Pi( );

        // If eccentricity is to be varied, generate random value
        if( !useConstantEccentricity )
        {
            testEccentricity = eccentricityGenerator( );
        }

        // If the Rootfinder does not converge, it will produce a runtime error. In order to make
        // sure that these values that led to the error will not be lost, they will be stored in
        // the failed input data vectors. To do so, a try-catch sequence is used.
        try
        {
            // Compute eccentric anomaly.
            eccentricAnomaly = convertMeanAnomalyToEccentricAnomaly< ScalarType>(
                        testEccentricity, testMeanAnomaly );
        }
        catch( std::runtime_error )
        {
            // Store the fact that a runtime error occurred, such that the values will be stored.
            aRuntimeErrorOccurred = true;
        }

        // Calculate the mean anomaly from this eccentric anomaly.
        reverseCalculatedMeanAnomaly = convertEccentricAnomalyToMeanAnomaly< ScalarType>(
                    eccentricAnomaly, testEccentricity );

        // Test whether the computed mean anomaly is equal to the mean anomaly from the input and
        // that no runtime errors occurred. If an error was found, store the values leading to this
        // error in a vector for later use. '!' operator is there to ensure that a NaN value will
        // result in the values being written away. It is also checked that the mean anomaly is
        // not equal to 0.0, because that would result in falsely writing an error.
        if ( ( ( !( std::abs( testMeanAnomaly - reverseCalculatedMeanAnomaly ) < testTolerance ) )
               && !( testMeanAnomaly == getFloatingInteger< ScalarType >( 0 ) ||
                  reverseCalculatedMeanAnomaly == getFloatingInteger< ScalarType >( 0 ) ) ) &&
             !aRuntimeErrorOccurred )
        {
            failedMeanAnomalies.push_back( testMeanAnomaly );
            failedEccentricities.push_back( testEccentricity );
        }

        // Reset boolean.
        aRuntimeErrorOccurred = false;

    }

    // Check that no values have been written to the failedMeanAnomalies vector.  If so, this test
    // is passed. Otherwisely these values will be written away and this test will fail.
    BOOST_CHECK( failedMeanAnomalies.empty( ) );

    // If the vector is not empty, write the failed cases of this test case to a file.
    if ( !( failedMeanAnomalies.empty( ) ) )
    {
        writeErrorsToFile( failedEccentricities, failedMeanAnomalies, caseId );
    }
}

//! Test 6: Test large number of anomalies for near-parabolic orbits (double).
BOOST_AUTO_TEST_CASE( test_convertMeanAnomalyToEccentricAnomaly_nearParabolic_random_double )
{
    long double ratioOfPrecision = std::numeric_limits< long double >::epsilon( ) /
            std::numeric_limits< double >::epsilon( );

    // Test random conversions for near-parabolic orbits
    testMeanToEccentricAnomalyConversions< double >(
                "DoubleParabolic", 1.0E-13, 1, TUDAT_NAN, TUDAT_NAN, 1.0 - 1.0e-15 );
    testMeanToEccentricAnomalyConversions< long double >(
                "LongDoubleParabolic", 1.0E-13L * ratioOfPrecision, 1, TUDAT_NAN, TUDAT_NAN,
                1.0L - 1.0e-15L * ratioOfPrecision );

    // Test random conversions for random orbits
    testMeanToEccentricAnomalyConversions< double >(
                "DoubleEccentric", 1.0E-15, 0, 0.0, 1.0 - 1.0e-11 );
    testMeanToEccentricAnomalyConversions< long double >(
                "LongDoubleEccentric", 1.0E-15L * ratioOfPrecision, 0, 0.0,
                1.0L - 1.0e-11L * ratioOfPrecision );
}

//! Test 7: Test functionality of specifying the initial guess. Case contained in Test 2 also.
BOOST_AUTO_TEST_CASE( test_convertMeanAnomalyToEccentricAnomaly_specificInitialGuess )
{
    // Set test value for eccentricity.
    const double testEccentricity = 0.78514;

    // Set test value for mean anomaly.
    const double testMeanAnomaly = convertDegreesToRadians( 120.0 );

    // Set reference value for eccentric anomaly;
    const double referenceEccentricAnomaly = 2.5392410896466027;

    // Specify initial guess.
    const bool useDefaultGuess = false;
    const double initialGuess = PI;

    // Compute eccentric anomaly.
    const double eccentricAnomaly = convertMeanAnomalyToEccentricAnomaly(
                testEccentricity, testMeanAnomaly, useDefaultGuess, initialGuess );

    // Check if computed eccentric anomaly is less than error tolerance.
    BOOST_CHECK_CLOSE( eccentricAnomaly,
                       referenceEccentricAnomaly,
                       1.0E-13 );
}

// End Boost test suite.
BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


/*!
@file
Defines `boost::hana::Orderable::laws`.

@copyright Louis Dionne 2014
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_HANA_ORDERABLE_LAWS_HPP
#define BOOST_HANA_ORDERABLE_LAWS_HPP

#include <boost/hana/orderable/orderable.hpp>

#include <boost/hana/logical/logical.hpp>
#include <boost/hana/searchable/searchable.hpp>

namespace boost { namespace hana {
    struct Orderable::laws {
        template <typename Orderables>
        static constexpr auto check(Orderables orderables) {
            auto implies = [](auto p, auto q) { return or_(not_(p), q); };
            auto incomparable = [](auto x, auto y) {
                return not_(or_(less(x, y), less(y, x)));
            };
            return all(orderables, [=](auto a) {
                return all(orderables, [=](auto b) {
                    return all(orderables, [=](auto c) {
                        return and_(
                            not(less(a, a)),
                            implies(less(a, b), not_(less(b, a))),
                            implies(and_(less(a, b), less(b, c)), less(a, c)),
                            implies(
                                and_(incomparable(a, b), incomparable(b, c)),
                                incomparable(a, c)
                            )
                        );
                    });
                });
            });
        }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_ORDERABLE_LAWS_HPP