/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

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

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <algorithm>
#include <iterator>
#include <boost/cstdint.hpp>
#include <sprig/binary.hpp>
#include <sprig/utility/iterator.hpp>
#include <sprig/uck/traits.hpp>
#include <sprig/uck/types.hpp>
#include <sprig/uck/std/std_types.hpp>
#include <sprig/uck/fun/fun_types.hpp>
#include <sprig/uck/fun/fun_manager.hpp>

namespace sprig {
	namespace uck {
		//
		// fun_arithmetic
		//
		template<typename Traits = traits<> >
		struct fun_arithmetic {
		public:
			typedef Traits traits_type;
			typedef types<traits_type> types_type;
			typedef std_types<traits_type> std_types_type;
			typedef fun_types<traits_type> fun_types_type;
		public:
			static typename fun_types_type::name_type const& name() {
				static typename fun_types_type::name_type const v("arithmetic");
				return v;
			}
		public:
			//
			// plus_int32
			//
			struct plus_int32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("+i4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 2) {
						return;
					}
					boost::int32_t v = get_from_big_endian<boost::int32_t>(arg_first->begin(), arg_first->end());
					for (typename fun_types_type::fun_argument_type i = sprig::get_advance(arg_first, 1), last = arg_last; i != last; ++i) {
						v += get_from_big_endian<boost::int32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::int32_t>(v, res_first, res_last);
				}
			};
			//
			// plus_uint32
			//
			struct plus_uint32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("+u4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 2) {
						return;
					}
					boost::uint32_t v = get_from_big_endian<boost::uint32_t>(arg_first->begin(), arg_first->end());
					for (typename fun_types_type::fun_argument_type i = sprig::get_advance(arg_first, 1), last = arg_last; i != last; ++i) {
						v += get_from_big_endian<boost::uint32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::uint32_t>(v, res_first, res_last);
				}
			};
			//
			// minus_int32
			//
			struct minus_int32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("-i4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 2) {
						return;
					}
					boost::int32_t v = get_from_big_endian<boost::int32_t>(arg_first->begin(), arg_first->end());
					for (typename fun_types_type::fun_argument_type i = sprig::get_advance(arg_first, 1), last = arg_last; i != last; ++i) {
						v -= get_from_big_endian<boost::int32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::int32_t>(v, res_first, res_last);
				}
			};
			//
			// minus_uint32
			//
			struct minus_uint32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("-u4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 2) {
						return;
					}
					boost::uint32_t v = get_from_big_endian<boost::uint32_t>(arg_first->begin(), arg_first->end());
					for (typename fun_types_type::fun_argument_type i = sprig::get_advance(arg_first, 1), last = arg_last; i != last; ++i) {
						v -= get_from_big_endian<boost::uint32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::uint32_t>(v, res_first, res_last);
				}
			};
			//
			// multiplies_int32
			//
			struct multiplies_int32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("*i4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 2) {
						return;
					}
					boost::int32_t v = get_from_big_endian<boost::int32_t>(arg_first->begin(), arg_first->end());
					for (typename fun_types_type::fun_argument_type i = sprig::get_advance(arg_first, 1), last = arg_last; i != last; ++i) {
						v *= get_from_big_endian<boost::int32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::int32_t>(v, res_first, res_last);
				}
			};
			//
			// multiplies_uint32
			//
			struct multiplies_uint32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("*u4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 2) {
						return;
					}
					boost::uint32_t v = get_from_big_endian<boost::uint32_t>(arg_first->begin(), arg_first->end());
					for (typename fun_types_type::fun_argument_type i = sprig::get_advance(arg_first, 1), last = arg_last; i != last; ++i) {
						v *= get_from_big_endian<boost::uint32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::uint32_t>(v, res_first, res_last);
				}
			};
			//
			// devides_int32
			//
			struct devides_int32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("/i4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 2) {
						return;
					}
					boost::int32_t v = get_from_big_endian<boost::int32_t>(arg_first->begin(), arg_first->end());
					for (typename fun_types_type::fun_argument_type i = sprig::get_advance(arg_first, 1), last = arg_last; i != last; ++i) {
						v /= get_from_big_endian<boost::int32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::int32_t>(v, res_first, res_last);
				}
			};
			//
			// devides_uint32
			//
			struct devides_uint32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("/u4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 2) {
						return;
					}
					boost::uint32_t v = get_from_big_endian<boost::uint32_t>(arg_first->begin(), arg_first->end());
					for (typename fun_types_type::fun_argument_type i = sprig::get_advance(arg_first, 1), last = arg_last; i != last; ++i) {
						v /= get_from_big_endian<boost::uint32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::uint32_t>(v, res_first, res_last);
				}
			};
			//
			// modulus_int32
			//
			struct modulus_int32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("%i4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 2) {
						return;
					}
					boost::int32_t v = get_from_big_endian<boost::int32_t>(arg_first->begin(), arg_first->end());
					for (typename fun_types_type::fun_argument_type i = sprig::get_advance(arg_first, 1), last = arg_last; i != last; ++i) {
						v %= get_from_big_endian<boost::int32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::int32_t>(v, res_first, res_last);
				}
			};
			//
			// modulus_uint32
			//
			struct modulus_uint32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("%u4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 2) {
						return;
					}
					boost::uint32_t v = get_from_big_endian<boost::uint32_t>(arg_first->begin(), arg_first->end());
					for (typename fun_types_type::fun_argument_type i = sprig::get_advance(arg_first, 1), last = arg_last; i != last; ++i) {
						v %= get_from_big_endian<boost::uint32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::uint32_t>(v, res_first, res_last);
				}
			};
			//
			// negate_int32
			//
			struct negate_int32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("-@i4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					boost::int32_t v = -get_from_big_endian<boost::int32_t>(res_first, res_last);
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::int32_t>(v, res_first, res_last);
				}
			};
		public:
			static typename fun_types_type::package_type const& package() {
				typedef typename fun_types_type::package_type result_type;
				typedef typename result_type::value_type value_type;
				static result_type v;
				struct initializer {
					int operator()() const {
						v.insert(value_type(plus_int32::name(), plus_int32()));
						v.insert(value_type(plus_uint32::name(), plus_uint32()));
						v.insert(value_type(minus_int32::name(), minus_int32()));
						v.insert(value_type(minus_uint32::name(), minus_uint32()));
						v.insert(value_type(multiplies_int32::name(), multiplies_int32()));
						v.insert(value_type(multiplies_uint32::name(), multiplies_uint32()));
						v.insert(value_type(devides_int32::name(), devides_int32()));
						v.insert(value_type(devides_uint32::name(), devides_uint32()));
						v.insert(value_type(modulus_int32::name(), modulus_int32()));
						v.insert(value_type(modulus_uint32::name(), modulus_uint32()));
						v.insert(value_type(negate_int32::name(), negate_int32()));
						return 0;
					}
				};
				static int const init = initializer()();
				return v;
			}
			static bool attach() {
				return fun_manager<traits_type>::get_mutable_instance().attach(name(), package().begin(), package().end());
			}
			static bool detach() {
				return fun_manager<traits_type>::get_mutable_instance().detach(name());
			}
		};
	}	// namespace uck
}	// namespace sprig

#endif	// #ifndef SPRIG_UCK_FUN_FUN_ARITHMETIC_HPP


#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>

#include <limits>
#include <Eigen/Dense>
#include <vector>
#include <range/v3/all.hpp>
#include "floatvectormatcher.hpp"

using namespace Eigen;
using namespace ranges;

TEST_CASE("Extract indices rows from a matrix")
{
      MatrixXi m(10, 4);
      m.reshaped() = VectorXi::LinSpaced(40, 1, 40);
      std::vector<size_t> indices{3, 5, 8};
      m = m(indices, all).eval();
      std::cout << m << std::endl;
}
TEST_CASE("Enumerate and find")
{
      std::vector<std::string> v{"blah", "blih", "blouh", "machin", "truc"};
      auto venum = v | views::enumerate | to<std::vector>();
      auto i = find_if(venum, [](auto &s) { return s.second == "machin"; });
      CHECK(i->first == 3);
}

TEST_CASE("Shuffle and split")
{
      std::default_random_engine gen;
      size_t n = 10;
      size_t s = 3;
      auto tosplit = views::ints(static_cast<size_t>(0), n) | to_vector | actions::shuffle(gen);
      auto a = tosplit | views::take(s);
      auto b = tosplit | views::slice(s, n);
      CHECK(distance(a) == 3);
      CHECK(distance(b) == 7);
}

