#include <functional>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>
#include <ros/ros.h>
#include <rocket/FiveThrustCommand.h>
#include <atom_esp_joy/joydata.h>

#include <boost/bind.hpp>
#include <gazebo/common/common.hh>
#include <gazebo/common/PID.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <gazebo_msgs/ModelStates.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <stdio.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <std_srvs/Empty.h>

#include <geometry_msgs/Vector3.h>
//#include <tf/Quaternion.h>

using gazebo::common::PID;
using gazebo::common::Time;

/*
Only for reference

    private: void joystate_cb(const atom_esp_joy::joydata::ConstPtr& _joy_data_msg)
    {
	this->joy_data = *_joy_data_msg;
        this->addaccel_center = this->joy_data.S;

      this->addaccel_side_1 = this->addaccel_center - this->joy_data.X;
      this->addaccel_side_2 = this->addaccel_center + this->joy_data.X;
      this->addaccel_side_3 = this->addaccel_center - this->joy_data.Y;
      this->addaccel_side_4 = this->addaccel_center + this->joy_data.Y;

	ROS_INFO_THROTTLE(5,"Joy msg received! %f %f", (float)this->joy_data.S, this->addaccel_center);
    }

*/

class _PID
{
	//public:
	//_PID(double _P, double _I, double _D);
	//void Update_Param(double _P, double _I, double _D);
	//void Reset();
	//double Update(double err, double dt);

	private:
	double integral_error;
	double last_error;
	double p, i , d;

	public: _PID(double _P, double _I, double _D){p=_P; i=_I; d=_D;integral_error=0;last_error=0;}
	public: void Update_Param(double _P, double _I, double _D){p=_P; i=_I; d=_D; this->Reset();}
	public: double Update(double err, double dt)
	{

		float error_derivative = (err - last_error)/dt;

		integral_error += err*dt;

		return (err*p + error_derivative*d + integral_error*i);

	}
	public: void Reset()
	{
		integral_error = 0;
		last_error = 0;
	}

};

geometry_msgs::Vector3 g_target_pos;//(0,0,0.2);
nav_msgs::Odometry g_input_odom;
geometry_msgs::Vector3 g_pid_param_top,  g_pid_param_side,  g_pid_param_front; //(500,0,0);
//double intError = 0; // PID interative error
//float errorPlast = 0;
ros::Time last_time;
rocket::FiveThrustCommand g_LastReqThrust;

_PID pid_center(150, 0.2, 1);
_PID pid_side(35, 0.01, 2);
_PID pid_front(35, 0.01, 2);

void calculateReqThrust(const nav_msgs::Odometry *odom, const geometry_msgs::Vector3 *targetPos, rocket::FiveThrustCommand *reqThrust)
{
	// probably will need PID trying with P only
        //static gazebo::common::PID pid_center(40,25,15,10, 0.1, 500, 10 );
        //static gazebo::common::PID pid_side(40,25,15,10, 0.1, 500, 10 );
        //static gazebo::common::PID pid_front(40,25,15,10, 0.1, 500, 10 );

        float center_thrust,
	right_plus_thrust, // opposite has to be applied
	right_minus_thrust,
        front_plus_thrust,
	front_minus_thrust;

        float right_plus_pos  = odom->pose.pose.position.x;
        float front_plus_pos  = odom->pose.pose.position.y;
        float center_plus_pos = odom->pose.pose.position.z;

        float right_vel  = odom->twist.twist.linear.x;
        float front_vel  = odom->twist.twist.linear.y;
        float center_vel = odom->twist.twist.linear.z;

        tf::Quaternion q(odom->pose.pose.orientation.x , odom->pose.pose.orientation.y, odom->pose.pose.orientation.z, odom->pose.pose.orientation.w);
	double roll, pitch, yaw;
	tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

	ROS_INFO_THROTTLE(1, "%2.3f, %2.3f, %2.3f, %2.3f, %2.3f, %2.3f", roll, pitch, yaw, right_vel, front_vel, center_vel);

        float right_plus_target  = targetPos->x;
        float front_plus_target  = targetPos->y;
        float center_plus_target = targetPos->z;
	//gazebo::common::Time
	ros::Time curr_time = ros::Time::now();

// https://www.ode-wiki.org/wiki/index.php?title=HOWTO_thrust_control_logic
// Owen Jones
	const ignition::math::Vector3<double> LOOKAHEAD(0.0f, 0.0f, /*(odom->twist.twist.linear.z>=0?1:-1)*/g_pid_param_top.x);
	const ignition::math::Vector3<double> SENSITIVITY(0.0f, 0.0f, g_pid_param_top.y);

	ignition::math::Vector3<double> future_pose(0.0f, 0.0f, 0.0f);
	ignition::math::Vector3<double> current_pose(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z);
	ignition::math::Vector3<double> target_pose(targetPos->x, targetPos->y, targetPos->z);
	ignition::math::Vector3<double> current_vel(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z);
	ignition::math::Vector3<double> applicable_force(0.0f, 0.0f, 0.0f);

	future_pose = current_pose + LOOKAHEAD * current_vel;
	applicable_force = (future_pose - target_pose) * SENSITIVITY;
	g_LastReqThrust.thrust_center = applicable_force.Z();

	//float errorP = center_plus_target - center_plus_pos;

	float dt = (curr_time-last_time).toSec();

	//g_LastReqThrust.thrust_center = pid_center.Update(errorP, dt);

        g_LastReqThrust.thrust_center = g_LastReqThrust.thrust_center > 0 ? g_LastReqThrust.thrust_center : 0 ;
        g_LastReqThrust.thrust_center = g_LastReqThrust.thrust_center > g_pid_param_top.z ? g_pid_param_top.z : g_LastReqThrust.thrust_center ;

	center_thrust = g_LastReqThrust.thrust_center;

	//roll applicable_force.Y(); //
	g_LastReqThrust.thrust_side_1 = pid_front.Update(-roll, dt);
	g_LastReqThrust.thrust_side_2 = -g_LastReqThrust.thrust_side_1; //pid_front.Update(roll, dt);

        g_LastReqThrust.thrust_side_1 = g_LastReqThrust.thrust_side_1 > 0 ? g_LastReqThrust.thrust_side_1 : 0 ;
        g_LastReqThrust.thrust_side_2 = g_LastReqThrust.thrust_side_2 > 0 ? g_LastReqThrust.thrust_side_2 : 0 ;

	//pitch // applicable_force.X(); //
	g_LastReqThrust.thrust_side_3 = pid_side.Update(-pitch, dt);
	g_LastReqThrust.thrust_side_4 = -g_LastReqThrust.thrust_side_3 ;//pid_side.Update(pitch, dt);

        g_LastReqThrust.thrust_side_3 = g_LastReqThrust.thrust_side_3 > 0 ? g_LastReqThrust.thrust_side_3 : 0 ;
        g_LastReqThrust.thrust_side_4 = g_LastReqThrust.thrust_side_4 > 0 ? g_LastReqThrust.thrust_side_4 : 0 ;

	g_LastReqThrust.thrust_side_1 = g_LastReqThrust.thrust_side_1 < 20 ? g_LastReqThrust.thrust_side_1 : 20;
	g_LastReqThrust.thrust_side_2 = g_LastReqThrust.thrust_side_2 < 20 ? g_LastReqThrust.thrust_side_2: 20;
	g_LastReqThrust.thrust_side_3 = g_LastReqThrust.thrust_side_3 < 20 ? g_LastReqThrust.thrust_side_3 : 20;
	g_LastReqThrust.thrust_side_4 = g_LastReqThrust.thrust_side_4 < 20 ? g_LastReqThrust.thrust_side_4 :20;

        *reqThrust = g_LastReqThrust;
	last_time = curr_time;
	//errorPlast = errorP;

/*
        ros::Time curr_time = ros::Time::now();
	//nav_msgs::Odometry odom;
	//odom->header.stamp = curr_time;
	//odom->header.frame_id = "odom";

	// set the position
	odom->pose.pose.position.x, y, z
	odom->pose.pose.orientation.x , y, z, w
	// set the velocity
	//odom->child_frame_id = this->model->GetName();
	// set the twists
	odom->twist.twist.linear.x, y, z
        odom->twist.twist.angular.x, y, z
*/
}

void target_pos_cb(const geometry_msgs::Vector3 _target_pos)
{
	g_target_pos = _target_pos;
        ROS_INFO("target received");
}

void pidparam_top_cb(const geometry_msgs::Vector3 _pid_param)
{
	g_pid_param_top = _pid_param;
	pid_center.Update_Param(g_pid_param_top.x, g_pid_param_top.y, g_pid_param_top.z);
	//pid_side.Update_Param(g_pid_param.x, g_pid_param.z, g_pid_param.y);
	//pid_front.Update_Param(g_pid_param.x, g_pid_param.z, g_pid_param.y);
	pid_side.Reset();
	pid_front.Reset();
	//intError = 0;
	//errorPlast = 0;
	last_time = ros::Time::now();
        g_LastReqThrust.thrust_center = 0;
	g_LastReqThrust.thrust_side_1 = 0;
	g_LastReqThrust.thrust_side_2 = 0;
	g_LastReqThrust.thrust_side_3 = 0;
	g_LastReqThrust.thrust_side_4 = 0;

        std_srvs::Empty resetWorldSrv;
	ros::service::call("/gazebo/reset_world", resetWorldSrv);
	//reset worls and pos to the bot
        ROS_INFO("pid params received");
}

void pidparam_front_cb(const geometry_msgs::Vector3 _pid_param)
{
	g_pid_param_front = _pid_param;
	//pid_center.Update_Param(g_pid_param.x, g_pid_param.z, g_pid_param.y);
	//pid_side.Update_Param(g_pid_param.x, g_pid_param.z, g_pid_param.y);
	pid_front.Update_Param(g_pid_param_front.x, g_pid_param_front.y, g_pid_param_front.z);
	pid_center.Reset();
	pid_side.Reset();
	//intError = 0;
	//errorPlast = 0;
	last_time = ros::Time::now();
        g_LastReqThrust.thrust_center = 0;
	g_LastReqThrust.thrust_side_1 = 0;
	g_LastReqThrust.thrust_side_2 = 0;
	g_LastReqThrust.thrust_side_3 = 0;
	g_LastReqThrust.thrust_side_4 = 0;

        //std_srvs::Empty resetWorldSrv;
	//ros::service::call("/gazebo/reset_world", resetWorldSrv);
	//reset worls and pos to the bot
        ROS_INFO("pid params received");
}

void pidparam_side_cb(const geometry_msgs::Vector3 _pid_param)
{
	g_pid_param_side = _pid_param;
	//pid_center.Update_Param(g_pid_param.x, g_pid_param.z, g_pid_param.y);
	pid_side.Update_Param(g_pid_param_side.x, g_pid_param_side.y, g_pid_param_side.z);
	//pid_front.Update_Param(g_pid_param.x, g_pid_param.z, g_pid_param.y);
	pid_center.Reset();
	pid_front.Reset();

        pidparam_front_cb(_pid_param);
	//intError = 0;
	//errorPlast = 0;
	last_time = ros::Time::now();
        g_LastReqThrust.thrust_center = 0;
	g_LastReqThrust.thrust_side_1 = 0;
	g_LastReqThrust.thrust_side_2 = 0;
	g_LastReqThrust.thrust_side_3 = 0;
	g_LastReqThrust.thrust_side_4 = 0;

        std_srvs::Empty resetWorldSrv;
	ros::service::call("/gazebo/reset_world", resetWorldSrv);
	//reset worls and pos to the bot
        ROS_INFO("pid params received");
}

