#include <benchmark/benchmark.h>

#include <utils/RandomValuePool.h>

#include <sofa/type/Mat.h>
#include <Eigen/Core>
#include <Eigen/LU> // for inverse(?)
#include <array>

static void BM_Matrix_typemat3x3f_construct(benchmark::State& state);
static void BM_Matrix_typemat3x3f_construct_noinit(benchmark::State& state);
static void BM_Matrix_eigenmat33_construct(benchmark::State& state);
static void BM_Matrix_typemat3x3f_transpose(benchmark::State& state);
static void BM_Matrix_eigenmat33_transpose(benchmark::State& state);
static void BM_Matrix_typemat3x3f_invert(benchmark::State& state);
static void BM_Matrix_eigenmat33_invert(benchmark::State& state);
static void BM_Matrix_typemat3x3f_determinant(benchmark::State& state);
static void BM_Matrix_eigenmat33_determinant(benchmark::State& state);
template<int N>
static void BM_Matrix_typematf_matmult(benchmark::State& state);
template<int N>
static void BM_Matrix_eigenmatf_matmult(benchmark::State& state);
template<int N>
static void BM_Matrix_typematf_vecmult(benchmark::State& state);
template<int N>
static void BM_Matrix_eigenmatf_vecmult(benchmark::State& state);
static void BM_Matrix_typemat3x3f_assign(benchmark::State& state);
static void BM_Matrix_eigenmat33_assign(benchmark::State& state);

constexpr int64_t minSubIterations = 8 << 4;
constexpr int64_t maxSubIterations = 8 << 6;

BENCHMARK(BM_Matrix_typemat3x3f_construct)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK(BM_Matrix_typemat3x3f_construct_noinit)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK(BM_Matrix_eigenmat33_construct)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK(BM_Matrix_typemat3x3f_transpose)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK(BM_Matrix_eigenmat33_transpose)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK(BM_Matrix_typemat3x3f_invert)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK(BM_Matrix_eigenmat33_invert)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK(BM_Matrix_typemat3x3f_determinant)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK(BM_Matrix_eigenmat33_determinant)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK_TEMPLATE(BM_Matrix_typematf_matmult, 3)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK_TEMPLATE(BM_Matrix_typematf_matmult, 24)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK_TEMPLATE(BM_Matrix_eigenmatf_matmult, 3)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK_TEMPLATE(BM_Matrix_eigenmatf_matmult, 24)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK_TEMPLATE(BM_Matrix_typematf_vecmult, 3)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK_TEMPLATE(BM_Matrix_typematf_vecmult, 24)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK_TEMPLATE(BM_Matrix_eigenmatf_vecmult, 3)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK_TEMPLATE(BM_Matrix_eigenmatf_vecmult, 24)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK(BM_Matrix_typemat3x3f_assign)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);
BENCHMARK(BM_Matrix_eigenmat33_assign)->RangeMultiplier(2)->Ranges({ {minSubIterations, maxSubIterations} })->Unit(benchmark::kMicrosecond);

void BM_Matrix_typemat3x3f_construct(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * 9;
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    for (auto _ : state)
    {
        for (unsigned int i = 0; i < state.range(0); ++i)
        {
            benchmark::DoNotOptimize(sofa::type::Mat3x3f{
                sofa::type::Vec<3, float>{values[i], values[i + 1], values[i + 2]},
                sofa::type::Vec<3, float>{values[i + 3], values[i + 4], values[i + 5]},
                sofa::type::Vec<3, float>{values[i + 6], values[i + 7], values[i + 8] }
            });
        }
    }
}

void BM_Matrix_typemat3x3f_construct_noinit(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * 9;
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    for (auto _ : state)
    {
        for (unsigned int i = 0; i < state.range(0); ++i)
        {
            benchmark::DoNotOptimize(sofa::type::Mat3x3f{
                sofa::type::Mat3x3f::LineNoInit{values[i], values[i + 1], values[i + 2]},
                sofa::type::Mat3x3f::LineNoInit{values[i + 3], values[i + 4], values[i + 5]},
                sofa::type::Mat3x3f::LineNoInit{values[i + 6], values[i + 7], values[i + 8] }
            });
        }
    }
}

void BM_Matrix_eigenmat33_construct(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * 9;
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    for (auto _ : state)
    {
        for (unsigned int i = 0; i < state.range(0); ++i)
        {
            //only eigen3.4
            //Eigen::Matrix<float, 3,3>(
            //    { vc[i], vc[i + 1], vc[i + 2] },
            //    { vc[i + 3], vc[i + 4], vc[i + 5] },
            //    { vc[i + 6], vc[i + 7], vc[i + 8] }
            //);

            Eigen::Matrix<float, 3, 3> m;
            m << values[i], values[i + 1], values[i + 2], values[i + 3], values[i + 4], values[i + 5], values[i + 6], values[i + 7], values[i + 8];
            benchmark::DoNotOptimize(m);
        }
    }
}

void BM_Matrix_typemat3x3f_assign(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * 9;
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    for (auto _ : state)
    {
        for (unsigned int i = 0; i < state.range(0); ++i)
        {
            sofa::type::Mat3x3f m1{
                sofa::type::Mat3x3f::LineNoInit{values[i], values[i + 1], values[i + 2]},
                sofa::type::Mat3x3f::LineNoInit{values[i + 3], values[i + 4], values[i + 5]},
                sofa::type::Mat3x3f::LineNoInit{values[i + 6], values[i + 7], values[i + 8] }
            };
            sofa::type::Mat3x3f m2;
            m2 = m1;
            benchmark::DoNotOptimize(m2);
        }
    }
}

void BM_Matrix_eigenmat33_assign(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * 9;
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    for (auto _ : state)
    {
        for (unsigned int i = 0; i < state.range(0); ++i)
        {
            Eigen::Matrix<float, 3, 3> m1;
            m1 << values[i], values[i + 1], values[i + 2], values[i + 3], values[i + 4], values[i + 5], values[i + 6], values[i + 7], values[i + 8];
            Eigen::Matrix<float, 3, 3> m2;
            m2 = m1;
            benchmark::DoNotOptimize(m2);
        }
    }
}

void BM_Matrix_typemat3x3f_transpose(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * 9;
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    std::vector<sofa::type::Mat3x3f> vc;
    vc.reserve(state.range(0));

    for (unsigned int i = 0; i < state.range(0); i++)
    {
        vc.push_back(sofa::type::Mat3x3f{
                sofa::type::Mat3x3f::LineNoInit{values[i * 9 + 0], values[i * 9 + 1], values[i * 9 + 2]},
                sofa::type::Mat3x3f::LineNoInit{values[i * 9 + 3], values[i * 9 + 4], values[i * 9 + 5]},
                sofa::type::Mat3x3f::LineNoInit{values[i * 9 + 6], values[i * 9 + 7], values[i * 9 + 8]}
            });
    }

    for (auto _ : state)
    {
        for (auto& mat : vc)
        {
            auto t = mat.transposed();
            benchmark::DoNotOptimize(t);
        }
    }
}

void BM_Matrix_eigenmat33_transpose(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * 9;
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    std::vector<Eigen::Matrix3f> vc;
    vc.resize(state.range(0));

    for (unsigned int i = 0; i < state.range(0); i++)
    {
        vc[i] << values[i * 9 + 0] , values[i * 9 + 1] , values[i * 9 + 2],
                 values[i * 9 + 3] , values[i * 9 + 4] , values[i * 9 + 5],
                 values[i * 9 + 6] , values[i * 9 + 7] , values[i * 9 + 8];
    }

    for (auto _ : state)
    {
        for (auto& mat : vc)
        {
            Eigen::Matrix3f t = mat.transpose();
            benchmark::DoNotOptimize(t);
        }
    }
}

template<int N>
void BM_Matrix_typematf_matmult(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * N*N * 2;
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    std::vector<sofa::type::Mat<N, N, float > > vc1;
    std::vector<sofa::type::Mat<N, N, float > > vc2;
    vc1.reserve(state.range(0));
    vc2.reserve(state.range(0));

    auto it = values.begin();

    for (unsigned int i = 0; i < state.range(0); i++)
    {
        sofa::type::Mat<N, N, float > mat1, mat2;
        for (int a = 0; a < N; ++a)
        {
            for (int b = 0; b < N; ++b)
            {
                mat1[a][b] = *it++;
                mat2[a][b] = *it++;
            }
        }
        vc1.push_back(mat1);
        vc2.push_back(mat2);
    }

    for (auto _ : state)
    {
        for (auto i = 0 ; i < state.range(0); i++)
        {
            benchmark::DoNotOptimize(vc1[i]*vc2[i]);
        }
    }
}

template<int N>
void BM_Matrix_eigenmatf_matmult(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * N*N * 2;
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    std::vector<Eigen::Matrix<float, N, N > > vc1;
    std::vector<Eigen::Matrix<float, N, N > > vc2;
    vc1.reserve(state.range(0));
    vc2.reserve(state.range(0));
    benchmark::DoNotOptimize(vc1);
    benchmark::DoNotOptimize(vc2);

    auto it = values.begin();

    for (auto i = 0; i < state.range(0); i++)
    {
        Eigen::Matrix<float, N, N > mat1, mat2;
        for (int a = 0; a < N; ++a)
        {
            for (int b = 0; b < N; ++b)
            {
                mat1(a,b) = *it++;
                mat2(a,b) = *it++;
            }
        }
        vc1.push_back(mat1);
        vc2.push_back(mat2);
    }

    for (auto _ : state)
    {
        for (auto i = 0; i < state.range(0); i++)
        {
            Eigen::Matrix<float, N, N > product = vc1[i] * vc2[i];
            benchmark::DoNotOptimize(product);
        }
    }
}

template <int N>
void BM_Matrix_typematf_vecmult(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * (N*N + N);
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    std::vector<sofa::type::Mat<N, N, float > > vc1;
    std::vector<sofa::type::Vec<N, float > > vc2;
    vc1.reserve(state.range(0));
    vc2.reserve(state.range(0));

    auto it = values.begin();

    for (unsigned int i = 0; i < state.range(0); i++)
    {
        sofa::type::Mat<N, N, float > mat;
        sofa::type::Vec<N, float > vec;
        for (int a = 0; a < N; ++a)
        {
            for (int b = 0; b < N; ++b)
            {
                mat[a][b] = *it++;
            }
            vec[a] = *it++;
        }
        vc1.push_back(mat);
        vc2.push_back(vec);
    }

    for (auto _ : state)
    {
        for (auto i = 0 ; i < state.range(0); i++)
        {
            benchmark::DoNotOptimize(vc1[i]*vc2[i]);
        }
    }
}

template <int N>
void BM_Matrix_eigenmatf_vecmult(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * (N*N + N);
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    std::vector<Eigen::Matrix<float, N, N > > vc1;
    std::vector<Eigen::Matrix<float, N, 1 > > vc2;
    vc1.reserve(state.range(0));
    vc2.reserve(state.range(0));
    benchmark::DoNotOptimize(vc1);
    benchmark::DoNotOptimize(vc2);

    auto it = values.begin();

    for (auto i = 0; i < state.range(0); i++)
    {
        Eigen::Matrix<float, N, N > mat1;
        Eigen::Matrix<float, N, 1 > mat2;
        for (int a = 0; a < N; ++a)
        {
            for (int b = 0; b < N; ++b)
            {
                mat1(a,b) = *it++;
            }
            mat2(a,0) = *it++;
        }
        vc1.push_back(mat1);
        vc2.push_back(mat2);
    }

    for (auto _ : state)
    {
        for (auto i = 0; i < state.range(0); i++)
        {
            Eigen::Matrix<float, N, 1 > product = vc1[i] * vc2[i];
            benchmark::DoNotOptimize(product);
        }
    }
}