TEST_CASE("Eigen with ranges")
{
      MatrixXi m1(2, 3);

      m1 << 1, std::numeric_limits<int>::quiet_NaN(), 3,
          4, 5, 6;

      //    const auto& expected = Catch::Range::EqualsMatcher<std::vector<int>,std::vector<int>>(std::vector{1,2,3});
      auto row0 = m1.row(0);
      std::vector<int> vect_expected{1, std::numeric_limits<int>::quiet_NaN(), 3};
      const auto &expected = Catch::Matchers::EqualsRng<std::vector<int>>(row0);
      CHECK_THAT(vect_expected, expected);

      MatrixXd m2(2, 3);
      // m2 << 1.0, std::numeric_limits<double>::quiet_NaN(), 3.0,
      m2 << 1.0, 2.0, 3.0,
          4.0, 5.0, 6.0;

      auto row1 = m2.row(0);
      std::vector<double> vect_expected2{1.0, 2.0, 3.0};
      const auto &expected2 = Catch::Matchers::ApproxRng<std::vector<double>>(row1);
      CHECK_THAT(vect_expected2, expected2);

      MatrixXd m3(2, 3);
      m3 << 1.0, std::numeric_limits<double>::quiet_NaN(), 3.0,
          4.0, 5.0, 6.0;

      auto row2 = m3.row(0);
      std::vector<double> vect_expected3{1.0, std::numeric_limits<double>::quiet_NaN(), 3.0};
      const auto &expected3 = Catch::Matchers::ApproxRng<std::vector<double>>(row2);
      CHECK_THAT(vect_expected3, expected3);

      MatrixXd m4(2, 3);
      m3 << 1.0, 2.001, 3.0,
          4.0, 5.0, 6.0;

      auto row3 = m3.row(0);
      const auto &expected4 = Catch::Matchers::ApproxRng<std::vector<double>>(row3);
      CHECK_THAT(vect_expected2, !expected4);

      RowVectorXd vec1(3);
      vec1 << 1, 2, 3;
      VectorXd res(10);
      res << 1, 2, 3 ,4 ,5, 7, 9, 10, 12, 15;

 // https://stackoverflow.com/questions/36820639/how-do-i-write-a-range-pipeline-that-uses-temporary-containers
    auto restmp = res(seq(3,7)).array();
      auto diff = restmp
            | views::sliding(2)
            | views::transform([](const auto& l) -> double {
                    return (2.0 * std::abs(l[1]-l[0])/(l[1]+l[0]) ) ; })
            | to<std::vector>();

}


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#pragma once

#include <string>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/inversive_congruential.hpp>

#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

#include "plog/Severity.h"

/**

BRng
======

* http://www.boost.org/doc/libs/1_65_1/doc/html/boost_random/tutorial.html

* https://stackoverflow.com/questions/4778797/setting-seed-boostrandom

**/

#include "BRAP_API_EXPORT.hh"

class BRAP_API BRng
{
    static const plog::Severity LEVEL ;

    typedef boost::mt19937          RNG_t;
    //typedef boost::hellekalek1995   RNG_t ;
    typedef boost::uniform_real<>   DST_t;
    typedef boost::variate_generator< RNG_t, DST_t > GEN_t ;

    public:
        BRng(float lo=0.f, float hi=1.f, unsigned seed=42, const char* label="A");
        float one();
        void two(float& a, float& b);
        void setSeed(unsigned _seed);
        std::string desc() const ;
        void dump();

        float getLo() const ;
        float getHi() const ;
    private:
        float m_lo ;
        float m_hi ;

        RNG_t* m_rng;
        DST_t* m_dst ;
        GEN_t* m_gen ;

        unsigned m_seed ;
        const char* m_label ;
        unsigned m_count ;

};


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

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

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

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_GIS_LATLONG_POINT_LL_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GIS_LATLONG_POINT_LL_HPP

#include <cstddef>
#include <sstream>
#include <string>

#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/geometries/point.hpp>

#include <boost/geometry/extensions/gis/latlong/detail/graticule.hpp>

namespace boost { namespace geometry
{

namespace model { namespace ll
{

/*!
    \brief Point using spherical coordinates \a lat and \a lon, on Earth
    \ingroup Geometry
    \details The point_ll class implements a point with lat and lon functions.
    It can be constructed using latitude and longitude classes. The latlong
    class can be defined in degrees or in radians. There is a conversion method
    from degree to radian, and from radian to degree.
    \tparam Units units,defaults to degree
    \tparam CoordinateType coordinate type, double (the default) or float
        (it might be int as well)
    \tparam CoordinateSystem coordinate system, should include NOT degree/radian
        indication, should be e.g. cs::geographic or cs::spherical
    \tparam Dimensions number of dimensions
    \note There is NO constructor with two values to avoid
        exchanging lat and long
    \note Construction with latitude and longitude can be done in both orders,
        so lat/long and long/lat
    \par Example:
    Example showing how the point_ll class can be constructed. Note that it
        can also be constructed using
    decimal degrees (43.123).
    \dontinclude doxygen_1.cpp
    \skip example_point_ll_construct
    \line {
    \until }
*/
template
<
    typename Units = degree,
    typename CoordinateType = double,
    template<typename> class CoordinateSystem = cs::geographic,
    std::size_t Dimensions = 2
>
class point : public model::point
                <
                    CoordinateType,
                    Dimensions,
                    CoordinateSystem<Units>
                >
{
    typedef model::point
        <
            CoordinateType,
            Dimensions,
            CoordinateSystem<Units>
        >
        base_type;
public:

    /// Default constructor, does not initialize anything
    inline point() : base_type() {}

    /// Constructor with longitude/latitude
    inline point(longitude<CoordinateType> const& lo,
                latitude<CoordinateType> const& la)
        : base_type(lo, la) {}

    /// Constructor with latitude/longitude
    inline point(latitude<CoordinateType> const& la,
                longitude<CoordinateType> const& lo)
        : base_type(lo, la) {}

    /// Get longitude
    inline CoordinateType const& lon() const
    { return this->template get<0>(); }

    /// Get latitude
    inline CoordinateType const& lat() const
    { return this->template get<1>(); }

    /// Set longitude
    inline void lon(CoordinateType const& v)
    { this->template set<0>(v); }

    /// Set latitude
    inline void lat(CoordinateType const& v)
    { this->template set<1>(v); }

    /// Set longitude using dms class
    inline void lon(dms<east, CoordinateType> const& v)
    {
        this->template set<0>(v.as_value());
    }
    inline void lon(dms<west, CoordinateType> const& v)
    {
        this->template set<0>(v.as_value());
    }

    inline void lat(dms<north, CoordinateType> const& v)
    {
        this->template set<1>(v.as_value());
    }
    inline void lat(dms<south, CoordinateType> const& v)
    {
        this->template set<1>(v.as_value());
    }
};

}} // namespace model::ll

// Adapt the point_ll to the concept
#ifndef DOXYGEN_NO_TRAITS_SPECIALIZATIONS
namespace traits
{

template
<
    typename Units,
    typename CoordinateType,
    template<typename> class CoordinateSystem,
    std::size_t DimensionCount
>
struct tag
        <
            model::ll::point
            <
                Units,
                CoordinateType,
                CoordinateSystem,
                DimensionCount
            >
        >
{
    typedef point_tag type;
};

template
<
    typename Units,
    typename CoordinateType,
    template<typename> class CoordinateSystem,
    std::size_t DimensionCount
>
struct coordinate_type
        <
            model::ll::point
            <
                Units,
                CoordinateType,
                CoordinateSystem,
                DimensionCount
            >
        >
{
    typedef CoordinateType type;
};

template
<
    typename Units,
    typename CoordinateType,
    template<typename> class CoordinateSystem,
    std::size_t DimensionCount
>
struct coordinate_system
        <
            model::ll::point
            <
                Units,
                CoordinateType,
                CoordinateSystem,
                DimensionCount
            >
        >
{
    typedef CoordinateSystem<Units> type;
};

template
<
    typename Units,
    typename CoordinateType,
    template<typename> class CoordinateSystem,
    std::size_t DimensionCount
>
struct dimension
        <
            model::ll::point
            <
                Units,
                CoordinateType,
                CoordinateSystem,
                DimensionCount
            >
        >
    : boost::mpl::int_<DimensionCount>
{};

template
<
    typename Units,
    typename CoordinateType,
    template<typename> class CoordinateSystem,
    std::size_t DimensionCount,
    std::size_t Dimension
>
struct access
        <
            model::ll::point
            <
                Units,
                CoordinateType,
                CoordinateSystem,
                DimensionCount
            >,
            Dimension
        >
{
    typedef model::ll::point
            <
                Units,
                CoordinateType,
                CoordinateSystem,
                DimensionCount
            > type;

    static inline CoordinateType get(type const& p)
    {
        return p.template get<Dimension>();
    }

    static inline void set(type& p, CoordinateType const& value)
    {
        p.template set<Dimension>(value);
    }
};

} // namespace traits
#endif // DOXYGEN_NO_TRAITS_SPECIALIZATIONS

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GIS_LATLONG_POINT_LL_HPP


// Copyright (c) 2018-2019 FRC Team 3512. All Rights Reserved.

#pragma once

#include <Eigen/Core>
#include <frc/controller/StateSpaceController.h>
#include <frc/controller/StateSpaceObserver.h>
#include <frc/controller/StateSpacePlant.h>

#include "Constants.hpp"
#include "control/ElevatorClimbCoeffs.hpp"
#include "control/ElevatorCoeffs.hpp"
#include "control/TrapezoidalMotionProfile.hpp"
#include "logging/CsvLogger.hpp"

namespace frc3512 {

class ElevatorController {
public:
    // State tolerances in meters and meters/sec respectively.
    static constexpr double kPositionTolerance = 0.05;
    static constexpr double kVelocityTolerance = 2.0;

    ElevatorController();

    ElevatorController(const ElevatorController&) = delete;
    ElevatorController& operator=(const ElevatorController&) = delete;

    void Enable();
    void Disable();

    void SetScoringIndex();
    void SetClimbingIndex();

    void SetGoal(double goal);

    /**
     * Sets the references.
     *
     * @param position Position of the carriage in meters.
     * @param velocity Velocity of the carriage in meters per second.
     */
    void SetReferences(units::meter_t position,
                       units::meters_per_second_t velocity);

    bool AtReferences() const;

    bool AtGoal() const;

    /**
     * Sets the current encoder measurement.
     *
     * @param measuredPosition Position of the carriage in meters.
     */
    void SetMeasuredPosition(double measuredPosition);