void odom_cb(const nav_msgs::Odometry _input_odom)
{

	g_input_odom = _input_odom;
	ROS_INFO_THROTTLE(10, "odom received");

}

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

  ros::init(argc, argv, "rocket_stabilization_node");
  ROS_INFO("stabilization control -> starting up...");

  g_target_pos.x = 0;
  g_target_pos.y = 0;
  g_target_pos.z = 20;

  g_pid_param_top.x = 0.10;
  g_pid_param_top.y = 0.10;
  g_pid_param_top.z = 80.0;

  ros::NodeHandle nh_;

  ros::Rate loop_rate(100);

  ros::Subscriber target_sub = nh_.subscribe("rocket_target_pos", 100, target_pos_cb);

  ros::Subscriber odom_sub = nh_.subscribe("/odom", 100, odom_cb);

  ros::Subscriber pidparam_top_sub = nh_.subscribe("rocket_pid_param_top", 10, pidparam_top_cb);
  ros::Subscriber pidparam_side_sub = nh_.subscribe("rocket_pid_param_side", 10, pidparam_side_cb);
  //ros::Subscriber pidparam_front_sub = nh_.subscribe("rocket_pid_param_front", 10, pidparam_front_cb);

  ros::Publisher rocket_5_thrusts_pub = nh_.advertise<rocket::FiveThrustCommand>("rocket_stabilization_control", 100);

  // setRightForce(center+(X))
  // setFrontForce(center+(Y))
  // setAltitudeForce(center)

  while(ros::ok())
  {
    	rocket::FiveThrustCommand five_thrusts;
        calculateReqThrust(&g_input_odom, &g_target_pos, &five_thrusts);
	rocket_5_thrusts_pub.publish(five_thrusts);

	ros::spinOnce();

        loop_rate.sleep();

  }

}


///////////////////////////////////////////////////////////////
//  Copyright 2019 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include "../performance_test.hpp"
#if defined(TEST_CPP_BIN_FLOAT)
#include <boost/multiprecision/cpp_bin_float.hpp>
#endif

void test34()
{
#ifdef TEST_CPP_BIN_FLOAT
   test<boost::multiprecision::number<boost::multiprecision::cpp_bin_float<500> > >("cpp_bin_float", 500);
#endif
}


//Link to Boost
#define BOOST_TEST_DYN_LINK

//VERY IMPORTANT - include this last
//#include <boost/test/included/unit_test.hpp>
#include <boost/test/unit_test.hpp>

#include "test.h"
#include "../InfiniteRoots.h"
using namespace std::literals::complex_literals;

// test suite
BOOST_FIXTURE_TEST_SUITE(InfiniteRoots_suite, SimpleTestFixture, * utf::label("InfiniteRoots"))

BOOST_DATA_TEST_CASE(twoRoots_test, bdata::random(0, 8) ^ bdata::random(2, 8) ^ bdata::xrange(20), diffLM, M, index)
{
    int L =  2 * M + diffLM;
    int m = 2;
    InfiniteRoots ir(L, M);
    std::vector<var_t>& roots = ir.getRoot(m);
    var_t r1 = 2.0L * L / var_t(3.0L + L - 2.0L * M, sqrt(3.0L + L - 2.0L * M));
    var_t r2 = 2.0L * L / var_t(3.0L + L - 2.0L * M, -sqrt(3.0L + L - 2.0L * M));
    BOOST_TEST(std::abs(roots[0] - std::conj(roots[1])) < EPS);
    BOOST_TEST(std::abs(roots[0].real() - r1.real()) < EPS);
    BOOST_TEST(abs(abs(roots[0].imag()) - abs(r1.imag())) < EPS);
}

BOOST_DATA_TEST_CASE(moreThanTwoRoots_test, bdata::random(0, 8) ^ bdata::random(3, 16) ^ bdata::xrange(20), diffLM, M, index)
{
    int L = 2 * M + diffLM;
    int m = random64(M - 2) + 2;
    InfiniteRoots ir(L, M);
    std::vector<var_t>& roots = ir.getRoot(m);
    int R = L - 2 * M + 2 * m;
    std::cout <<"L=" << L << ", M=" << M << ", roots=" << roots << std::endl;
    for (int i = 0; i < m; i++) {
        var_t res = roots[i] * (elem_t)R - 2.0L * L;
        for (int j = 0; j < m; j++) {
            if (j == i) continue;
            res += 2.0L * roots[i] * roots[j] / (roots[i] - roots[j]);
        }
        BOOST_TEST_INFO("m=" << m << ", i=" << i);
        BOOST_TEST(std::abs(res) < 1e-8);
    }
}

BOOST_AUTO_TEST_SUITE_END()


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include "fputils/log_space_matrix_multiply.hpp"
#include "test_case.hpp"

using fputils::Matrix ;

bool operator==( Matrix const& left, Matrix const& right ) {
	if( left.size1() != right.size1() || left.size2() != right.size2() ) {
		return false ;
	}

	for( std::size_t i = 0; i < left.size1(); ++i ) {
		for( std::size_t j = 0; j < left.size2(); ++j ) {
			if( left( i, j ) != right( i, j )) {
				return false ;
			}
		}
	}

	return true ;
}

bool operator!=( Matrix const& left, Matrix const& right ) {
	return !(left == right ) ;
}

AUTO_TEST_CASE( test_log_space_matrix_multiply ) {
	std::cerr << "test_log_space_matrix_multiply...\n" ;
	using fputils::ConstantMatrix ;
	typedef std::pair< Matrix, Matrix > P ;
	std::vector< P > multiplicands ;
	std::vector< Matrix > expected_results ;

	// multiply 1x1 matrices
	multiplicands.push_back(
		P(
			ConstantMatrix( 1, 1, 0.0 ),
			ConstantMatrix( 1, 1, 0.0 )
		)
	) ;

	expected_results.push_back( ConstantMatrix( 1, 1, 0.0 ) ) ;

	multiplicands.push_back(
		P(
			ConstantMatrix( 1, 1, 0.0 ),
			ConstantMatrix( 1, 1, 1.0 )
		)
	) ;

	expected_results.push_back( ConstantMatrix( 1, 1, 1.0 ) ) ;

	multiplicands.push_back(
		P(
			ConstantMatrix( 1, 1, 1.0 ),
			ConstantMatrix( 1, 1, 0.0 )
		)
	) ;

	expected_results.push_back( ConstantMatrix( 1, 1, 1.0 ) ) ;

	multiplicands.push_back(
		P(
			ConstantMatrix( 1, 1, 1.0 ),
			ConstantMatrix( 1, 1, 1.0 )
		)
	) ;

	expected_results.push_back( ConstantMatrix( 1, 1, 2.0 ) ) ;

	// multiply 1x2 by 2x1 matrices
	multiplicands.push_back(
		P(
			ConstantMatrix( 1, 2, 0.0 ),
			ConstantMatrix( 2, 1, 0.0 )
		)
	) ;

	expected_results.push_back( ConstantMatrix( 1, 1, std::log( 2.0 ))) ;

	multiplicands.push_back(
		P(
			ConstantMatrix( 1, 2, 0.0 ),
			ConstantMatrix( 2, 1, 1.0 )
		)
	) ;

	expected_results.push_back( ConstantMatrix( 1, 1, std::log( 2.0 ) + 1.0 )) ;

	multiplicands.push_back(
		P(
			ConstantMatrix( 1, 2, 1.0 ),
			ConstantMatrix( 2, 1, 0.0 )
		)
	) ;

	expected_results.push_back( ConstantMatrix( 1, 1, std::log( 2.0 ) + 1.0 )) ;

	multiplicands.push_back(
		P(
			ConstantMatrix( 1, 2, 1.0 ),
			ConstantMatrix( 2, 1, 1.0 )
		)
	) ;

	expected_results.push_back( ConstantMatrix( 1, 1, std::log( 2.0 ) + 2.0 )) ;

	// multiply 2x2 by 2x2 matrices
	multiplicands.push_back(
		P(
			ConstantMatrix( 2, 2, 0.0 ),
			ConstantMatrix( 2, 2, 0.0 )
		)
	) ;

	expected_results.push_back( ConstantMatrix( 2, 2, std::log( 2.0 ))) ;

	multiplicands.push_back(
		P(
			ConstantMatrix( 2, 2, 0.0 ),
			ConstantMatrix( 2, 2, 1.0 )
		)
	) ;

	expected_results.push_back( ConstantMatrix( 2, 2, std::log( 2.0 ) + 1.0 )) ;

	multiplicands.push_back(
		P(
			ConstantMatrix( 2, 2, 1.0 ),
			ConstantMatrix( 2, 2, 0.0 )
		)
	) ;

	expected_results.push_back( ConstantMatrix( 2, 2, std::log( 2.0 ) + 1.0 )) ;

	multiplicands.push_back(
		P(
			ConstantMatrix( 2, 2, 1.0 ),
			ConstantMatrix( 2, 2, 1.0 )
		)
	) ;

	expected_results.push_back( ConstantMatrix( 2, 2, std::log( 2.0 ) + 2.0 )) ;

	assert( multiplicands.size() == expected_results.size() ) ;
	for( std::size_t i = 0; i < multiplicands.size(); ++i ) {
		std::cerr << "Test case " << i+1 << " of " << multiplicands.size() << "...\n" ;
		Matrix m = fputils::log_space_matrix_multiply( multiplicands[i].first, multiplicands[i].second ) ;
		if( m != expected_results[i] ) {
			std::cerr << "Multiplying\n  " << multiplicands[i].first << "\n  by\n" << multiplicands[i].second << ",\n" ;
			std::cerr << "Expected:\n  " << expected_results[i] << "\n  but got\n" << m << ".\n" ;
		}
		TEST_ASSERT( m == expected_results[i] ) ;
	}
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    int n; cin >> n;
    cpp_int ans = 0;
    vector<cpp_int> v(5, 0);
    for (int i = 0; i < n; i++) {
        string s; cin >> s;
        if (s[0] == 'M') v[0]++;
        if (s[0] == 'A') v[1]++;
        if (s[0] == 'R') v[2]++;
        if (s[0] == 'C') v[3]++;
        if (s[0] == 'H') v[4]++;
    }
    for (int i = 0; i < 3; i++) {
        for (int j = i + 1; j < 4; j++) {
            for (int k = j + 1; k < 5; k++) {
                ans += v[i] * v[j] * v[k];
            }
        }
    }
    cout << ans << endl;
}


#pragma once

#include <Eigen/Dense>

#include "Point.hpp"

namespace Geometry2d {
// A 2x3 transformation matrix.
//
// This is the 2D equivalent of the usual 3D tranformation matrix with the
// bottom row omitted because the bottom (third) element of a 2D point is always
// 1.  The third row of the matrix is understood to be [0 0 1].
class TransformMatrix {
public:
    TransformMatrix() {
        _m[0] = 1;
        _m[1] = 0;
        _m[2] = 0;
        _m[3] = 0;
        _m[4] = 1;
        _m[5] = 0;
    }

    TransformMatrix(float a, float b, float c, float d, float e, float f) {
        _m[0] = a;
        _m[1] = b;
        _m[2] = c;
        _m[3] = d;
        _m[4] = e;
        _m[5] = f;
    }

    TransformMatrix(Geometry2d::Point origin, float rotation = 0,
                    bool mirror = false, float scale = 1);

    TransformMatrix(const Eigen::Matrix<double, 3, 3>& other) {
        _m[0] = other(0, 0);
        _m[1] = other(0, 1);
        _m[2] = other(0, 2);
        _m[3] = other(1, 0);
        _m[4] = other(1, 1);
        _m[5] = other(1, 2);
    }