void BM_Matrix_typemat3x3f_determinant(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * 9;
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    std::vector<sofa::type::Mat3x3f> vc;
    vc.reserve(state.range(0));

    for (unsigned int i = 0; i < state.range(0); i++)
    {
        vc.push_back(sofa::type::Mat3x3f{
                sofa::type::Mat3x3f::LineNoInit{values[i * 9 + 0], values[i * 9 + 1], values[i * 9 + 2]},
                sofa::type::Mat3x3f::LineNoInit{values[i * 9 + 3], values[i * 9 + 4], values[i * 9 + 5]},
                sofa::type::Mat3x3f::LineNoInit{values[i * 9 + 6], values[i * 9 + 7], values[i * 9 + 8]}
            });
    }

    for (auto _ : state)
    {
        for (auto& mat : vc)
        {
            benchmark::DoNotOptimize(sofa::type::determinant(mat));
        }
    }
}

void BM_Matrix_eigenmat33_determinant(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * 9;
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    std::vector<Eigen::Matrix3f> vc;
    vc.resize(state.range(0));

    for (unsigned int i = 0; i < state.range(0); i++)
    {
        vc[i] << values[i * 9 + 0], values[i * 9 + 1], values[i * 9 + 2],
            values[i * 9 + 3], values[i * 9 + 4], values[i * 9 + 5],
            values[i * 9 + 6], values[i * 9 + 7], values[i * 9 + 8];
    }

    for (auto _ : state)
    {
        for (auto& mat : vc)
        {
            benchmark::DoNotOptimize(mat.determinant());
        }
    }
}

void BM_Matrix_typemat3x3f_invert(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * 9;
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    std::vector<sofa::type::Mat3x3f> vc;
    vc.reserve(state.range(0));

    for (unsigned int i = 0; i < state.range(0); i++)
    {
        vc.push_back(sofa::type::Mat3x3f{
                sofa::type::Mat3x3f::LineNoInit{values[i * 9 + 0], values[i * 9 + 1], values[i * 9 + 2]},
                sofa::type::Mat3x3f::LineNoInit{values[i * 9 + 3], values[i * 9 + 4], values[i * 9 + 5]},
                sofa::type::Mat3x3f::LineNoInit{values[i * 9 + 6], values[i * 9 + 7], values[i * 9 + 8]}
            });
    }

    for (auto _ : state)
    {
        for (auto& mat : vc)
        {
            benchmark::DoNotOptimize(mat.inverted());
        }
    }
}

void BM_Matrix_eigenmat33_invert(benchmark::State& state)
{
    constexpr auto totalsize = maxSubIterations * 9;
    const std::array<float, totalsize>& values = RandomValuePool<float, totalsize>::get();

    std::vector<Eigen::Matrix3f> vc;
    vc.resize(state.range(0));

    for (unsigned int i = 0; i < state.range(0); i++)
    {
        vc[i] << values[i * 9 + 0], values[i * 9 + 1], values[i * 9 + 2],
            values[i * 9 + 3], values[i * 9 + 4], values[i * 9 + 5],
            values[i * 9 + 6], values[i * 9 + 7], values[i * 9 + 8];
    }

    for (auto _ : state)
    {
        for (auto& mat : vc)
        {
            Eigen::Matrix3f inv = mat.inverse();
            benchmark::DoNotOptimize(inv);
        }
    }
}


/***************************************************************************************************************************
 * circular.cpp
 *
 * Author: Ydm
 *
 * Update Time: 2020.8.14
 *
 * descripation: demostration of circular flying using mavros
 *      1.
 *      2.
 *      3.
***************************************************************************************************************************/

#include <ros/ros.h>
#include <fstream>
#include <math.h>
#include <string>
#include <time.h>
#include <queue>
#include <vector>
#include <cstdlib>
#include <stdlib.h>
#include <iostream>
#include <stdio.h>
#include <math_utils.h>
#include <Eigen/Eigen>

#include <std_msgs/Bool.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/Imu.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/ActuatorControl.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

using namespace std;

float desire_x = 200.0;				//desired altitude
float desire_y = 100.0;				//desired altitude
float desire_z = 50.0;				//desired altitude
float desire_Radius = 10.0;		//desired radius of circle
float MoveTimeCnt = 0.0;
float priod = 2000.0;			//to change velocity of flying using it

float DESIRE_V = 10;

Eigen::Vector3d P_T0 = {desire_x, desire_y, desire_z};		//initial position of Target
Eigen::Vector3d P_M0 = {0, 0, 5};			//initial position of Missile
Eigen::Vector3d V_T = {-3, 0, 0};			//velocity of Target
Eigen::Vector3d V_M = {0, 0, 0};
float VM = 5;						//velocity of Missile

mavros_msgs::State current_state;

Eigen::Vector3d pos_target; 	//desired value in offboard mode
Eigen::Vector3d pos_drone;
Eigen::Vector3d temp_pos_drone;
Eigen::Vector3d temp_pos_target;
Eigen::Vector3d velocity_sp;

mavros_msgs::SetMode mode_cmd;
ros::Publisher setpoint_raw_local_pub;
ros::ServiceClient set_mode_client;

enum
{
	WATTING,
	CHECKING,
	PREPARE,
	REST,
	FLY,
	FLYOVER,
}FlyState = WATTING;		//the initial state

Eigen::Matrix3d L;
Eigen::Vector3d P_r;
Eigen::Vector3d P_T = P_T0;
Eigen::Vector3d P_M = P_M0;

//receive the current position of drone from controller

void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
	current_state = *msg;

	//cout << "P_T = " << endl << P_T << endl;
}

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
	//read the drone postion from the Mavros Package [Frame: ENU]
	Eigen::Vector3d pos_drone_fcu_enu(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);

	pos_drone = pos_drone_fcu_enu;

}

void att_cb(const sensor_msgs::Imu::ConstPtr& msg)
{
	Eigen::Quaterniond q_fcu = Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
      //Transform the Quaternion to euler Angles
      Eigen::Vector3d euler_fcu = quaternion_to_euler(q_fcu);
 }

//send the desired position to controller (xyz, yaw)
void send_vel_setpoint(const Eigen::Vector3d& vel_sp)
{
	mavros_msgs::PositionTarget pos_setpoint;
	pos_setpoint.type_mask = 0b110111000111;	//0b 100 111 000 111  velocity

	//Bitmask to indicate which dimensions should be ignored (1 means ignore, 0 means not ignore; Bit 10 must set to 0)
	//Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7: ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11: yaw, bit 12:yaw_rate
	//Bit 10 should set to 0, means no force sp

	pos_setpoint.coordinate_frame = 1;

	pos_setpoint.velocity.x = -3;
	pos_setpoint.velocity.y = 0;
	pos_setpoint.velocity.z = 0;

	setpoint_raw_local_pub.publish(pos_setpoint);
}

//send the desired position to controller (xyz, yaw)
void send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp)
{
	mavros_msgs::PositionTarget pos_setpoint;
	pos_setpoint.type_mask = 0b100111111000;	//0b 100 111 111 000 xyz + yaw

	//Bitmask to indicate which dimensions should be ignored (1 means ignore, 0 means not ignore; Bit 10 must set to 0)
	//Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7: ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11: yaw, bit 12:yaw_rate
	//Bit 10 should set to 0, means no force sp

	pos_setpoint.coordinate_frame = 1;

	pos_setpoint.position.x = pos_sp[0];
	pos_setpoint.position.y = pos_sp[1];
	pos_setpoint.position.z = pos_sp[2];

	pos_setpoint.yaw = yaw_sp;

	setpoint_raw_local_pub.publish(pos_setpoint);
}

void FlyState_update(void)
{
	switch(FlyState)
	{
		case WATTING:
			if(current_state.mode != "OFFBOARD")	//waitting for offboard mode
			{
				pos_target[0] = pos_drone[0];
				pos_target[1] = pos_drone[1];
				pos_target[2] = pos_drone[2];
				temp_pos_drone[0] = pos_drone[0];
				temp_pos_drone[1] = pos_drone[1];
				temp_pos_drone[2] = pos_drone[2];
				send_pos_setpoint(pos_target, 0);
			}
			else
			{
				pos_target[0] = temp_pos_drone[0];
				pos_target[1] = temp_pos_drone[1];
				pos_target[2] = temp_pos_drone[2];
				send_pos_setpoint(pos_target, 0);
				FlyState = CHECKING;
			}
			//cout << "WATTING" << endl;
			break;

		case CHECKING:
			if(pos_drone[0] == 0 && pos_drone[1] == 0)	//no position information, land
			{
				cout << "Check error, make sure have local location" <<endl;
				mode_cmd.request.custom_mode = "AUTO.LAND";
				set_mode_client.call(mode_cmd);
				FlyState = WATTING;
			}
			else
			{
				FlyState = PREPARE;
			}
			cout << "CHECKING" << endl;
			break;
		case PREPARE:									//fly to the first point located in [200, 100, 50]

			if(abs(pos_drone[0]-desire_x) < 3 && abs(pos_drone[1]-desire_y < 3) && abs(pos_drone[2]-desire_z) < 3)
			{
				FlyState = REST;
				cout << "!!! I am in the REST mode !!!";
			}

			pos_target[0] = desire_x;
			pos_target[1] = desire_y;
			pos_target[2] = desire_z;
			send_pos_setpoint(pos_target, 0);
			if(current_state.mode != "OFFBOARD")			//if it is switched to "onboard" mode, jump to the "WATTING"
			{
				FlyState = WATTING;
			};
			//cout << "PREPARE" << endl;
			break;
		case REST:
			pos_target[0] = desire_x;
			pos_target[1] = desire_y;
			pos_target[2] = desire_z;
			send_pos_setpoint(pos_target, 0);
			MoveTimeCnt += 1;
			if(MoveTimeCnt >= 20)
			{
				MoveTimeCnt = 0;
				FlyState = FLY;
			}
			if(current_state.mode != "OFFBOARD")			//if it is switched to "onboard" mode, jump to the "WATTING"
			{
				FlyState = WATTING;
			}
			//cout << "REST" << endl;
			break;
		case FLY:
			{

				//velocity_sp = {3,3,3};
				//pos_target = {200, 100, 50};
				//send_pos_setpoint(pos_target, 0);
				send_vel_setpoint(velocity_sp);
				//cout << "I am here!!! " << endl;
				if(current_state.mode != "OFFBOARD")			//if it is switched to "onboard" mode, jump to the "WATTING"
				{
					FlyState = WATTING;
				}
			}
			//cout << "FLY" << endl;
			break;
		case FLYOVER:
			{
				mode_cmd.request.custom_mode = "AUTO.LAND";
				set_mode_client.call(mode_cmd);
				FlyState = WATTING;
			}
			//cout << "FLYOVER" << endl;
			break;
		default:
			cout << "ERROR!!!" << endl;
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "target_uav1");
	ros::NodeHandle nh;

	ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/uav1/mavros/state", 10, state_cb);			//handle return function
    ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>("/uav1/mavros/local_position/pose", 100, pos_cb);
	ros::Subscriber attitude_sub = nh.subscribe<sensor_msgs::Imu>("/uav1/mavros/imu/data", 10, att_cb);

	setpoint_raw_local_pub = nh.advertise<mavros_msgs::PositionTarget>("/uav1/mavros/setpoint_raw/local", 10);

	set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/uav1/mavros/set_mode");			//little question

	cout << "target node started!!!" << endl;
	ros::Rate rate(20.0);
	while(ros::ok())
	{
		FlyState_update();
		ros::spinOnce();
		rate.sleep();
	}
	return 0;
}