    /**
     * Returns the control loop calculated voltage.
     */
    double ControllerVoltage() const;

    /**
     * Returns the estimated position.
     */
    double EstimatedPosition() const;

    /**
     * Returns the estimated velocity.
     */
    double EstimatedVelocity() const;

    /**
     * Returns the error between the position reference and the position
     * estimate.
     */
    double PositionError() const;

    /**
     * Returns the error between the velocity reference and the velocity
     * estimate.
     */
    double VelocityError() const;

    /**
     * Returns the current reference set by the profile
     */
    double PositionReference();

    /**
     * Executes the control loop for a cycle.
     */
    void Update();

    /**
     * Resets any internal state.
     */
    void Reset();

    void SetClimbingProfile();

    void SetScoringProfile();

private:
    // The current sensor measurement.
    Eigen::Matrix<double, 1, 1> m_Y;
    TrapezoidalMotionProfile::State m_goal;

    TrapezoidalMotionProfile::Constraints scoringConstraints{
        Constants::Elevator::kMaxV, Constants::Elevator::kMaxA};
    TrapezoidalMotionProfile::Constraints climbingConstraints{
        Constants::Elevator::kClimbMaxV, Constants::Elevator::kClimbMaxA};
    TrapezoidalMotionProfile::Constraints m_activeConstraints =
        scoringConstraints;

    TrapezoidalMotionProfile m_positionProfile{scoringConstraints,
                                               {0_m, 0_mps}};

    TrapezoidalMotionProfile::State m_profiledReference;

    frc::StateSpacePlant<2, 1, 1> m_plant = [&] {
        frc::StateSpacePlant<2, 1, 1> plant{MakeElevatorPlantCoeffs()};
        plant.AddCoefficients(MakeElevatorClimbPlantCoeffs());
        return plant;
    }();

    frc::StateSpaceController<2, 1, 1> m_controller = [&] {
        frc::StateSpaceController<2, 1, 1> controller{
            MakeElevatorControllerCoeffs(), m_plant};
        controller.AddCoefficients(MakeElevatorClimbControllerCoeffs());
        return controller;
    }();

    frc::StateSpaceObserver<2, 1, 1> m_observer = [&] {
        frc::StateSpaceObserver<2, 1, 1> observer{MakeElevatorObserverCoeffs(),
                                                  m_plant};
        observer.AddCoefficients(MakeElevatorClimbObserverCoeffs());
        return observer;
    }();

    Eigen::Matrix<double, 2, 1> m_nextR;

    bool m_atReferences = false;

    CsvLogger elevatorLogger{"/home/lvuser/Elevator.csv",
                             "Time,EstPos,PosRef,Voltage"};
};

}  // namespace frc3512


#include "core.hpp"
#include "PMK.hpp"
#define BOOST_CHRONO_HEADER_ONLY 1
#include <boost/chrono.hpp>
#include "Constants.hpp"

namespace pmk
{
double period(double val, double a, double b) {
    if(val >= b)
        val -= b - a;
    else if(val < a)
        val += b - a;
    return val;
}

double angleDistance(double a, double b) {
    auto delta = fmod(b - a, dpi2);
    if(delta < -dpi)
        return delta + dpi2;
    if(delta > dpi)
        return delta - dpi2;
    return delta;
}

double mixAngles(double angle, double target, double mix) {
    return angleDistance(angle, target) * mix;
}

std::default_random_engine generator;
std::uniform_int_distribution<u32> idistribution(0, 0xffffffff);
std::uniform_real_distribution<float> rdistribution(0.0, 1.0);
std::normal_distribution<float> ndistribution(0.0, 0.4);

u32 irng() {
    return idistribution(generator);
}
u32 irng(u32 min, u32 max) {
    std::uniform_int_distribution<u32> d(min, max);
    return d(generator);
}
float rng() {
    return rdistribution(generator);
}
float rng(float min, float max) {
    return rdistribution(generator);
}
float gauss() {
    return ndistribution(generator);
}

void init() {
    generator.seed(boost::chrono::high_resolution_clock::now().time_since_epoch().count());
}

float triangleInterpolate(float v00, float v01, float v10, float v11, float u, float v) {
    // return v00*(1.f-u)*(1.f-v) + v10*u*(1.f-v) + v01*(1.f-u)*v + v11*u*v;
    // if(u > v) return 500;
    if(u > v)
        return v00 + u * (v01 - v00) + v * (v10 - v00);
    else
        return v11 - u * (v11 - v01) - v * (v11 - v10);
}
glm::vec3 triangleNormal(float v00, float v01, float v10, float v11, float u, float v, float gridSize) {
    if(u > v) {
        glm::vec3 p00(0, 0, v00);
        glm::vec3 p10(gridSize, 0, v10);
        glm::vec3 p01(0, gridSize, v01);
        return glm::normalize(glm::triangleNormal(p01, p00, p10));
    }
    else {
        glm::vec3 p11(gridSize, gridSize, v11);
        glm::vec3 p10(gridSize, 0, v10);
        glm::vec3 p01(0, gridSize, v01);
        return glm::normalize(glm::triangleNormal(p10, p11, p01));
    }
}

glm::mat4 boxAroundAABB(const glm::vec3& bbMin, const glm::vec3& bbMax, const glm::vec3& dir) {
    return {};
}
std::function<void(glm::vec4& bbMin, glm::vec4& bbMax, glm::vec4 position)> sunSpaceViewBox;
std::function<void(glm::vec4, glm::vec4)> updateSunTransform;

}


#ifndef IOSQP_HPP
#define IOSQP_HPP

#include <osqp/osqp.h>
#include <Eigen/Sparse>
#include <memory>

/**
 * osqp interface:
 * minimize     0.5 x' P x + q' x
 * subject to   l <= A x <= u
 **/

class IOSQP
{
public:
    IOSQP() : UNBOUNDED_VAL(OSQP_INFTY),
              pWork(nullptr),
              pSettings(nullptr),
              pData(nullptr)
    {
        pSettings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
        pData = (OSQPData *)c_malloc(sizeof(OSQPData));
        if (pSettings)
            osqp_set_default_settings(pSettings);
    }

    ~IOSQP()
    {
        if (pWork)
            osqp_cleanup(pWork);
        if (pSettings)
            c_free(pSettings);
        if (pData)
            c_free(pData);
    }

    const double UNBOUNDED_VAL;

    inline c_int setMats(Eigen::SparseMatrix<double> &P,
                         Eigen::VectorXd &q,
                         Eigen::SparseMatrix<double> &A,
                         Eigen::VectorXd &l,
                         Eigen::VectorXd &u,
                         const double &eps_abs,
                         const double &eps_rel)
    {
        if (pWork)
            osqp_cleanup(pWork);

        P = P.triangularView<Eigen::Upper>();
        P.makeCompressed();
        A.makeCompressed();

        pData->n = P.rows();
        pData->m = A.rows();

        Eigen::Map<const Eigen::VectorXi> iIdxP(P.innerIndexPtr(), P.nonZeros());
        Eigen::Map<const Eigen::VectorXi> oIdxP(P.outerIndexPtr(), P.cols() + 1);
        Eigen::Matrix<c_int, -1, 1> innerIdxP = iIdxP.cast<c_int>();
        Eigen::Matrix<c_int, -1, 1> outerIdxP = oIdxP.cast<c_int>();
        pData->P = csc_matrix(pData->n,
                              pData->n,
                              P.nonZeros(),
                              P.valuePtr(),
                              innerIdxP.data(),
                              outerIdxP.data());

        Eigen::Map<const Eigen::VectorXi> iIdxA(A.innerIndexPtr(), A.nonZeros());
        Eigen::Map<const Eigen::VectorXi> oIdxA(A.outerIndexPtr(), A.cols() + 1);
        Eigen::Matrix<c_int, -1, 1> innerIdxA = iIdxA.cast<c_int>();
        Eigen::Matrix<c_int, -1, 1> outerIdxA = oIdxA.cast<c_int>();
        pData->A = csc_matrix(pData->m,
                              pData->n,
                              A.nonZeros(),
                              A.valuePtr(),
                              innerIdxA.data(),
                              outerIdxA.data());

        Eigen::Matrix<c_float, -1, 1> pDqVec = q.cast<c_float>();
        pData->q = pDqVec.data();
        Eigen::Matrix<c_float, -1, 1> pDlVec = l.cast<c_float>();
        pData->l = pDlVec.data();
        Eigen::Matrix<c_float, -1, 1> pDuVec = u.cast<c_float>();
        pData->u = pDuVec.data();

        pSettings->eps_abs = eps_abs;
        pSettings->eps_rel = eps_rel;

        c_int exitflag = osqp_setup(&pWork, pData, pSettings);

        if (pData->A)
            c_free(pData->A);
        if (pData->P)
            c_free(pData->P);

        return exitflag;
    }

    inline c_int solve() const
    {
        return osqp_solve(pWork);
    }

    inline c_int getStatus() const
    {
        return pWork->info->status_val;
    }

    inline Eigen::VectorXd getPrimalSol() const
    {
        return Eigen::Map<const Eigen::VectorXd>(pWork->solution->x,
                                                 pWork->data->n);
    }

private:
    OSQPWorkspace *pWork;
    OSQPSettings *pSettings;
    OSQPData *pData;
};

#endif


// Modified version of SpatialHash.hpp from IPC codebase.
// Originally created by Minchen Li.
#pragma once

#include <vector>

#include <Eigen/Core>
#include <ipc/utils/eigen_ext.hpp>

#include <ipc/broad_phase/collision_candidate.hpp>
#include <ipc/utils/unordered_map_and_set.hpp>

namespace ipc {

class SpatialHash {
public: // data
    VectorMax3d leftBottomCorner, rightTopCorner;
    ArrayMax3i voxelCount;
    double one_div_voxelSize;
    int voxelCount0x1;