    TransformMatrix operator*(const TransformMatrix& other) const {
        float a = _m[0] * other._m[0] + _m[1] * other._m[3];
        float b = _m[0] * other._m[1] + _m[1] * other._m[4];
        float c = _m[0] * other._m[2] + _m[1] * other._m[5] + _m[2];
        float d = _m[3] * other._m[0] + _m[4] * other._m[3];
        float e = _m[3] * other._m[1] + _m[4] * other._m[4];
        float f = _m[3] * other._m[2] + _m[4] * other._m[5] + _m[5];

        return TransformMatrix(a, b, c, d, e, f);
    }

    TransformMatrix& operator*=(const TransformMatrix& other) {
        float a = _m[0] * other._m[0] + _m[1] * other._m[3];
        float b = _m[0] * other._m[1] + _m[1] * other._m[4];
        float c = _m[0] * other._m[2] + _m[1] * other._m[5] + _m[2];
        float d = _m[3] * other._m[0] + _m[4] * other._m[3];
        float e = _m[3] * other._m[1] + _m[4] * other._m[4];
        float f = _m[3] * other._m[2] + _m[4] * other._m[5] + _m[5];

        _m[0] = a;
        _m[1] = b;
        _m[2] = c;
        _m[3] = d;
        _m[4] = e;
        _m[5] = f;

        return *this;
    }

    operator Eigen::Matrix<double, 3, 3>() const {
        Eigen::Matrix<double, 3, 3> result;
        result << _m[0], _m[1], _m[2], _m[3], _m[4], _m[5], 0, 0, 1;
        return result;
    }

    Point operator*(const Point& pt) const {
        return Point(pt.x() * _m[0] + pt.y() * _m[1] + _m[2],
                     pt.x() * _m[3] + pt.y() * _m[4] + _m[5]);
    }

    // Transforms a direction vector (3rd element is zero)
    Point transformDirection(const Point& dir) const {
        return Point(dir.x() * _m[0] + dir.y() * _m[1],
                     dir.x() * _m[3] + dir.y() * _m[4]);
    }

    // Transforms the given angle in radians
    float transformAngle(float angle) const;

    // Returns the vector that represents the direction of the transformed
    // X-axis.
    Point x() const { return Point(_m[0], _m[3]); }

    // Returns the vector that represents the direction of the transformed
    // Y-axis.
    Point y() const { return Point(_m[1], _m[4]); }

    // Returns the origin of the transformed coordinate system
    Point origin() const { return Point(_m[2], _m[5]); }

    // Returns the scaling along the transformed X-axis.
    float xScale() const { return x().mag(); }

    // Returns the scaling along the transformed Y-axis.
    float yScale() const { return y().mag(); }

    // Returns the clockwise angle from the transformed Y axis to the original Y
    // axis. This is not affected by horizontal reflection.
    float rotation() const;

    // Returns true if the coordinate system has been mirrored (i.e. is now
    // left-handed).
    bool mirrored() const;

    const float* m() const { return _m; }

    ////////////////
    // Functions to build common transformations:

    // Translation
    static TransformMatrix translate(const Point& delta) {
        return TransformMatrix(1, 0, delta.x(), 0, 1, delta.y());
    }

    static TransformMatrix translate(float x, float y) {
        return translate(Point(x, y));
    }

    // Rotation in radians around origin
    static TransformMatrix rotate(float angle) {
        float c = cos(angle);
        float s = sin(angle);

        return TransformMatrix(c, -s, 0, s, c, 0);
    }

    // Uniform scale
    static TransformMatrix scale(float s) {
        return TransformMatrix(s, 0, 0, 0, s, 0);
    }

    // Non-uniform scale
    static TransformMatrix scale(float x, float y) {
        return TransformMatrix(x, 0, 0, 0, y, 0);
    }

    // Returns a matrix to rotate <angle> radians CCW around <center>.
    static TransformMatrix rotateAroundPoint(const Point& center, float angle);

    // Returns a matrix to reflect along the line parallel to the Y-axis
    // containing <center>.
    static TransformMatrix mirrorAroundPoint(const Point& center);

    static const TransformMatrix identity;
    static const TransformMatrix mirrorX;

protected:
    // Matrix values in row-major order.
    //
    // Indices:
    //   [0 1 2
    //    3 4 5]
    float _m[6];
};
}  // namespace Geometry2d


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#ifndef BOOST_SIMD_NO_NANS
#include <boost/simd/function/scalar/is_nan.hpp>
#endif
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/exp.hpp>
#include <boost/simd/function/scalar/log1p.hpp>
#include <boost/simd/function/scalar/max.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

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

  BOOST_DISPATCH_OVERLOAD ( logspace_add_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      A0 tmp = -bs::abs(a0-a1);
      A0 r = bs::max(a0,a1)+bs::log1p(bs::exp(tmp));
    #ifndef BOOST_SIMD_NO_NANS
      return is_nan(tmp) ? a0+a1 : r;
    #else
      return r;
    #endif
    }
  };
} } }

#endif


/*
 * Shared copyright notice and LGPLv3 license statement.
 *
 * Copyright (C) 2010 The Board of Trustees of The Leland Stanford Junior University. All rights reserved.
 * Copyright (C) 2010 University of Texas at Austin. All rights reserved.
 *
 * Authors: Roland Philippsen (Stanford) and Luis Sentis (UT Austin)
 *          http://cs.stanford.edu/group/manips/
 *          http://www.me.utexas.edu/~hcrl/
 *
 * This program is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this program.  If not, see
 * <http://www.gnu.org/licenses/>
 */

#include <utils/pseudo_inverse.hpp>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <stdio.h>

using namespace std;

namespace sejong {

    void pseudoInverse(Matrix const & matrix,
                       double sigmaThreshold,
                       Matrix & invMatrix,
                       Vector * opt_sigmaOut)    {

        if ((1 == matrix.rows()) && (1 == matrix.cols())) {
            // workaround for Eigen2
            invMatrix.resize(1, 1);
            if (matrix.coeff(0, 0) > sigmaThreshold) {
                invMatrix.coeffRef(0, 0) = 1.0 / matrix.coeff(0, 0);
            }
            else {
                invMatrix.coeffRef(0, 0) = 0.0;
            }
            if (opt_sigmaOut) {
                opt_sigmaOut->resize(1);
                opt_sigmaOut->coeffRef(0) = matrix.coeff(0, 0);
            }
            return;
        }

        Eigen::JacobiSVD<Matrix> svd(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
        // not sure if we need to svd.sort()... probably not
        int const nrows(svd.singularValues().rows());
        Matrix invS;
        invS = Matrix::Zero(nrows, nrows);
        for (int ii(0); ii < nrows; ++ii) {
            if (svd.singularValues().coeff(ii) > sigmaThreshold) {
                invS.coeffRef(ii, ii) = 1.0 / svd.singularValues().coeff(ii);
            }
            else{
                // invS.coeffRef(ii, ii) = 1.0/ sigmaThreshold;
                // printf("sigular value is too small: %f\n", svd.singularValues().coeff(ii));
            }
        }
        invMatrix = svd.matrixV() * invS * svd.matrixU().transpose();
        if (opt_sigmaOut) {
            *opt_sigmaOut = svd.singularValues();
        }
    }

}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE CISHW1Test

// system includes
#include <boost/test/unit_test.hpp>
#include <exception>
#include <unistd.h>
#include <boost/math/constants/constants.hpp>
#include <iostream>
#include <vector>
#include <boost/bind.hpp>
#include <boost/math/special_functions/binomial.hpp>

// local includes
#include "matrixOperations.hpp"
#include "parsePA1_2.hpp"
#include "PivotCalibration.hpp"
#include "PointEstimation.hpp"
#include "PA1_2_DataConstants.hpp"
#include "DistortionCalibration.hpp"

static const bool debug = false;
static const double tolerance = 0.01l;

/// @todo The unit test requires a symlink from the executable folder to the location of the PA1-2 data folder, fix this using CMake.

template<typename T>
bool isWithinTolerance(const T& result, const T& groundTruth, double toleranceVal = tolerance){
    return (result.isApprox(groundTruth,toleranceVal) || (result - groundTruth).norm() < toleranceVal);
}

BOOST_AUTO_TEST_SUITE(cisPA2test)

BOOST_AUTO_TEST_CASE(BernsteinTest)
{

    // Testing
    double test = boost::math::binomial_coefficient<double>(3, 1);
    if(debug) std::cout << "\n\nbinomial coefficient test is " << test << std::endl;
    double a=5.0;
    int b=3;
    int c=1;
    double Btest = BersteinPolynomial(a, b, c);
    if(debug) std::cout << "\n\nBtest is " << Btest << std::endl;

    Eigen::MatrixXd test2(1,3);
    test2 << 0, 0.5, 1;
    Eigen::MatrixXd TestF = FMatrix(test2);
    if(debug) std::cout << "\n\nF is " << TestF.transpose() << std::endl;
    //std::cout << "\n\nThe size of F is " << TestF.rows() << "x" << TestF.cols() <<std::endl;

    // 1-D Unit Test
    // A uniform distortion is applied to a set of points (y=x^2)
    // The expected result is that UndistortedUnitTest is about equal to the GroundTruth
    int max = 100;
    int min = 0;
    Eigen::VectorXd X = Eigen::VectorXd::LinSpaced(max+1,min,max);
    Eigen::VectorXd Y = 2*X;
    Eigen::MatrixXd Y3distorted = Eigen::MatrixXd::Zero(X.size(),3);
    Eigen::MatrixXd X3GroundTruth = Eigen::MatrixXd::Zero(X.size(),3);
    Y3distorted.col(0) = Y;
    X3GroundTruth.col(0) = X;
    Eigen::Vector3d minCorner;
    Eigen::Vector3d maxCorner;

    Eigen::MatrixXd undistorted = correctDistortion(Y3distorted, Y3distorted, X3GroundTruth, minCorner, maxCorner);
    if(debug) std::cout << "\n\nUnitTestUndistorted\n\n" << undistorted << "\n\nGroundTruth\n\n" << X3GroundTruth;
    BOOST_CHECK(isWithinTolerance(undistorted,X3GroundTruth));

}

template<typename T>
void CompareFrames(std::string file, const T& f1, const T& f2){
    double frameTolerance = 2.0;
    auto f1b = f1.begin();
    auto f2b = f2.begin();
    for(;f1b!=f1.end(); ++f1b, ++f2b){
        auto f11b = f1b->begin();
        auto f22b = f2b->begin();
        for(;f11b != f1b->end(); ++f11b, ++f22b){
            bool isWithinToleranceB = isWithinTolerance(*f11b, *f22b, frameTolerance);
            BOOST_CHECK(isWithinToleranceB);
            if(!isWithinToleranceB){
                std::cout << "\n\n" << file << " outside of tolerances! check these values:\n\n" << *f11b << "\n\n";

            }
        }

    }

}

void CompareOutputFiles(std::string filenamePrefix){

    DataSource pclp;
    DataSource pclpOut;
    const bool required = true;
    // optional == not required == false
    const bool optional = false;
    // check if the user supplied a full path, if not assemble a path
    // from the default paths and the defualt prefix/suffix combos
    assemblePathIfFullPathNotSupplied(relativeOutputDataPath,filenamePrefix,dataFileNameSuffix_output1       ,pclpOut.output1Path      ,required);
    assemblePathIfFullPathNotSupplied(relativeOutputDataPath,filenamePrefix,dataFileNameSuffix_output2       ,pclpOut.output2Path      ,required);
    assemblePathIfFullPathNotSupplied(relativeDataPath,filenamePrefix,dataFileNameSuffix_output1       ,pclp.output1Path      ,required);
    assemblePathIfFullPathNotSupplied(relativeDataPath,filenamePrefix,dataFileNameSuffix_output2       ,pclp.output2Path      ,required);

    AlgorithmData ad;
    loadPointCloudFromFile(pclp.output1Path       ,ad.output1                    ,debug);
    loadPointCloudFromFile(pclp.output2Path       ,ad.output2                    ,debug);

    AlgorithmData adOut;
    loadPointCloudFromFile(pclpOut.output1Path       ,adOut.output1                    ,debug);
    loadPointCloudFromFile(pclpOut.output2Path       ,adOut.output2                    ,debug);

    //AlgorithmData adOut = initAlgorithmData(relativeOutputDataPath,filenamePrefix);
    //AlgorithmData ad = initAlgorithmData(relativeDataPath, filenamePrefix);

    CompareFrames(relativeOutputDataPath + filenamePrefix + dataFileNameSuffix_output1, adOut.output1.frames, ad.output1.frames);
    CompareFrames(relativeOutputDataPath + filenamePrefix + dataFileNameSuffix_output2, adOut.output2.frames, ad.output2.frames);
}

/// unit test output of all debug files
BOOST_AUTO_TEST_CASE(EMPivotCalibrationResults)
{
    CompareOutputFiles(pa2debuga);
    CompareOutputFiles(pa2debugb);
    CompareOutputFiles(pa2debugc);
    CompareOutputFiles(pa2debugd);
    CompareOutputFiles(pa2debuge);
    CompareOutputFiles(pa2debugf);
}

BOOST_AUTO_TEST_SUITE_END()


#include <blitzml/dataset/dataset.h>

#include <cmath>

namespace BlitzML {

value_t Column::l2_norm() const {
  return sqrt(l2_norm_sq());
}

value_t Column::l2_norm_sq_centered() const {
  value_t mean_value = this->mean();
  value_t ret = l2_norm_sq() - mean_value * mean_value * length();
  if (ret <= 0) {
    return 0.;
  }
  return ret;
}

value_t Column::l2_norm_centered() const {
  return sqrt(l2_norm_sq_centered());
};

void Dataset::contiguous_submatrix_multiply(
    const std::vector<value_t> &values, value_t* result,
    index_t first_column_submatrix, index_t end_column_submatrix) const {

  index_t j, result_ind;
  for (j = first_column_submatrix, result_ind = 0;
       j < end_column_submatrix; ++j, ++result_ind) {
    result[result_ind] = column(j)->inner_product(values);
  }
}

} // namespace BlitzML


#pragma once

#include <armadillo>

class Composition;

namespace utils
{
    double calculateHeatCapacityConstantVolumeJFH(
            const double pressure);