// -*- C++ -*-
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
//                                   Jiao Lin
//                      California Institute of Technology
//                         (C) 2007 All Rights Reserved
//
// {LicenseText}
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

#include <sstream>
#include <boost/python.hpp>
#include "mccomponents/kernels/sample/phonon/AbstractDispersion_3D.h"
#include "mccomponents/kernels/sample/phonon/AbstractDebyeWallerFactor.h"
#include "mccomponents/kernels/sample/phonon/CoherentInelastic_PolyXtal.h"
#include "mccomponents/boostpython_binding/wrap_kernel.h"

namespace wrap_mccomponents {

  void wrap_Phonon_CoherentInelastic_PolyXtal_kernel()
  {
    using namespace boost::python;
    using namespace mccomponents::boostpython_binding;

    typedef mccomponents::kernels::phonon::CoherentInelastic_PolyXtal w_t;

    kernel_wrapper<w_t>::wrap
      ( "Phonon_CoherentInelastic_PolyXtal_kernel",
	init<
	const w_t::dispersion_t &,
	const w_t::atoms_t &,
	const w_t::R_t &, const w_t::R_t &, const w_t::R_t &,
	w_t::dwcalculator_t & ,
	w_t::float_t,
	w_t::float_t
	> ()
	[with_custodian_and_ward<1,2,
	 with_custodian_and_ward<1,3,
	 with_custodian_and_ward<1,7 > > > () ]
	)
      ;

  }

}

// version
// $Id$

// End of file


#pragma once

#include <random>
#include <numeric>
#include <set>
#include <boost/random.hpp>
#include <boost/random/uniform_real_distribution.hpp>

namespace mhcpp
{
	namespace random
	{
		using namespace std;

		// In order to solve WIRADA-392 we cannot use the distributions in std::
		// as they behave differently across compilers.
		typedef boost::random::uniform_real_distribution<double> uniform_real_distribution_double;
		typedef boost::random::discrete_distribution<int> discrete_distribution_int;
		typedef boost::random::normal_distribution<double> normal_distribution_double;

		// using typedef for GCC backward compatibility - replace with using statements later.
		typedef boost::mt19937 default_wila_random_engine;

		template<typename RNG, typename Distribution>
		class VariateGenerator
		{
		public:
			typedef typename Distribution::result_type result_type;

			VariateGenerator() { }

			VariateGenerator(const Distribution& d) : _dist(d) { }

			/**
			* Constructs a @c VariateGenerator object with the associated
			* \uniform_random_number_generator eng and the associated
			* \random_distribution d.
			*
			* Throws: If and what the copy constructor of RNG or
			* Distribution throws.
			*/

			VariateGenerator(const RNG& e, const Distribution& d)
				: _eng(e), _dist(d) { }

			VariateGenerator(const VariateGenerator& vg)
				: _eng(vg._eng), _dist(vg._dist) { }

			VariateGenerator(VariateGenerator&& vg)
			{
				this->_eng = std::move(vg._eng);
				this->_dist = std::move(vg._dist);
			}

		private:
			template<typename RNG1>
			void Clean(typename enable_if<std::is_pointer<RNG1>::value, RNG1>::type e)
			{
				delete e;
			}

			template<typename RNG1>
			void Clean(typename enable_if<!std::is_pointer<RNG1>::value, RNG1>::type e) {}
		public:

			~VariateGenerator()
			{
				Clean<RNG>(this->_eng);
			}

			VariateGenerator& operator=(const VariateGenerator& vg)
			{
				if (&vg == this) {
					return *this;
				}
				this->_eng = vg._eng;
				this->_dist = vg._dist;
				return *this;
			}

			VariateGenerator& operator=(VariateGenerator&& vg)
			{
				if (&vg == this) {
					return *this;
				}
				this->_eng = std::move(vg._eng);
				this->_dist = std::move(vg._dist);
				return *this;
			}

			bool RngEngineEquals(const VariateGenerator& other) const
			{
				if (&other == this) {
					return true;
				}
				return _eng == (other._eng);
			}

			result_type operator()() { return _dist(_eng); }

			const Distribution& distribution()
			{
				return _dist;
			}

			vector<result_type> Generate(size_t n, std::function<double(double)>& f)
			{
				vector<result_type> v(n);
				auto g = [&]()
				{
					result_type x = (*this)();
					return f(x);
				};
				std::generate(v.begin(), v.end(), g);
				return v;
			}

			vector<result_type> Generate(size_t n)
			{
				auto identity = [&](double x) { return x; };
				return Generate(n, identity);
			}

		private:
			RNG _eng;
			Distribution _dist;
		};

#ifdef __GNUC__
#define GCC_VERSION (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__)
#if GCC_VERSION < 40800
#define WILA_USE_TYPEDEF_RNG
#endif
#endif

#ifdef WILA_USE_TYPEDEF_RNG
		typedef VariateGenerator < default_wila_random_engine, mhcpp::random::discrete_distribution_int > RngInt;
		typedef VariateGenerator < default_wila_random_engine, mhcpp::random::uniform_real_distribution_double > RngReal;
		typedef VariateGenerator < default_wila_random_engine, mhcpp::random::normal_distribution_double > RngNorm;
#else
		/*template <typename RNG = default_wila_random_engine>
		typedef VariateGenerator < RNG, mhcpp::random::discrete_distribution_int > RngInt;
*/
		template <typename RNG = default_wila_random_engine>
		using RngInt = VariateGenerator < RNG, mhcpp::random::discrete_distribution_int >;

		template <typename RNG = default_wila_random_engine>
		using RngReal = VariateGenerator < RNG, mhcpp::random::uniform_real_distribution_double >;

		template <typename RNG = default_wila_random_engine>
		using RngNorm = VariateGenerator < RNG, mhcpp::random::normal_distribution_double >;
#endif

		template<typename RNG = default_wila_random_engine>
		class IRandomNumberGeneratorFactory
		{

		private:
			/** \brief	A random number generator factory. */

			template<typename RngEngine = default_wila_random_engine>
			class RandomNumberGeneratorFactory
			{
			private:
				RngEngine seedEngine;
			public:
				typedef RngEngine engine_type; // No typename needed here. See http://stackoverflow.com/questions/6489351/nested-name-specifier
				//	http://stackoverflow.com/questions/495021/why-can-templates-only-be-implemented-in-the-header-file

				RandomNumberGeneratorFactory(unsigned int seed) : seedEngine(seed)
				{
				}

				RandomNumberGeneratorFactory() : seedEngine(0)
				{
				}

				RandomNumberGeneratorFactory(const RandomNumberGeneratorFactory& src)
				{
					this->seedEngine = src.seedEngine;
				}

				RandomNumberGeneratorFactory(RandomNumberGeneratorFactory&& src)
				{
					this->seedEngine = std::move(src.seedEngine);
				}

				RandomNumberGeneratorFactory& operator = (const RandomNumberGeneratorFactory& src)
				{
					if (&src == this) {
						return *this;
					}
					this->seedEngine = src.seedEngine;
					return *this;
				}

				RandomNumberGeneratorFactory& operator = (RandomNumberGeneratorFactory&& src)
				{
					if (&src == this) {
						return *this;
					}
					this->seedEngine = std::move(src.seedEngine);
					return *this;
				}

				virtual ~RandomNumberGeneratorFactory()
				{
				}

				bool Equals(const RandomNumberGeneratorFactory& other) const
				{
					return seedEngine == (other.seedEngine);
				}

				RngEngine * CreateNewEngine()
				{
					return new RngEngine(seedEngine());
				}

				RandomNumberGeneratorFactory<RngEngine> * CreateNewFactory()
				{
					return new RandomNumberGeneratorFactory<RngEngine>(seedEngine());
				}

				unsigned int operator()() {
					return seedEngine();
				}

				template<class DistributionType = mhcpp::random::uniform_real_distribution_double>
				static VariateGenerator<RngEngine*, DistributionType> * CreateVariateGenerator(RandomNumberGeneratorFactory<RngEngine>& rngf, DistributionType& dist)
				{
					return new VariateGenerator<RngEngine*, DistributionType>(rngf.CreateNewEngine(), dist);
				}

			};

			RandomNumberGeneratorFactory<RNG> rng;

		public:
			IRandomNumberGeneratorFactory() : rng(0)
			{
			}

			IRandomNumberGeneratorFactory(unsigned int seed) : rng(seed)
			{
			}

			IRandomNumberGeneratorFactory(const IRandomNumberGeneratorFactory& src)
			{
				this->rng = src.rng;
			}

			IRandomNumberGeneratorFactory(IRandomNumberGeneratorFactory&& src)
			{
				this->rng = std::move(src.rng);
			}

			IRandomNumberGeneratorFactory& operator = (const IRandomNumberGeneratorFactory& src)
			{
				if (&src == this) {
					return *this;
				}
				this->rng = src.rng;
				return *this;
			}

			IRandomNumberGeneratorFactory& operator = (IRandomNumberGeneratorFactory&& src)
			{
				if (&src == this) {
					return *this;
				}
				this->rng = std::move(src.rng);
				return *this;
			}

			bool Equals(const IRandomNumberGeneratorFactory& other) const
			{
				return rng.Equals(other.rng);
			}

			unsigned int Next()
			{
				return rng();
			}

			unsigned int PeekNext() const
			{
				RandomNumberGeneratorFactory<RNG> rngTmp(rng);
				return rngTmp();
			}

			std::vector<unsigned int> Next(size_t size)
			{
				std::vector<unsigned int> result;
				for (size_t i = 0; i < size; i++)
				{
					result.push_back(Next());
				}
				return result;
			}

			IRandomNumberGeneratorFactory CreateNew()
			{
				return IRandomNumberGeneratorFactory(Next());
			}

			default_wila_random_engine CreateNewStd()
			{
				unsigned int seed = Next();
				return CreateNewStd(seed);
			}

			static default_wila_random_engine CreateNewStd(unsigned int seed)
			{
				return default_wila_random_engine(seed);
			}

			RNG CreateNewEngine()
			{
				unsigned int seed = Next();
				return CreateNewEngine(seed);
			}

			static RNG CreateNewEngine(unsigned int seed)
			{
				return RNG(seed);
			}