    int edgeStartInd, triStartInd;

    unordered_map<int, std::vector<int>> voxel;
    std::vector<std::vector<int>> pointAndEdgeOccupancy;

protected:
    int dim;
    double builtInRadius;

public: // constructor
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    SpatialHash() {}

    SpatialHash(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double inflation_radius = 0,
        double voxelSize = -1)
    {
        build(V, E, F, inflation_radius, voxelSize);
    }

    SpatialHash(
        const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double inflation_radius = 0,
        double voxelSize = -1)
    {
        build(V0, V1, E, F, inflation_radius, voxelSize);
    }

public: // API
    void build(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double inflation_radius = 0,
        double voxelSize = -1);

    void build(
        const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double inflation_radius = 0,
        double voxelSize = -1);

    void clear()
    {
        voxel.clear();
        pointAndEdgeOccupancy.clear();
    }

    void queryPointForTriangles(
        const VectorMax3d& p,
        unordered_set<int>& triInds,
        double radius = 0) const;

    void queryPointForTriangles(
        const VectorMax3d& p_t0,
        const VectorMax3d& p_t1,
        unordered_set<int>& triInds,
        double radius = 0) const;

    void queryPointForPrimitives(
        const VectorMax3d& p_t0,
        const VectorMax3d& p_t1,
        unordered_set<int>& vertInds,
        unordered_set<int>& edgeInds,
        unordered_set<int>& triInds,
        double radius = 0) const;

    void queryEdgeForPE(
        const VectorMax3d& e0,
        const VectorMax3d& e1,
        std::vector<int>& vertInds,
        std::vector<int>& edgeInds,
        double radius = 0) const;

    void queryEdgeForEdges(
        const VectorMax3d& ea0,
        const VectorMax3d& ea1,
        std::vector<int>& edgeInds,
        double radius = 0,
        int eai = -1) const;

    void queryEdgeForEdgesWithBBoxCheck(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const VectorMax3d& ea0,
        const VectorMax3d& ea1,
        std::vector<int>& edgeInds,
        double radius = 0,
        int eai = -1) const;

    void queryEdgeForEdges(
        const VectorMax3d& ea0_t0,
        const VectorMax3d& ea1_t0,
        const VectorMax3d& ea0_t1,
        const VectorMax3d& ea1_t1,
        std::vector<int>& edgeInds,
        double radius = 0,
        int eai = -1) const;

    void queryTriangleForPoints(
        const VectorMax3d& t0,
        const VectorMax3d& t1,
        const VectorMax3d& t2,
        unordered_set<int>& pointInds,
        double radius = 0) const;

    void queryTriangleForPoints(
        const VectorMax3d& t0_t0,
        const VectorMax3d& t1_t0,
        const VectorMax3d& t2_t0,
        const VectorMax3d& t0_t1,
        const VectorMax3d& t1_t1,
        const VectorMax3d& t2_t1,
        unordered_set<int>& pointInds,
        double radius = 0) const;

    void queryTriangleForEdges(
        const VectorMax3d& t0,
        const VectorMax3d& t1,
        const VectorMax3d& t2,
        unordered_set<int>& edgeInds,
        double radius = 0) const;

    void queryEdgeForTriangles(
        const VectorMax3d& e0,
        const VectorMax3d& e1,
        unordered_set<int>& triInds,
        double radius = 0) const;

    void queryPointForPrimitives(
        int vi,
        unordered_set<int>& vertInds,
        unordered_set<int>& edgeInds,
        unordered_set<int>& triInds) const;

    void queryPointForEdges(int vi, unordered_set<int>& edgeInds) const;

    void queryPointForTriangles(int vi, unordered_set<int>& triInds) const;

    // will only put edges with larger than ei index into edgeInds
    void queryEdgeForEdges(int eai, unordered_set<int>& edgeInds) const;

    void queryEdgeForEdgesWithBBoxCheck(
        const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        int eai,
        unordered_set<int>& edgeInds) const;

    void queryMeshForCandidates(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        Candidates& candidates,
        bool queryEV = false,
        bool queryEE = true,
        bool queryFV = true) const;

    void queryMeshForCandidates(
        const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        Candidates& candidates,
        bool queryEV = false,
        bool queryEE = true,
        bool queryFV = true) const;

public: // helper functions
    int locateVoxelIndex(const VectorMax3d& p) const;

    void locateVoxelAxisIndex(
        const VectorMax3d& p, ArrayMax3i& voxelAxisIndex) const;

    int voxelAxisIndex2VoxelIndex(const ArrayMax3i& voxelAxisIndex) const;

    int voxelAxisIndex2VoxelIndex(int ix, int iy, int iz) const;
};

} // namespace ipc


// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include "carla/road/element/Geometry.h"

#include "carla/Debug.h"
#include "carla/Exception.h"
#include "carla/geom/Location.h"
#include "carla/geom/Math.h"
#include "carla/geom/Vector2D.h"

#include <boost/array.hpp>
#include <boost/math/tools/rational.hpp>

#include <odrSpiral/odrSpiral.h>

#include <algorithm>
#include <cmath>
#include <stdexcept>

namespace carla {
namespace road {
namespace element {

  void DirectedPoint::ApplyLateralOffset(float lateral_offset) {
    /// @todo Z axis??
    auto normal_x =  std::sin(static_cast<float>(tangent));
    auto normal_y = -std::cos(static_cast<float>(tangent));
    location.x += lateral_offset * normal_x;
    location.y += lateral_offset * normal_y;
  }

  DirectedPoint GeometryLine::PosFromDist(double dist) const {
    DEBUG_ASSERT(_length > 0.0);
    dist = geom::Math::Clamp(dist, 0.0, _length);
    DirectedPoint p(_start_position, _heading);
    p.location.x += static_cast<float>(dist * std::cos(p.tangent));
    p.location.y += static_cast<float>(dist * std::sin(p.tangent));
    return p;
  }

  DirectedPoint GeometryArc::PosFromDist(double dist) const {
    dist = geom::Math::Clamp(dist, 0.0, _length);
    DEBUG_ASSERT(_length > 0.0);
    DEBUG_ASSERT(std::fabs(_curvature) > 1e-15);
    const double radius = 1.0 / _curvature;
    constexpr double pi_half = geom::Math::Pi<double>() / 2.0;
    DirectedPoint p(_start_position, _heading);
    p.location.x += static_cast<float>(radius * std::cos(p.tangent + pi_half));
    p.location.y += static_cast<float>(radius * std::sin(p.tangent + pi_half));
    p.tangent += dist * _curvature;
    p.location.x -= static_cast<float>(radius * std::cos(p.tangent + pi_half));
    p.location.y -= static_cast<float>(radius * std::sin(p.tangent + pi_half));
    return p;
  }

  // helper function for rotating points
  geom::Vector2D RotatebyAngle(double angle, double x, double y) {
    const double cos_a = std::cos(angle);
    const double sin_a = std::sin(angle);
    return geom::Vector2D(
    static_cast<float>(x * cos_a - y * sin_a),
    static_cast<float>(y * cos_a + x * sin_a));
  }

  DirectedPoint GeometrySpiral::PosFromDist(double dist) const {
    dist = geom::Math::Clamp(dist, 0.0, _length);
    DEBUG_ASSERT(_length > 0.0);
    DirectedPoint p(_start_position, _heading);

    const double curve_end = (_curve_end);
    const double curve_start = (_curve_start);
    const double curve_dot = (curve_end - curve_start) / (_length);
    const double s_o = curve_start / curve_dot;
    double s = s_o + dist;

    double x;
    double y;
    double t;
    odrSpiral(s, curve_dot, &x, &y, &t);

    double x_o;
    double y_o;
    double t_o;
    odrSpiral(s_o, curve_dot, &x_o, &y_o, &t_o);

    x = x - x_o;
    y = y - y_o;
    t = t - t_o;

    geom::Vector2D pos = RotatebyAngle(_heading - t_o, x, y);
    p.location.x += pos.x;
    p.location.y += pos.y;
    p.tangent = _heading + t;

    return p;
  }

  /// @todo
  std::pair<float, float> GeometrySpiral::DistanceTo(const geom::Location &location) const {
    // Not analytic, discretize and find nearest point
    // throw_exception(std::runtime_error("not implemented"));
    return {location.x - _start_position.x, location.y - _start_position.y};
  }

  DirectedPoint GeometryPoly3::PosFromDist(double dist) const {
    auto result = _rtree.GetNearestNeighbours(
        Rtree::BPoint(static_cast<float>(dist))).front();

    auto &val1 = result.second.first;
    auto &val2 = result.second.second;

    double rate = (val2.s - dist) / (val2.s - val1.s);
    double u = rate * val1.u + (1.0 - rate) * val2.u;
    double v = rate * val1.v + (1.0 - rate) * val2.v;
    double tangent = atan((rate * val1.t + (1.0 - rate) * val2.t)); // ?

    geom::Vector2D pos = RotatebyAngle(_heading, u, v);
    DirectedPoint p(_start_position, _heading + tangent);
    p.location.x += pos.x;
    p.location.y += pos.y;
    return p;
  }

  std::pair<float, float> GeometryPoly3::DistanceTo(const geom::Location & /*p*/) const {
    // No analytical expression (Newton-Raphson?/point search)
    // throw_exception(std::runtime_error("not implemented"));
    return {_start_position.x, _start_position.y};
  }