    double calculateHeatCapacityConstantVolumeTGNet(
            const double molarMass,
            const double pressure,
            const double temperature);

    double calculateHeatCapacityConstantPressureJFH(
            const double molarMass,
            const double pressure,
            const double temperature);

    double calculateHeatCapacityConstantPressureLangelandsvik(
            const double molarMass,
            const double pressure,
            const double temperature);

    double calculateHeatCapacityConstantPressureTGNet(
            const double molarMassOfMixture,
            const double pressure,
            const double temperature);

    double calculateHeatCapacityConstantPressureKIO(
            const Composition& comp,
            const double pressure,
            const double temperature);

    double calculateIsobaricHeatCapacityJKH(
            const Composition& comp,
            const double pressure,
            const double temperature,
            const double Z = 0);

    arma::vec calculateViscosity(
            const arma::vec& molarMass,
            const arma::vec& temperature,
            const arma::vec& density);

    arma::vec calculateReynoldsNumber(
            const arma::vec& massFlow,
            const arma::vec& diameter,
            const arma::vec& viscosity);

    arma::vec calculateColebrookWhiteFrictionFactor(
            const arma::vec& sandGrainEquivalentRoughness,
            const arma::vec& diameter,
            const arma::vec& reynoldsNumber);

    double calculateColebrookWhiteFrictionFactor(
            const double sandGrainEquivalentRoughness,
            const double diameter,
            const double reynoldsNumber);

    double calculateHaalandFrictionFactor(
            const double sandGrainEquivalentRoughness,
            const double diameter,
            const double reynoldsNumber);

    namespace details
    {
        double KIOidealGasCP(
                const double specificGravity,
                const double temperature);

        double KIOdimensionlessResidualCP(
                const double reducedPressure,
                const double reducedTemperature);

        double JKHdimensionlessCP(
                const double molarMass,
                const double H2S,
                const double CO2,
                const double N2,
                const double H2,
                const double H2O,
                const double pressure,
                const double temperature,
                const double compressibility);

        double JKHidealGasCP(
                const Composition& comp,
                const double specificGravity,
                const double temperature);

        double JKHidealGasCP(
                const double specificGravity,
                const double H2S,
                const double CO2,
                const double N2,
                const double H2,
                const double H2O,
                const double temperature);

        double calculateHeatCapacityConstantPressureKIO(
                const Composition& comp,
                const double H2S,
                const double pressure,
                const double temperature);

        double colebrookWhiteFrictionFactor(
                const double sandGrainEquivalentRoughness,
                const double diameter,
                const double reynoldsNumber);

        double colebrookWhite(
                const double f,
                const double sandGrainEquivalentRoughness,
                const double diameter,
                const double reynoldsNumber);