			// http://stackoverflow.com/questions/7166799/specialize-template-function-for-template-class
			template<class DistributionType = mhcpp::random::uniform_real_distribution_double>
			VariateGenerator<RNG, DistributionType> CreateVariateGenerator(DistributionType& dist, unsigned int seed) const
			{
				return VariateGenerator<RNG, DistributionType>(CreateNewEngine(seed), dist);
			}

			template<class DistributionType = mhcpp::random::uniform_real_distribution_double>
			VariateGenerator<RNG, DistributionType> CreateVariateGenerator(DistributionType& dist)
			{
				return VariateGenerator<RNG, DistributionType>(CreateNewEngine(), dist);
			}
		};

		template<typename RNG = default_wila_random_engine>
		RngInt<RNG> CreateTrapezoidalRng(size_t n, const RNG& generator, double trapezoidalFactor = -1)
		{
			std::vector<double> weights(n);
			double m = (double)n;
			double sumWeights = n*(n + 1) / 2.0;
			double avgWeight = sumWeights / m;

			if ((trapezoidalFactor <= 0) || (trapezoidalFactor >= 2))
			{
				// default as per the original SCE paper. Note that we do not need
				// to normalize: std::discrete_distribution takes care of it
				for (size_t i = 1; i <= n; i++)
					weights[i - 1] = (double)(n + 1 - i);
			}
			else
			{
				// y = ax + b
				double b = trapezoidalFactor; // y(0) = b
				double a = 2 * (1 - b) / (m - 1);  // because y(n-1) = (2-b) = a * (n-1) + b
				for (size_t i = 0; i < n; i++)
					weights[i] = a * i + b;
			}
			// Would expect to be able to do:
			// mhcpp::random::discrete_distribution_int distribution(weights.begin(), weights.end());
			// but missing constructor in MS implementation. Using workaround derived from
			// http://stackoverflow.com/questions/21959404/initialising-stddiscrete-distribution-in-vs2013
			std::size_t i(0);
			mhcpp::random::discrete_distribution_int distribution(weights.size(),
				0.0, // dummy!
				1.0, // dummy!
				[&weights, &i](double)
			{
				auto w = weights[i];
				++i;
				return w;
			});
			return VariateGenerator<default_wila_random_engine, mhcpp::random::discrete_distribution_int>(generator, distribution);
		}

		template<typename X>
		static std::vector<const X*> AsPointers(const std::vector<X>& vec)
		{
			std::vector<const X*> result;
			for (size_t i = 0; i < vec.size(); i++)
			{
				const X& e = vec[i];
				result.push_back(&e);
			}
			return result;
		}

		template<typename RNG = default_wila_random_engine>
		vector<int> SampleFrom(RngInt<RNG>& drng, size_t nsampling)
		{
			size_t size = drng.distribution().max() - drng.distribution().min() + 1;
			std::vector<int>p(size);

			for (size_t i = 0; i < nsampling; ++i) {
				int number = drng();
				++p[number];
			}
			return p;
		}

		template<typename ElemType>
		vector<ElemType> SampleFrom(RngInt<>& drng, const std::vector<ElemType>& population, size_t n,
			vector<ElemType>& leftOut, bool replace = false)
		{
			if (!replace && population.size() <= n)
				throw std::logic_error("If elements are sampled once, the output size must be less than the population sampled from");

			std::set<int> selected;
			std::set<int> notSelected;
			for (size_t i = 0; i < population.size(); i++)
				notSelected.emplace(i);
			std::vector<ElemType> result(n);
			if (replace)
			{
				for (size_t i = 0; i < n; i++)
				{
					result[i] = population[drng()];
					selected.emplace(i);
					notSelected.erase(i);
				}
			}
			else
			{
				auto src = AsPointers(population);
				int counter = 0;
				while (counter < n)
				{
					int i = drng();
					if (!(src[i] == nullptr))
					{
						result[counter] = *(src[i]);
						src[i] = nullptr;
						selected.emplace(i);
						notSelected.erase(i);
						counter++;
					}
#ifdef _DEBUG
					//CheckParameterFeasible(population);
#endif
				}
				leftOut.clear();
				for (auto index : notSelected)
					leftOut.push_back(population[index]);
#ifdef _DEBUG
				//CheckParameterFeasible(population);
				//CheckParameterFeasible(leftOut);
#endif
			}
			return result;
		}

	}
}


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Wakker, K. F. (2007), Lecture Notes Astrodynamics II (Chapter 18), TU Delft course AE4-874,
 *          Delft University of technology, Delft, The Netherlands.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>
#include <Eigen/Dense>
#include <math.h>
#include <iostream>

#include "Tudat/Astrodynamics/Ephemerides/approximatePlanetPositions.h"
#include "Tudat/Astrodynamics/LowThrustTrajectories/hybridMethodModel.h"
#include "Tudat/Astrodynamics/LowThrustTrajectories/hybridOptimisationSetup.h"
#include "Tudat/Astrodynamics/LowThrustTrajectories/hybridMethod.h"
#include "pagmo/algorithms/de1220.hpp"
#include "Tudat/Astrodynamics/LowThrustTrajectories/lowThrustLeg.h"

namespace tudat
{
namespace unit_tests
{

//! Test hybrid method implementation.
BOOST_AUTO_TEST_SUITE( test_hybrid_method )

BOOST_AUTO_TEST_CASE( test_hybrid_method_implementation )
{
    using namespace low_thrust_trajectories;

    spice_interface::loadStandardSpiceKernels( );

    double maximumThrust = 0.450;
    double specificImpulse = 3000.0;
    double mass = 1800.0;

    std::function< double( const double ) > specificImpulseFunction = [ = ] ( const double currentTime )
    {
        return specificImpulse;
    };

    double julianDate = 1000.0 * physical_constants::JULIAN_DAY;
    double timeOfFlight = 100.0 * physical_constants::JULIAN_DAY;

    // Define body settings for simulation.
    std::vector< std::string > bodiesToCreate;
    bodiesToCreate.push_back( "Sun" );
    bodiesToCreate.push_back( "Earth" );

    // Create body objects.
    std::map< std::string, std::shared_ptr< simulation_setup::BodySettings > > bodySettings =
            simulation_setup::getDefaultBodySettings( bodiesToCreate, julianDate - 300.0, julianDate + timeOfFlight + 300.0 );
    for( unsigned int i = 0; i < bodiesToCreate.size( ); i++ )
    {
        bodySettings[ bodiesToCreate.at( i ) ]->ephemerisSettings->resetFrameOrientation( "J2000" );
        bodySettings[ bodiesToCreate.at( i ) ]->rotationModelSettings->resetOriginalFrame( "J2000" );
    }
    simulation_setup::NamedBodyMap bodyMap = createBodies( bodySettings );

    // Create spacecraft object.
    bodyMap[ "Vehicle" ] = std::make_shared< simulation_setup::Body >( );

    // Finalize body creation.
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "J2000" );

    std::string bodyToPropagate = "Vehicle";
    std::string centralBody = "Earth";

    // Set vehicle mass.
    bodyMap[ bodyToPropagate ]->setConstantBodyMass( mass );

    // Initial and final states in keplerian elements.
    Eigen::Vector6d initialKeplerianElements = ( Eigen::Vector6d( ) << 24505.9e3, 0.725, 7.0 * mathematical_constants::PI / 180.0,
                                                 0.0, 0.0, 0.0 ).finished( );
    Eigen::Vector6d finalKeplerianElements = ( Eigen::Vector6d( ) << 42164.65e3, 5.53e-4, 7.41e-5 * mathematical_constants::PI / 180.0,
                                               0.0, 0.0, 0.0 ).finished( );

    // Initial and final states in cartesian coordinates.
    Eigen::Vector6d stateAtDeparture = orbital_element_conversions::convertKeplerianToCartesianElements(
                initialKeplerianElements, bodyMap[ "Earth" ]->getGravityFieldModel()->getGravitationalParameter() );
    Eigen::Vector6d stateAtArrival = orbital_element_conversions::convertKeplerianToCartesianElements(
                finalKeplerianElements, bodyMap[ "Earth" ]->getGravityFieldModel()->getGravitationalParameter() );

    // Define integrator settings.
    double stepSize = ( timeOfFlight ) / static_cast< double >( 40000 );
    std::shared_ptr< numerical_integrators::IntegratorSettings< double > > integratorSettings =
            std::make_shared< numerical_integrators::IntegratorSettings< double > >
            ( numerical_integrators::rungeKutta4, 0.0, stepSize );

    // Define optimisation algorithm.
    algorithm optimisationAlgorithm{ pagmo::de1220() };

    std::shared_ptr< simulation_setup::OptimisationSettings > optimisationSettings =
            std::make_shared< simulation_setup::OptimisationSettings >( optimisationAlgorithm, 1, 10, 1.0e-3 );

    // Create hybrid method trajectory.
    HybridMethod hybridMethod = HybridMethod( stateAtDeparture, stateAtArrival, maximumThrust, specificImpulse,
                                              timeOfFlight, bodyMap, bodyToPropagate, centralBody, integratorSettings,
                                              optimisationSettings );

    // Retrieve optimisation output.
    std::vector< double > fitnessVector =  hybridMethod.getBestIndividualFitness( );
    std::vector< double > bestDesignVariables = hybridMethod.getBestIndividual( );

    // Define MEE costates function.
    Eigen::VectorXd bestInitialMEEcostates; bestInitialMEEcostates.resize( 5 );
    Eigen::VectorXd bestFinalMEEcostates; bestFinalMEEcostates.resize( 5 );
    for ( int i = 0 ; i < 5 ; i++ )
    {
        bestInitialMEEcostates[ i ] = hybridMethod.getBestIndividual( )[ i ];
        bestFinalMEEcostates[ i ] = hybridMethod.getBestIndividual( )[ i + 5 ];
    }

    std::function< Eigen::VectorXd( const double ) > costatesFunction = [ = ]( const double currentTime )
    {
        Eigen::VectorXd currentCostates;
        currentCostates.resize( 5 );

        for ( int i = 0 ; i < 5 ; i++ )
        {
            currentCostates[ i ] = bestInitialMEEcostates[ i ]
                    + ( currentTime / timeOfFlight ) * ( bestFinalMEEcostates[ i ] - bestInitialMEEcostates[ i ] );
        }
        return currentCostates;
    };

    // Retrieve trajectory, mass, thrust, and thrust acceleration for a given set of epochs.
    int numberSteps = 10;
    std::vector< double > epochsVector;
    for ( int i = 1 ; i <= numberSteps ; i++ )
    {
        epochsVector.push_back( timeOfFlight / numberSteps * i );
    }

    std::map< double, Eigen::Vector6d > trajectory;
    std::map< double, Eigen::VectorXd > massProfile;
    std::map< double, Eigen::VectorXd > thrustProfile;
    std::map< double, Eigen::VectorXd > thrustAccelerationProfile;

    hybridMethod.getTrajectory( epochsVector, trajectory );
    hybridMethod.getMassProfile( epochsVector, massProfile, specificImpulseFunction, integratorSettings );
    hybridMethod.getThrustForceProfile( epochsVector, thrustProfile, specificImpulseFunction, integratorSettings );
    hybridMethod.getThrustAccelerationProfile( epochsVector, thrustAccelerationProfile, specificImpulseFunction, integratorSettings );