  void GeometryPoly3::PreComputeSpline() {
    const double delta_u = 0.01; // interval between values of u
    double current_s = 0;
    double current_u = 0;
    double last_u = 0;
    double last_v = 0;
    double last_s = 0;
    RtreeValue last_val;
    while (current_s < _length + delta_u) {
      double current_v = _poly.Evaluate(current_u);
      double du = current_u - last_u;
      double dv = current_v - last_v;
      double ds = sqrt(du * du + dv * dv);
      current_s += ds;
      double current_t = _poly.Tangent(current_u);
      RtreeValue current_val{current_u, current_v, current_s, current_t};

      Rtree::BPoint p1(static_cast<float>(last_s));
      Rtree::BPoint p2(static_cast<float>(current_s));
      _rtree.InsertElement(Rtree::BSegment(p1, p2), last_val, current_val);

      last_u = current_u;
      last_v = current_v;
      last_s = current_s;
      last_val = current_val;

      current_u += delta_u;
    }
  }

  DirectedPoint GeometryParamPoly3::PosFromDist(double dist) const {
    auto result = _rtree.GetNearestNeighbours(
        Rtree::BPoint(static_cast<float>(dist))).front();

    auto &val1 = result.second.first;
    auto &val2 = result.second.second;

    double rate = (val2.s - dist) / (val2.s - val1.s);
    double u = rate * val1.u + (1.0 - rate) * val2.u;
    double v = rate * val1.v + (1.0 - rate) * val2.v;
    double t_u = (rate * val1.t_u + (1.0 - rate) * val2.t_u);
    double t_v = (rate * val1.t_v + (1.0 - rate) * val2.t_v);
    double tangent = atan2(t_v, t_u); // ?

    geom::Vector2D pos = RotatebyAngle(_heading, u, v);
    DirectedPoint p(_start_position, _heading + tangent);
    p.location.x += pos.x;
    p.location.y += pos.y;
    return p;
  }
  std::pair<float, float> GeometryParamPoly3::DistanceTo(const geom::Location &) const {
    // No analytical expression (Newton-Raphson?/point search)
    // throw_exception(std::runtime_error("not implemented"));
    return {_start_position.x, _start_position.y};
  }