        double colebrookWhiteDerivative(
                const double f,
                const double sandGrainEquivalentRoughness,
                const double diameter,
                const double reynoldsNumber);
    }
}


#include <Eigen/Core>
#include <functional>

#include "update_ops_cpp.hpp"
#include "utility.hpp"

void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list,
    UINT target_qubit_index_count, const CTYPE* matrix, CTYPE* state,
    ITYPE dim) {
    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(
        target_qubit_index_list, target_qubit_index_count);
    Eigen::Map<const Eigen::Matrix<std::complex<double>, Eigen::Dynamic,
                   Eigen::Dynamic, Eigen::RowMajor>,
        Eigen::Aligned>
        eigen_matrix((std::complex<double>*)matrix, matrix_dim, matrix_dim);
    Eigen::VectorXcd buffer(matrix_dim);
    std::complex<double>* eigen_state =
        reinterpret_cast<std::complex<double>*>(state);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(
        target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for (state_index = 0; state_index < loop_dim; ++state_index) {
        // create base index
        ITYPE basis_0 = state_index;
        for (UINT cursor = 0; cursor < target_qubit_index_count; cursor++) {
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(
                basis_0, 1ULL << insert_index, insert_index);
        }

        // fetch vector
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            buffer[y] = eigen_state[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            eigen_state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}

void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list,
    UINT target_qubit_index_count,
    const Eigen::Matrix<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic,
        Eigen::RowMajor>& eigen_matrix,
    CTYPE* state, ITYPE dim) {
    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(
        target_qubit_index_list, target_qubit_index_count);
    Eigen::VectorXcd buffer(matrix_dim);
    std::complex<double>* eigen_state =
        reinterpret_cast<std::complex<double>*>(state);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(
        target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for (state_index = 0; state_index < loop_dim; ++state_index) {
        // create base index
        ITYPE basis_0 = state_index;
        for (UINT cursor = 0; cursor < target_qubit_index_count; cursor++) {
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(
                basis_0, 1ULL << insert_index, insert_index);
        }

        // fetch vector
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            buffer[y] = eigen_state[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            eigen_state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}

void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list,
    UINT target_qubit_index_count, const Eigen::MatrixXcd& eigen_matrix,
    CTYPE* state, ITYPE dim) {
    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(
        target_qubit_index_list, target_qubit_index_count);
    std::complex<double>* cppstate =
        reinterpret_cast<std::complex<double>*>(state);
    Eigen::VectorXcd buffer(matrix_dim);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(
        target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for (state_index = 0; state_index < loop_dim; ++state_index) {
        // create base index
        ITYPE basis_0 = state_index;
        for (UINT cursor = 0; cursor < target_qubit_index_count; cursor++) {
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(
                basis_0, 1ULL << insert_index, insert_index);
        }

        // fetch vector
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            buffer[y] = cppstate[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            cppstate[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}

void multi_qubit_sparse_matrix_gate_eigen(const UINT* target_qubit_index_list,
    UINT target_qubit_index_count,
    const Eigen::SparseMatrix<std::complex<double>>& eigen_matrix, CTYPE* state,
    ITYPE dim) {
    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(
        target_qubit_index_list, target_qubit_index_count);
    Eigen::VectorXcd buffer(matrix_dim);
    std::complex<double>* eigen_state =
        reinterpret_cast<std::complex<double>*>(state);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(
        target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for (state_index = 0; state_index < loop_dim; ++state_index) {
        // create base index
        ITYPE basis_0 = state_index;
        for (UINT cursor = 0; cursor < target_qubit_index_count; cursor++) {
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(
                basis_0, 1ULL << insert_index, insert_index);
        }

        // fetch vector
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            buffer[y] = eigen_state[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            eigen_state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}


/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2009: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/

/** Example partys_height_average.cpp \file partys_height_average.cpp
    \brief Using <i>aggregate on overlap</i> a history of height averages of
           party guests is computed.

    In partys_height_average.cpp we compute yet another aggregation:
    The average height of guests as it changes over time. This is done by
    defining a class counted_sum that sums up heights and counts the number
    of guests via an operator +=.

    Based on the operator += we can aggregate counted sums on addition
    of interval value pairs into an interval_map.

    \include partys_height_average_/partys_height_average.cpp
*/
//[example_partys_height_average
// The next line includes <boost/date_time/posix_time/posix_time.hpp>
// and a few lines of adapter code.
#include <boost/icl/ptime.hpp>
#include <iostream>
#include <boost/icl/interval_map.hpp>
#include <boost/icl/split_interval_map.hpp>

using namespace std;
using namespace boost::posix_time;
using namespace boost::icl;

class counted_sum
{
public:
    counted_sum():_sum(0),_count(0){}
    counted_sum(int sum):_sum(sum),_count(1){}

    int sum()const  {return _sum;}
    int count()const{return _count;}
    double average()const{ return _count==0 ? 0.0 : _sum/static_cast<double>(_count); }

    counted_sum& operator += (const counted_sum& right)
    { _sum += right.sum(); _count += right.count(); return *this; }

private:
    int _sum;
    int _count;
};

bool operator == (const counted_sum& left, const counted_sum& right)
{ return left.sum()==right.sum() && left.count()==right.count(); }

void partys_height_average()
{
    interval_map<ptime, counted_sum> height_sums;

    height_sums +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 19:30"),
          time_from_string("2008-05-20 23:00")),
        counted_sum(165)); // Mary is 1,65 m tall.

    height_sums +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 19:30"),
          time_from_string("2008-05-20 23:00")),
        counted_sum(180)); // Harry is 1,80 m tall.

    height_sums +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 20:10"),
          time_from_string("2008-05-21 00:00")),
        counted_sum(170)); // Diana is 1,70 m tall.

    height_sums +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 20:10"),
          time_from_string("2008-05-21 00:00")),
        counted_sum(165)); // Susan is 1,65 m tall.

    height_sums +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 22:15"),
          time_from_string("2008-05-21 00:30")),
        counted_sum(200)); // Peters height is 2,00 m

    interval_map<ptime, counted_sum>::iterator height_sum_ = height_sums.begin();
    cout << "-------------- History of average guest height -------------------\n";
    while(height_sum_ != height_sums.end())
    {
        discrete_interval<ptime> when = height_sum_->first;

        double height_average = (*height_sum_++).second.average();
        cout << setprecision(3)
             << "[" << first(when) << " - " << upper(when) << ")"
             << ": " << height_average <<" cm = " << height_average/30.48 << " ft" << endl;
    }
}

int main()
{
    cout << ">>Interval Container Library: Sample partys_height_average.cpp  <<\n";
    cout << "------------------------------------------------------------------\n";
    partys_height_average();
    return 0;
}

// Program output:
/*-----------------------------------------------------------------------------
>>Interval Container Library: Sample partys_height_average.cpp  <<
------------------------------------------------------------------
-------------- History of average guest height -------------------
[2008-May-20 19:30:00 - 2008-May-20 20:10:00): 173 cm = 5.66 ft
[2008-May-20 20:10:00 - 2008-May-20 22:15:00): 170 cm = 5.58 ft
[2008-May-20 22:15:00 - 2008-May-20 23:00:00): 176 cm = 5.77 ft
[2008-May-20 23:00:00 - 2008-May-21 00:00:00): 178 cm = 5.85 ft
[2008-May-21 00:00:00 - 2008-May-21 00:30:00): 200 cm = 6.56 ft
-----------------------------------------------------------------------------*/
//]


#include <Eigen/Core>
#include <ceres/ceres.h>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include <ceres-error-terms/generic-prior-error-term.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

class PriorErrorTerms : public ::testing::Test {
 public:
  static constexpr int VectorDim = 9;

  static constexpr int PriorBlockSize = 3;
  static constexpr int PriorBlockIndex = 4;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 protected:
  virtual void SetUp() {
    prior_mean_ << 1, 2, 3, 4, 5, 6, 7, 8, -3;
    block_prior_mean_ = prior_mean_.segment<PriorBlockSize>(PriorBlockIndex);

    current_value_ = prior_mean_;
    covariance_matrix_.setIdentity();
    block_covariance_matrix_ =
        covariance_matrix_.block<PriorBlockSize, PriorBlockSize>(
            PriorBlockIndex, PriorBlockIndex);
  }

  void addResidual();
  void addMoreGenericResidual();
  void solve();

  ceres::Problem problem_;
  ceres::Solver::Summary summary_;
  Eigen::Matrix<double, VectorDim, 1> prior_mean_;
  Eigen::Matrix<double, VectorDim, VectorDim> covariance_matrix_;

  Eigen::Matrix<double, PriorBlockSize, 1> block_prior_mean_;
  Eigen::Matrix<double, PriorBlockSize, PriorBlockSize>
      block_covariance_matrix_;

  Eigen::Matrix<double, VectorDim, 1> current_value_;
};

void PriorErrorTerms::addResidual() {
  ceres::CostFunction* cost_function =
      new ceres_error_terms::GenericPriorErrorTerm<VectorDim, PriorBlockIndex,
                                                   PriorBlockSize>(
          block_prior_mean_, block_covariance_matrix_);
  problem_.AddResidualBlock(cost_function, NULL, current_value_.data());
}

void PriorErrorTerms::solve() {
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.parameter_tolerance = 1e-20;
  options.gradient_tolerance = 1e-20;
  options.function_tolerance = 1e-20;
  options.max_num_iterations = 1e3;
  ceres::Solve(options, &problem_, &summary_);

  LOG(INFO) << summary_.BriefReport() << std::endl;
  LOG(INFO) << summary_.message << std::endl;
}

TEST_F(PriorErrorTerms, TestGenericPriorErrorTermZeroCost) {
  addResidual();
  solve();

  EXPECT_EQ(summary_.initial_cost, 0.0);
  EXPECT_EQ(summary_.final_cost, 0.0);
  EXPECT_EQ(summary_.iterations.size(), 1u);
}

TEST_F(PriorErrorTerms, TestGenericPriorErrorTermOptimization) {
  current_value_.segment<PriorBlockSize>(PriorBlockIndex).setRandom();
  current_value_.segment<PriorBlockSize>(PriorBlockIndex) *= 1000.0;

  addResidual();
  solve();

  EXPECT_NEAR_EIGEN(current_value_, prior_mean_, 1e-10);
  EXPECT_LT(summary_.final_cost, 1e-10);
}

MAPLAB_UNITTEST_ENTRYPOINT


// Copyright © 2018 Thomas Nagler
//
// This file is part of the wdm library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory
// or https://github.com/tnagler/wdmcpp/blob/master/LICENSE.

#pragma once

#include <Eigen/Dense>
#include "../wdm.hpp"

namespace wdm {

namespace utils {

    inline std::vector<double> convert_vec(const Eigen::VectorXd& x)
    {
        std::vector<double> xx(x.size());
        if (x.size() > 0)
            Eigen::VectorXd::Map(&xx[0], x.size()) = x;
        return xx;
    }

}

//! calculates (weighted) dependence measures.
//! @param x, y input data.
//! @param method the dependence measure; see details for possible values.
//! @param weights an optional vector of weights for the data.
//! @param remove_missing if `true`, all observations containing a `nan` are
//!    removed; otherwise throws an error if `nan`s are present.
//! @details
//! Available methods:
//!   - `"pearson"`, `"prho"`, `"cor"`: Pearson correlation
//!   - `"spearman"`, `"srho"`, `"rho"`: Spearman's \f$ \rho \f$
//!   - `"kendall"`, `"ktau"`, `"tau"`: Kendall's \f$ \tau \f$
//!   - `"blomqvist"`, `"bbeta"`, `"beta"`: Blomqvist's \f$ \beta \f$
//!   - `"hoeffding"`, `"hoeffd"`, `"d"`: Hoeffding's \f$ D \f$
//!
//! @return the dependence measure
inline double wdm(const Eigen::VectorXd& x,
                  const Eigen::VectorXd& y,
                  std::string method,
                  Eigen::VectorXd weights = Eigen::VectorXd(),
                  bool remove_missing = true)
{
    return wdm(utils::convert_vec(x),
               utils::convert_vec(y),
               method,
               utils::convert_vec(weights),
               remove_missing);
}

//! calculates a matrix of (weighted) dependence measures.
//! @param x input data.
//! @param method the dependence measure; see details for possible values.
//! @param weights an optional vector of weights for the data.
//! @param remove_missing if `true`, all observations containing a `nan` are
//!    removed; otherwise throws an error if `nan`s are present.
//! @details
//! Available methods:
//!   - `"pearson"`, `"prho"`, `"cor"`: Pearson correlation
//!   - `"spearman"`, `"srho"`, `"rho"`: Spearman's \f$ \rho \f$
//!   - `"kendall"`, `"ktau"`, `"tau"`: Kendall's \f$ \tau \f$
//!   - `"blomqvist"`, `"bbeta"`, `"beta"`: Blomqvist's \f$ \beta \f$
//!   - `"hoeffding"`, `"hoeffd"`, `"d"`: Hoeffding's \f$ D \f$
//!
//! @return a matrix of pairwise dependence measures.
inline Eigen::MatrixXd wdm(const Eigen::MatrixXd& x,
                           std::string method,
                           Eigen::VectorXd weights = Eigen::VectorXd(),
                           bool remove_missing = true)
{
    size_t d = x.cols();
    if (d == 1)
        throw std::runtime_error("x must have at least 2 columns.");

    Eigen::MatrixXd ms = Eigen::MatrixXd::Identity(d, d);
    for (size_t i = 0; i < d; i++) {
        for (size_t j = i + 1; j < d; j++) {
            ms(i, j) = wdm(utils::convert_vec(x.col(i)),
                           utils::convert_vec(x.col(j)),
                           method,
                           utils::convert_vec(weights),
                           remove_missing);
            ms(j, i) = ms(i, j);
        }
    }

    return ms;
}

}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <string>

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/domains/gis/io/wkt/read_wkt.hpp>

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

#include <boost/geometry/multi/algorithms/distance.hpp>
#include <boost/geometry/multi/geometries/multi_point.hpp>
#include <boost/geometry/multi/geometries/multi_linestring.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>
#include <boost/geometry/domains/gis/io/wkt/read_wkt_multi.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename Geometry1, typename Geometry2>
void test_distance(std::string const& wkt1, std::string const& wkt2, double expected)
{
    Geometry1 g1;
    Geometry2 g2;
    bg::read_wkt(wkt1, g1);
    bg::read_wkt(wkt2, g2);
    typename bg::default_distance_result<Geometry1, Geometry2>::type d = bg::distance(g1, g2);

    BOOST_CHECK_CLOSE(d, expected, 0.0001);
}

template <typename Geometry1, typename Geometry2, typename Strategy>
void test_distance(Strategy const& strategy, std::string const& wkt1,
                   std::string const& wkt2, double expected)
{
    Geometry1 g1;
    Geometry2 g2;
    bg::read_wkt(wkt1, g1);
    bg::read_wkt(wkt2, g2);
    typename bg::default_distance_result<Geometry1, Geometry2>::type d = bg::distance(g1, g2, strategy);

    BOOST_CHECK_CLOSE(d, expected, 0.0001);
}

template <typename P>
void test_2d()
{
    typedef bg::model::multi_point<P> mp;
    typedef bg::model::multi_linestring<bg::model::linestring<P> > ml;
    test_distance<P, P>("POINT(0 0)", "POINT(1 1)", sqrt(2.0));
    test_distance<P, mp>("POINT(0 0)", "MULTIPOINT((1 1),(1 0),(0 2))", 1.0);
    test_distance<mp, P>("MULTIPOINT((1 1),(1 0),(0 2))", "POINT(0 0)", 1.0);
    test_distance<mp, mp>("MULTIPOINT((1 1),(1 0),(0 2))", "MULTIPOINT((2 2),(2 3))", sqrt(2.0));
    test_distance<P, ml>("POINT(0 0)", "MULTILINESTRING((1 1,2 2),(1 0,2 0),(0 2,0 3))", 1.0);
    test_distance<ml, P>("MULTILINESTRING((1 1,2 2),(1 0,2 0),(0 2,0 3))", "POINT(0 0)", 1.0);
    test_distance<ml, mp>("MULTILINESTRING((1 1,2 2),(1 0,2 0),(0 2,0 3))", "MULTIPOINT((0 0),(1 1))", 0.0);

    // Test with a strategy
    bg::strategy::distance::pythagoras<P, P> pyth;
    test_distance<P, P>(pyth, "POINT(0 0)", "POINT(1 1)", sqrt(2.0));
    test_distance<P, mp>(pyth, "POINT(0 0)", "MULTIPOINT((1 1),(1 0),(0 2))", 1.0);
    test_distance<mp, P>(pyth, "MULTIPOINT((1 1),(1 0),(0 2))", "POINT(0 0)", 1.0);
}

template <typename P>
void test_3d()
{
    typedef bg::model::multi_point<P> mp;
    test_distance<P, P>("POINT(0 0 0)", "POINT(1 1 1)", sqrt(3.0));
    test_distance<P, mp>("POINT(0 0 0)", "MULTIPOINT((1 1 1),(1 0 0),(0 1 2))", 1.0);
    test_distance<mp, mp>("MULTIPOINT((1 1 1),(1 0 0),(0 0 2))", "MULTIPOINT((2 2 2),(2 3 4))", sqrt(3.0));
}

template <typename P1, typename P2>
void test_mixed()
{
    typedef bg::model::multi_point<P1> mp1;
    typedef bg::model::multi_point<P2> mp2;

    test_distance<P1, P2>("POINT(0 0)", "POINT(1 1)", sqrt(2.0));

    test_distance<P1, mp1>("POINT(0 0)", "MULTIPOINT((1 1),(1 0),(0 2))", 1.0);
    test_distance<P1, mp2>("POINT(0 0)", "MULTIPOINT((1 1),(1 0),(0 2))", 1.0);
    test_distance<P2, mp1>("POINT(0 0)", "MULTIPOINT((1 1),(1 0),(0 2))", 1.0);
    test_distance<P2, mp2>("POINT(0 0)", "MULTIPOINT((1 1),(1 0),(0 2))", 1.0);

    // Test automatic reversal
    test_distance<mp1, P1>("MULTIPOINT((1 1),(1 0),(0 2))", "POINT(0 0)", 1.0);
    test_distance<mp1, P2>("MULTIPOINT((1 1),(1 0),(0 2))", "POINT(0 0)", 1.0);
    test_distance<mp2, P1>("MULTIPOINT((1 1),(1 0),(0 2))", "POINT(0 0)", 1.0);
    test_distance<mp2, P2>("MULTIPOINT((1 1),(1 0),(0 2))", "POINT(0 0)", 1.0);

    // Test multi-multi using different point types for each
    test_distance<mp1, mp2>("MULTIPOINT((1 1),(1 0),(0 2))", "MULTIPOINT((2 2),(2 3))", sqrt(2.0));

    // Test with a strategy
    using namespace bg::strategy::distance;

    test_distance<P1, P2>(pythagoras<P1, P2>(), "POINT(0 0)", "POINT(1 1)", sqrt(2.0));

    test_distance<P1, mp1>(pythagoras<P1, P1>(), "POINT(0 0)", "MULTIPOINT((1 1),(1 0),(0 2))", 1.0);
    test_distance<P1, mp2>(pythagoras<P1, P2>(), "POINT(0 0)", "MULTIPOINT((1 1),(1 0),(0 2))", 1.0);
    test_distance<P2, mp1>(pythagoras<P2, P1>(), "POINT(0 0)", "MULTIPOINT((1 1),(1 0),(0 2))", 1.0);
    test_distance<P2, mp2>(pythagoras<P2, P2>(), "POINT(0 0)", "MULTIPOINT((1 1),(1 0),(0 2))", 1.0);

    // Most interesting: reversal AND a strategy (note that the stategy must be reversed automatically
    test_distance<mp1, P1>(pythagoras<P1, P1>(), "MULTIPOINT((1 1),(1 0),(0 2))", "POINT(0 0)", 1.0);
    test_distance<mp1, P2>(pythagoras<P1, P2>(), "MULTIPOINT((1 1),(1 0),(0 2))", "POINT(0 0)", 1.0);
    test_distance<mp2, P1>(pythagoras<P2, P1>(), "MULTIPOINT((1 1),(1 0),(0 2))", "POINT(0 0)", 1.0);
    test_distance<mp2, P2>(pythagoras<P2, P2>(), "MULTIPOINT((1 1),(1 0),(0 2))", "POINT(0 0)", 1.0);
}

int test_main( int , char* [] )
{
    test_2d<boost::tuple<float, float> >();
    test_2d<bg::model::d2::point_xy<float> >();
    test_2d<bg::model::d2::point_xy<double> >();

    test_3d<boost::tuple<float, float, float> >();
    test_3d<bg::model::point<double, 3, bg::cs::cartesian> >();

    test_mixed<bg::model::d2::point_xy<float>, bg::model::d2::point_xy<double> >();

#ifdef HAVE_TTMATH
    test_2d<bg::model::d2::point_xy<ttmath_big> >();
    test_mixed<bg::model::d2::point_xy<ttmath_big>, bg::model::d2::point_xy<double> >();
#endif

    return 0;
}


#ifndef HYPSYS1D_FVM_RATE_OF_CHANGE_HPP
#define HYPSYS1D_FVM_RATE_OF_CHANGE_HPP

#include <memory>
#include <Eigen/Dense>

#include <ancse/config.hpp>
#include <ancse/grid.hpp>
#include <ancse/model.hpp>
#include <ancse/rate_of_change.hpp>
#include <ancse/simulation_time.hpp>

/// Compute the rate of change due to FVM.
/** The semidiscrete approximation of a PDE using FVM is
 *      du_i/dt = - (F_{i+0.5} - F_{i-0.5}) / dx.
 *  This computes the right hand side of the ODE.
 *
 * @tparam NumericalFlux see e.g. `CentralFlux`.
 * @tparam Reconstruction see e.g. `PWConstantReconstruction`.
 */
template <class NumericalFlux, class Reconstruction>
class FVMRateOfChange : public RateOfChange
{
    public:
        FVMRateOfChange(const Grid& grid,
                        const std::shared_ptr<Model>& model,
                        const NumericalFlux& numerical_flux,
                        const Reconstruction& reconstruction)
            : grid(grid),
              model(model),
              numerical_flux(numerical_flux),
              reconstruction(reconstruction) {}

        virtual void operator()(Eigen::MatrixXd& dudt,
                                const Eigen::MatrixXd& u0) const override
        {
            // implement the flux loop here.
            const int n_cells= grid.n_cells;
            const int n_ghost= grid.n_ghost;

            const int n_vars= model->get_nvars();

            const double dx= grid.dx;
            Eigen::VectorXd fL= Eigen::VectorXd::Zero(n_vars), fR= Eigen::VectorXd::Zero(n_vars);
            Eigen::VectorXd uL, uR;

            for (int i= n_ghost - 1; i < n_cells - n_ghost; ++i)
            {
                std::tie(uL, uR)= reconstruction(u0, i);

                fL= fR;
                fR= numerical_flux(uL, uR);

                dudt.col(i)= (fL - fR) / dx;
            }
        }

    private:
        Grid grid;
        std::shared_ptr<Model> model;
        NumericalFlux numerical_flux;
        Reconstruction reconstruction;
};

std::shared_ptr<RateOfChange>
make_fvm_rate_of_change(const nlohmann::json &config,
                        const Grid &grid,
                        const std::shared_ptr<Model> &model,
                        const std::shared_ptr<SimulationTime> &simulation_time);

#endif // HYPSYS1D_FVM_RATE_OF_CHANGE_HPP


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <Eigen/Core>

namespace pyinterp {

template <typename T>
using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;

template <typename T>
using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

}  // namespace pyinterp


#include "clock.h"
#include <boost/format.hpp>
#include <sstream>

namespace date_independent {
    using namespace std;
    using namespace boost;

    clock::clock(const int hour, const int minute){
        #define cx_minperday 60*24

        time_in_minutes = hour*60 + minute;
        time_in_minutes %= cx_minperday;
        if(time_in_minutes < 0)
            time_in_minutes += cx_minperday;
    }

    clock clock::at(const int hour, const int minute){
        return clock(hour, minute);
    }

    clock clock::plus(int minute){
        return clock(0, time_in_minutes + minute);
    }

    clock::operator std::string() const {
        stringstream stream;
        stream << format("%02d:%02d") % int(time_in_minutes/60) % (time_in_minutes%60);
        return stream.str();
    }
}  // namespace date_independent


/*=============================================================================
    Copyright (c) 1998-2003 Joel de Guzman
    http://spirit.sourceforge.net/

    Use, modification and distribution is subject to the Boost Software
    License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
    http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
////////////////////////////////////////////////////////////////////////////
//
//  The calculator using a simple virtual machine and compiler.
//
//  Ported to v1.5 from the original v1.0 code by JDG
//  [ JDG 9/18/2002 ]
//
////////////////////////////////////////////////////////////////////////////
#include <boost/spirit/include/classic_core.hpp>
#include <iostream>
#include <vector>
#include <string>

////////////////////////////////////////////////////////////////////////////
using namespace std;
using namespace BOOST_SPIRIT_CLASSIC_NS;

///////////////////////////////////////////////////////////////////////////////
//
//  The VMachine
//
///////////////////////////////////////////////////////////////////////////////
enum ByteCodes
{
    OP_NEG,     //  negate the top stack entry
    OP_ADD,     //  add top two stack entries
    OP_SUB,     //  subtract top two stack entries
    OP_MUL,     //  multiply top two stack entries
    OP_DIV,     //  divide top two stack entries
    OP_INT,     //  push constant integer into the stack
    OP_RET      //  return from the interpreter
};

class vmachine
{
public:
                vmachine(unsigned stackSize = 1024)
                :   stack(new int[stackSize]),
                    stackPtr(stack) {}
                ~vmachine() { delete [] stack; }
    int         top() const { return stackPtr[-1]; };
    void        execute(int code[]);

private:

    int*        stack;
    int*        stackPtr;
};

void
vmachine::execute(int code[])
{
    int const*  pc = code;
    bool        running = true;
    stackPtr = stack;

    while (running)
    {
        switch (*pc++)
        {
            case OP_NEG:
                stackPtr[-1] = -stackPtr[-1];
                break;

            case OP_ADD:
                stackPtr--;
                stackPtr[-1] += stackPtr[0];
                break;

            case OP_SUB:
                stackPtr--;
                stackPtr[-1] -= stackPtr[0];
                break;

            case OP_MUL:
                stackPtr--;
                stackPtr[-1] *= stackPtr[0];
                break;

            case OP_DIV:
                stackPtr--;
                stackPtr[-1] /= stackPtr[0];
                break;

            case OP_INT:
                // Check stack overflow here!
                *stackPtr++ = *pc++;
                break;

            case OP_RET:
                running = false;
                break;
        }
    }
}

///////////////////////////////////////////////////////////////////////////////
//
//  The Compiler
//
///////////////////////////////////////////////////////////////////////////////
struct push_int
{
    push_int(vector<int>& code_)
    : code(code_) {}

    void operator()(char const* str, char const* /*end*/) const
    {
        using namespace std;
        int n = strtol(str, 0, 10);
        code.push_back(OP_INT);
        code.push_back(n);
        cout << "push\t" << int(n) << endl;
    }

    vector<int>& code;
};