    /// PROPAGATE THE TRAJECTORY NUMERICALLY (SHOULD BE EQUIVALENT TO HYBRID METHOD)

    // Define thrust direction settings from the MEE costates.
    std::shared_ptr< simulation_setup::MeeCostateBasedThrustDirectionSettings > thrustDirectionSettings =
            std::make_shared< simulation_setup::MeeCostateBasedThrustDirectionSettings >( bodyToPropagate, centralBody, costatesFunction );

    // Define bang-bang thrust magnitude settings based on MEE co-states.
    std::shared_ptr< simulation_setup::FromMeeCostatesBangBangThrustMagnitudeSettings > thrustMagnitudeSettings
            = std::make_shared< simulation_setup::FromMeeCostatesBangBangThrustMagnitudeSettings >(
                maximumThrust, specificImpulseFunction, costatesFunction, bodyToPropagate, centralBody );

    // Define thrust acceleration settings.
    std::shared_ptr< simulation_setup::ThrustAccelerationSettings > thrustAccelerationSettings =
            std::make_shared< simulation_setup::ThrustAccelerationSettings >( thrustDirectionSettings, thrustMagnitudeSettings );

    // Define acceleration map.
    std::map< std::string, std::vector< std::shared_ptr< simulation_setup::AccelerationSettings > > > bodyToPropagateAccelerations;
    bodyToPropagateAccelerations[ centralBody ].push_back( std::make_shared< simulation_setup::AccelerationSettings >(
                                                                    basic_astrodynamics::central_gravity ) );
    bodyToPropagateAccelerations[ bodyToPropagate ].push_back( thrustAccelerationSettings );

    bodyMap[ bodyToPropagate ]->setConstantBodyMass( mass );

    simulation_setup::SelectedAccelerationMap accelerationMap;
    accelerationMap[ bodyToPropagate ] = bodyToPropagateAccelerations;

    // Create the acceleration map.
    basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodyMap, accelerationMap, std::vector< std::string >{ bodyToPropagate }, std::vector< std::string >{ centralBody } );

    // Create mass rate models
    std::map< std::string, std::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels;
    massRateModels[ bodyToPropagate ] = simulation_setup::createMassRateModel( bodyToPropagate, std::make_shared< simulation_setup::FromThrustMassModelSettings >( 1 ),
                                                       bodyMap, accelerationModelMap );

    // Define list of dependent variables to save.
    std::vector< std::shared_ptr< propagators::SingleDependentVariableSaveSettings > > dependentVariablesList;
    dependentVariablesList.push_back( std::make_shared< propagators::SingleAccelerationDependentVariableSaveSettings >(
                        basic_astrodynamics::thrust_acceleration, "Vehicle", "Vehicle", 0 ) );

    // Create object with list of dependent variables
    std::shared_ptr< propagators::DependentVariableSaveSettings > dependentVariablesToSave =
            std::make_shared< propagators::DependentVariableSaveSettings >( dependentVariablesList );

    // Define list of dependent variables to save.
    dependentVariablesList.push_back( std::make_shared< propagators::SingleDependentVariableSaveSettings >(
                    propagators::total_mass_rate_dependent_variables, bodyToPropagate ) );

    // Create object with list of dependent variables
    dependentVariablesToSave = std::make_shared< propagators::DependentVariableSaveSettings >( dependentVariablesList );

    double initialTime = 0.0;
    Eigen::Vector6d currentState = stateAtDeparture;
    double currentMass = mass;
    for ( std::map< double, Eigen::Vector6d >::iterator itr = trajectory.begin( ) ; itr != trajectory.end( ) ; itr++ )
    {
        double currentEpoch = itr->first;

        // Create termination conditions settings.
        std::shared_ptr< propagators::PropagationTerminationSettings > terminationSettings =
                std::make_shared< propagators::PropagationTimeTerminationSettings >( currentEpoch, true );

        // Define translational state propagation settings
        std::shared_ptr< propagators::TranslationalStatePropagatorSettings< double > > translationalStatePropagatorSettings =
                std::make_shared< propagators::TranslationalStatePropagatorSettings< double > >
                            ( std::vector< std::string >{ centralBody }, accelerationModelMap,
                              std::vector< std::string >{ bodyToPropagate }, currentState,
                              terminationSettings, propagators::gauss_modified_equinoctial, dependentVariablesToSave );

        // Create settings for propagating the mass of the vehicle.
        std::shared_ptr< propagators::MassPropagatorSettings< double > > massPropagatorSettings =
                std::make_shared< propagators::MassPropagatorSettings< double > >(
                    std::vector< std::string >{ bodyToPropagate }, massRateModels, ( Eigen::Matrix< double, 1, 1 >( ) << currentMass ).finished( ),
                    terminationSettings );

        integratorSettings->initialTimeStep_ = std::fabs( integratorSettings->initialTimeStep_ );
        integratorSettings->initialTime_ = initialTime;

        // Create list of propagation settings.
        std::vector< std::shared_ptr< propagators::SingleArcPropagatorSettings< double > > > propagatorSettingsVector;

        // Backward propagator settings vector.
        propagatorSettingsVector.push_back( translationalStatePropagatorSettings );
        propagatorSettingsVector.push_back( massPropagatorSettings );

        // Define propagator settings.
        std::shared_ptr< propagators::PropagatorSettings< double > > propagatorSettings = std::make_shared< propagators::MultiTypePropagatorSettings< double > >(
                    propagatorSettingsVector, terminationSettings, dependentVariablesToSave );

        bodyMap[ bodyToPropagate ]->setConstantBodyMass( currentMass );

        // Perform propagation.
        propagators::SingleArcDynamicsSimulator< > dynamicsSimulator( bodyMap, integratorSettings, propagatorSettings );
        std::map< double, Eigen::VectorXd > stateHistory = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
        std::map< double, Eigen::VectorXd > dependentVariableHistory = dynamicsSimulator.getDependentVariableHistory( );

        Eigen::Vector6d currentExpectedState = stateHistory.rbegin( )->second.segment( 0, 6 );
        double currentExpectedMass = stateHistory.rbegin( )->second[ 6 ];
        Eigen::Vector3d currentExpectedThrustAcceleration =  dependentVariableHistory.rbegin( )->second.segment( 0, 3 );
        Eigen::Vector3d currentExpectedThrust = currentExpectedThrustAcceleration * currentExpectedMass;

        // Check consistency between hybrid method and numerical propagation results.
        for ( int i = 0 ; i < 3 ; i++ )
        {
            BOOST_CHECK_SMALL( ( std::fabs( trajectory[ itr->first ][ i ] - currentExpectedState[ i ] ) / currentExpectedState.segment( 0, 3 ).norm( ) ), 1.0e-12 );
            BOOST_CHECK_SMALL( ( std::fabs( trajectory[ itr->first ][ i + 3 ] - currentExpectedState[ i + 3 ] ) / currentExpectedState.segment( 3, 3 ).norm( ) ), 1.0e-12 );
            BOOST_CHECK_SMALL( ( std::fabs( thrustProfile[ itr->first ][ i ] - currentExpectedThrust[ i ] ) ), 1.0e-8 );
            BOOST_CHECK_SMALL( ( std::fabs( thrustAccelerationProfile[ itr->first ][ i ] - currentExpectedThrustAcceleration[ i ] ) ), 1.0e-10 );
        }
        BOOST_CHECK_SMALL( std::fabs( massProfile[ itr->first ][ 0 ] - currentExpectedMass ), 1.0e-15 );

        initialTime = currentEpoch;
        currentMass = currentExpectedMass;
        currentState = currentExpectedState;

    }

    /// Test the computeSemiAnalyticalAndFullPropagation function
    /// (the difference between hybrid method and full propagation results should be integration errors only)

    bodyMap[ bodyToPropagate ]->setConstantBodyMass( mass );

    // Create termination conditions settings.
    std::pair< std::shared_ptr< propagators::PropagationTerminationSettings >,
            std::shared_ptr< propagators::PropagationTerminationSettings > > terminationConditions;

    terminationConditions.first = std::make_shared< propagators::PropagationTimeTerminationSettings >( 0.0, true );
    terminationConditions.second = std::make_shared< propagators::PropagationTimeTerminationSettings >( timeOfFlight, true );

    // Define empty maps to store the propagation results.
    std::map< double, Eigen::VectorXd > fullPropagationResults;
    std::map< double, Eigen::Vector6d > hybridMethodResults;
    std::map< double, Eigen::VectorXd > dependentVariablesHistory;

    basic_astrodynamics::AccelerationMap perturbingAccelerationsMap;

    // Create complete propagation settings (backward and forward propagations).
    std::pair< std::shared_ptr< propagators::PropagatorSettings< double > >,
            std::shared_ptr< propagators::PropagatorSettings< double > > > propagatorSettings = hybridMethod.createLowThrustPropagatorSettings(
                 specificImpulseFunction, perturbingAccelerationsMap, integratorSettings, dependentVariablesToSave );

    // Compute full propagation.
    hybridMethod.computeSemiAnalyticalAndFullPropagation( integratorSettings, propagatorSettings, fullPropagationResults,
                                                                  hybridMethodResults, dependentVariablesHistory );

    for ( int i = 0 ; i < 3 ; i++ )
    {
        BOOST_CHECK_SMALL( ( std::fabs(  hybridMethodResults.begin()->second[ i ] - fullPropagationResults.begin()->second[ i ] ) / stateAtDeparture.segment( 0, 3 ).norm( ) ), 2.0e-6 );
        BOOST_CHECK_SMALL( ( std::fabs(  hybridMethodResults.begin()->second[ i + 3 ] - fullPropagationResults.begin()->second[ i + 3 ] ) / stateAtDeparture.segment( 3, 3 ).norm( ) ), 2.0e-6 );

        BOOST_CHECK_SMALL( ( std::fabs(  hybridMethodResults.rbegin()->second[ i ] - fullPropagationResults.rbegin()->second[ i ] ) / stateAtArrival.segment( 0, 3 ).norm( ) ), 2.0e-6 );
        BOOST_CHECK_SMALL( ( std::fabs(  hybridMethodResults.rbegin()->second[ i + 3 ] - fullPropagationResults.rbegin()->second[ i + 3 ] ) / stateAtArrival.segment( 3, 3 ).norm( ) ), 2.0e-6 );

    }

}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Astrodynamics/GroundStations/groundStationState.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/oblateSpheroidBodyShapeModel.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateRepresentationConversions.h"
#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"
#include "Tudat/External/SpiceInterface/spiceEphemeris.h"
#include "Tudat/External/SpiceInterface/spiceRotationalEphemeris.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/body.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createGroundStations.h"
#include "Tudat/SimulationSetup/EstimationSetup/createLightTimeCalculator.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat::coordinate_conversions;
using namespace tudat::basic_astrodynamics;
using namespace tudat::ground_stations;
using namespace tudat::simulation_setup;
using namespace tudat::unit_conversions;
using namespace tudat::spice_interface;

BOOST_AUTO_TEST_SUITE( test_ground_station_state )