  void GeometryParamPoly3::PreComputeSpline() {
    size_t number_intervals = 1000;
    double delta_p = 1.0 / number_intervals;
    if (_arcLength) {
        delta_p *= _length;
    }
    double param_p = 0;
    double current_s = 0;
    double last_u = 0;
    double last_v = 0;
    double last_s = 0;
    RtreeValue last_val;
    for(size_t i = 0; i < number_intervals; ++i) {
      double current_u = _polyU.Evaluate(param_p);
      double current_v = _polyV.Evaluate(param_p);
      double du = current_u - last_u;
      double dv = current_v - last_v;
      double ds = sqrt(du * du + dv * dv);
      current_s += ds;
      double current_t_u = _polyU.Tangent(param_p);
      double current_t_v = _polyV.Tangent(param_p);
      RtreeValue current_val{
          current_u,
          current_v,
          current_s,
          current_t_u,
          current_t_v };

      Rtree::BPoint p1(static_cast<float>(last_s));
      Rtree::BPoint p2(static_cast<float>(current_s));
      _rtree.InsertElement(Rtree::BSegment(p1, p2), last_val, current_val);

      last_u = current_u;
      last_v = current_v;
      last_s = current_s;
      last_val = current_val;

      param_p += delta_p;
      if(current_s > _length){
        break;
      }
    }
  }
} // namespace element
} // namespace road
} // namespace carla


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    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
 *      Noomen, R., Lambert targeter Excel file.
 *      Mengali, G., and A.A. Quarta, Fondamenti di Meccanica del volo Spaziale.
 *      Izzo, D., Keplerian_Toolbox.
 *
 *    Notes
 *      The elliptical case was taken from Example 6.1, page 159-162 of ( Mengali, Quarta ). The
 *      hyperbolic case was taken from ( Noomen, R. ). The retrograde and near-pi cases are
 *      verified against values found with the Lambert routine available in the Keplerian_Toolbox
 *      from ESA/ACT.
 *
 *      DISCLAIMER: At the moment, the Gooding Lambert targeter only converges for about half of
 *      the cases. This is not evident from the tests below, but it was observed during simulations
 *      carried out by the author. The reason might very well be an erroneous definition of the
 *      starters.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/MissionSegments/lambertRoutines.h"

namespace tudat
{
namespace unit_tests
{

using namespace unit_conversions;

//! Test the Lambert targeting routines.
BOOST_AUTO_TEST_SUITE( test_lambert_routines )

//! Test the Izzo time-of-flight computation.
BOOST_AUTO_TEST_CASE( testIzzoTimeOfFlightComputation )
{
    // Set tolerance.
    const double tolerance = 1.0e-7;

    // Set input values (taken from testEllipticalCase in test_lambert_targeter_izzo) [-].
    const double testXParameter = -0.5, testSemiPerimeter = 2.36603, testChord = 1.73205,
            testSemiMajorAxisOfTheMinimumEnergyEllipse = 1.18301;
    const bool testIsLongway = false;

    // Set expected time-of-flight [-].
    const double expectedTimeOfFlight = 9.759646;

    // Check that returned value is equal to expected value.
    BOOST_CHECK_CLOSE_FRACTION( expectedTimeOfFlight,
                                mission_segments::computeTimeOfFlightIzzo(
                                    testXParameter, testSemiPerimeter, testChord, testIsLongway,
                                    testSemiMajorAxisOfTheMinimumEnergyEllipse ),
                                tolerance );
}

//! Test the Izzo Lambert routine for an elliptical transfer.
BOOST_AUTO_TEST_CASE( testSolveLambertProblemIzzoElliptical )
{
    // Set tolerance.
    const double tolerance = 1.0e-6;

    // Set canonical units for Earth (see page 29 [1]).
    const double distanceUnit = 6.378136e6;
    const double timeUnit = 806.78;

    // Set expected inertial vectors.
    Eigen::Vector3d expectedInertialVelocityAtDeparture( 2735.8, 6594.3, 0.0 ),
            expectedInertialVelocityAtArrival( -1367.9, 4225.03, 0.0 );

    // Time conversions.
    const double testTimeOfFlight = 5.0 * timeUnit;

    // Set central body graviational parameter.
    const double testGravitationalParameter = 398600.4418e9;

    // Set position at departure and arrival.
    const Eigen::Vector3d testCartesianPositionAtDeparture( 2.0 * distanceUnit, 0.0, 0.0 ),
            testCartesianPositionAtArrival( 2.0 * distanceUnit, 2.0 * sqrt( 3.0 ) * distanceUnit,
                                            0.0 );

    // Declare velocity vectors.
    Eigen::Vector3d testInertialVelocityAtDeparture, testInertialVelocityAtArrival;

    // Solve Lambert problem.
    mission_segments::solveLambertProblemIzzo( testCartesianPositionAtDeparture,
                                                      testCartesianPositionAtArrival,
                                                      testTimeOfFlight,
                                                      testGravitationalParameter,
                                                      testInertialVelocityAtDeparture,
                                                      testInertialVelocityAtArrival );

    // Check that returned vectors are equal to expected vectors.
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.x( ),
                                testInertialVelocityAtDeparture.x( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.y( ),
                                testInertialVelocityAtDeparture.y( ), tolerance );
    BOOST_CHECK_SMALL( testInertialVelocityAtDeparture.z( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.x( ),
                                testInertialVelocityAtArrival.x( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.y( ),
                                testInertialVelocityAtArrival.y( ), tolerance );
    BOOST_CHECK_SMALL( testInertialVelocityAtArrival.z( ), tolerance );
}

//! Test the Izzo Lambert routine for a hyperbolic transfer.
BOOST_AUTO_TEST_CASE( testSolveLambertProblemIzzoHyperbolic )
{
    // Set tolerance.
    const double tolerance = 1.0e-5;

    // Set expected inertial vectors.
    Eigen::Vector3d expectedInertialVelocityAtDeparture( -745.457, 156.743, 0.0 ),
            expectedInertialVelocityAtArrival( 104.495, -693.209, 0.0 );

    // Time conversions.
    const double testTimeOfFlightInDaysHyperbola = 100.0;
    const double testTimeOfFlightHyperbola = convertJulianDaysToSeconds(
            testTimeOfFlightInDaysHyperbola );

    // Set central body graviational parameter.
    const double testGravitationalParameter = 398600.4418e9;

    // Set position at departure and arrival.
    const Eigen::Vector3d testCartesianPositionAtDeparture(
                convertAstronomicalUnitsToMeters( 0.02 ), 0.0, 0.0 ),
            testCartesianPositionAtArrival(
                0.0, convertAstronomicalUnitsToMeters( -0.03 ), 0.0 );

    // Declare velocity vectors.
    Eigen::Vector3d testInertialVelocityAtDeparture, testInertialVelocityAtArrival;

    // Solve Lambert problem.
    mission_segments::solveLambertProblemIzzo( testCartesianPositionAtDeparture,
                                                      testCartesianPositionAtArrival,
                                                      testTimeOfFlightHyperbola,
                                                      testGravitationalParameter,
                                                      testInertialVelocityAtDeparture,
                                                      testInertialVelocityAtArrival );

    // Check that returned vectors are equal to expected vectors.
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.x( ),
                                testInertialVelocityAtDeparture.x( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.y( ),
                                testInertialVelocityAtDeparture.y( ), tolerance );
    BOOST_CHECK_SMALL( testInertialVelocityAtDeparture.z( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.x( ),
                                testInertialVelocityAtArrival.x( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.y( ),
                                testInertialVelocityAtArrival.y( ), tolerance );
    BOOST_CHECK_SMALL( testInertialVelocityAtArrival.z( ), tolerance );
}

//! Test the Izzo Lambert routine for retrograde orbits.
BOOST_AUTO_TEST_CASE( testSolveLambertProblemIzzoRetrograde )
{
    // Set tolerance.
    const double tolerance = 1.0e-9;

    // Set positions at departure and arrival.
    /* Values taken from http://ccar.colorado.edu/~rla/lambert_j2000.html for JDi = 2456036 and
     * JDf = 2456336.
     */
    const Eigen::Vector3d
            testPositionAtDeparture( -131798187443.90068, -72114797019.4148, 2343782.3918863535 ),
            testPositionAtArrival( 202564770723.92966, -42405023055.01754, -5861543784.413235);

    // Set time-of-flight, coherent with initial and final positions.
    const double testTimeOfFlight = convertJulianDaysToSeconds( 300.0 );

    // Set central body (the Sun) gravitational parameter. Value taken from keptoolbox.
    const double testSolarGravitationalParameter = 1.32712428e20;

    // Set expected values for inertial velocities. Values obtained with keptoolbox.
    const Eigen::Vector3d
            expectedInitialVelocity( -14157.8507230353, 28751.266655828, 1395.46037631136 ),
            expectedFinalVelocity( -6609.91626743654, -22363.5220239692, -716.519714631494 );

    // Declare initial and final velocity vectors.
    Eigen::Vector3d initialVelocity, finalVelocity;

    // Compute Lambert solution.
    mission_segments::solveLambertProblemIzzo(
                testPositionAtDeparture, testPositionAtArrival,
                testTimeOfFlight, testSolarGravitationalParameter,
                initialVelocity, finalVelocity, true );

    // Check that velocities match expected values within the defined tolerance.
    TUDAT_CHECK_MATRIX_CLOSE( expectedInitialVelocity, initialVelocity, tolerance );
    TUDAT_CHECK_MATRIX_CLOSE( expectedFinalVelocity, finalVelocity, tolerance );
}

//! Test the Izzo Lambert routine for near-pi transfers.
BOOST_AUTO_TEST_CASE( testSolveLambertProblemIzzoNearPi )
{
    // Set tolerance.
    const double tolerance = 1.0e-9;

    // Set time-of-flight, coherent with initial and final positions.
    const double testTimeOfFlight = convertJulianDaysToSeconds( 300.0 );

    // Set central body (the Sun) gravitational parameter. Value taken from keptoolbox.
    const double testSolarGravitationalParameter = 1.32712428e20;

    // Set Keplerian elements at departure and arrival.
    Eigen::Matrix< double, 6, 1 > keplerianStateAtDeparture, keplerianStateAtArrival;
    keplerianStateAtDeparture << convertAstronomicalUnitsToMeters( 1.0 ),
                                    0.0, 0.0, 0.0, 0.0, 0.0;
    keplerianStateAtArrival << convertAstronomicalUnitsToMeters( 1.5 ),
            0.0, 0.0, 0.0, 0.0, convertDegreesToRadians( 179.999 );

    //  Convert to Cartesian elements.
    const Eigen::Matrix< double, 6, 1 > cartesianStateAtDeparture =
            orbital_element_conversions::convertKeplerianToCartesianElements(
                keplerianStateAtDeparture, testSolarGravitationalParameter );
    const Eigen::Matrix< double, 6, 1 > cartesianStateAtArrival =
            orbital_element_conversions::convertKeplerianToCartesianElements(
                keplerianStateAtArrival, testSolarGravitationalParameter );

    // Extract positions at departure and arrival.
    const Eigen::Vector3d positionAtDeparture = cartesianStateAtDeparture.head( 3 );
    const Eigen::Vector3d positionAtArrival = cartesianStateAtArrival.head( 3 );

    // Set expected values for inertial velocities. Values obtained with keptoolbox.
    const Eigen::Vector3d expectedInitialVelocity( 3160.36638344209, 32627.4771454454, 0.0 ),
            expectedFinalVelocity( 3159.89183582648, -21751.7065841264, 0.0 );

    // Declare initial and final velocity vectors.
    Eigen::Vector3d initialVelocity, finalVelocity;

    // Compute Lambert solution.
    mission_segments::solveLambertProblemIzzo( positionAtDeparture, positionAtArrival,
                                               testTimeOfFlight, testSolarGravitationalParameter,
                                               initialVelocity, finalVelocity );

    // Check that velocities match expected values within the defined tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedInitialVelocity.x( ), initialVelocity.x( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInitialVelocity.y( ), initialVelocity.y( ), tolerance );
    BOOST_CHECK_SMALL( initialVelocity.z( ), tolerance );

    BOOST_CHECK_CLOSE_FRACTION( expectedFinalVelocity.x( ), finalVelocity.x( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedFinalVelocity.y( ), finalVelocity.y( ), tolerance );
    BOOST_CHECK_SMALL( finalVelocity.z( ), tolerance );
}

//! Test the positive Gooding Lambert function.
BOOST_AUTO_TEST_CASE( testLambertFunctionPositiveGooding )
{
    // Set tolerance.
    const double tolerance = 1.0e-6;

    // Create input parameters. Values taken from validated test_lambert_targeter_gooding.
    double testXParameter = 1.09806;
    const double testQParameter = -0.402543;
    const double testNormalizedTimeOfFlight = 0.944749;

    // Create expected value.
    const double expectedLambertFunctionPositiveValue = -0.4004214;

    // Create LambertFunctionsGooding object.
    mission_segments::LambertFunctionsGooding testLambertFunctionsGooding(
                testQParameter, testNormalizedTimeOfFlight);

    // Check that the result is equal to the expected value.
    BOOST_CHECK_CLOSE_FRACTION( expectedLambertFunctionPositiveValue,
                                testLambertFunctionsGooding.lambertFunctionPositiveGooding(
                                    testXParameter ), tolerance );
}

//! Test the negative Gooding Lambert function.
BOOST_AUTO_TEST_CASE( testLambertFunctionNegativeGooding )
{
    // Set tolerance.
    const double tolerance = 1.0e-6;

    // Create input parameters. Values taken from validated test_lambert_targeter_gooding.
    double testXParameter = 0.434564;
    const double testQParameter = -0.402543;
    const double testNormalizedTimeOfFlight = 0.944749;

    // Create expected value.
    const double expectedLambertFunctionNegativeValue = -1.1439925;

    // Create LambertFunctionsGooding object.
    mission_segments::LambertFunctionsGooding testLambertFunctionsGooding(
                testQParameter, testNormalizedTimeOfFlight);

    // Check that the result is equal to the expected value.
    BOOST_CHECK_CLOSE_FRACTION( expectedLambertFunctionNegativeValue,
                                testLambertFunctionsGooding.lambertFunctionNegativeGooding(
                                    testXParameter ), tolerance );
}

//! Test the Gooding Lambert function.
BOOST_AUTO_TEST_CASE( testLambertFunctionGooding )
{
    // Set tolerance.
    const double tolerance = 1.0e-6;

    // Test 1: Test positive case.
    {
        // Create input parameters. Values taken from validated test_lambert_targeter_gooding.
        double testXParameter = 1.09806;
        const double testQParameter = -0.402543;
        const double testNormalizedTimeOfFlight = 0.944749;

        // Create expected value.
        const double expectedLambertFunctionValue = -0.4004214;

        // Create LambertFunctionsGooding object.
        mission_segments::LambertFunctionsGooding testLambertFunctionsGooding(
                    testQParameter, testNormalizedTimeOfFlight );

        // Check that the result is equal to the expected value.
        BOOST_CHECK_CLOSE_FRACTION( expectedLambertFunctionValue,
                                    testLambertFunctionsGooding.computeLambertFunctionGooding(
                                        testXParameter ), tolerance );
    }

    // Test 2: Test negative case.
    {
        // Create input parameters. Values taken from validated test_lambert_targeter_gooding.
        double testXParameter = 0.434564;
        const double testQParameter = -0.402543;
        const double testNormalizedTimeOfFlight = 0.944749;

        // Create expected value.
        const double expectedLambertFunctionValue = -1.1439925;

        // Create LambertFunctionsGooding object.
        mission_segments::LambertFunctionsGooding testLambertFunctionsGooding(
                    testQParameter, testNormalizedTimeOfFlight );

        // Check that the result is equal to the expected value.
        BOOST_CHECK_CLOSE_FRACTION( expectedLambertFunctionValue,
                                    testLambertFunctionsGooding.computeLambertFunctionGooding(
                                        testXParameter ), tolerance );
    }
}

//! Test the positive Gooding Lambert first derivative function.
BOOST_AUTO_TEST_CASE( testLambertFirstDerivativeFunctionPositiveGooding )
{
    // Set tolerance.
    const double tolerance = 1.0e-6;

    // Create input parameters. Values taken from validated test_lambert_targeter_gooding.
    double testXParameter = 1.09806;
    const double testQParameter = -0.402543;
    const double testNormalizedTimeOfFlight = 0.944749;

    // Create expected value.
    const double expectedLambertFirstDerivativeFunctionPositiveValue = 0.7261451;

    // Create LambertFunctionsGooding object.
    mission_segments::LambertFunctionsGooding testLambertFunctionsGooding(
                testQParameter, testNormalizedTimeOfFlight);

    // Check that the result is equal to the expected value.
    BOOST_CHECK_CLOSE_FRACTION( expectedLambertFirstDerivativeFunctionPositiveValue,
              testLambertFunctionsGooding.lambertFirstDerivativeFunctionPositiveGooding(
                                    testXParameter ), tolerance );
}

//! Test the negative Gooding Lambert first derivative function.
BOOST_AUTO_TEST_CASE( testLambertFirstDerivativeFunctionNegativeGooding )
{
    // Set tolerance.
    const double tolerance = 1.0e-6;

    // Create input parameters. Values taken from validated test_lambert_targeter_gooding.
    double testXParameter = 0.434564;
    const double testQParameter = -0.402543;
    const double testNormalizedTimeOfFlight = 0.944749;

    // Create expected value.
    const double expectedLambertFirstDerivativeFunctionNegativeValue = 1.72419;

    // Create LambertFunctionsGooding object.
    mission_segments::LambertFunctionsGooding testLambertFunctionsGooding(
                testQParameter, testNormalizedTimeOfFlight);

    // Check that the result is equal to the expected value.
    BOOST_CHECK_CLOSE_FRACTION( expectedLambertFirstDerivativeFunctionNegativeValue,
              testLambertFunctionsGooding.lambertFirstDerivativeFunctionNegativeGooding(
                                    testXParameter ), tolerance );
}

//! Test the Gooding Lambert first derivative function.
BOOST_AUTO_TEST_CASE( testLambertFirstDerivativeFunctionGooding )
{
    // Set tolerance.
    const double tolerance = 1.0e-6;

    // Test 1: Test the positive case.
    {
        // Create input parameters. Values taken from validated test_lambert_targeter_gooding.
        double testXParameter = 1.09806;
        const double testQParameter = -0.402543;
        const double testNormalizedTimeOfFlight = 0.944749;

        // Create expected value.
        const double expectedLambertFirstDerivativeFunctionValue = 0.7261451;

        // Create LambertFunctionsGooding object.
        mission_segments::LambertFunctionsGooding testLambertFunctionsGooding(
                    testQParameter, testNormalizedTimeOfFlight);

        // Check that the result is equal to the expected value.
        BOOST_CHECK_CLOSE_FRACTION( expectedLambertFirstDerivativeFunctionValue,
                  testLambertFunctionsGooding.computeFirstDerivativeLambertFunctionGooding(
                                        testXParameter ), tolerance );
    }

    // Test 2: Test the negative case.
    {
        // Create input parameters. Values taken from validated test_lambert_targeter_gooding.
        double testXParameter = 0.434564;
        const double testQParameter = -0.402543;
        const double testNormalizedTimeOfFlight = 0.944749;

        // Create expected value.
        const double expectedLambertFirstDerivativeFunctionValue = 1.72419;

        // Create LambertFunctionsGooding object.
        mission_segments::LambertFunctionsGooding testLambertFunctionsGooding(
                    testQParameter, testNormalizedTimeOfFlight);

        // Check that the result is equal to the expected value.
        BOOST_CHECK_CLOSE_FRACTION( expectedLambertFirstDerivativeFunctionValue,
                  testLambertFunctionsGooding.computeFirstDerivativeLambertFunctionGooding(
                                        testXParameter ), tolerance );
    }
}

//! Test the Gooding Lambert routine for a hyperbolic transfer.
BOOST_AUTO_TEST_CASE( testsolveLambertProblemGoodingHyperbolic )
{
    // Set tolerance.
    const double tolerance = 1.0e-5;

    // Set expected inertial vectors.
    Eigen::Vector3d expectedInertialVelocityAtDeparture( -745.457, 156.743, 0.0 ),
            expectedInertialVelocityAtArrival( 104.495, -693.209, 0.0 );

    // Time conversions.
    const double testTimeOfFlightInDaysHyperbola = 100.0;
    const double testTimeOfFlightHyperbola = convertJulianDaysToSeconds(
            testTimeOfFlightInDaysHyperbola );

    // Set central body graviational parameter.
    const double testGravitationalParameter = 398600.4418e9;

    // Set position at departure and arrival.
    const Eigen::Vector3d testCartesianPositionAtDeparture(
                convertAstronomicalUnitsToMeters( 0.02 ), 0.0, 0.0 ),
            testCartesianPositionAtArrival(
                0.0, convertAstronomicalUnitsToMeters( -0.03 ), 0.0 );

    // Declare velocity vectors.
    Eigen::Vector3d testInertialVelocityAtDeparture, testInertialVelocityAtArrival;

    // Solve Lambert problem.
    mission_segments::solveLambertProblemGooding( testCartesianPositionAtDeparture,
                                                        testCartesianPositionAtArrival,
                                                        testTimeOfFlightHyperbola,
                                                        testGravitationalParameter,
                                                        testInertialVelocityAtDeparture,
                                                        testInertialVelocityAtArrival);

    // Check that returned vectors are equal to expected vectors.
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.x( ),
                                testInertialVelocityAtDeparture.x( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.y( ),
                                testInertialVelocityAtDeparture.y( ), tolerance );
    BOOST_CHECK_SMALL( testInertialVelocityAtDeparture.z( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.x( ),
                                testInertialVelocityAtArrival.x( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.y( ),
                                testInertialVelocityAtArrival.y( ), tolerance );
    BOOST_CHECK_SMALL( testInertialVelocityAtArrival.z( ), tolerance );
}

//! Test the Gooding Lambert routine for an elliptical transfer.
BOOST_AUTO_TEST_CASE( testsolveLambertProblemGoodingElliptical )
{
    // Set tolerance.
    const double tolerance = 1.0e-6;

    // Set canonical units for Earth (see page 29 [1]).
    const double distanceUnit = 6.378136e6;
    const double timeUnit = 806.78;

    // Set expected inertial vectors.
    const Eigen::Vector3d expectedInertialVelocityAtDeparture( 2735.8, 6594.3, 0.0 ),
            expectedInertialVelocityAtArrival( -1367.9, 4225.03, 0.0 );

    // Time conversions.
    const double testTimeOfFlight = 5.0 * timeUnit;

    // Set central body graviational parameter.
    const double testGravitationalParameter = 398600.4418e9;

    // Set position at departure and arrival.
    const Eigen::Vector3d testCartesianPositionAtDeparture( 2.0 * distanceUnit, 0.0, 0.0 ),
            testCartesianPositionAtArrival( 2.0 * distanceUnit, 2.0 * sqrt( 3.0 ) * distanceUnit,
                                            0.0 );

    // Declare velocity vectors.
    Eigen::Vector3d testInertialVelocityAtDeparture, testInertialVelocityAtArrival;

    // Solve Lambert problem.
    mission_segments::solveLambertProblemGooding( testCartesianPositionAtDeparture,
                                                         testCartesianPositionAtArrival,
                                                         testTimeOfFlight,
                                                         testGravitationalParameter,
                                                         testInertialVelocityAtDeparture,
                                                         testInertialVelocityAtArrival );

    // Check that returned vectors are equal to expected vectors.
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.x( ),
                                testInertialVelocityAtDeparture.x( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtDeparture.y( ),
                                testInertialVelocityAtDeparture.y( ), tolerance );
    BOOST_CHECK_SMALL( testInertialVelocityAtDeparture.z( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.x( ),
                                testInertialVelocityAtArrival.x( ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( expectedInertialVelocityAtArrival.y( ),
                                testInertialVelocityAtArrival.y( ), tolerance );
    BOOST_CHECK_SMALL( testInertialVelocityAtArrival.z( ), tolerance );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//! OPENOR_INTERFACE_FILE(openOR_core)
//****************************************************************************
//!
//! @file
//! @ingroup openOR_core

#ifndef openOR_core_Math_ublasmatrix_hpp
#define openOR_core_Math_ublasmatrix_hpp

#if defined(_MSC_VER)
#  pragma warning(disable:4172)
#endif

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/quaternion.hpp>

#if defined(_MSC_VER)
#  pragma warning(default:4172)
#endif

#include <openOR/Math/traits.hpp>
#include <openOR/Math/matrixconcept.hpp>
#include <openOR/Math/matrix_types.hpp>

namespace openOR {
   namespace Math {

      //----------------------------------------------------------------------------
      // Adapting the corresponding traits
      //----------------------------------------------------------------------------

      template<typename MatrixType>
      struct UBLASMatrixAccess {
         typedef typename MatrixType::reference AccessType;
         typedef typename MatrixType::const_reference ConstAccessType;

         template <int I, int J, typename PP>
            static typename if_const<PP, ConstAccessType, AccessType>::
         type get(PP& p) {
            assert(I < p.size1() && J < p.size2());
            return p(I, J);
         }
      };

      template<typename Value, std::size_t Row, std::size_t Col, typename Layout>
      struct MatrixTraits<boost::numeric::ublas::bounded_matrix<Value, Row, Col, Layout> > {
         typedef boost::numeric::ublas::bounded_matrix<Value, Row, Col, Layout> MatrixType;
         typedef boost::numeric::ublas::bounded_matrix<typename ScalarTraits<Value>::RealType, Row, Col, Layout> RealMatrixType;
         typedef typename boost::numeric::ublas::bounded_matrix<Value, Row, Col, Layout>::value_type ValueType;
         typedef boost::mpl::int_<Row> RowDimension;
         typedef boost::mpl::int_<Col> ColDimension;
         typedef boost::numeric::ublas::bounded_vector<Value, Col> RowVectorType;
         typedef boost::numeric::ublas::bounded_vector<Value, Row> ColVectorType;
         typedef boost::numeric::ublas::bounded_vector<Value, 3> Vector3Type;
		 typedef boost::numeric::ublas::bounded_vector<Value, 2> Vector2Type;
         typedef boost::numeric::ublas::bounded_matrix<typename ScalarTraits<Value>::RealType, 3, 3, Layout> Matrix33Type;
         typedef UBLASMatrixAccess<MatrixType> Access;
         typedef boost::mpl::true_ IsMatrix;

         static MatrixType ZEROS;
         static MatrixType IDENTITY;
      };

      template<typename Value, std::size_t Row, std::size_t Col, typename Layout>
      typename MatrixTraits<boost::numeric::ublas::bounded_matrix<Value, Row, Col, Layout> >::MatrixType
      MatrixTraits<boost::numeric::ublas::bounded_matrix<Value, Row, Col, Layout> >::ZEROS = boost::numeric::ublas::zero_matrix<Value>(Row, Col);

      template<typename Value, std::size_t Row, std::size_t Col, typename Layout>
      typename MatrixTraits<boost::numeric::ublas::bounded_matrix<Value, Row, Col, Layout> >::MatrixType
      MatrixTraits<boost::numeric::ublas::bounded_matrix<Value, Row, Col, Layout> >::IDENTITY = boost::numeric::ublas::identity_matrix<Value>(Row, Col);

      //----------------------------------------------------------------------------
      //! @brief
      //! @ingroup openOR_core
      //----------------------------------------------------------------------------
      template<class E1, class E2>
         typename boost::numeric::ublas::matrix_vector_binary1_traits <
            typename E1::value_type, E1,
            typename E2::value_type, E2 >::
      result_type prod( const boost::numeric::ublas::matrix_expression<E1> &e1,
                        const boost::numeric::ublas::vector_expression<E2> &e2)
      {
         return boost::numeric::ublas::prod(e1, e2);
      }

      //----------------------------------------------------------------------------
      //! @brief
      //! @ingroup openOR_core
      //----------------------------------------------------------------------------
      template<class E1, class E2>
         typename boost::numeric::ublas::matrix_matrix_binary_traits <
            typename E1::value_type, E1,
            typename E2::value_type, E2 >::
      result_type prod( const boost::numeric::ublas::matrix_expression<E1> &e1,
                        const boost::numeric::ublas::matrix_expression<E2> &e2)
      {
         return boost::numeric::ublas::prod(e1, e2);
      }

   }
}

namespace boost {
   namespace numeric {
      namespace ublas {

         // TODO: boost ublas is in the process of defining the usual operaters more sensibly
         // (probably part of 1.42.0) we should get rid of these then

         template<class V, class E1, class E2>
         inline V operator% (const matrix_expression<E1>& e1, const vector_expression<E2>& e2) {
            return prod(e1, e2);
         }

         template<class M, class E1, class E2>
         inline M operator% (const matrix_expression<E1>& e1, const matrix_expression<E2>& e2) {
            return prod(e1, e2);
         }

      }
   }
}

#endif


#ifndef DRACO_FOOTSTEP_OBJECT_H
#define DRACO_FOOTSTEP_OBJECT_H

#include <vector>
#include <Eigen/Dense>
#include <iostream>
#include <string>

#define DRACO_LEFT_FOOTSTEP 0
#define DRACO_RIGHT_FOOTSTEP 1
#define DRACO_MID_FOOTSTEP 2

// Data container for a footstep by Draco

class DracoFootstep{
public:
  DracoFootstep();
  DracoFootstep(const Eigen::Vector3d & pos_in, const Eigen::Quaterniond & quat_in, const int & robot_side_in);

  ~DracoFootstep();

  // Position and orientation of the sole of the footstep
  Eigen::Vector3d position;
  Eigen::Quaternion<double> orientation;

  Eigen::Matrix3d R_ori;

  // Setters
  void setPosOriSide(const Eigen::Vector3d & pos_in, const Eigen::Quaterniond & quat_in, const int & robot_side_in);
  void setPosOri(const Eigen::Vector3d & pos_in, const Eigen::Quaterniond & quat_in);
  void setRightSide();
  void setLeftSide();
  void setMidFoot();

  void setWalkingParams(const double double_contact_time_in,
                        const double contact_transition_time_in,
                        const double swing_time_in,
                        const double swing_height_in);

  // Left or right side
  int robot_side;

  void computeMidfeet(const DracoFootstep & footstep1, const DracoFootstep & footstep2, DracoFootstep & midfeet);

  int getSide();
  void printInfo();

  // distance of toe and heel contacts from the center of the foot.
  double toe_dist_from_center = 0.1;
  double heel_dist_from_center = 0.05;

  // MPC default walking parameters
  double double_contact_time = 0.03; // seconds
  double contact_transition_time = 0.2; // seconds
  double swing_time = 0.3; // seconds
  double swing_height = 0.05; // meters

  std::vector<Eigen::Vector3d> local_contact_point_list;
  std::vector<Eigen::Vector3d> global_contact_point_list;

  Eigen::Vector3d getToePosition();
  Eigen::Vector3d getHeelPosition();

private:
  void common_initialization();
  void updateContactLocations();

};

#endif


#ifndef BOOST_TEST_HH
#define BOOST_TEST_HH

#include <boost/geometry/index/rtree.hpp>
#include <boost/geometry/geometry.hpp>

typedef boost::geometry::model::point<double, NUM_DIMS, boost::geometry::cs::cartesian> boost_point;
typedef std::pair<boost_point, size_t> boost_point_w_index;
typedef boost::geometry::model::box<boost_point> boost_bbox;
typedef std::pair<boost_bbox, size_t> boost_bbox_w_index;

inline boost_point make_boost_point(const point &pt) {
    return boost_point(pt[0], pt[1], pt[2]);
}

inline boost_bbox make_boost_bbox(const point &pt0, const point &pt1) {
    return boost_bbox(make_boost_point(pt0),make_boost_point(pt1));
}

static void print_boost_box(boost_bbox my_box) {
    std::cout << "(" << my_box.min_corner().get<0>() << " " << my_box.min_corner().get<1>() << ", " << my_box.min_corner().get<2>() << ")-";
    std::cout << "(" << my_box.max_corner().get<0>() << " " << my_box.max_corner().get<1>() << ", " << my_box.max_corner().get<2>() << ")" << std::endl;
}

static void print_boost_point(boost_point my_pt, bool suppress_newline = true) {
    std::cout << "(" << my_pt.get<0>() << " " << my_pt.get<1>() << ", " << my_pt.get<2>() << ")";
    if(! suppress_newline) {
        std::cout << std::endl;
    }
}

template <class BuildAlg>
class TestBoost : public BboxIntersectionTest {

    private:
        boost::geometry::index::rtree<boost_point_w_index, BuildAlg> *tree;
        boost::geometry::index::rtree<boost_bbox_w_index, BuildAlg> *tree_boxes;
        bool uses_boxes = false;

        struct boost_point_w_index_iterator : std::vector<boost_point_w_index>::const_iterator
        {
            using base_class = std::vector<boost_point_w_index>::const_iterator;
            using value_type = size_t;

            using base_class::base_class;

            value_type operator*() const {
                auto const& src = base_class::operator*();
                return (src.second );
            }
        };

        struct boost_bbox_w_index_iterator : std::vector<boost_bbox_w_index>::const_iterator
        {
            using base_class = std::vector<boost_bbox_w_index>::const_iterator;
            using value_type = size_t;

            using base_class::base_class;

            value_type operator*() const {
                auto const& src = base_class::operator*();
                return (src.second);
            }
        };
    public:
        bool intersections_exact() { return true; } //bounding box search

        TestBoost() {}
        ~TestBoost() {
            delete tree;
            delete tree_boxes;
        }

        void build_tree(const std::vector<point> &pts, const std::vector<size_t> &indices) {
            std::vector<boost_point_w_index> data;
            for(int i = 0; i < pts.size(); i++) {
                data.push_back(std::make_pair(make_boost_point(pts[i]),indices[i]));
            }
             tree = new boost::geometry::index::rtree<boost_point_w_index, BuildAlg>(data.begin(), data.end());
        }

        void build_tree_bbox(const std::vector<point> &pts, const std::vector<size_t> &indices) {
            if((pts.size() % 2) !=0) {
                std::cerr << "error. your point list size has to be even to insert bounding boxes" << std::endl;
                return;
            }
            uses_boxes = true;
            std::vector<boost_bbox_w_index> data;
            for(int i = 0; i < pts.size(); i += 2) {
                data.push_back(std::make_pair(make_boost_bbox(pts[i], pts[i+1]),indices[i/2]));
            }
             tree_boxes = new boost::geometry::index::rtree<boost_bbox_w_index, BuildAlg>(data.begin(), data.end());
        }

        void get_intersections(const bbox &my_bbox, std::vector<size_t> &intersections_indices) {
            if(uses_boxes) {
                std::vector<boost_bbox_w_index> results;
                tree_boxes->query(boost::geometry::index::intersects(make_boost_bbox(my_bbox.first, my_bbox.second)), std::back_inserter(results));
                intersections_indices.assign(boost_bbox_w_index_iterator(results.begin()),
                                           boost_bbox_w_index_iterator(results.end()));
            }
            else {
                std::vector<boost_point_w_index> results;
                tree->query(boost::geometry::index::intersects(make_boost_bbox(my_bbox.first, my_bbox.second)), std::back_inserter(results));
                intersections_indices.assign(boost_point_w_index_iterator(results.begin()),
                                           boost_point_w_index_iterator(results.end()));
            }
        }
};

#endif //BOOST_TEST_HH