struct push_op
{
    push_op(int op_, vector<int>& code_)
    : op(op_), code(code_) {}

    void operator()(char const*, char const*) const
    {
        code.push_back(op);

        switch (op) {

            case OP_NEG:
                cout << "neg\n";
                break;

            case OP_ADD:
                cout << "add\n";
                break;

            case OP_SUB:
                cout << "sub\n";
                break;

            case OP_MUL:
                cout << "mul\n";
                break;

            case OP_DIV:
                cout << "div\n";
                break;
        }
    }

    int op;
    vector<int>& code;
};

template <typename GrammarT>
static bool
compile(GrammarT const& calc, char const* expr)
{
    cout << "\n/////////////////////////////////////////////////////////\n\n";

    parse_info<char const*>
        result = parse(expr, calc, space_p);

    if (result.full)
    {
        cout << "\t\t" << expr << " Parses OK\n\n\n";
        calc.code.push_back(OP_RET);
        return true;
    }
    else
    {
        cout << "\t\t" << expr << " Fails parsing\n";
        cout << "\t\t";
        for (int i = 0; i < (result.stop - expr); i++)
            cout << " ";
        cout << "^--Here\n\n\n";
        return false;
    }
}

////////////////////////////////////////////////////////////////////////////
//
//  Our calculator grammar
//
////////////////////////////////////////////////////////////////////////////
struct calculator : public grammar<calculator>
{
    calculator(vector<int>& code_)
    : code(code_) {}

    template <typename ScannerT>
    struct definition
    {
        definition(calculator const& self)
        {
            integer =
                lexeme_d[ (+digit_p)[push_int(self.code)] ]
                ;

            factor =
                    integer
                |   '(' >> expression >> ')'
                |   ('-' >> factor)[push_op(OP_NEG, self.code)]
                |   ('+' >> factor)
                ;

            term =
                factor
                >> *(   ('*' >> factor)[push_op(OP_MUL, self.code)]
                    |   ('/' >> factor)[push_op(OP_DIV, self.code)]
                    )
                    ;

            expression =
                term
                >> *(   ('+' >> term)[push_op(OP_ADD, self.code)]
                    |   ('-' >> term)[push_op(OP_SUB, self.code)]
                    )
                    ;
        }

        rule<ScannerT> expression, term, factor, integer;

        rule<ScannerT> const&
        start() const { return expression; }
    };

    vector<int>& code;
};