//! Test if ground stations are correctly created.
BOOST_AUTO_TEST_CASE( test_GroundStationState )
{
    // Create Earth object
    std::shared_ptr< Body > earth = std::make_shared< Body >( );
    NamedBodyMap bodyMap;
    bodyMap[ "Earth" ] = earth;

    // Central body characteristics (WGS84 Earth ellipsoid).
    const double flattening = 1.0 / 298.257223563;
    const double equatorialRadius = 6378137.0;

    std::shared_ptr< basic_astrodynamics::OblateSpheroidBodyShapeModel > oblateSpheroidModel =
            std::make_shared< basic_astrodynamics::OblateSpheroidBodyShapeModel >(
                equatorialRadius, flattening );

    earth->setShapeModel( oblateSpheroidModel );

    // Expected Cartesian state, Montenbruck & Gill (2000) Exercise 5.3.
    const Eigen::Vector3d testCartesianPosition( 1917032.190, 6029782.349, -801376.113 );

    // Expected Cartesian state, Montenbruck & Gill (2000) Exercise 5.3.
    const Eigen::Vector3d testGeodeticPosition( -63.667,
                                                convertDegreesToRadians( -7.26654999 ),
                                                convertDegreesToRadians( 72.36312094 ) );

    // Manually compute associated spherical position
    Eigen::Vector3d testSphericalPosition = coordinate_conversions::convertCartesianToSpherical(
                testCartesianPosition );
    testSphericalPosition( 1 ) = mathematical_constants::PI / 2.0 - testSphericalPosition( 1 );

    // Creatre ground stations: same position, but different representation
    createGroundStation( earth, "Station1", testCartesianPosition, cartesian_position );
    createGroundStation( earth, "Station2", testSphericalPosition, spherical_position );
    createGroundStation( earth, "Station3", testGeodeticPosition, geodetic_position );

    std::shared_ptr< GroundStationState > station1 = earth->getGroundStation( "Station1" )->getNominalStationState( );
    std::shared_ptr< GroundStationState > station2 = earth->getGroundStation( "Station2" )->getNominalStationState( );
    std::shared_ptr< GroundStationState > station3 = earth->getGroundStation( "Station3" )->getNominalStationState( );

    // Check if ground station representations are correctly converted
    Eigen::Vector3d position1, position2;

    {
        position1 = station1->getNominalCartesianPosition( );
        position2 = station2->getNominalCartesianPosition( );

        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL( position1( i ) - position2( i ), 1.0E-3 );
        }

        position1 = station1->getNominalCartesianPosition( );
        position2 = station3->getNominalCartesianPosition( );

        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL( position1( i ) - position2( i ), 1.0E-3 );
        }

        position1 = station2->getNominalCartesianPosition( );
        position2 = station3->getNominalCartesianPosition( );

        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL( position1( i ) - position2( i ), 1.0E-3 );
        }
    }

    {
        position1 = station1->getNominalGeodeticPosition( );
        position2 = station2->getNominalGeodeticPosition( );

        BOOST_CHECK_SMALL( position1( 0 ) - position2( 0 ), 1.0E-3 );
        BOOST_CHECK_SMALL( position1( 1 ) - position2( 1 ), 1.0E-3 / equatorialRadius );
        BOOST_CHECK_SMALL( position1( 2 ) - position2( 2 ), 1.0E-3 / equatorialRadius );

        position1 = station1->getNominalGeodeticPosition( );
        position2 = station3->getNominalGeodeticPosition( );

        BOOST_CHECK_SMALL( position1( 0 ) - position2( 0 ), 1.0E-3 );
        BOOST_CHECK_SMALL( position1( 1 ) - position2( 1 ), 1.0E-3 / equatorialRadius );
        BOOST_CHECK_SMALL( position1( 2 ) - position2( 2 ), 1.0E-3 / equatorialRadius );

        position1 = station2->getNominalGeodeticPosition( );
        position2 = station3->getNominalGeodeticPosition( );

        BOOST_CHECK_SMALL( position1( 0 ) - position2( 0 ), 1.0E-3 );
        BOOST_CHECK_SMALL( position1( 1 ) - position2( 1 ), 1.0E-3 / equatorialRadius );
        BOOST_CHECK_SMALL( position1( 2 ) - position2( 2 ), 1.0E-3 / equatorialRadius );
    }

    {
        position1 = station1->getNominalSphericalPosition( );
        position2 = station2->getNominalSphericalPosition( );

        BOOST_CHECK_SMALL( position1( 0 ) - position2( 0 ), 1.0E-3 );
        BOOST_CHECK_SMALL( position1( 1 ) - position2( 1 ), 1.0E-3 / equatorialRadius );
        BOOST_CHECK_SMALL( position1( 2 ) - position2( 2 ), 1.0E-3 / equatorialRadius );

        position1 = station1->getNominalSphericalPosition( );
        position2 = station3->getNominalSphericalPosition( );

        BOOST_CHECK_SMALL( position1( 0 ) - position2( 0 ), 1.0E-3 );
        BOOST_CHECK_SMALL( position1( 1 ) - position2( 1 ), 1.0E-3 / equatorialRadius );
        BOOST_CHECK_SMALL( position1( 2 ) - position2( 2 ), 1.0E-3 / equatorialRadius );

        position1 = station2->getNominalSphericalPosition( );
        position2 = station3->getNominalSphericalPosition( );

        BOOST_CHECK_SMALL( position1( 0 ) - position2( 0 ), 1.0E-3 );
        BOOST_CHECK_SMALL( position1( 1 ) - position2( 1 ), 1.0E-3 / equatorialRadius );
        BOOST_CHECK_SMALL( position1( 2 ) - position2( 2 ), 1.0E-3 / equatorialRadius );
    }
}

//! Test if global state function for ground station is correctly created.
BOOST_AUTO_TEST_CASE( test_GroundStationGlobalState )
{
    // Load Spice kernels
    spice_interface::loadStandardSpiceKernels( );

    // Create Earth object
    std::shared_ptr< Body > earth = std::make_shared< Body >( );
    NamedBodyMap bodyMap;
    bodyMap[ "Earth" ] = earth;

    // Central body characteristics (WGS84 Earth ellipsoid).
    const double flattening = 1.0 / 298.257223563;
    const double equatorialRadius = 6378137.0;

    std::shared_ptr< basic_astrodynamics::OblateSpheroidBodyShapeModel > oblateSpheroidModel =
            std::make_shared< basic_astrodynamics::OblateSpheroidBodyShapeModel >(
                equatorialRadius, flattening );

    earth->setShapeModel( oblateSpheroidModel );
    earth->setEphemeris( std::make_shared< ephemerides::SpiceEphemeris >(
                             "Earth", "SSB", false, true, true, "ECLIPJ2000" ) );
    earth->setRotationalEphemeris( std::make_shared< ephemerides::SpiceRotationalEphemeris >(
                                       "ECLIPJ2000", "IAU_Earth" ) );

    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Define ground station state
    const Eigen::Vector3d groundStationPosition( 1917032.190, 6029782.349, -801376.113 );
    Eigen::Vector6d groundStationState;
    groundStationState << groundStationPosition, 0.0, 0.0, 0.0;

    // Create ground station
    createGroundStation( earth, "Station1", groundStationPosition, cartesian_position );

    // Make state function of ground station w.r.t. SSB in inertial frame
    std::function< Eigen::Matrix< double, 6, 1 >( const double ) > stateFunction =
            observation_models::getLinkEndCompleteEphemerisFunction(
                std::make_pair( "Earth", "Station1" ), bodyMap );

    // Compare state function with manual computation.
    Eigen::Vector6d currentGlobalState, currentGlobalStateFromFunction;
    for( double testTime = 1.0E7; testTime < 5.0E7; testTime += 2.5E6 )
    {
        currentGlobalState = earth->getEphemeris( )->getCartesianState( testTime ) +
                ephemerides::transformStateToGlobalFrame( groundStationState, testTime, earth->getRotationalEphemeris( ) );
        currentGlobalStateFromFunction = stateFunction( testTime );
        for( unsigned int i = 0; i < 6; i++ )
        {
            BOOST_CHECK_EQUAL( currentGlobalState( i ), currentGlobalStateFromFunction( i ) );
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


//
// Copyright (c) 2017 CNRS
//
// This file is part of tsid
// tsid 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.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include <pinocchio/algorithm/joint-configuration.hpp>

#include <tsid/contacts/contact-6d.hpp>
#include <tsid/robots/robot-wrapper.hpp>

using namespace tsid;
using namespace trajectories;
using namespace math;
using namespace contacts;
using namespace tsid::robots;
using namespace std;

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

#define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(isFinite(A), #A<<": "<<A)

const string romeo_model_path = TSID_SOURCE_DIR"/models/romeo";

BOOST_AUTO_TEST_CASE ( test_contact_6d )
{
  const double lx = 0.07;
  const double ly = 0.12;
  const double lz = 0.105;
  const double mu = 0.3;
  const double fMin = 10.0;
  const double fMax = 1000.0;
  const std::string frameName = "r_sole_joint";

  vector<string> package_dirs;
  package_dirs.push_back(romeo_model_path);
  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
  RobotWrapper robot(urdfFileName,
                     package_dirs,
                     pinocchio::JointModelFreeFlyer(),
                     false);

  BOOST_REQUIRE(robot.model().existFrame(frameName));

  Vector3 contactNormal = Vector3::UnitZ();
  Matrix3x contactPoints(3,4);
  contactPoints << -lx, -lx, +lx, +lx,
                   -ly, +ly, -ly, +ly,
                    lz,  lz,  lz,  lz;
  Contact6d contact("contact6d", robot, frameName,
                    contactPoints, contactNormal,
                    mu, fMin, fMax);

  BOOST_CHECK(contact.n_motion()==6);
  BOOST_CHECK(contact.n_force()==12);

  Vector Kp = Vector::Ones(6);
  Vector Kd = 2*Vector::Ones(6);
  contact.Kp(Kp);
  contact.Kd(Kd);
  BOOST_CHECK(contact.Kp().isApprox(Kp));
  BOOST_CHECK(contact.Kd().isApprox(Kd));

  Vector q = neutral(robot.model());
  Vector v = Vector::Zero(robot.nv());
  pinocchio::Data data(robot.model());
  robot.computeAllTerms(data, q, v);

  pinocchio::SE3 H_ref = robot.position(data, robot.model().getJointId(frameName));
  contact.setReference(H_ref);

  double t = 0.0;
  contact.computeMotionTask(t, q, v, data);

  const ConstraintInequality & forceIneq = contact.computeForceTask(t, q, v, data);
  Vector3 f3;
  Vector f(12);
  f3 << 0.0, 0.0, 100.0;
  f << f3, f3, f3, f3;
  BOOST_CHECK(forceIneq.checkConstraint(f));
  BOOST_CHECK(((forceIneq.matrix()*f).array() <= forceIneq.upperBound().array()).all());
  BOOST_CHECK(((forceIneq.matrix()*f).array() >= forceIneq.lowerBound().array()).all());
  f(0) = f(2)*mu*1.1;
  BOOST_CHECK(forceIneq.checkConstraint(f)==false);

  const Matrix & forceGenMat = contact.getForceGeneratorMatrix();
  BOOST_CHECK(forceGenMat.rows()==6 && forceGenMat.cols()==12);

  contact.computeForceRegularizationTask(t, q, v, data);
}

BOOST_AUTO_TEST_SUITE_END ()


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

// Copyright (c) 2007-2014 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014, Oracle and/or its affiliates.

// 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)

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_RELATE_LESS_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_RELATE_LESS_HPP

#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/math.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace detail_dispatch { namespace relate {

// TODO: Integrate it with geometry::less?

template <typename Point1,
          typename Point2,
          std::size_t I = 0,
          std::size_t D = geometry::dimension<Point1>::value>
struct less
{
    static inline bool apply(Point1 const& left, Point2 const& right)
    {
        typename geometry::coordinate_type<Point1>::type
            cleft = geometry::get<I>(left);
        typename geometry::coordinate_type<Point2>::type
            cright = geometry::get<I>(right);

        if ( geometry::math::equals(cleft, cright) )
        {
            return less<Point1, Point2, I + 1, D>::apply(left, right);
        }
        else
        {
            return cleft < cright;
        }
    }
};

template <typename Point1, typename Point2, std::size_t D>
struct less<Point1, Point2, D, D>
{
    static inline bool apply(Point1 const&, Point2 const&)
    {
        return false;
    }
};

}} // namespace detail_dispatch::relate

#endif

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace relate {

struct less
{
    template <typename Point1, typename Point2>
    inline bool operator()(Point1 const& point1, Point2 const& point2) const
    {
        return detail_dispatch::relate::less<Point1, Point2>::apply(point1, point2);
    }
};

}} // namespace detail::relate
#endif // DOXYGEN_NO_DETAIL

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_RELATE_LESS_HPP