////////////////////////////////////////////////////////////////////////////
//
//  Main program
//
////////////////////////////////////////////////////////////////////////////
int
main()
{
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tA simple virtual machine...\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "Type an expression...or [q or Q] to quit\n\n";

    vmachine    mach;       //  Our virtual machine
    vector<int> code;       //  Our VM code
    calculator  calc(code); //  Our parser

    string str;
    while (getline(cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        code.clear();
        if (compile(calc, str.c_str()))
        {
            mach.execute(&*code.begin());
            cout << "\n\nresult = " << mach.top() << "\n\n";
        }
    }

    cout << "Bye... :-) \n\n";
    return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>

template<typename Scalar> void eulerangles(void)
{
  typedef Matrix<Scalar,3,3> Matrix3;
  typedef Matrix<Scalar,3,1> Vector3;
  typedef Quaternion<Scalar> Quaternionx;
  typedef AngleAxis<Scalar> AngleAxisx;

  Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
  Quaternionx q1;
  q1 = AngleAxisx(a, Vector3::Random().normalized());
  Matrix3 m;
  m = q1;

  #define VERIFY_EULER(I,J,K, X,Y,Z) { \
    Vector3 ea = m.eulerAngles(I,J,K); \
    VERIFY_IS_APPROX(m,  Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \
  }
  VERIFY_EULER(0,1,2, X,Y,Z);
  VERIFY_EULER(0,1,0, X,Y,X);
  VERIFY_EULER(0,2,1, X,Z,Y);
  VERIFY_EULER(0,2,0, X,Z,X);

  VERIFY_EULER(1,2,0, Y,Z,X);
  VERIFY_EULER(1,2,1, Y,Z,Y);
  VERIFY_EULER(1,0,2, Y,X,Z);
  VERIFY_EULER(1,0,1, Y,X,Y);

  VERIFY_EULER(2,0,1, Z,X,Y);
  VERIFY_EULER(2,0,2, Z,X,Z);
  VERIFY_EULER(2,1,0, Z,Y,X);
  VERIFY_EULER(2,1,2, Z,Y,Z);
}

void test_geo_eulerangles()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( eulerangles<float>() );
    CALL_SUBTEST_2( eulerangles<double>() );
  }
}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_VECTOR3D_HPP
#define RW_MATH_VECTOR3D_HPP

/**
 * @file Vector3D.hpp
 */

#if !defined(SWIG)
#include <rw/common/Serializable.hpp>

#include <Eigen/Eigen>
#endif
namespace rw { namespace math {

    /** @addtogroup math */
    /*@{*/