#ifdef OpenMVS

#include "IO/mvIO.h"
#include <base/cgal_typedefs.h>
// fix with PI from here: https://github.com/cdcseacave/openMVS/issues/643
// otherwise nameclash with some CGAL headers
#pragma push_macro("PI")
#undef PI
#include "MVS.h"
#pragma pop_macro("PI")
#include <boost/filesystem.hpp>

using namespace MVS;
using namespace std;

int loadOMVSScene(dirHolder dir, dataHolder& data){

    auto start = std::chrono::high_resolution_clock::now();
    cout << "\nRead dense point cloud..." << endl;
    cout << "\t-from " << dir.path+"openMVS/densify_file.mvs" << endl;

    if ( !boost::filesystem::exists(dir.path+"openMVS/densify_file.mvs")){
        cout << "\nERROR: densify_file.mvs does not exist in the given path!" << std::endl;
        return 1;
    }

    Scene scene(0);
    scene.Load(dir.path+"openMVS/densify_file.mvs");

    // make a sensor map (for debbuging)
    FOREACH(k, scene.images){
        Image& imageData = scene.images[k];
        // skip invalid, uncalibrated or discarded images
        if (!imageData.IsValid())
            continue;
        // reset image resolution
//        imageData.ReloadImage(0, false);
        if(!imageData.ReloadImage(0, false)){
            #ifdef RECMESH_USE_OPENMP
            bAbort = true;
            #pragma omp flush (bAbort)
            continue;
//            #else
//            return EXIT_FAILURE;
            #endif
        }
        imageData.UpdateCamera(scene.platforms);
        // select neighbor views
        if (imageData.neighbors.IsEmpty()) {
            IndexArr points;
            scene.SelectNeighborViews(k, points);
        }
        Camera& camera = imageData.camera;
        data.sensor_map[k] = Point(camera.C.x,camera.C.y,camera.C.z);
    };

    // fetch points
    FOREACH(i, scene.pointcloud.points){
        // first check if the point has valid images attached
        const PointCloud::ViewArr& views = scene.pointcloud.pointViews[i];
        if(views.IsEmpty())
            continue;

        // read points
        const PointCloud::Point& P(scene.pointcloud.points[i]);
        Point current_point(P.x, P.y, P.z);
        data.points.push_back(current_point);

        vertex_info vinf;
        // read normal
        if(scene.pointcloud.normals.size()>0){
            const PointCloud::Normal& N(scene.pointcloud.normals[i]);
            Vector current_normal(N.x, N.y, N.z);
            vinf.normal = current_normal;
            data.has_normal = 1;
        }

        // read color
        const PointCloud::Color& C(scene.pointcloud.colors[i]);
        vinf.color = {C.r,C.g,C.b};

        // read views
        FOREACH(j, views) {
            const PointCloud::View viewID(views[j]);
            const Image& imageData = scene.images[viewID];
            if(!imageData.IsValid())
                continue;
            const Camera& camera = imageData.camera;
            vinf.sensor_positions.push_back(Point(camera.C.x, camera.C.y, camera.C.z));
        }
        // if no sensor is associated with point, delete point and go to next one (without adding the point info of course)
        if(vinf.sensor_positions.size() < 1){
            data.points.pop_back();
            continue;
        }
        vinf.sensor_vec = Vector(vinf.sensor_positions[0]-current_point);
        vinf.global_idx=i;
        data.infos.push_back(vinf);
    }

    // set has color and has normal
    data.has_color = 1;
    data.has_sensor = 1;

    auto stop = chrono::high_resolution_clock::now();
    auto duration = chrono::duration_cast<chrono::seconds>(stop - start);
    cout << "\t-" << data.points.size() << " points read" << endl;
    cout << "\t-in " << duration.count() << "s" << endl;

    return 0;
}

int omvsCleanMesh(dirHolder& dir, dataHolder& data){

    auto start = std::chrono::high_resolution_clock::now();
    cout << "\nOMVS clean mesh..." << endl;
    cout << "\t-from densify file " << dir.path+"openMVS/densify_file.mvs" << endl;
    cout << "\t-from mesh " << dir.path+dir.write_file+dir.suffix+".ply" << endl;

    Scene scene(0);
    scene.Load(dir.path+"openMVS/densify_file.mvs");

    if(!scene.mesh.Load(dir.path+dir.write_file+dir.suffix+".ply")){
            cout << "ERROR in loading the mesh file" << endl;
            return 1;
    };

    int nItersFixNonManifold = 4; // also hardcoded to 4 in omvs code
    for (unsigned i=0; i<nItersFixNonManifold; ++i)
        if (!scene.mesh.FixNonManifold())
            break;

//    dir.suffix = "_nm";
//    scene.mesh.Save(dir.path+dir.write_file+dir.suffix+".ply");

    // clean the mesh
    scene.mesh.Clean(1.f, 20.f, true, 30.f, 2.f, false);
    scene.mesh.Clean(1.f, 0.f, true, 30.f, 0, false); // extra cleaning trying to close more holes
    scene.mesh.Clean(1.f, 0.f, false, 0, 0, true); // extra cleaning to remove non-manifold problems created by closing holes

    dir.suffix = "_cleaned";
    scene.mesh.Save(dir.path+dir.write_file+dir.suffix+".ply");

    auto stop = chrono::high_resolution_clock::now();
    auto duration = chrono::duration_cast<chrono::seconds>(stop - start);
    cout << "\t-saved to " << dir.path+dir.write_file+dir.suffix+".ply" << endl;
    cout << "\t-done after " << duration.count() << "s" << endl;

    return 0;
}

#endif


/*
 * csll is for serial port (RS232)
 */
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "MotorControl.h"
#include <boost/program_options.hpp>
#include <stdio.h>
#include <termios.h>  // for tcxxxattr, ECHO, etc ..
#include <unistd.h>    // for STDIN_FILENO
#include "std_msgs/Int32.h"

//#define Debug
//#include <conio.h>

using std::string;

//-------variable-------//
const char *motion_topic_name = "/cmd_vel";

//const double mAngle1Cos(cos(5*M_PI/3));
//const double mAngle2Cos(cos(M_PI/3));
//const double mAngle3Cos(cos(M_PI));

//const double mAngle1Sin(sin(5*M_PI/3));
//const double mAngle2Sin(sin(M_PI/3));
//const double mAngle3Sin(sin(M_PI));
const double mAngle1Cos(cos(5*M_PI/6));
const double mAngle2Cos(cos(M_PI/6));
const double mAngle3Cos(cos(3*M_PI/2));

const double mAngle1Sin(sin(M_PI/6));
const double mAngle2Sin(sin(5*M_PI/6));
const double mAngle3Sin(sin(3*M_PI/2));
const double rad2rpm =  60.0  / (2.0*M_PI);
const double percentRange =  PWM_Limit_Percent_Max - PWM_Limit_Percent_Min;

double w1,w2,w3;
double en1,en2,en3;

#define ESCAPE 27

/*==============================================================================*/
//Initialize
/*==============================================================================*/
void Initialize()
{
    w1=0;
    w2=0;
    w3=0;
}
/*==============================================================================*/
//Topic call back
/*==============================================================================*/
void motionCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
    w1 = mAngle1Cos*msg->linear.y + mAngle1Sin*msg->linear.x + ROBOT_RADIUS*msg->angular.z*(-1);
    w2 = mAngle2Cos*msg->linear.y + mAngle2Sin*msg->linear.x + ROBOT_RADIUS*msg->angular.z*(-1);
    w3 = mAngle3Cos*msg->linear.y + mAngle3Sin*msg->linear.x + ROBOT_RADIUS*msg->angular.z*(-1);

    en1 = 1; en2 = 1; en3 = 1;
//    printf("=====================================================\n");
    if(w1 != 0 || w2!=0 || w3!=0){
//	printf("-->w1:%0.2f w2:%0.2f w3:%0.2f en1:%f en2:%f en3:%f<--\n\n",w1,w2,w3,en1,en2,en3);
    }

//    ROS_INFO("<=============Kinematics=================>");
//    ROS_INFO("w1:%f\tw2:%f\tw3:%f\n",w1,w2,w3);
//    ROS_INFO("<========================================>");
    //speed2pwm(en);
    //ROS_INFO("w1:%f\tw2:%f\tw3:%f\n",w1,w2,w3);
    //ROS_INFO("*******************************");

    mcssl_send2motor(w1,w2,w3,en1,en2,en3,0);
}

void shootCallback(const std_msgs::Int32::ConstPtr& msg)
{
    int shoot_power = msg->data;
    char shoot=(char)shoot_power;
    mcssl_send2motor(w1,w2,w3,en1,en2,en3,shoot);
}