    /**
     * @brief A 3D vector @f$ \mathbf{v}\in \mathbb{R}^3 @f$
     *
     * @f$ \robabx{i}{j}{\mathbf{v}} = \left[
     *  \begin{array}{c}
     *  v_x \\
     *  v_y \\
     *  v_z
     *  \end{array}
     *  \right]
     *  @f$
     *
     *  Usage example:
     *
     *  \code
     *  const Vector3D<> v1(1.0, 2.0, 3.0);
     *  const Vector3D<> v2(6.0, 7.0, 8.0);
     *  const Vector3D<> v3 = cross(v1, v2);
     *  const double d = dot(v1, v2);
     *  const Vector3D<> v4 = v2 - v1;
     *  \endcode
     */
    template< class T = double > class Vector3D
    {
      public:
        //! Eigen type equivalent to Vector3D
        typedef Eigen::Matrix< T, 3, 1 > EigenVector3D;

        //! Value type.
        typedef T value_type;

        /**
         * @brief Creates a 3D vector initialized with 0's
         */
        Vector3D ()
        {
            _vec[0] = 0;
            _vec[1] = 0;
            _vec[2] = 0;
        }

        /**
         * @brief Creates a 3D vector
         * @param x [in] @f$ x @f$
         * @param y [in] @f$ y @f$
         * @param z [in] @f$ z @f$
         */
        Vector3D (T x, T y, T z)
        {
            _vec[0] = x;
            _vec[1] = y;
            _vec[2] = z;
        }

        /**
         * @brief Copy constructor
         * @param vec [in] vector to copy
         */
        Vector3D (const Vector3D<T>& copy_vec): _vec(copy_vec._vec)
        {
        }

        /**
         * @brief Creates a 3D vector from vector_expression
         * @param r [in] an Eigen Vector
         */
        template< class R > explicit Vector3D (const Eigen::MatrixBase< R >& r)
        {
            _vec[0] = T( r.row (0) (0));
            _vec[1] = T( r.row (1) (0));
            _vec[2] = T( r.row (2) (0));
        }

        /**
         * @brief construct vector from std::vector
         * @param vec [in] the vector to construct from
         */
        Vector3D (const std::vector< T >& vec)
        {
            if (vec.size () != 3u) {
                RW_THROW ("Wrong Size vector matrix: N of size:" << 3 << " and vector of size: "
                                                                 << vec.size () << "given");
            }
            for (size_t i = 0; i < 3u; i++) {
                _vec[i] = vec[i];
            }
        }

        /**
         *  @brief The dimension of the vector (i.e. 3).
         * This method is provided to help support generic algorithms using
         * size() and operator[].
         * @return the size
         */
        size_t size () const { return 3u; }

        /**
         * @brief Get zero vector.
         * @return vector.
         */
        static Vector3D< T > zero () { return Vector3D< T > (0, 0, 0); }

        /**
         * @brief Get x vector (1,0,0)
         * @return vector.
         */
        static Vector3D< T > x () { return Vector3D< T > (1.0, 0, 0); }

        /**
         * @brief Get y vector (0,1,0)
         * @return vector.
         */
        static Vector3D< T > y () { return Vector3D< T > (0, 1.0, 0); }

        /**
         * @brief Get z vector (0,0,1)
         * @return vector.
         */
        static Vector3D< T > z () { return Vector3D< T > (0, 0, 1.0); }

        // ###################################################
        // #                 Math Operations                 #
        // ###################################################

        // ########## Eigen Operations

        /**
         * @brief element wise division.
         * @param rhs [in] the vector being devided with
         * @return the resulting Vector3D
         */
        template< class R > Vector3D< T > elemDivide (const Eigen::MatrixBase< R >& rhs) const
        {
            Vector3D< T > ret = *this;
            for (size_t i = 0; i < size (); i++) {
                ret._vec[i] /= rhs[i];
            }
            return ret;
        }

        /**
         * @brief Elementweise multiplication.
         * @param rhs [in] vector
         * @return the element wise product
         */
        template< class R > Vector3D< T > elemMultiply (const Eigen::MatrixBase< R >& rhs) const
        {
            Vector3D< T > ret = *this;
            for (size_t i = 0; i < size (); i++) {
                ret._vec[i] *= rhs[i];
            }
            return ret;
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R > Vector3D< T > operator- (const Eigen::MatrixBase< R >& rhs) const
        {
            return Vector3D< T > (_vec - rhs);
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R >
        friend Vector3D< T > operator- (const Eigen::MatrixBase< R >& lhs, const Vector3D< T >& rhs)
        {
            return Vector3D< T > (lhs - rhs.e ());
        }

        /**
         * @brief Vector addition.
         */
        template< class R > Vector3D< T > operator+ (const Eigen::MatrixBase< R >& rhs) const
        {
            return Vector3D< T > (_vec + rhs);
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R >
        friend Vector3D< T > operator+ (const Eigen::MatrixBase< R >& lhs, const Vector3D< T >& rhs)
        {
            return Vector3D< T > (lhs + rhs.e ());
        }

        // ########## Vector3D Operations

        /**
         * @brief element wise division.
         * @param rhs [in] the vector being devided with
         * @return the resulting Vector3D
         */
        Vector3D< T > elemDivide (const Vector3D< T >& rhs) const
        {
            return Vector3D< T > (
                _vec[0] / rhs._vec[0], _vec[1] / rhs._vec[1], _vec[2] / rhs._vec[2]);
        }

        /**
         * @brief Elementweise multiplication.
         * @param rhs [in] vector
         * @return the element wise product
         */
        Vector3D< T > elemMultiply (const Vector3D< T >& rhs) const
        {
            return Vector3D< T > (
                _vec[0] * rhs._vec[0], _vec[1] * rhs._vec[1], _vec[2] * rhs._vec[2]);
        }

        /**
         * @brief Vector subtraction.
         */
        Vector3D< T > operator- (const Vector3D< T >& b) const
        {
            return Vector3D< T > (_vec[0] - b[0], _vec[1] - b[1], _vec[2] - b[2]);
        }

        /**
         * @brief Vector addition.
         */
        Vector3D< T > operator+ (const Vector3D< T >& b) const
        {
            return Vector3D< T > (_vec[0] + b[0], _vec[1] + b[1], _vec[2] + b[2]);
        }

        /**
         * @brief Unary minus.
         * @brief negative version
         */
        Vector3D< T > operator- () const { return Vector3D< T > (-_vec[0], -_vec[1], -_vec[2]); }

        // ########## Scalar Operations

        /**
         * @brief Scalar division.
         * @param s [in] the scalar to devide with
         * @return result of devision
         */
        Vector3D< T > operator/ (T s) const
        {
            return Vector3D< T > (_vec[0] / s, _vec[1] / s, _vec[2] / s);
        }
#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar division.
         * @param lhs [in] the scalar to devide with
         * @param rhs [out] the vector beind devided
         * @return result of devision
         */
        friend Vector3D< T > operator/ (T lhs, const Vector3D< T >& rhs)
        {
            return Vector3D< T > (lhs / rhs._vec[0], lhs / rhs._vec[1], lhs / rhs._vec[2]);
        }
#endif
        /**
         * @brief Scalar multiplication.
         * @param rhs [in] the scalar to multiply with
         * @return the product
         */
        Vector3D< T > operator* (T rhs) const
        {
            return Vector3D< T > (_vec[0] * rhs, _vec[1] * rhs, _vec[2] * rhs);
        }

        /**
         * @brief Scalar multiplication.
         * @param lhs [in] the scalar to multiply with
         * @param rhs [in] the Vector to be multiplied
         * @return the product
         */
        friend Vector3D< T > operator* (T lhs, const Vector3D< T >& rhs)
        {
            return Vector3D< T > (lhs * rhs[0], lhs * rhs[1], lhs * rhs[2]);
        }

        /**
         * @brief Scalar multiplication.
         * @param rhs [in] the Eigen vector^T or matrix to multiply with
         * @return the product
         */
        template< class R > Vector3D< T > operator* (const Eigen::MatrixBase< R >& rhs) const
        {
            return Vector3D< T > (this->e()*rhs);
        }

        /**
         * @brief Scalar multiplication.
         * @param lhs [in] the Eigen vector^T or matrix to multiply with
         * @param rhs [in] the Vector to be multiplied
         * @return the product
         */
        template< class R > friend Vector3D< T > operator* (const Eigen::MatrixBase< R >& lhs, const Vector3D< T >& rhs)
        {
            return Vector3D< T > (lhs*rhs.e());
        }

        /**
         * @brief Scalar subtraction.
         */
        Vector3D< T > elemSubtract (const T rhs) const
        {
            return Vector3D< T > (_vec[0] - rhs, _vec[1] - rhs, _vec[2] - rhs);
        }

        /**
         * @brief Scalar addition.
         */
        Vector3D< T > elemAdd (const T rhs) const
        {
            return Vector3D< T > (_vec[0] + rhs, _vec[1] + rhs, _vec[2] + rhs);
        }

        // ########### Math Functions

        /**
         * @brief Returns the Euclidean norm (2-norm) of the vector
         * @return the norm
         */
        T norm2 () const
        {
            return sqrt (_vec[0] * _vec[0] + _vec[1] * _vec[1] + _vec[2] * _vec[2]);
        }

        /**
         * @brief Returns the Manhatten norm (1-norm) of the vector
         * @return the norm
         */
        T norm1 () const { return fabs (_vec[0]) + fabs (_vec[1]) + fabs (_vec[2]); }

        /**
         * @brief Returns the infinte norm (\f$\inf\f$-norm) of the vector
         * @return the norm
         */
        T normInf () const
        {
            T res      = fabs (_vec[0]);
            const T f1 = fabs (_vec[1]);
            if (f1 > res)
                res = f1;
            const T f2 = fabs (_vec[2]);
            if (f2 > res)
                res = f2;
            return res;
        }

        /**
         * @brief Calculate cross product
         * @param vec [in] the vector to cross with
         * @return the cross product
         */
        Vector3D< T > cross (const Vector3D& vec) const
        {
            return Vector3D< T > (_vec.cross (vec._vec));
        }

        /**
         * @brief calculate the dot product
         * @param vec [in] the vecor to be dotted
         * @return the dot product
         */
        T dot (const Vector3D& vec) const { return _vec.dot (vec._vec); }

        /**
         * @brief normalize vector to get length 1
         * @return the normalized Vector
         */
        Vector3D< T > normalize ()
        {
            T length = norm2 ();
            if (length != 0)
                return (*this) / length;
            else
                return Vector3D< T > (0, 0, 0);
        }

        // ###################################################
        // #                Acces Operators                  #
        // ###################################################

        /**
         * @brief Returns Reference to Eigen Vector
         * @return reference to underling eigen
         */
        EigenVector3D& e () { return _vec; }

        /**
         * @brief Returns Reference to Eigen Vector
         * @return copy of eigen vector
         */
        const EigenVector3D e () const { return _vec; }

#if !defined(SWIG)
        /**
         * @brief Returns reference to vector element
         * @param i [in] index in the vector \f$i\in \{0,1,2\} \f$
         * @return const reference to element
         */
        const T& operator() (size_t i) const { return _vec[i]; }

        /**
         * @brief Returns reference to vector element
         * @param i [in] index in the vector \f$i\in \{0,1,2\} \f$
         * @return reference to element
         */
        T& operator() (size_t i) { return _vec[i]; }

        /**
         * @brief Returns reference to vector element
         * @param i [in] index in the vector \f$i\in \{0,1,2\} \f$
         * @return const reference to element
         */
        const T& operator[] (size_t i) const { return _vec[i]; }

        /**
         * @brief Returns reference to vector element
         * @param i [in] index in the vector \f$i\in \{0,1,2\} \f$
         * @return reference to element
         */
        T& operator[] (size_t i) { return _vec[i]; }
#else
        ARRAYOPERATOR (T);
#endif
#if !defined(SWIG)
        /**
         * @brief Streaming operator.
         * @param out [in/out] the stream to continue
         * @param v [in] the vector to stream
         * @param reference to \b out
         */
        friend std::ostream& operator<< (std::ostream& out, const Vector3D< T >& v)
        {
            return out << "Vector3D(" << v[0] << ", " << v[1] << ", " << v[2] << ")";
        }
#else
        TOSTRING (rw::math::Vector3D< T >);
#endif
        // ###################################################
        // #             assignement Operators               #
        // ###################################################

        /**
         * @brief Scalar multiplication.
         */
        Vector3D< T >& operator*= (T s)
        {
            _vec[0] *= s;
            _vec[1] *= s;
            _vec[2] *= s;
            return *this;
        }

        /**
         * @brief Scalar division.
         */
        Vector3D< T >& operator/= (T s)
        {
            _vec[0] /= s;
            _vec[1] /= s;
            _vec[2] /= s;
            return *this;
        }

        /**
         * @brief Vector addition.
         */
        Vector3D< T >& operator+= (const Vector3D< T >& v)
        {
            _vec[0] += v._vec[0];
            _vec[1] += v._vec[1];
            _vec[2] += v._vec[2];
            return *this;
        }

        /**
         * @brief Vector subtraction.
         */
        Vector3D< T >& operator-= (const Vector3D< T >& v)
        {
            _vec[0] -= v._vec[0];
            _vec[1] -= v._vec[1];
            _vec[2] -= v._vec[2];
            return *this;
        }

        /**
         * @brief copy a vector from eigen type
         * @param r [in] an Eigen Vector
         */
        template< class R > Vector3D< T >& operator= (const Eigen::MatrixBase< R >& r)
        {
            _vec = r;
            return *this;
        }

        /**
         * @brief Vector addition.
         */
        template< class R > Vector3D< T >& operator+= (const Eigen::MatrixBase< R >& r)
        {
            _vec += r;
            return *this;
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R > Vector3D< T >& operator-= (const Eigen::MatrixBase< R >& r)
        {
            _vec -= r;
            return *this;
        }

        // ###################################################
        // #                    Comparetors                  #
        // ###################################################

        /**
         * @brief Compare with \b b for equality.
         * @param b [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        bool operator== (const Vector3D< T >& b) const
        {
            return _vec[0] == b[0] && _vec[1] == b[1] && _vec[2] == b[2];
        }

        /**
           @brief Compare with \b b for inequality.
           @param b [in] other vector.
           @return True if a and b are different, false otherwise.
        */
        bool operator!= (const Vector3D< T >& b) const { return !(*this == b); }

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        template< class R > bool operator== (const Eigen::MatrixBase< R >& rhs) const
        {
            return this->_vec == rhs;
        }

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        template< class R >
        friend bool operator== (const Eigen::MatrixBase< R >& lhs, const Vector3D< T >& rhs)
        {
            return lhs == rhs._vec;
        }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param b [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        template< class R > bool operator!= (const Eigen::MatrixBase< R >& rhs) const
        {
            return !(*this == rhs);
        }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param b [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        template< class R >
        friend bool operator!= (const Eigen::MatrixBase< R >& lhs, const Vector3D< T >& rhs)
        {
            return !(lhs == rhs);
        }
#if !defined(SWIG)
        /**
         * @brief implicit conversion to EigenVector
         */
        operator EigenVector3D () const { return this->e (); }

        /**
         * @brief implicit conversion to EigenVector
         */
        operator EigenVector3D& () { return this->e (); }
#endif

      private:
        EigenVector3D _vec;
    };

    /**
     * @brief Calculates the 3D vector cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the 3D vector cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     *
     * The 3D vector cross product is defined as:
     * @f$
     * \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c}
     *  v1_y * v2_z - v1_z * v2_y \\
     *  v1_z * v2_x - v1_x * v2_z \\
     *  v1_x * v2_y - v1_y * v2_x
     * \end{array}\right]
     * @f$
     *
     * @relates Vector3D
     */
    template< class T > const Vector3D< T > cross (const Vector3D< T >& v1, const Vector3D< T >& v2)
    {
        return Vector3D< T > (v1[1] * v2[2] - v1[2] * v2[1],
                              v1[2] * v2[0] - v1[0] * v2[2],
                              v1[0] * v2[1] - v1[1] * v2[0]);
    }

    /**
     * @brief Calculates the 3D vector cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     * @param dst [out] the 3D vector cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     *
     * The 3D vector cross product is defined as:
     * @f$
     * \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c}
     *  v1_y * v2_z - v1_z * v2_y \\
     *  v1_z * v2_x - v1_x * v2_z \\
     *  v1_x * v2_y - v1_y * v2_x
     * \end{array}\right]
     * @f$
     *
     * @relates Vector3D
     */
    template< class T >
    void cross (const Vector3D< T >& v1, const Vector3D< T >& v2, Vector3D< T >& dst)
    {
        dst[0] = v1[1] * v2[2] - v1[2] * v2[1];
        dst[1] = v1[2] * v2[0] - v1[0] * v2[2];
        dst[2] = v1[0] * v2[1] - v1[1] * v2[0];
    }

    /**
     * @brief Calculates the dot product @f$ \mathbf{v1} . \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the dot product @f$ \mathbf{v1} . \mathbf{v2} @f$
     *
     * @relates Vector3D
     */
    template< class T > T dot (const Vector3D< T >& v1, const Vector3D< T >& v2)
    {
        return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
        // return inner_prod(v1.m(), v2.m());
    }

    /**
     * @brief Returns the normalized vector \f$\mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} \f$.
     * In case \f$ \|mathbf{v}\| = 0\f$ the zero vector is returned.
     * @param v [in] \f$ \mathbf{v} \f$ which should be normalized
     * @return the normalized vector \f$ \mathbf{n} \f$
     *
     * @relates Vector3D
     */
    template< class T > const Vector3D< T > normalize (const Vector3D< T >& v)
    {
        T length = v.norm2 ();
        if (length != 0)
            return Vector3D< T > (v (0) / length, v (1) / length, v (2) / length);
        else
            return Vector3D< T > (0, 0, 0);
    }

    /**
     * @brief Calculates the angle from @f$ \mathbf{v1}@f$ to @f$ \mathbf{v2} @f$
     * around the axis defined by @f$ \mathbf{v1} \times \mathbf{v2} @f$ with n
     * determining the sign.
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     * @param n [in] @f$ \mathbf{n} @f$
     *
     * @return the angle
     *
     * @relates Vector3D
     */
    template< class T >
    double angle (const Vector3D< T >& v1, const Vector3D< T >& v2, const Vector3D< T >& n)
    {
        const Vector3D< T > nv1 = normalize (v1);
        const Vector3D< T > nv2 = normalize (v2);
        const Vector3D< T > nn  = normalize (n);
        return atan2 (dot (nn, cross (nv1, nv2)), dot (nv1, nv2));
    }

    /**
     * @brief Calculates the angle from @f$ \mathbf{v1}@f$ to @f$ \mathbf{v2} @f$
     * around the axis defined by @f$ \mathbf{v1} \times \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the angle
     *
     * @relates Vector3D
     */
    template< class T > double angle (const Vector3D< T >& v1, const Vector3D< T >& v2)
    {
        Vector3D< T > n = cross (v1, v2);
        return angle (v1, v2, n);
    }

    /**
     * @brief Casts Vector3D<T> to Vector3D<Q>
     * @param v [in] Vector3D with type T
     * @return Vector3D with type Q
     *
     * @relates Vector3D
     */
    template< class Q, class T > const Vector3D< Q > cast (const Vector3D< T >& v)
    {
        return Vector3D< Q > (
            static_cast< Q > (v (0)), static_cast< Q > (v (1)), static_cast< Q > (v (2)));
    }
#if !defined(SWIG)
    extern template class rw::math::Vector3D< double >;
    extern template class rw::math::Vector3D< float >;
#else
    SWIG_DECLARE_TEMPLATE (Vector3Dd, rw::math::Vector3D< double >);
    SWIG_DECLARE_TEMPLATE (Vector3Df, rw::math::Vector3D< float >);
#endif

    using Vector3Dd = Vector3D< double >;
    using Vector3Df = Vector3D< float >;

    /**@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Vector3D
         */
        template<>
        void write (const rw::math::Vector3D< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Vector3D
         */
        template<>
        void write (const rw::math::Vector3D< float >& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Vector3D
         */
        template<>
        void read (rw::math::Vector3D< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Vector3D
         */
        template<>
        void read (rw::math::Vector3D< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

namespace boost { namespace serialization {
    /**
     * @brief Boost serialization.
     * @param archive [in] the boost archive to read from or write to.
     * @param vector [in/out] the vector to read/write.
     * @param version [in] class version (currently version 0).
     * @relatedalso rw::math::Vector3D
     */
    template< class Archive, class T >
    void serialize (Archive& archive, rw::math::Vector3D< T >& vector, const unsigned int version)
    {
        archive& vector[0];
        archive& vector[1];
        archive& vector[2];
    }
}}    // namespace boost::serialization

#endif    // end include guard