/*==============================================================================*/
//Main
/*==============================================================================*/
int main(int argc, char **argv)
{
    //Initial
    ros::init(argc, argv, "motion");
    ros::NodeHandle n("~");

//    std::string port_name;
//    n.param<std::string>("port", port_name, "/dev/ttyUSB3");

//    ROS_INFO("port3=%s",port_name.c_str());

    //if(mcssl_init()<=0){return 0;}
//    ROS_INFO("Initialize Motion with port=%s...",port_name.c_str());
    char *command;
    //scanf("%s", command);

    Initialize();

    geometry_msgs::Twist feedback_msg;
    //motion subscriber
    ros::Subscriber motion_sub = n.subscribe(motion_topic_name, 1, motionCallback);
    ros::Subscriber shoot_sub  = n.subscribe("/shoot",1,shootCallback);
    ros::Publisher feedback_pub  = n.advertise<geometry_msgs::Twist>("/motorFB",0);
#ifdef Debug
    ROS_INFO("Debug mode (motion)\n");
#else
    do{

        if(mcssl_init(/*port_name.c_str()*/) > 0){
            break;
        }else{
          usleep(1000000);//1s = 1,000,000 us
        }

    }while(ros::ok());
#endif
    ROS_INFO("Motion is running\n");
    ros::Rate loop_rate(30);
//    static double time = 0;
    while(ros::ok())
    {
//	mcssl_send2motor(w1,w2,w3);
#ifdef Debug

#else
        feedback_msg = getmotionfeedback();
        feedback_pub.publish(feedback_msg);
#endif

//        double n_time,duration;
//        n_time = ros::Time::now().toSec();
//        duration = n_time - time;
//        time = n_time;
//        printf("\nduration = %f ",duration);
        //spin
        ros::spinOnce();
        loop_rate.sleep();
    }
    //RS232 finish
    mcssl_finish();

    return 0;
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/refine_rsqrt.hpp>

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

   BOOST_DISPATCH_OVERLOAD( rsqrt_
                          , (typename A0)
                          , bs::vmx_
                          , bs::pack_< bd::single_<A0>, bs::vmx_>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(const A0& a00) const BOOST_NOEXCEPT
      {
        A0 a0 = refine_rsqrt(a00, raw_(rsqrt)(a00));
        #ifndef BOOST_SIMD_NO_INFINITIES
        a0 = if_zero_else(a00 == Inf<A0>(),a0);
        #endif
        return if_else(is_eqz(a00), Inf<A0>(), a0);
      }
   };

   BOOST_DISPATCH_OVERLOAD( rsqrt_
                          , (typename A0)
                          , bs::vmx_
                          , bs::raw_tag
                          , bs::pack_< bd::single_<A0>, bs::vmx_>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(bs::raw_tag const&, const A0& a0) const BOOST_NOEXCEPT
      {
        return vec_rsqrte( a0.storage() );
      }
   };
} } }

#endif


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

// Copyright (c) 2007-2012 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)

#ifndef BOOST_GEOMETRY_TEST_LENGTH_HPP
#define BOOST_GEOMETRY_TEST_LENGTH_HPP

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/io/wkt/read.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/variant/variant.hpp>

template <typename Geometry>
void test_length(Geometry const& geometry, long double expected_length)
{
    typename bg::default_length_result<Geometry>::type
        length = bg::length(geometry);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::ostringstream out;
    out << typeid(typename bg::coordinate_type<Geometry>::type).name()
        << std::endl
        << typeid(typename bg::default_length_result<Geometry>::type).name()
        << std::endl
        << "length : " << bg::length(geometry)
        << std::endl;
    std::cout << out.str();
#endif

    BOOST_CHECK_CLOSE(length, expected_length, 0.0001);
}

template <typename Geometry>
void test_geometry(std::string const& wkt, double expected_length)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);
    test_length(geometry, expected_length);
#if !defined(BOOST_GEOMETRY_TEST_DEBUG)
    test_length(boost::variant<Geometry>(geometry), expected_length);
#endif
}

template <typename Geometry>
void test_empty_input(Geometry const& geometry)
{
    try
    {
        bg::length(geometry);
    }
    catch(bg::empty_input_exception const& )
    {
        return;
    }
    BOOST_CHECK_MESSAGE(false, "A empty_input_exception should have been thrown" );
}

#endif


/*=========================================================================
 *
 *  Copyright Leiden University Medical Center, Erasmus University Medical
 *  Center and contributors
 *
 *  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.txt
 *
 *  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.
 *
 *=========================================================================*/

#include "selxItkGradientDescentOptimizerv4Component.h"
#include <boost/lexical_cast.hpp>
#include "selxPodString.h"

namespace selx
{
template< class InternalComputationValueType >
ItkGradientDescentOptimizerv4Component< InternalComputationValueType >::ItkGradientDescentOptimizerv4Component( const std::string & name,
  LoggerImpl & logger ) :
  Superclass( name, logger )
{
  m_Optimizer = GradientDescentOptimizerv4Type::New();
  m_Optimizer->SetNumberOfIterations( 100 );
  m_Optimizer->SetLearningRate( 1.0 );

  //TODO: instantiating the filter in the constructor might be heavy for the use in component selector factory, since all components of the database are created during the selection process.
  // we could choose to keep the component light weighted (for checking criteria such as names and connections) until the settings are passed to the filter, but this requires an additional initialization step.
}

template< class InternalComputationValueType >
ItkGradientDescentOptimizerv4Component< InternalComputationValueType >::~ItkGradientDescentOptimizerv4Component()
{
}

template< class InternalComputationValueType >
typename ItkGradientDescentOptimizerv4Component< InternalComputationValueType >::Optimizerv4Pointer
ItkGradientDescentOptimizerv4Component< InternalComputationValueType >::GetItkOptimizerv4()
{
  return (Optimizerv4Pointer)this->m_Optimizer;
}

template< class InternalComputationValueType >
bool
ItkGradientDescentOptimizerv4Component< InternalComputationValueType >
::MeetsCriterion( const ComponentBase::CriterionType & criterion )
{
  bool hasUndefinedCriteria( false );
  bool meetsCriteria( false );
  auto status = CheckTemplateProperties( this->TemplateProperties(), criterion );
  if( status == CriterionStatus::Satisfied )
  {
    return true;
  }
  else if( status == CriterionStatus::Failed )
  {
    return false;
  } // else: CriterionStatus::Unknown

  if( criterion.first == "NumberOfIterations" ) //Supports this?
  {
    if( criterion.second.size() != 1 )
    {
      meetsCriteria = false;
      //itkExceptionMacro("The criterion Sigma may have only 1 value");
    }
    else
    {
      auto const & criterionValue = *criterion.second.begin();
      try
      {
        this->m_Optimizer->SetNumberOfIterations( std::stoi( criterionValue ) );
        meetsCriteria = true;
      }
      catch( itk::ExceptionObject & err )
      {
        //TODO log the error message?
        meetsCriteria = false;
      }
    }
  }
  else if( criterion.first == "LearningRate" ) //Supports this?
  {
    if( criterion.second.size() != 1 )
    {
      meetsCriteria = false;
      //itkExceptionMacro("The criterion Sigma may have only 1 value");
    }
    else
    {
      auto const & criterionValue = *criterion.second.begin();
      try
      {
        this->m_Optimizer->SetLearningRate( boost::lexical_cast< InternalComputationValueType >( criterionValue ) );
        //this->m_Optimizer->SetLearningRate(std::stod(criterionValue));
        meetsCriteria = true;
      }
      catch( itk::ExceptionObject & err ) // TODO: should catch(const bad_lexical_cast &) too
      {
        //TODO log the error message?
        meetsCriteria = false;
      }
    }
  }
  return meetsCriteria;
}
} //end namespace selx


/**
 *  @file

 *  @copyright defined in eos/LICENSE

 */
#include <eosio/chain/asset.hpp>
#include <boost/rational.hpp>
#include <fc/reflect/variant.hpp>

namespace eosio { namespace chain {

uint8_t asset::decimals()const {
   return sym.decimals();
}

string asset::symbol_name()const {
   return sym.name();
}

int64_t asset::precision()const {
   return sym.precision();
}

string asset::to_string()const {
   string sign = amount < 0 ? "-" : "";
   int64_t abs_amount = std::abs(amount);
   string result = fc::to_string( static_cast<int64_t>(abs_amount) / precision());
   if( decimals() )
   {
      auto fract = static_cast<int64_t>(abs_amount) % precision();
      result += "." + fc::to_string(precision() + fract).erase(0,1);
   }
   return sign + result + " " + symbol_name();
}

asset asset::from_string(const string& from)
{
   try {
      string s = fc::trim(from);

      // Find space in order to split amount and symbol
      auto space_pos = s.find(' ');
      EOS_ASSERT((space_pos != string::npos), asset_type_exception, "Asset's amount and symbol should be separated with space");
      auto symbol_str = fc::trim(s.substr(space_pos + 1));
      auto amount_str = s.substr(0, space_pos);

      // Ensure that if decimal point is used (.), decimal fraction is specified
      auto dot_pos = amount_str.find('.');
      if (dot_pos != string::npos) {
         EOS_ASSERT((dot_pos != amount_str.size() - 1), asset_type_exception, "Missing decimal fraction after decimal point");
      }

      // Parse symbol
      string precision_digit_str;
      if (dot_pos != string::npos) {
         precision_digit_str = eosio::chain::to_string(amount_str.size() - dot_pos - 1);
      } else {
         precision_digit_str = "0";
      }

      string symbol_part = precision_digit_str + ',' + symbol_str;
      symbol sym = symbol::from_string(symbol_part);

      // Parse amount
      safe<int64_t> int_part, fract_part;
      if (dot_pos != string::npos) {
         int_part = fc::to_int64(amount_str.substr(0, dot_pos));
         fract_part = fc::to_int64(amount_str.substr(dot_pos + 1));
         if (amount_str[0] == '-') fract_part *= -1;
      } else {
         int_part = fc::to_int64(amount_str);
      }

      safe<int64_t> amount = int_part;
      amount *= safe<int64_t>(sym.precision());
      amount += fract_part;

      return asset(amount.value, sym);
   }
   FC_CAPTURE_LOG_AND_RETHROW( (from) )
}

} }  // eosio::types


#define BOOST_TEST_MODULE max_flow_algorithms_named_parameters_and_bundled_params_test

#include <boost/graph/adjacency_list.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>

#include "min_cost_max_flow_utils.hpp"

typedef boost::adjacency_list_traits<boost::vecS,boost::vecS,boost::directedS> traits;
struct edge_t {
  double capacity;
  float cost;
  float residual_capacity;
  traits::edge_descriptor reversed_edge;
};
struct node_t {
  traits::edge_descriptor predecessor;
  int dist;
  int dist_prev;
  boost::vertex_index_t id;
  boost::default_color_type color;
};
typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS, node_t, edge_t > Graph;

BOOST_AUTO_TEST_CASE(using_named_parameters_and_bundled_params_on_edmonds_karp_max_flow_test)
{
  Graph g;
  traits::vertex_descriptor s,t;

  boost::property_map<Graph,double edge_t::* >::type capacity = get(&edge_t::capacity, g);
  boost::property_map<Graph,float edge_t::* >::type cost = get(&edge_t::cost, g);
  boost::property_map<Graph,float edge_t::* >::type residual_capacity = get(&edge_t::residual_capacity, g);
  boost::property_map<Graph,traits::edge_descriptor edge_t::* >::type rev = get(&edge_t::reversed_edge, g);
  boost::property_map<Graph,traits::edge_descriptor node_t::* >::type pred = get(&node_t::predecessor, g);
  boost::property_map<Graph,boost::default_color_type node_t::* >::type col = get(&node_t::color, g);

  boost::SampleGraph::getSampleGraph(g,s,t,capacity,residual_capacity,cost,rev);

  // The "named parameter version" (producing errors)
  // I chose to show the error with edmonds_karp_max_flow().
  int flow_value = edmonds_karp_max_flow(g, s, t,
    boost::capacity_map(capacity)
    .residual_capacity_map(residual_capacity)
    .reverse_edge_map(rev)
    .color_map(col)
    .predecessor_map(pred));

  BOOST_CHECK_EQUAL(flow_value,4);
}