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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_TRIG_BASE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_TRIG_BASE_HPP_INCLUDED
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/arch/common/detail/scalar/trig_reduction.hpp>
#include <boost/simd/function/is_invalid.hpp>
#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/is_invalid.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/quarter.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/deginrad.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/mtwo.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/config.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

// trigonometric functions are implemented using the classical cephes/fdlibm/libc principles
// however the formal reduce/eval/return is properly divided to allow choices versus
// speed, accuracy and  SIMD vectorisation
//
// On small ranges performance can be obtained with almost full accuracy (1 ulp) at 1 to 2
// cycles per element for a cosine computation using SIMD. But if one require full accuracy
// on the full float or double range, the prize can grow up to few hundreds of cycle for
// some big values of input.
// See the boost/simd technical documentation for more insights

namespace boost { namespace simd
{
  namespace detail
  {
    // in the class defined below:
    // A0 is the argument type of the trigonometric functions
    //
    // Up to now, only floating real based on IEEE 754 float or double are supported
    // (sorry no long double for lack of portability)

    template < typename A0, typename unit_tag> struct trig_ranges;
    template < typename A0>  struct trig_ranges<A0, tag::radian_tag>
    {
      static BOOST_FORCEINLINE A0    max_range() BOOST_NOEXCEPT {return Pio_4<A0>(); }
      static BOOST_FORCEINLINE A0    scale()     BOOST_NOEXCEPT {return One<A0>(); }
    };
    template < typename A0>  struct trig_ranges<A0, tag::pi_tag>
    {
      static BOOST_FORCEINLINE A0    max_range() BOOST_NOEXCEPT {return Quarter<A0>(); }
      static BOOST_FORCEINLINE A0    scale()     BOOST_NOEXCEPT {return Pi<A0>(); }
    };
    template < typename A0>  struct trig_ranges<A0, tag::degree_tag>
    {
      static BOOST_FORCEINLINE A0    max_range() BOOST_NOEXCEPT {return Ratio<A0, 45>(); }
      static BOOST_FORCEINLINE A0    scale()     BOOST_NOEXCEPT {return Deginrad<A0>(); }
    };

    // This class expose the public static members:
    // cosa     x -> cos(a*x)
    // sina     x -> sin(a*x)
    // tana     x -> tan(a*x)
    // cota     x -> cot(a*x)
    // sincosa  x -> cos(a*x) and sin(a*x)
    // "a" is not a run time parameter !
    // It stands as a remainder that according the Tag it can be considered
    // as a scale factor for the input of values:
    //   1           radian_tag,
    //   pi          pi_tag
    //   pi/180      degree_tag,
    // but the scaling is internal to the algorithm to provide better accuracy
    // and better performance.

    // It heritates of a reduction and an evaluation class to perform
    // the appropriate range reductions and function evaluations.

    // In this file stands the scalar version !
    // Some definitions are useless but provide facilities of comparaison with
    // the SIMD implementation
    //
    template < typename A0,
               typename unit_tag,
               typename style,
               typename mode =  tag::big_tag>
    struct trig_base{};

    template < typename A0,
               typename unit_tag,
               typename mode>
     struct trig_base<A0,unit_tag,tag::not_simd_type,mode>
     {
       using redu_t = trig_reduction<A0,unit_tag,tag::not_simd_type, mode>;
       using eval_t = trig_evaluation<A0,tag::not_simd_type>;
       using i_t    = bd::as_integer_t<A0, signed>;           // signed integer type associated to A0
       using style_t = typename mode::type;

       // for all functions the scalar algorithm is:
       // * quick return (or exception raising TODO) for inf and nan or specific invalid cases inputs
       // * range reduction
       // * computation of sign and evaluation selections flags
       // * evaluations
       // * return with flag based corrections

       static BOOST_FORCEINLINE A0 cosa(A0 a0){ return cosa(a0, style_t()); }
       static BOOST_FORCEINLINE A0 sina(A0 a0){ return sina(a0, style_t()); }
       static BOOST_FORCEINLINE A0 tana(A0 a0){ return tana(a0, style_t()); }
       static BOOST_FORCEINLINE A0 cota(A0 a0){ return cota(a0, style_t()); }
       static BOOST_FORCEINLINE std::pair<A0,A0> sincosa(const A0& a0){ return sincosa(a0,style_t()); }

       static BOOST_FORCEINLINE A0 cosa(A0 a0, const tag::regular&)
       {
         if (is_invalid(a0)) return Nan<A0>(); //Nan or Inf input
         const A0 x =  abs(a0);
         A0 xr = Nan<A0>();
         const i_t n = toint(redu_t::reduce(x, xr));
         const i_t swap_bit = n&One<i_t>();
         const i_t sign_bit = shift_left(bitwise_xor(swap_bit, (n&Two<i_t>())>>1), sizeof(i_t)*8-1);
         A0 z = sqr(xr);
         if (swap_bit)
           z = eval_t::sin_eval(z, xr);
         else
           z = eval_t::cos_eval(z);
         return bitwise_xor(z,sign_bit);
       }

       static BOOST_FORCEINLINE A0 sina(A0 a0, const tag::regular&) BOOST_NOEXCEPT
       {
         if (is_invalid(a0)) return Nan<A0>();
         const A0 x =  abs(a0);
         A0 xr = Nan<A0>();
         const i_t n = toint(redu_t::reduce(x, xr));
         const i_t swap_bit = n&One<i_t>();
         const A0 sign_bit = bitwise_xor(bitofsign(a0), shift_left(n&Two<i_t>(), sizeof(i_t)*8-2));
         A0 z = sqr(xr);
         if (swap_bit)
           z = eval_t::cos_eval(z);
         else
           z = eval_t::sin_eval(z, xr);
         return bitwise_xor(z,sign_bit);
       }

       static BOOST_FORCEINLINE A0 tana(A0 a0, const tag::regular&) BOOST_NOEXCEPT
       {
         if (is_invalid(a0)||redu_t::tan_invalid(a0)) return Nan<A0>();
         if (is_eqz(a0)) return a0;
         const A0 x =  abs(a0);
         A0 xr = Nan<A0>(), y;
         const i_t n = toint(redu_t::reduce(x, xr));
         y = eval_t::tan_eval(xr, 1-((n&1)<<1));
         // 1 -- n even
         //-1 -- n odd
         return bitwise_xor(y, bitofsign(a0));
       }

       static BOOST_FORCEINLINE A0 cota(A0 a0, const tag::regular&) BOOST_NOEXCEPT
       {
         if (is_invalid(a0)||redu_t::cot_invalid(a0)) return Nan<A0>();
         const A0 x = abs(a0);
         const A0 bos =  bitofsign(a0);
         if (!a0) return bitwise_or(Inf<A0>(), bos);
         A0 xr = Nan<A0>();
         const i_t n = toint(redu_t::reduce(x, xr));
         const A0 y = eval_t::cot_eval(xr, 1-((n&1)<<1));
         return bitwise_xor(y, bos);
       }

       static BOOST_FORCEINLINE std::pair<A0,A0> sincosa(A0 a0, const tag::regular&) BOOST_NOEXCEPT
       {
         A0 s, c;
         if (is_invalid(a0)) return {Nan<A0>(), Nan<A0>()};
         const A0 x =  abs(a0);
         constexpr i_t de = static_cast<i_t>(sizeof(i_t)*8-1);
         A0 xr;
         const i_t n = toint(redu_t::reduce(x, xr));
         const i_t swap_bit = n&One<i_t>();
         const A0 z = sqr(xr);
         const i_t cos_sign_bit = shift_left(bitwise_xor(swap_bit, (n&Two<i_t>())>>1), de);
         const A0 sin_sign_bit = bitwise_xor(bitofsign(a0), shift_left(n&Two<i_t>(), de-1));

         if (is_nez(swap_bit))
         {
           c = eval_t::sin_eval(z, xr);
           s = eval_t::cos_eval(z);
         }
         else
         {
           c = eval_t::cos_eval(z);
           s = eval_t::sin_eval(z, xr);
         }
         return {bitwise_xor(s,sin_sign_bit), bitwise_xor(c,cos_sign_bit)};
       }

       static BOOST_FORCEINLINE A0 cosa(A0 a0, const tag::restricted &) BOOST_NOEXCEPT
       {
         A0 x =  scale(a0);
         if(not_in_range(a0))
           return Nan<A0>();
         else
           return eval_t::cos_eval(sqr(x));
       }

       static BOOST_FORCEINLINE A0 sina(A0 a0, const tag::restricted&) BOOST_NOEXCEPT
       {
         if (is_eqz(a0)) return a0;
         A0 x =  scale(a0);
         if(not_in_range(a0))
           return Nan<A0>();
         else
           return eval_t::sin_eval(sqr(x), x);
       }

       static BOOST_FORCEINLINE A0 tana(A0 a0, const tag::restricted&) BOOST_NOEXCEPT
       {
         if (is_eqz(a0)) return a0;
         A0 x =  scale(a0);
         if(not_in_range(a0))
           return Nan<A0>();
         else
           return eval_t::base_tan_eval(x);
       }

       static BOOST_FORCEINLINE A0 cota(A0 a0, const tag::restricted&) BOOST_NOEXCEPT
       {
         if (is_eqz(a0)) return rec(a0);
          if(not_in_range(a0))
           return Nan<A0>();
          else
          {

            A0 x =  scale(a0);
            return rec(eval_t::base_tan_eval(x));
          }
       }

       static BOOST_FORCEINLINE std::pair<A0, A0>sincosa(A0 a0, const tag::restricted&) BOOST_NOEXCEPT
       {
         if (is_eqz(a0))       return {a0, One<A0>()} ;
         if(not_in_range(a0))  return {Nan<A0>(), Nan<A0>()};
         A0 x =  scale(a0);
         A0 z =  sqr(x);
         return { eval_t::sin_eval(z, x), eval_t::cos_eval(z)};
       }

       static BOOST_FORCEINLINE bool not_in_range(A0 a0) BOOST_NOEXCEPT
       {
         return bs::abs(a0) > trig_ranges<A0,unit_tag>::max_range();
       }
       static BOOST_FORCEINLINE A0 scale (A0 a0) BOOST_NOEXCEPT
       {
         return a0*trig_ranges<A0,unit_tag>::scale();
       }
    };
  }
} }
#endif


/*
 * The MIT License (MIT)
 * Copyright (c) 2011-2016 Bo Zhou<bo.schwarzstein@gmail.com>
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
 * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include <omp.h>

#include <cmath>
#include <iostream>
#include <vector>

#include <boost/timer/timer.hpp>

#include <WetaHair/Implementation/Origin.hpp>

#include "rgbe.h"

int main(int iArgc, char * aArgv[])
{
    -- iArgc , ++ aArgv ;
    if ( iArgc != 5 )
    {
        return EXIT_FAILURE ;
    }

    int iP = atoi( aArgv[0] ) ;
    double lfBeta = atof( aArgv[1] ) * boost::math::constants::degree<double>() ;
    double lfEta = atof( aArgv[2] ) ;
    double aMuA[3] = { 0.42f , 0.70f , 1.37f } ;

    int iSize = atoi( aArgv[3] ) ;
    FILE * pOutput = fopen( aArgv[4] , "wb" ) ;
    if ( ! pOutput )
    {
        return EXIT_FAILURE ;
    }
    RGBE_WriteHeader( pOutput , iSize , iSize , NULL ) ;

    //
    std::vector<float> vPixels;
    vPixels.resize( iSize * iSize * 3 ) ;

    double lfDelta = boost::math::constants::two_pi<double>() / iSize ;

    boost::timer::auto_cpu_timer cTimer ;
    #pragma omp parallel for
    for ( int y = 0 ; y < iSize ; ++ y )
    {
        double lfThetaD = - boost::math::constants::pi<double>() + lfDelta * ( y + 0.5 ) ;
        double lfEtaPrime = EtaPrime( lfEta , lfThetaD ) ;
        for ( int x = 0 ; x < iSize ; ++ x )
        {
            double aResult[3] = { 0.0 , 0.0 , 0.0 } ;
            if ( iP == 0 )
            {
                // $ \acos \in [ 0 , \pi ] $
                double lfPhi = - boost::math::constants::pi<double>() + lfDelta * ( x + 0.5 ) ;
                double lfAngle = cos( lfDelta * 0.5 * ( x + 0.5 ) ) ;
                N( 3 , aResult, aMuA , iP , lfBeta , lfPhi , lfEta , lfEtaPrime , lfAngle ) ;
            }
            else
            {
                // $\phi \in [ - \pi , pi ]$
                double lfPhi = - boost::math::constants::pi<double>() + lfDelta * ( x + 0.5 ) ;
                N( 3 , aResult, aMuA , iP , lfBeta , lfPhi , lfEta , lfEtaPrime , lfThetaD ) ;
            }
            int z = ( x + y * iSize ) * 3 ;
            vPixels[z + 0] = aResult[0] ;
            vPixels[z + 1] = aResult[1] ;
            vPixels[z + 2] = aResult[2] ;
        }
    }

    RGBE_WritePixels( pOutput , reinterpret_cast<float *>( & vPixels[0] ) , vPixels.size() );
    fclose( pOutput ) ;

    return EXIT_SUCCESS ;
}


/**
 * \file boost/numeric/ublasx/operation/exp.hpp
 *
 * \brief Apply the \c std::exp function to each element of a vector or
 *  matrix expression.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_EXP_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_EXP_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <cmath>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT>
struct vector_exp_functor_traits
{
	typedef VectorExprT input_expression_type;
	typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef vector_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_exp_functor_traits
{
	typedef MatrixExprT input_expression_type;
	typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef matrix_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

namespace /*<unnamed>*/ {

/// Auxiliary function used to replace ::std::exp2 when that is not available.
template <typename T>
BOOST_UBLAS_INLINE
T exp(T x)
{
	return ::std::exp(x);
}

} // Namespace <unnamed>

} // Namespace detail

/**
 * \brief Applies the \c std::exp function to each element of a given vector
 *  expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::exp to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_exp_functor_traits<VectorExprT>::result_type exp(vector_expression<VectorExprT> const& ve)
{
	typedef typename detail::vector_exp_functor_traits<VectorExprT>::expression_type expression_type;
	typedef typename detail::vector_exp_functor_traits<VectorExprT>::signature_result_type signature_result_type;

	return expression_type(ve(), detail::exp<signature_result_type>);
}

/**
 * \brief Applies the \c std::exp function to each element of a given matrix
 *  expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::exp to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_exp_functor_traits<MatrixExprT>::result_type exp(matrix_expression<MatrixExprT> const& me)
{
	typedef typename detail::matrix_exp_functor_traits<MatrixExprT>::expression_type expression_type;
	typedef typename detail::matrix_exp_functor_traits<MatrixExprT>::signature_result_type signature_result_type;

	return expression_type(me(), detail::exp<signature_result_type>);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_LOG_HPP


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/constant/mcd.hpp>

#include <boost/hana/constant/laws.hpp>
#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/minimal/constant.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

template <typename mcd, typename T>
void test() {
    BOOST_HANA_CONSTANT_ASSERT(Constant::laws::check(
        list(
            detail::minimal::constant<T, 0, mcd>,
            detail::minimal::constant<T, 1, mcd>,
            detail::minimal::constant<T, 2, mcd>,
            detail::minimal::constant<T, 3, mcd>
        )
    ));
}

int main() {
    test<Constant::mcd, int>();
    test<Constant::mcd, long>();
}


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <cmath>
#include <boost/array.hpp>
#include <comma/base/exception.h>
#include "angle.h"

namespace snark {  namespace velodyne { namespace impl {

class prefilled_sin_table
{
    public:
        prefilled_sin_table()
        {
            for( std::size_t i = 0; i < m_sin.size(); ++i ) { m_sin[i] = ::sin( M_PI / 180 * i ); }
            for( std::size_t i = 0; i < m_fractionSin.size(); ++i )
            {
                static double a( M_PI * 0.01 / 180 );
                m_fractionSin[i] = ::sin( a * i );
                m_fractionCos[i] = ::cos( a * i );
            }
        }

        double sin( unsigned int degrees, unsigned int fractions )
        {
            degrees %= 360;
            int sign( 1 );
            if( degrees >= 180 ) { sign = -1; degrees -= 180; }
            if( degrees >= 90 )
            {
                degrees = 180 - degrees;
                if( fractions > 0 ) { --degrees; fractions = 100 - fractions; }
            }
            int signOfSum( 1 );
            if( fractions > 50 )
            {
                signOfSum = -1;
                fractions = 100 - fractions;
                ++degrees;
            }
            return (   m_fractionCos[ fractions ] * m_sin[ degrees ]
                     + m_fractionSin[ fractions ] * m_sin[ 90 - degrees ] * signOfSum ) * sign;
        }

        double cos( unsigned int degrees, unsigned int fractions )
        {
            degrees %= 360;
            if( fractions == 0 ) { return prefilled_sin_table::sin( 360 + 90 - degrees, 0 ); }
            return prefilled_sin_table::sin( 360 + 90 - 1 - degrees, 100 - fractions );
        }

    private:
        // it could be further optimized, considering angle divided by
        // 256 instread 100 (thus saving on divisions), but it would become
        // even less readable
        boost::array< double, 91 > m_sin;
        boost::array< double, 51 > m_fractionSin;
        boost::array< double, 51 > m_fractionCos;
};

static prefilled_sin_table table;

double angle::sin( unsigned int angle )
{
    return table.sin( angle / 100, angle % 100 );
}

double angle::cos( unsigned int angle )
{
    return table.cos( angle / 100, angle % 100 );
}

} } } // namespace snark {  namespace velodyne { namespace impl {


#ifndef BEARING_HPP
#define BEARING_HPP

#include <algorithm>
#include <boost/assert.hpp>
#include <cmath>
#include <string>

namespace osrm
{
namespace util
{
namespace bearing
{

inline std::string get(const double heading)
{
    BOOST_ASSERT(heading >= 0);
    BOOST_ASSERT(heading <= 360);

    if (heading <= 22.5)
    {
        return "N";
    }
    if (heading <= 67.5)
    {
        return "NE";
    }
    if (heading <= 112.5)
    {
        return "E";
    }
    if (heading <= 157.5)
    {
        return "SE";
    }
    if (heading <= 202.5)
    {
        return "S";
    }
    if (heading <= 247.5)
    {
        return "SW";
    }
    if (heading <= 292.5)
    {
        return "W";
    }
    if (heading <= 337.5)
    {
        return "NW";
    }
    return "N";
}

// Checks whether A is between B-range and B+range, all modulo 360
// e.g. A = 5, B = 5, range = 10 == true
//      A = -6, B = 5, range = 10 == false
//      A = -2, B = 355, range = 10 == true
//      A = 6, B = 355, range = 10 == false
//      A = 355, B = -2, range = 10 == true
//
// @param A the bearing to check, in degrees, 0-359, 0=north
// @param B the bearing to check against, in degrees, 0-359, 0=north
// @param range the number of degrees either side of B that A will still match
// @return true if B-range <= A <= B+range, modulo 360
inline bool CheckInBounds(const int A, const int B, const int range)
{

    if (range >= 180)
        return true;
    if (range < 0)
        return false;

    // Map both bearings into positive modulo 360 space
    const int normalized_B = (B < 0) ? (B % 360) + 360 : (B % 360);
    const int normalized_A = (A < 0) ? (A % 360) + 360 : (A % 360);

    if (normalized_B - range < 0)
    {
        return (normalized_B - range + 360 <= normalized_A && normalized_A < 360) ||
               (0 <= normalized_A && normalized_A <= normalized_B + range);
    }
    else if (normalized_B + range > 360)
    {
        return (normalized_B - range <= normalized_A && normalized_A < 360) ||
               (0 <= normalized_A && normalized_A <= normalized_B + range - 360);
    }
    else
    {
        return normalized_B - range <= normalized_A && normalized_A <= normalized_B + range;
    }
}

inline double reverse(const double bearing)
{
    if (bearing >= 180)
        return bearing - 180.;
    return bearing + 180;
}

// Compute the angle between two bearings on a normal turn circle
//
//      Bearings                      Angles
//
//         0                           180
//   315         45               225       135
//
// 270     x       90           270     x      90
//
//   225        135               315        45
//        180                           0
//
// A turn from north to north-east offerst bearing 0 and 45 has to be translated
// into a turn of 135 degrees. The same holdes for 90 - 135 (east to south
// east).
// For north, the transformation works by angle = 540 (360 + 180) - exit_bearing
// % 360;
// All other cases are handled by first rotating both bearings to an
// entry_bearing of 0.
inline double angleBetween(const double entry_bearing, const double exit_bearing)
{
    // transform bearing from cw into ccw order
    const double offset = 360 - entry_bearing;
    const double rotated_exit = [](double bearing, const double offset) {
        bearing += offset;
        return bearing > 360 ? bearing - 360 : bearing;
    }(exit_bearing, offset);

    const auto angle = 540 - rotated_exit;
    return angle >= 360 ? angle - 360 : angle;
}

} // namespace bearing

// compute the minimum distance in degree between two angles/bearings
inline double angularDeviation(const double angle_or_bearing, const double from)
{
    const double deviation = std::abs(angle_or_bearing - from);
    return std::min(360 - deviation, deviation);
}

/* Angles in OSRM are expressed in the range of [0,360). During calculations, we might violate
 * this range via offsets. This function helps to ensure the range is kept. */
inline double restrictAngleToValidRange(const double angle)
{
    if (angle < 0)
        return restrictAngleToValidRange(angle + 360.);
    else if (angle > 360)
        return restrictAngleToValidRange(angle - 360.);
    else
        return angle;
}

// finds the angle between two angles, based on the minum difference between the two
inline double angleBetween(const double lhs, const double rhs)
{
    const auto difference = std::abs(lhs - rhs);
    const auto is_clockwise_difference = difference <= 180;
    const auto angle_between_candidate = .5 * (lhs + rhs);
    if (is_clockwise_difference)
        return angle_between_candidate;
    else
        return restrictAngleToValidRange(angle_between_candidate + 180);
}

} // namespace util
} // namespace osrm

#endif // BEARING_HPP


//------------------------------------------------------------------------------
// \file Sorting_tests.cpp
//------------------------------------------------------------------------------
#include "Algorithms/BubbleSort.h"
#include "Algorithms/InsertionSort.h"
#include "Algorithms/MergeSort.h"
#include "Algorithms/QuickSort.h"
#include "Cpp/Std/TypeTraitsProperties.h"
#include "DataStructures/LinkedLists.h"

#include <array>
#include <boost/test/unit_test.hpp>
#include <deque>
#include <forward_list>
#include <list>
#include <string>
#include <vector>

using Algorithms::Sorting::Details::naive_single_pass;
using Algorithms::Sorting::Details::single_pass;
using Algorithms::Sorting::Details::single_swap;
using Algorithms::Sorting::InsertionSort::binary_insertion_sort;
using Algorithms::Sorting::InsertionSort::insertion;
using Algorithms::Sorting::InsertionSort::insertion_sort;
using Algorithms::Sorting::InsertionSort::insertion_sort_list;
using Algorithms::Sorting::QuickSort::Details::partition_from_first;
using Algorithms::Sorting::QuickSort::Details::quick_sort_from_first;
using Algorithms::Sorting::QuickSort::partition_from_last_basic;
using Algorithms::Sorting::QuickSort::quick_sort_basic;
using Algorithms::Sorting::QuickSort::quick_sort_from_first;
using Algorithms::Sorting::QuickSort::quick_sort_from_last;
using Algorithms::Sorting::bubble_sort;
using Algorithms::Sorting::merge_sort;
using Algorithms::Sorting::merge_sort_with_temp;
using Algorithms::Sorting::naive_bubble_sort;
using DataStructures::LinkedLists::UsingPointers::ListNode;
using DataStructures::LinkedLists::UsingPointers::clean_up_ListNode_setup;

using DataStructures::LinkedLists::UsingPointers::get_size;

using DataStructures::LinkedLists::UsingPointers::setup_ListNode_linked_list;
using Std::CompositeTypeTraits;
using Std::PrimaryTypeTraits;
using std::vector;

BOOST_AUTO_TEST_SUITE(Algorithms)
BOOST_AUTO_TEST_SUITE(Sorting_tests)

BOOST_AUTO_TEST_SUITE(InsertionSort_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SortsCStyleIntArrays)
{
  int arr[] {12, 11, 13, 5, 6};
  const int n {sizeof(arr) / sizeof(arr[0])};

  insertion_sort(arr, n);
  BOOST_TEST(arr[0] == 5);
  BOOST_TEST(arr[1] == 6);
  BOOST_TEST(arr[2] == 11);
  BOOST_TEST(arr[3] == 12);
  BOOST_TEST(arr[4] == 13);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(InsertionSortLinkedList)
{
  {
    ListNode** lnl {setup_ListNode_linked_list({6,-5})};
    BOOST_TEST(lnl[0]->value_ == 6);
    BOOST_TEST(lnl[1]->value_ == -5);

    BOOST_TEST(get_size(lnl[0]) == 2);

    ListNode* new_head {insertion_sort_list(lnl[0])};

    BOOST_TEST(new_head->value_ == -5);
    BOOST_TEST(new_head->next_->value_ == 6);

    clean_up_ListNode_setup(lnl, 2);
  }
  {
    ListNode** lnl {setup_ListNode_linked_list({6,-5, 4})};
    ListNode* new_head {insertion_sort_list(lnl[0])};

    BOOST_TEST(new_head->value_ == -5);
    BOOST_TEST(new_head->next_->value_ == 4);
    BOOST_TEST(new_head->next_->next_->value_ == 6);

    clean_up_ListNode_setup(lnl, 3);
  }
  {
    ListNode** lnl {setup_ListNode_linked_list({6,5,3,1,8,7,2,4})};
    BOOST_TEST(lnl[0]->value_ == 6);
    BOOST_TEST(lnl[1]->value_ == 5);
    BOOST_TEST(lnl[2]->value_ == 3);

    clean_up_ListNode_setup(lnl, 8);
  }

}

BOOST_AUTO_TEST_SUITE(BinaryInsertionSort_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SortsStdArrays)
{
  constexpr int N {5};
  std::array<int, N> arr {12, 11, 13, 5, 6};

  binary_insertion_sort(arr);

  BOOST_TEST(arr[0] == 5);
  BOOST_TEST(arr[1] == 6);
  BOOST_TEST(arr[2] == 11);
  BOOST_TEST(arr[3] == 12);
  BOOST_TEST(arr[4] == 13);
}

BOOST_AUTO_TEST_SUITE_END() // BinaryInsertionSort_tests

BOOST_AUTO_TEST_SUITE_END() // InsertionSort_tests

BOOST_AUTO_TEST_SUITE(BubbleSort_tests)

BOOST_AUTO_TEST_SUITE(Details_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateSingleSwapping)
{
  {
    std::array<unsigned int, 5> a {5, 1, 4, 2, 8};

    single_swap(a, 2, 4);

    BOOST_TEST(a[2] == 8);
    BOOST_TEST(a[4] == 4);
  }
  {
    std::vector<unsigned int> a {5, 1, 4, 2, 8};

    single_swap(a, 2, 4);

    BOOST_TEST(a[2] == 8);
    BOOST_TEST(a[4] == 4);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateContainerProperties)
{
  {
    std::cout << "\n std::array \n";
    PrimaryTypeTraits<std::array<int, 42>> array_primary_type_traits;
    std::cout << array_primary_type_traits << '\n';
    CompositeTypeTraits<std::array<int, 42>> array_composite_type_traits;
    std::cout << array_composite_type_traits << '\n';

    std::cout << "\n std::list \n";
    PrimaryTypeTraits<std::list<int>> list_primary_type_traits;
    std::cout << list_primary_type_traits << '\n';
    CompositeTypeTraits<std::list<int>> list_composite_type_traits;
    std::cout << list_composite_type_traits << '\n';

    std::cout << "\n std::vector \n";
    PrimaryTypeTraits<std::vector<int>> vector_primary_type_traits;
    std::cout << vector_primary_type_traits << '\n';
    CompositeTypeTraits<std::vector<int>> vector_composite_type_traits;
    std::cout << vector_composite_type_traits << '\n';
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateNaiveSinglePass)
{
  {
    std::array<unsigned int, 5> a {5, 1, 4, 2, 8};

    BOOST_TEST(naive_single_pass(a));

    BOOST_TEST(a[0] == 1);
    BOOST_TEST(a[1] == 4);
    BOOST_TEST(a[2] == 2);
    BOOST_TEST(a[3] == 5);
    BOOST_TEST(a[4] == 8);
  }
  {
    std::vector<unsigned int> a {5, 1, 4, 2, 8};

    BOOST_TEST(naive_single_pass(a));

    BOOST_TEST(a[2] == 2);
    BOOST_TEST(a[4] == 8);

    BOOST_TEST(naive_single_pass(a));

    BOOST_TEST(a[0] == 1);
    BOOST_TEST(a[1] == 2);
    BOOST_TEST(a[2] == 4);
    BOOST_TEST(a[3] == 5);
    BOOST_TEST(a[4] == 8);

    BOOST_TEST(!naive_single_pass(a));
  }
  {
    std::vector<unsigned int> a {21, 4, 1, 3, 9, 20, 25, 6, 21, 14};

    BOOST_TEST(naive_single_pass(a));

    BOOST_TEST(a[2] == 3);
    BOOST_TEST(a[4] == 20);

    BOOST_TEST(naive_single_pass(a));

    BOOST_TEST(a[0] == 1);
    BOOST_TEST(a[1] == 3);
    BOOST_TEST(a[2] == 4);
    BOOST_TEST(a[3] == 9);
    BOOST_TEST(a[4] == 20);

    BOOST_TEST(naive_single_pass(a));

    BOOST_TEST(a[0] == 1);
    BOOST_TEST(a[1] == 3);
    BOOST_TEST(a[2] == 4);
    BOOST_TEST(a[3] == 9);
    BOOST_TEST(a[4] == 6);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateSinglePass)
{
  {
    std::array<unsigned int, 5> a {5, 1, 4, 2, 8};

    single_pass(a, 2);

    BOOST_TEST(a[0] == 1);
    BOOST_TEST(a[1] == 4);
    BOOST_TEST(a[2] == 2);
    BOOST_TEST(a[3] == 5);
    BOOST_TEST(a[4] == 8);
  }
  {
    std::vector<unsigned int> a {5, 1, 4, 2, 8};

    single_pass(a, 3);

    BOOST_TEST(a[2] == 5);
    BOOST_TEST(a[4] == 8);

    single_pass(a, 4);

    BOOST_TEST(a[0] == 1);
    BOOST_TEST(a[1] == 4);
    BOOST_TEST(a[2] == 5);
    BOOST_TEST(a[3] == 2);
    BOOST_TEST(a[4] == 8);
  }
}

BOOST_AUTO_TEST_SUITE_END() // Details_tests

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateNaiveBubbleSort)
{
  {
    std::array<unsigned int, 5> a {5, 1, 4, 2, 8};

    naive_bubble_sort(a);

    BOOST_TEST(a[0] == 1);
    BOOST_TEST(a[1] == 2);
    BOOST_TEST(a[2] == 4);
    BOOST_TEST(a[3] == 5);
    BOOST_TEST(a[4] == 8);
  }
  {
    std::vector<unsigned int> a {5, 1, 4, 2, 8};

    naive_bubble_sort(a);

    BOOST_TEST(a[2] == 4);
    BOOST_TEST(a[4] == 8);

    naive_bubble_sort(a);

    BOOST_TEST(a[0] == 1);
    BOOST_TEST(a[1] == 2);
    BOOST_TEST(a[2] == 4);
    BOOST_TEST(a[3] == 5);
    BOOST_TEST(a[4] == 8);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateBubbleSort)
{
  {
    std::array<unsigned int, 5> a {5, 1, 4, 2, 8};

    bubble_sort(a);

    BOOST_TEST(a[0] == 1);
    BOOST_TEST(a[1] == 2);
    BOOST_TEST(a[2] == 4);
    BOOST_TEST(a[3] == 5);
    BOOST_TEST(a[4] == 8);
  }
  {
    std::vector<unsigned int> a {5, 1, 4, 2, 8};

    bubble_sort(a);

    BOOST_TEST(a[2] == 4);
    BOOST_TEST(a[4] == 8);

    bubble_sort(a);

    BOOST_TEST(a[0] == 1);
    BOOST_TEST(a[1] == 2);
    BOOST_TEST(a[2] == 4);
    BOOST_TEST(a[3] == 5);
    BOOST_TEST(a[4] == 8);
  }
}

BOOST_AUTO_TEST_SUITE_END() // BubbleSort_tests

BOOST_AUTO_TEST_SUITE(MergeSort_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateMergeSort)
{
  {
    std::array<unsigned int, 6> a {12, 11, 13, 5, 6, 7};

    BOOST_TEST(sizeof(a) / sizeof(a[0]) == 6);

    merge_sort(a);

    BOOST_TEST(a[0] == 5);
    BOOST_TEST(a[1] == 6);
    BOOST_TEST(a[2] == 7);
    BOOST_TEST(a[3] == 11);
    BOOST_TEST(a[4] == 12);
    BOOST_TEST(a[5] == 13);
  }
  {
    std::vector<unsigned int> a {12, 11, 13, 5, 6, 7};

    BOOST_TEST(sizeof(a) / sizeof(a[0]) == 6);

    merge_sort(a);

    BOOST_TEST(a[0] == 5);
    BOOST_TEST(a[1] == 6);
    BOOST_TEST(a[2] == 7);
    BOOST_TEST(a[3] == 11);
    BOOST_TEST(a[4] == 12);
    BOOST_TEST(a[5] == 13);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateMergeSortWithTemp)
{
  vector<int> a {14, 20, 78, 98, 20, 45};

  merge_sort_with_temp<vector<int>, int>(a, 0, a.size() - 1);

  BOOST_TEST(a[0] == 14);
  BOOST_TEST(a[1] == 20);
  BOOST_TEST(a[2] == 20);
  // ...
  BOOST_TEST((a == vector<int>{14, 20, 20, 45, 78, 98}));
}

BOOST_AUTO_TEST_SUITE_END() // MergeSort_tests

BOOST_AUTO_TEST_SUITE(QuickSort_tests)

BOOST_AUTO_TEST_SUITE(Details_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateQuickSortPartitionFromLast)
{
  {
    std::vector<int> a {8, 3, 1, 7, 0, 10, 2};

    std::size_t new_pivot {partition_from_first(a, 0, a.size() - 1)};

    BOOST_TEST(new_pivot == 4);

    BOOST_TEST(a[0] == 0);
    BOOST_TEST(a[1] == 3);
    BOOST_TEST(a[2] == 1);
    BOOST_TEST(a[3] == 7);
    BOOST_TEST(a[4] == 8);
    BOOST_TEST(a[5] == 10);
    BOOST_TEST(a[6] == 2);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateQuickSortFromLast)
{
  {
    std::vector<int> a {8, 3, 1, 7, 0, 10, 2};

    quick_sort_from_first(a, 0, a.size());

    BOOST_TEST(a[0] == 0);
    BOOST_TEST(a[1] == 1);
    BOOST_TEST(a[2] == 2);
    BOOST_TEST(a[3] == 3);
    BOOST_TEST(a[4] == 7);
    BOOST_TEST(a[5] == 8);
    BOOST_TEST(a[6] == 10);
  }
}

BOOST_AUTO_TEST_SUITE_END() // Details_tests

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateQuickSortFromFirst)
{
  {
    std::vector<int> a {12, 11, 13, 5, 6, 7};

    quick_sort_from_first(a);

    BOOST_TEST(a[0] == 5);
    BOOST_TEST(a[1] == 6);
    BOOST_TEST(a[2] == 7);
    BOOST_TEST(a[3] == 11);
    BOOST_TEST(a[4] == 12);
    BOOST_TEST(a[5] == 13);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateQuickSortFromLast)
{
    std::array<unsigned int, 7> a {8, 3, 1, 7, 0, 10, 2};

    quick_sort_from_last(a);

    BOOST_TEST(a[0] == 0);
    BOOST_TEST(a[1] == 1);
    BOOST_TEST(a[2] == 2);
    BOOST_TEST(a[3] == 3);
    BOOST_TEST(a[4] == 7);
    BOOST_TEST(a[5] == 8);
    BOOST_TEST(a[6] == 10);

  {
    std::vector<int> a {12, 11, 13, 5, 6, 7};

    quick_sort_from_last(a);

    BOOST_TEST(a[0] == 5);
    BOOST_TEST(a[1] == 6);
    BOOST_TEST(a[2] == 7);
    BOOST_TEST(a[3] == 11);
    BOOST_TEST(a[4] == 12);
    BOOST_TEST(a[5] == 13);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstratePartitionFromLastBasic)
{
  {
    vector<int> arr {10, 7, 8, 9, 1, 5};

    const auto result =
      partition_from_last_basic<vector<int>>(arr, 0, arr.size() - 1);

    BOOST_TEST(result == 1);
    BOOST_TEST(arr[0] == 1);
    BOOST_TEST(arr[1] == 5);
    BOOST_TEST(arr[2] == 8);
    BOOST_TEST(arr[3] == 9);
    BOOST_TEST(arr[4] == 10);
    BOOST_TEST(arr[5] == 7);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateQuickSortBasic)
{
  //int arr[] {10, 7, 8, 9, 1, 5};
  //int n {sizeof(arr) / sizeof(arr[0])};
  vector<int> arr {10, 7, 8, 9, 1, 5};

  quick_sort_basic(arr, 0, arr.size() - 1);

  BOOST_TEST((arr == vector<int>{1, 5, 7, 8, 9, 10}));
}

BOOST_AUTO_TEST_SUITE_END() // QuickSort_tests

BOOST_AUTO_TEST_SUITE_END() // Sorting_tests
BOOST_AUTO_TEST_SUITE_END() // Algorithms


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

#include <boost/config.hpp>

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

#include <boost/graph/visitors.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/neighbor_bfs.hpp>
#include <boost/property_map.hpp>

/*

  Sample Output:

  0 --> 2
  1 --> 1 3 4
  2 --> 1 3 4
  3 --> 1 4
  4 --> 0 1
  distances: 0 2 1 2 1
  parent[0] = 0
  parent[1] = 2
  parent[2] = 0
  parent[3] = 2
  parent[4] = 0

*/

using namespace boost;

template <class ParentDecorator>
struct print_parent {
  print_parent(const ParentDecorator& p_) : p(p_) { }
  template <class Vertex>
  void operator()(const Vertex& v) const {
    std::cout << "parent[" << v << "] = " <<  p[v]  << std::endl;
  }
  ParentDecorator p;
};

template <class DistanceMap, class PredecessorMap, class ColorMap>
class distance_and_pred_visitor : public neighbor_bfs_visitor<>
{
  typedef typename property_traits<ColorMap>::value_type ColorValue;
  typedef color_traits<ColorValue> Color;
public:
  distance_and_pred_visitor(DistanceMap d, PredecessorMap p, ColorMap c)
    : m_distance(d), m_predecessor(p), m_color(c) { }

  template <class Edge, class Graph>
  void tree_out_edge(Edge e, const Graph& g) const
  {
    typename graph_traits<Graph>::vertex_descriptor
      u = source(e, g), v = target(e, g);
    put(m_distance, v, get(m_distance, u) + 1);
    put(m_predecessor, v, u);
  }
  template <class Edge, class Graph>
  void tree_in_edge(Edge e, const Graph& g) const
  {
    typename graph_traits<Graph>::vertex_descriptor
      u = source(e, g), v = target(e, g);
    put(m_distance, u, get(m_distance, v) + 1);
    put(m_predecessor, u, v);
  }

  DistanceMap m_distance;
  PredecessorMap m_predecessor;
  ColorMap m_color;
};

int main(int , char* [])
{
  typedef adjacency_list<
    mapS, vecS, bidirectionalS,
    property<vertex_color_t, default_color_type>
  > Graph;

  typedef property_map<Graph, vertex_color_t>::type
    ColorMap;

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

  typedef Graph::vertex_descriptor Vertex;

  // Array to store predecessor (parent) of each vertex. This will be
  // used as a Decorator (actually, its iterator will be).
  std::vector<Vertex> p(num_vertices(G));
  // VC++ version of std::vector has no ::pointer, so
  // I use ::value_type* instead.
  typedef std::vector<Vertex>::value_type* Piter;

  // Array to store distances from the source to each vertex .  We use
  // a built-in array here just for variety. This will also be used as
  // a Decorator.
  typedef graph_traits<Graph>::vertices_size_type size_type;
  size_type d[5];
  std::fill_n(d, 5, 0);

  // The source vertex
  Vertex s = *(vertices(G).first);
  p[s] = s;
  distance_and_pred_visitor<size_type*, Vertex*, ColorMap>
    vis(d, &p[0], get(vertex_color, G));
  neighbor_breadth_first_search
    (G, s, visitor(vis).
     color_map(get(vertex_color, G)));

  print_graph(G);

  if (num_vertices(G) < 11) {
    std::cout << "distances: ";
#ifdef BOOST_OLD_STREAM_ITERATORS
    std::copy(d, d + 5, std::ostream_iterator<int, char>(std::cout, " "));
#else
    std::copy(d, d + 5, std::ostream_iterator<int>(std::cout, " "));
#endif
    std::cout << std::endl;

    std::for_each(vertices(G).first, vertices(G).second,
                  print_parent<Piter>(&p[0]));
  }

  return 0;
}


#include <NTL/lzz_pE.h>

#include <NTL/new.h>

NTL_START_IMPL

zz_pEInfoT::zz_pEInfoT(const zz_pX& NewP)
{
   ref_count = 1;

   build(p, NewP);

   _card_init = 0;
   _card_base = zz_p::modulus();
   _card_exp = deg(NewP);
}

const ZZ& zz_pE::cardinality()
{
   if (!zz_pEInfo) Error("zz_pE::cardinality: undefined modulus");

   if (!zz_pEInfo->_card_init) {
      power(zz_pEInfo->_card, zz_pEInfo->_card_base, zz_pEInfo->_card_exp);
      zz_pEInfo->_card_init = 1;
   }

   return zz_pEInfo->_card;
}

zz_pEInfoT *zz_pEInfo = 0;

typedef zz_pEInfoT *zz_pEInfoPtr;

static
void CopyPointer(zz_pEInfoPtr& dst, zz_pEInfoPtr src)
{
   if (src == dst) return;

   if (dst) {
      dst->ref_count--;

      if (dst->ref_count < 0)
         Error("internal error: negative zz_pEContext ref_count");

      if (dst->ref_count == 0) delete dst;
   }

   if (src) {
      if (src->ref_count == NTL_MAX_LONG)
         Error("internal error: zz_pEContext ref_count overflow");

      src->ref_count++;
   }

   dst = src;
}

void zz_pE::init(const zz_pX& p)
{
   zz_pEContext c(p);
   c.restore();
}

zz_pEContext::zz_pEContext(const zz_pX& p)
{
   ptr = NTL_NEW_OP zz_pEInfoT(p);
}

zz_pEContext::zz_pEContext(const zz_pEContext& a)
{
   ptr = 0;
   CopyPointer(ptr, a.ptr);
}

zz_pEContext& zz_pEContext::operator=(const zz_pEContext& a)
{
   CopyPointer(ptr, a.ptr);
   return *this;
}

zz_pEContext::~zz_pEContext()
{
   CopyPointer(ptr, 0);
}

void zz_pEContext::save()
{
   CopyPointer(ptr, zz_pEInfo);
}

void zz_pEContext::restore() const
{
   CopyPointer(zz_pEInfo, ptr);
}

zz_pEBak::~zz_pEBak()
{
   if (MustRestore)
      CopyPointer(zz_pEInfo, ptr);

   CopyPointer(ptr, 0);
}

void zz_pEBak::save()
{
   MustRestore = 1;
   CopyPointer(ptr, zz_pEInfo);
}

void zz_pEBak::restore()
{
   MustRestore = 0;
   CopyPointer(zz_pEInfo, ptr);
}

const zz_pE& zz_pE::zero()
{
   static zz_pE z(zz_pE_NoAlloc);
   return z;
}

zz_pE::zz_pE()
{
   _zz_pE__rep.rep.SetMaxLength(zz_pE::degree());
}

istream& operator>>(istream& s, zz_pE& x)
{
   zz_pX y;

   s >> y;
   conv(x, y);

   return s;
}

void div(zz_pE& x, const zz_pE& a, const zz_pE& b)
{
   zz_pE t;

   inv(t, b);
   mul(x, a, t);
}

void div(zz_pE& x, const zz_pE& a, long b)
{
   NTL_zz_pRegister(B);
   B = b;
   inv(B, B);
   mul(x, a, B);
}

void div(zz_pE& x, const zz_pE& a, const zz_p& b)
{
   NTL_zz_pRegister(B);
   B = b;
   inv(B, B);
   mul(x, a, B);
}

void div(zz_pE& x, long a, const zz_pE& b)
{
   zz_pE t;
   inv(t, b);
   mul(x, a, t);
}

void div(zz_pE& x, const zz_p& a, const zz_pE& b)
{
   zz_pE t;
   inv(t, b);
   mul(x, a, t);
}

void inv(zz_pE& x, const zz_pE& a)
{
   InvMod(x._zz_pE__rep, a._zz_pE__rep, zz_pE::modulus());
}

NTL_END_IMPL


#include <boost/numeric/odeint/stepper/explicit_generic_rk.hpp>


#include "config.h"
#include "Scene_c3t3_item.h"
#include "Scene_surface_mesh_item.h"

#include <QVector>
#include <QColor>
#include <QPixmap>
#include <QApplication>
#include <QPainter>
#include <QtCore/qglobal.h>
#include <QGuiApplication>
#include <QSlider>
#include <QWidgetAction>
#include <QKeyEvent>
#include <QMouseEvent>

#include <map>
#include <vector>

#include <CGAL/Three/Scene_interface.h>
#include <CGAL/Three/Triangle_container.h>
#include <CGAL/Three/Edge_container.h>
#include <CGAL/Three/Point_container.h>
#include <CGAL/Three/Three.h>

#include <CGAL/Real_timer.h>

#include <CGAL/Qt/manipulatedFrame.h>
#include <CGAL/Qt/qglviewer.h>

#include <boost/iterator/function_output_iterator.hpp>

#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/AABB_triangulation_3_cell_primitive.h>
#include <CGAL/facets_in_complex_3_to_triangle_mesh.h>

#include "Scene_polygon_soup_item.h"

typedef CGAL::AABB_triangulation_3_cell_primitive<EPICK,
                                                  C3t3::Triangulation> Primitive;
typedef CGAL::AABB_traits<EPICK, Primitive> Traits;
typedef CGAL::AABB_tree<Traits> Tree;
typedef Tree::Point_and_primitive_id Point_and_primitive_id;
using namespace CGAL::Three;
typedef Triangle_container Tc;
typedef Edge_container Ec;
typedef Point_container Pc;
typedef Viewer_interface Vi;

struct Scene_c3t3_item_priv {

  void init_default_values() {
    sharp_edges_angle = -1;
    detect_borders = false;
  }

  Scene_c3t3_item_priv(Scene_c3t3_item* item)
    : item(item), c3t3(), is_valid(true), computed_stats(false){
  }

  Scene_c3t3_item_priv(const C3t3& c3t3_, Scene_c3t3_item* item)
    : item(item), c3t3(c3t3_), is_valid(true), computed_stats(false){
  }

  ~Scene_c3t3_item_priv()
  {
    c3t3.clear();
  }

  void draw_triangle_edges_cnc(const Tr::Bare_point& pa,
                               const Tr::Bare_point& pb,
                               const Tr::Bare_point& pc) const;
  Scene_c3t3_item* item;
  C3t3 c3t3;
  bool is_surface;
  //only for optimizers
  double sharp_edges_angle;
  bool detect_borders;
  bool is_valid;
  bool cnc_are_shown;

  enum STATS {
    MIN_EDGES_LENGTH = 0,
    MAX_EDGES_LENGTH,
    MEAN_EDGES_LENGTH,
    MIN_DIHEDRAL_ANGLE,
    MAX_DIHEDRAL_ANGLE,
    MEAN_DIHEDRAL_ANGLE,
    NB_SPHERES,
    NB_VERTICES,
    NB_TETS,
    SMALLEST_RAD_RAD,
    SMALLEST_EDGE_RAD,
    BIGGEST_VL3_CUBE,
    NB_SUBDOMAINS,
    NB_CNC
  };

  mutable std::vector<float> positions_lines_not_in_complex;
  mutable std::size_t positions_lines_not_in_complex_size;
  mutable std::size_t nb_cnc;
  mutable bool computed_stats;

  void push_point(std::vector<float>& points, const EPICK::Point_3& p,
                  const CGAL::qglviewer::Vec& offset) const
  {
    points.push_back(static_cast<float>(p.x()+offset.x));
    points.push_back(static_cast<float>(p.y()+offset.y));
    points.push_back(static_cast<float>(p.z()+offset.z));
  }

  void push_edge(std::vector<float>& edges,
                 const EPICK::Point_3& pa,
                 const EPICK::Point_3& pb,
                 const CGAL::qglviewer::Vec& offset) const
  {
    push_point(edges, pa, offset);
    push_point(edges, pb, offset);
  }
};

void Scene_c3t3_item_priv::draw_triangle_edges_cnc(const Tr::Bare_point& pa,
                                                   const Tr::Bare_point& pb,
                                                   const Tr::Bare_point& pc) const
{
  const CGAL::qglviewer::Vec offset = Three::mainViewer()->offset();
  push_edge(positions_lines_not_in_complex, pa, pb, offset);
  push_edge(positions_lines_not_in_complex, pb, pc, offset);
  push_edge(positions_lines_not_in_complex, pc, pa, offset);
}

void Scene_c3t3_item::common_constructor(bool is_surface)
{
  d->is_surface = is_surface;
  d->cnc_are_shown = false;
  setEdgeContainer(CNC, new Ec(Vi::PROGRAM_NO_SELECTION, false));
}

Scene_c3t3_item::Scene_c3t3_item(bool is_surface)
  : Scene_triangulation_3_item(!is_surface)
{
  d = new Scene_c3t3_item_priv(this);
  common_constructor(is_surface);

}

Scene_c3t3_item::Scene_c3t3_item(const C3t3& c3t3, bool is_surface)
  : Scene_triangulation_3_item(c3t3.triangulation(), !is_surface)
{
  d = new Scene_c3t3_item_priv(c3t3, this);
  common_constructor(is_surface);
}

Scene_c3t3_item::~Scene_c3t3_item()
{
  delete d;
}

const C3t3&
Scene_c3t3_item::c3t3() const {
  return d->c3t3;
}

C3t3&
Scene_c3t3_item::c3t3()
{
  return d->c3t3;
}

void
Scene_c3t3_item::c3t3_changed()
{
  triangulation_changed();
}

const T3&
Scene_c3t3_item::triangulation() const {
  return d->c3t3.triangulation();
}

T3&
Scene_c3t3_item::triangulation()
{
  return d->c3t3.triangulation();
}

bool Scene_c3t3_item::do_take_cell(const T3::Cell_handle& c) const
{
  return d->c3t3.is_in_complex(c);
}

bool Scene_c3t3_item::do_take_facet(const T3::Facet& f)const
{
  return (d->c3t3.is_in_complex(f));
}

bool Scene_c3t3_item::do_take_vertex(const T3::Vertex_handle& v)const
{
  return d->c3t3.is_in_complex(v);
}

bool Scene_c3t3_item::is_facet_oriented(const T3::Facet& f)const
{
  const Tr::Cell_handle& cell = f.first;
  const int& index = f.second;
  return (index % 2 == 1) == d->c3t3.is_in_complex(cell);
}

QMenu* Scene_c3t3_item::contextMenu()
{

  const char* prop_name = "Menu modified by Scene_c3t3_item.";

  QMenu* menu = Scene_triangulation_3_item::contextMenu();

  // Use dynamic properties:
  // https://doc.qt.io/qt-5/qobject.html#property
  bool menuChanged = menu->property(prop_name).toBool();

  if (!menuChanged) {

    if(is_valid())
    {
      QAction* actionShowCNC =
          menu->addAction(tr("Show cells not in complex"));
      actionShowCNC->setCheckable(true);
      actionShowCNC->setObjectName("actionShowCNC");
      connect(actionShowCNC, SIGNAL(toggled(bool)),
              this, SLOT(show_cnc(bool)));
    }

    QAction* actionExportFacetsInComplex =
      menu->addAction(tr("Export facets in complex"));
    actionExportFacetsInComplex->setObjectName("actionExportFacetsInComplex");
    connect(actionExportFacetsInComplex,
      SIGNAL(triggered()), this,
      SLOT(export_facets_in_complex()));

    menu->setProperty(prop_name, true);
  }
  return menu;
}

bool Scene_c3t3_item::is_valid() const
{
  return d->is_valid;
}

void Scene_c3t3_item::set_valid(bool b)
{
  d->is_valid = b;
}

void Scene_c3t3_item::show_cnc(bool b)
{
  if(is_valid())
  {
    d->cnc_are_shown = b;
    contextMenu()->findChild<QAction*>("actionShowCNC")->setChecked(b);
    Q_EMIT redraw();
  }
}

bool Scene_c3t3_item::load_binary(std::istream& is)
{
  if(!CGAL::Mesh_3::load_binary_file(is, c3t3())) return false;
  resetCutPlane();
  if(is.good()) {
    c3t3_changed();
    changed();
    return true;
  }
  else
    return false;
}

void Scene_c3t3_item::export_facets_in_complex()
{
  SMesh outmesh;
  CGAL::facets_in_complex_3_to_triangle_mesh(c3t3(), outmesh);
  Scene_surface_mesh_item* item = new Scene_surface_mesh_item(std::move(outmesh));
  item->setName(QString("%1_%2").arg(this->name()).arg("facets"));
  scene->addItem(item);
  this->setVisible(false);
}

void Scene_c3t3_item::drawEdges(Viewer_interface *viewer) const
{
  Scene_triangulation_3_item::drawEdges(viewer);
//add cnc
  if(d->cnc_are_shown)
  {
    getEdgeContainer(CNC)->setColor(QColor(Qt::black));
    getEdgeContainer(CNC)->draw(viewer, true);
  }
}

void Scene_c3t3_item::initializeBuffers(CGAL::Three::Viewer_interface *viewer)const
{
  Scene_triangulation_3_item::initializeBuffers(viewer);
  // add cnc
  {
    getEdgeContainer(Scene_c3t3_item::CNC)->initializeBuffers(viewer);
    getEdgeContainer(Scene_c3t3_item::CNC)->setFlatDataSize(
          d->positions_lines_not_in_complex_size);
    d->positions_lines_not_in_complex.clear();
    d->positions_lines_not_in_complex.shrink_to_fit();
  }
}
void Scene_c3t3_item::computeElements()const
{
  Scene_triangulation_3_item::computeElements();
  QApplication::setOverrideCursor(Qt::WaitCursor);
  //add cnc
  //the cells not in the complex
  Geom_traits::Construct_point_3 wp2p
    = c3t3().triangulation().geom_traits().construct_point_3_object();
  for(C3t3::Triangulation::Cell_iterator
      cit = c3t3().triangulation().finite_cells_begin(),
      end = c3t3().triangulation().finite_cells_end();
      cit != end; ++cit)
  {
    if(!c3t3().is_in_complex(cit))
    {

      bool has_far_point = false;
      for(int i=0; i<4; i++)
        if(c3t3().in_dimension(cit->vertex(i)) == -1)
        {
          has_far_point = true;
          break;
        }
      if(!has_far_point)
      {
        const Tr::Bare_point& p1 = wp2p(cit->vertex(0)->point());
        const Tr::Bare_point& p2 = wp2p(cit->vertex(1)->point());
        const Tr::Bare_point& p3 = wp2p(cit->vertex(2)->point());
        const Tr::Bare_point& p4 = wp2p(cit->vertex(3)->point());
        d->draw_triangle_edges_cnc(p1, p2, p4);
        d->draw_triangle_edges_cnc(p1, p3, p4);
        d->draw_triangle_edges_cnc(p2, p3, p4);
        d->draw_triangle_edges_cnc(p1, p2, p3);
      }
    }
  }
  getEdgeContainer(CNC)->allocate(
        Ec::Vertices,
        d->positions_lines_not_in_complex.data(),
        static_cast<int>(d->positions_lines_not_in_complex.size()*sizeof(float)));
  d->positions_lines_not_in_complex_size = d->positions_lines_not_in_complex.size();
  QApplication::restoreOverrideCursor();
}

QString Scene_c3t3_item::computeStats(int type)
{
  if(type != Scene_c3t3_item_priv::NB_CNC)
    return Scene_triangulation_3_item::computeStats(type);
  if(!d->computed_stats)
  {
    d->nb_cnc = 0;
    for(C3t3::Triangulation::Cell_iterator
        cit = d->c3t3.triangulation().finite_cells_begin(),
        end = d->c3t3.triangulation().finite_cells_end();
        cit != end; ++cit)
    {
      if(!d->c3t3.is_in_complex(cit))
      {

        bool has_far_point = false;
        for(int i=0; i<4; i++)
          if(d->c3t3.in_dimension(cit->vertex(i)) == -1)
          {
            has_far_point = true;
            break;
          }
        if(!has_far_point)
          ++d->nb_cnc;
      }
    }
    d->computed_stats = true;
  }
  return QString::number(d->nb_cnc);
}

CGAL::Three::Scene_item::Header_data Scene_c3t3_item::header() const
{
  CGAL::Three::Scene_item::Header_data data = Scene_triangulation_3_item::header();
  data.categories[0] = std::pair<QString,int>(QString("Properties"),14);
  data.titles.append(QString("#Cells not in Complex"));
  return data;
}

bool Scene_c3t3_item::has_cnc()const { return d->cnc_are_shown;}

bool Scene_c3t3_item::is_surface() const
{
  return d->is_surface;
}

void Scene_c3t3_item::set_sharp_edges_angle(double a) { d->sharp_edges_angle = a; }
double Scene_c3t3_item::get_sharp_edges_angle() { return d->sharp_edges_angle; }

void Scene_c3t3_item::set_detect_borders(bool b) { d->detect_borders = b;}
bool Scene_c3t3_item::get_detect_borders() { return d->detect_borders; }

Scene_c3t3_item* Scene_c3t3_item::clone() const
{
  return new Scene_c3t3_item(d->c3t3, d->is_surface);
}

void Scene_c3t3_item::copyProperties(Scene_item *item)
{
  Scene_triangulation_3_item::copyProperties(item);
  Scene_c3t3_item* c3t3_item = qobject_cast<Scene_c3t3_item*>(item);
  if(!c3t3_item)
    return;
  show_cnc(c3t3_item->has_cnc());

}


//==================================================================================================
/*!
  @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_CONSTANT_DEFINITION_FOURPI_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_DEFINITION_FOURPI_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    struct fourpi_ : boost::dispatch::constant_value_<fourpi_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,fourpi_,boost::dispatch::constant_value_<fourpi_>);
      BOOST_SIMD_REGISTER_CONSTANT(12, 0X41490FDBUL, 0X402921FB54442D18ULL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, fourpi_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::fourpi_,fourpi);
  }

  template<typename T> BOOST_FORCEINLINE auto Fourpi()
  BOOST_NOEXCEPT_DECLTYPE(detail::fourpi( boost::dispatch::as_<T>{}))
  {
    return detail::fourpi( boost::dispatch::as_<T>{} );
  }
} }

#endif


/*
 [auto_generated]
 boost/numeric/odeint/stepper/euler.hpp

 [begin_description]
 Implementation of the classical explicit Euler stepper. This method is really simple and should only
 be used for demonstration purposes.
 [end_description]

 Copyright 2010-2013 Karsten Ahnert
 Copyright 2010-2013 Mario Mulansky

 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 BOOST_NUMERIC_ODEINT_STEPPER_EULER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_EULER_HPP_INCLUDED

#include <boost/numeric/odeint/stepper/base/explicit_stepper_base.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = typename algebra_dispatcher< State >::algebra_type ,
class Operations = typename operations_dispatcher< State >::operations_type ,
class Resizer = initially_resizer
>
#ifndef DOXYGEN_SKIP
class euler
: public explicit_stepper_base<
  euler< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
  1 , State , Value , Deriv , Time , Algebra , Operations , Resizer >
#else
class euler : public explicit_stepper_base
#endif
{
public :

    #ifndef DOXYGEN_SKIP
    typedef explicit_stepper_base< euler< State , Value , Deriv , Time , Algebra , Operations , Resizer > , 1 , State , Value , Deriv , Time , Algebra , Operations , Resizer > stepper_base_type;
    #else
    typedef explicit_stepper_base< euler< ... > , ... > stepper_base_type;
    #endif
    typedef typename stepper_base_type::state_type state_type;
    typedef typename stepper_base_type::value_type value_type;
    typedef typename stepper_base_type::deriv_type deriv_type;
    typedef typename stepper_base_type::time_type time_type;
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::operations_type operations_type;
    typedef typename stepper_base_type::resizer_type resizer_type;

    #ifndef DOXYGEN_SKIP
    typedef typename stepper_base_type::stepper_type stepper_type;
    typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
    #endif

    euler( const algebra_type &algebra = algebra_type() ) : stepper_base_type( algebra )
    { }

    template< class System , class StateIn , class DerivIn , class StateOut >
    void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
    {
        stepper_base_type::m_algebra.for_each3( out , in , dxdt ,
                typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , dt ) );

    }

    template< class StateOut , class StateIn1 , class StateIn2 >
    void calc_state( StateOut &x , time_type t ,  const StateIn1 &old_state , time_type t_old , const StateIn2 &current_state , time_type t_new ) const
    {
        const time_type delta = t - t_old;
        stepper_base_type::m_algebra.for_each3( x , old_state , stepper_base_type::m_dxdt.m_v ,
                typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , delta ) );
    }

    template< class StateType >
    void adjust_size( const StateType &x )
    {
        stepper_base_type::adjust_size( x );
    }
};

/********** DOXYGEN ***********/

/**
 * \class euler
 * \brief An implementation of the Euler method.
 *
 * The Euler method is a very simply solver for ordinary differential equations. This method should not be used
 * for real applications. It is only useful for demonstration purposes. Step size control is not provided but
 * trivial continuous output is available.
 *
 * This class derives from explicit_stepper_base and inherits its interface via CRTP (current recurring template pattern),
 * see explicit_stepper_base
 *
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn euler::euler( const algebra_type &algebra )
     * \brief Constructs the euler class. This constructor can be used as a default
     * constructor of the algebra has a default constructor.
     * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
     */

    /**
     * \fn euler::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
     * \brief This method performs one step. The derivative `dxdt` of `in` at the time `t` is passed to the method.
     * The result is updated out of place, hence the input is in `in` and the output in `out`.
     * Access to this step functionality is provided by explicit_stepper_base and
     * `do_step_impl` should not be called directly.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param dxdt The derivative of x at t.
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dt The step size.
     */

    /**
     * \fn euler::calc_state( StateOut &x , time_type t ,  const StateIn1 &old_state , time_type t_old , const StateIn2 &current_state , time_type t_new ) const
     * \brief This method is used for continuous output and it calculates the state `x` at a time `t` from the
     * knowledge of two states `old_state` and `current_state` at time points `t_old` and `t_new`.
     */

    /**
     * \fn euler::adjust_size( const StateType &x )
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_EULER_HPP_INCLUDED


// Boost.Geometry
// Unit Test

// Copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#include <geometry_test_common.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/srs/epsg.hpp>
#include <boost/geometry/srs/projection.hpp>
#include <boost/geometry/srs/transformation.hpp>
#include <boost/geometry/strategies/transform/srs_transformer.hpp>

#include <boost/geometry/io/wkt/read.hpp>

#include "check_geometry.hpp"

int test_main(int, char*[])
{
    using namespace boost::geometry;
    using namespace boost::geometry::model;
    using namespace boost::geometry::srs;
    using namespace bg::strategy::transform;

    typedef point<double, 2, cs::geographic<degree> > point_ll;
    typedef point<double, 2, cs::cartesian> point_xy;
    //typedef polygon<point_ll> polygon_ll;
    //typedef polygon<point_xy> polygon_xy;

    {
        point_ll pt_ll(1, 1);
        point_ll pt_ll2(0, 0);
        point_xy pt_xy(0, 0);
        point_xy pt_xy2(0, 0);

        srs_forward_transformer<projection<> > strategy_pf = proj4("+proj=tmerc +ellps=WGS84 +units=m");
        srs_inverse_transformer<projection<> > strategy_pi = proj4("+proj=tmerc +ellps=WGS84 +units=m");
        srs_forward_transformer<transformation<> > strategy_tf(proj4("+proj=tmerc +ellps=WGS84 +units=m"),
                                                               proj4("+proj=tmerc +ellps=clrk66 +units=m"));
        srs_inverse_transformer<transformation<> > strategy_ti(proj4("+proj=tmerc +ellps=WGS84 +units=m"),
                                                               proj4("+proj=tmerc +ellps=clrk66 +units=m"));

        bg::transform(pt_ll, pt_xy, strategy_pf);
        test::check_geometry(pt_xy, "POINT(111308.33561309829 110591.34223734379)", 0.0001);

        bg::transform(pt_xy, pt_ll2, strategy_pi);
        test::check_geometry(pt_ll2, "POINT(1 1)", 0.0001);

        bg::transform(pt_xy, pt_xy2, strategy_tf);
        test::check_geometry(pt_xy2, "POINT(111309.54843459482 110584.27813586517)", 0.0001);

        bg::transform(pt_xy2, pt_xy, strategy_ti);
        test::check_geometry(pt_xy, "POINT(111308.33561309829 110591.34223734379)", 0.0001);
    }

    {
        srs_forward_transformer<projection<> > strategy_pf = epsg(2000);
        srs_inverse_transformer<projection<> > strategy_pi = epsg(2000);
        srs_forward_transformer<transformation<> > strategy_tf(epsg(2000), epsg(2001));
        srs_inverse_transformer<transformation<> > strategy_ti(epsg(2000), epsg(2001));
    }

    {
        using namespace bg::srs::spar;

        srs_forward_transformer
            <
                projection<parameters<proj_tmerc, ellps_wgs84> >
            > strategy_pf;
        srs_forward_transformer
            <
                projection<parameters<proj_tmerc, ellps_wgs84> >
            > strategy_pi;
        srs_forward_transformer
            <
                transformation
                    <
                        parameters<proj_tmerc, ellps_wgs84>,
                        parameters<proj_tmerc, ellps_clrk66>
                    >
            > strategy_tf;
        srs_forward_transformer
            <
                transformation
                    <
                        parameters<proj_tmerc, ellps_wgs84>,
                        parameters<proj_tmerc, ellps_clrk66>
                    >
            > strategy_ti;
    }

    {
        srs_forward_transformer<projection<static_epsg<2000> > > strategy_pf;
        srs_forward_transformer<projection<static_epsg<2000> > > strategy_pi;
        srs_forward_transformer<transformation<static_epsg<2000>, static_epsg<2001> > > strategy_tf;
        srs_forward_transformer<transformation<static_epsg<2000>, static_epsg<2001> > > strategy_ti;
    }

    return 0;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120209    K. Kumar          File created.
 *
 *    References
 *      Easy calculation. Newton's Law of Gravity Tutorial,
 *          http://easycalculation.com/physics/classical-physics/learn-newtons-law.php, last
 *          accessed: 12th February, 2012.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_gravitational_force )

//! Test if gravitational force is computed correctly.
BOOST_AUTO_TEST_CASE( testGravitationalForce )
{
    // Case 1: Compute gravitational force exerted on boy, due to Earth (Easy calculation, 2012).
    {
        // Set gravitational parameter of Earth [m^3 s^-2].
        double gravitationalParameterOfEarth = 6.6726e-11 * 5.98e24;

        // Set position vector of Earth [m].
        Eigen::Vector3d positionOfEarth = Eigen::Vector3d::Zero( );

        // Set mass of boy [kg]
        double massOfBoy = 70.0;

        // Set position vector of boy [m].
        Eigen::Vector3d positionOfBoy( 6.38e6, 0.0, 0.0 );

        // Compute gravitational force acting on boy [N].
        Eigen::Vector3d gravitationalForceExertedOnBoy
                = gravitation::computeGravitationalForce(
                    massOfBoy, positionOfBoy,
                    gravitationalParameterOfEarth, positionOfEarth );

        // Check if computed gravitational force matches expected value.
        BOOST_CHECK_CLOSE_FRACTION( 685.54, gravitationalForceExertedOnBoy.norm( ), 1.0e-3 );
    }

    // Case 2: Compute gravitational force exerted on arbitrary body1, due to arbitrary body2
    //         (Easy calculation, 2012).
    {
        // Set universal gravitational constant [m^3 kg^-1 s^-2].
        double universalGravitationalConstant = 6.6726e-11;

        // Set mass of body1 [kg].
        double massOfBody1 = 1.0e4;

        // Set position vector of body1 [m].
        Eigen::Vector3d positionOfBody1( 0.0, 5.0, 0.0 );

        // Set mass of body2 [kg].
        double massOfBody2 = 2.0e4;

        // Set position vector of body2 [m].
        Eigen::Vector3d positionOfBody2( 0.0, -5.0, 0.0 );

        // Compute gravitational force acting on body1 [N].
        Eigen::Vector3d gravitationalForceExertedOnBody1
                = gravitation::computeGravitationalForce(
                    universalGravitationalConstant, massOfBody1,
                    positionOfBody1, massOfBody2, positionOfBody2 );

        // Check if computed gravitational force matches expected value.
        BOOST_CHECK_CLOSE_FRACTION( 13345200.0e-11, gravitationalForceExertedOnBody1.norm( ),
                                    std::numeric_limits< double >::epsilon( ) );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// Copyright (c) 2014 The Bitcoin Core developers
// Copyright (c) 2014-2015 The Dash developers
// Copyright (c) 2015-2017 The PIVX developers
// Copyright (c) 2015-2017 The ALQO developers
// Copyright (c) 2017-2018 The TimeIsMoney developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "primitives/transaction.h"
#include "main.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

CAmount nMoneySupplyPoWEnd = 41769651 * COIN;

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    CAmount nSum = 0;
    for (int nHeight = 0; nHeight < 1; nHeight += 1) {
        /* premine in block 1 */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 5000000 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 1; nHeight < 75000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 10 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 75000; nHeight < 150000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 10.75 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 150000; nHeight < 225000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 11.5 * COIN);
        nSum += nSubsidy;
    }
	for (int nHeight = 225000; nHeight < 300000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 12.25 * COIN);
        nSum += nSubsidy;
    }
	for (int nHeight = 300000; nHeight < 375000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 13 * COIN);
        nSum += nSubsidy;
    }
	for (int nHeight = 375000; nHeight < 450000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 13.75 * COIN);
        nSum += nSubsidy;
    }
	for (int nHeight = 450000; nHeight < 525000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 14.5 * COIN);
        nSum += nSubsidy;
    }
	for (int nHeight = 525000; nHeight < 600000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 15.25 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 600000; nHeight < 675000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 16 * COIN);
        nSum += nSubsidy;
    }

	BOOST_CHECK(nSum > 0 && nSum <= nMoneySupplyPoWEnd);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef WAVE_TRANSFORMATION_HPP
#define WAVE_TRANSFORMATION_HPP

#include <memory>
#include <Eigen/Core>
#include "unsupported/Eigen/MatrixFunctions"

#include "wave/utils/utils.hpp"

namespace wave {

template <typename T_str = Mat34, bool approximate = false>
class Transformation {
 public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    static constexpr const double tol = 1.0e-5;
    T_str storage;

    Transformation() {
        this->setIdentity();
    }

    Transformation(Transformation const &other) : storage(other.storage) {}

    Transformation(const T_str &storage) : storage(storage) {}
    Transformation(T_str &&storage) noexcept : storage(std::move(storage)) {}

    template <typename Other, bool OtherApprox>
    Transformation &operator=(const Transformation<Other, OtherApprox> &other) {
        this->storage = other.storage;
        return *this;
    }

    template <typename Other, bool OtherApprox>
    Transformation &operator=(Transformation<Other, OtherApprox> &&other) noexcept {
        this->storage = std::move(other.storage);
        return *this;
    }

    /** Constructs from Euler angles.
     *
     * @param eulers X-Y-Z Euler angles in radians, such that
     * @param translation Values to initialize transformation matrix
     * @f[
     * R = R_z R_y R_x
     * @f]
     * where @f$ R_x, R_y @f$, and @f$ R_z @f$, are rotation matrices
     * constructed using the rotation about @f x, y @f, and @fz@f, respectively.
     */
    explicit Transformation(const Vec3 &eulers, const Vec3 &translation);

    /** Sets from Euler angles.
     *
     * @param input_vector X-Y-Z Euler angles; see `Rotation(const Vec3 &)`.
     * @param translation Values to initialize transformation matrix
     * @return a reference to `*this`
     */
    Transformation &setFromEulerXYZ(const Vec3 &eulers, const Vec3 &translation);

    /** Sets from exponential map.
     *
     * Converts from the se(3) Lie algebra, to the SE(3) Lie group using the
     * exponential map
     * @f[
     * R = exp(w_{\times})
     * @f]
     * where @f$ w_{\times} @f$ is the input `se3_vector` represented in
     * skew-symmetric form.
     *
     * @param se3_vector Twist vector corresponding to the se3 lie algebra for the
     * transformation object. Parameters are rotation, then translation
     * @return a reference to `*this`
     */
    template <typename VType>
    Transformation &setFromExpMap(const Eigen::MatrixBase<VType> &se3_vector);

    /** Sets from a Transformation matrix.
     * @return a reference to `*this`
     */
    Transformation &setFromMatrix(const Mat4 &input_matrix);

    /** Sets to the identity Transformation.
     * @return a reference to `*this`
     */
    Transformation &setIdentity();

    /** Checks if matrix determinant is within given tolerance of 1 and normalizes
     * rotation component if not
     */
    Transformation &normalize();

    template <typename InType, typename Other, bool InApprox, bool O_approx>
    static void interpolate(const Transformation<InType, InApprox> &T_k,
                            const Transformation<InType, InApprox> &T_kp1,
                            const Vec6 &twist_k,
                            const Vec6 &twist_kp1,
                            const Mat12 &hat,
                            const Mat12 &candle,
                            Transformation<Other, O_approx> &T_int,
                            Vec6* interpTwist = nullptr);

    template <typename InType, typename Other, bool InApprox, bool O_approx>
    static void interpolateReduced(const Transformation<InType, InApprox> &T_k,
                            const Transformation<InType, InApprox> &T_kp1,
                            const Vec6 &twist_k,
                            const Vec6 &twist_kp1,
                            const Mat2 &hat,
                            const Mat2 &candle,
                            Transformation<Other, O_approx> &T_int,
                            Vec6* interpTwist = nullptr);

    /**
     * Returns an interpolated tranform at time t where t is between t_k and t_kp1.
     * Method is the one proposed in "Full STEAM Ahead" Anderson & Barfoot
     * @param T_k: Transform at time t_k
     * @param T_kp1: Transform at time t_kp1
     * @param twist_k: Twist at time t_k
     * @param twist_kp1: Twist at time t_kp1
     * @param hat: first interpolation factor
     * @param candle: second interpolation factor
     * @return Transformation at time t
     */
    template <typename InType, typename Other, bool InApprox, bool O_approx>
    static void interpolateAndJacobians(const Transformation<InType, InApprox> &T_k,
                                        const Transformation<InType, InApprox> &T_kp1,
                                        const Vec6 &twist_k,
                                        const Vec6 &twist_kp1,
                                        const Mat12 &hat,
                                        const Mat12 &candle,
                                        Transformation<Other, O_approx> &Tint,
                                        Mat6 &J_Tk,
                                        Mat6 &J_Tkp1,
                                        Mat6 &J_twist_k,
                                        Mat6 &J_twist_kp1);

    /** Returns the se3 Lie algebra parameters for this rotation.
     * @return the vector @f$ w @f$ given by
     * @f[
     * w = log(R)_{\times}
     * @f]
     * where @f$ w_\times @f$ is the skew-symmetric matrix form of the Lie
     * algebra parameters @f$ w \in \mathbb{R}^3 @f$.
     */
    Vec6 logMap(double tolerance = 1e-4) const;

    Mat6 adjointRep() const;

    /** Computes the exp map of the input and computes the Jacobian
     * of the exp map wrt @f$ w @f$.
     *
     * @param[in] W The input vector we are taking the expmap of.
     * @param[in]TOL Tolerance to switch between exact and taylor approximation (avoids singularity at zero)
     * @f$ W @f$.
     */
    template <typename VType, typename MType>
    static void expMap(const Eigen::MatrixBase<VType> &W, Eigen::MatrixBase<MType> &retval);

    /** Computes a first order approximation of exp map of the input and computes the Jacobian
     * of the exp map wrt @f$ w @f$.
     *
     * @param[in] W The input vector we are taking the expmap of.
     * @param[in]TOL Tolerance to switch between exact and taylor approximation (avoids singularity at zero)
     * @f$ W @f$.
     */
    template <typename VType, typename MType>
    static void expMap1st(const Eigen::MatrixBase<VType> &W, Eigen::MatrixBase<MType> &retval);

    /**
     * Computes the exponential map of the for the adjoint
     * @param W The input vector we are taking the expmap of.
     * @param TOL Tolerance to switch between exact and taylor approximation (avoids singularity at zero)
     * @return
     */
    static Mat6 expMapAdjoint(const Vec6 &W);

    /** Computes the log map of the input and computes the Jacobian
     * of the log map wrt @f$ R @f$.
     *
     * @param[in] R The input rotation we are taking the log of.
     * @f$ R @f$.
     */
    static Vec6 logMap(const Transformation &T);

    /**
     * Computes the left side Jacobian of SE3
     * @param W The twist location to calculate the jacobian of
     * @param[in]TOL Tolerance to switch between exact and taylor approximation
     * @return Left SE3 jacobian
     */
    static Mat6 SE3LeftJacobian(const Vec6 &W);

    /**
     * Computes the left side Jacobian using a 1st order Taylor Expansion
     * @param W The twist location to calculate the jacobian of
     * @return Left SE3 jacobian
     */
    template<typename Derived, typename OtherDerived>
    static void SE3ApproxLeftJacobian(const Eigen::MatrixBase<Derived> &W, Eigen::MatrixBase<OtherDerived> &J);

    /**
     * Computes the inverse left side Jacobian using a 1st order Taylor Expansion
     * @param W The twist
     * @return Inverse Left SE3 Jacobian
     */
     template<typename OtherDerived>
    static Eigen::Matrix<typename OtherDerived::Scalar, 6, 6> SE3ApproxInvLeftJacobian(const Eigen::MatrixBase<OtherDerived> &W);

    /**
     * Implements ^ operator mapping a vector to a skew symmetric matrix
     * @param V vector
     * @return V^
     */
    template <typename Derived, typename OtherDerived>
    static void skewSymmetric3(const Eigen::MatrixBase<Derived> &V, Eigen::MatrixBase<OtherDerived> &retval);

    template <typename Derived, typename OtherDerived>
    static void skewSymmetric3(const Eigen::MatrixBase<Derived> &V, Eigen::MatrixBase<OtherDerived> &&retval);

    template <typename Derived>
    static Mat3 skewSymmetric3(const Eigen::MatrixBase<Derived> &V);

    template <typename Derived, typename OtherDerived>
    static void skewSymmetric6(const Eigen::MatrixBase<Derived> &W, Eigen::MatrixBase<OtherDerived> &retval);

    /**
     * Returns the approximate Jacobian of an interpolated transform wrt to
     * the end perturbation. Called A(alpha, epsilon) in Barfoot State Estimation
     * twist quantity must be small for approximation to hold
     * @param twist of transform
     * @return A(alpha, twist)
     */
    static void Jinterpolated(const Vec6 &twist, const double &alpha, Mat6 &retval);

    static void Adjoint(const Vec6 &twist, Mat6 &retval);

    /**
     * Returns the "lift" Jacobian for use with Ceres solver
     * @return
     */
    void J_lift(Eigen::Matrix<double, 12, 6> &retval) const;

    /** transforms the input vector by this transformation.
     *
     * @return the vector @f$p_o@f$, the rotated result given by @f[
     * \label{eqn:rot}
     * p_o = R p_i
     * @f]
     * where @f$p_i@f$ is the input vector.
     */
    Vec3 transform(const Vec3 &input_vector) const;

    template <typename IP_T, typename OP_T>
    void transform(const Eigen::MatrixBase<IP_T> &ip_vec, Eigen::MatrixBase<OP_T> &op_vec) const;

    /** Rotates the input vector and calculate Jacobians.
     *
     * @param[in] input_vector The vector to be rotated
     * @param[out] Jpoint Jacobian of rotation function with respect to the
     * point input
     * @param[out] Jparam Jacobian of rotation function with respect to this
     * rotation
     * @return the rotated vector as given by `rotate()`.
     */
    template <typename IP_T, typename OP_T, typename M_T1, typename M_T2>
    void transformAndJacobian(const Eigen::MatrixBase<IP_T> &input_vector,
                              Eigen::MatrixBase<OP_T> &output_vector,
                              Eigen::MatrixBase<M_T1> *Jpoint = nullptr,
                              Eigen::MatrixBase<M_T2> *Jparam = nullptr) const;

    /** transforms the input vector by the inverse of this transformation.
     *
     * @return the vector @f$p_o@f$, given by @f[
     * p_o = T^{-1} p_i
     * @f]
     * where @f$p_i@f$ is the input vector.
     */
    Vec3 inverseTransform(const Vec3 &input_vector) const;

    template <typename IP_T, typename OP_T>
    void inverseTransform(const Eigen::MatrixBase<IP_T> &ip_vec, Eigen::MatrixBase<OP_T> &op_vec) const;

    /** Invert the transformation in place. */
    Transformation &invert();

    /**
     * Return the inverse of the transformation matrix, while preserving original
     * @return the inverse of the transformation object;
     */
    template <typename T_OUT, bool approx>
    void transformInverse(Transformation<T_OUT, approx> &T_inv) const;

    Transformation<Mat34, approximate> transformInverse() const;

    /** Checks if the input transformation is sufficiently close to this transformation
     *
     * The function computes a disparity transformation @f$R_d@f$ as
     * @f[
     * R_d = T_{other} T^{-1}.
     * @f]
     * If the transformations are (approximately) equal, @f$R_d@f$ will be
     * (approximately) the identity matrix,
     * or equivalently, the log map, @f$ w_d = log(T_d) @f$, will be
     * (approximately) zero.
     *
     * @return true if @f$ ||w_d|| \leq \epsilon @f$,
     * where @f$ \epsilon @f$ is the precision given by `comparison_threshold`.
     */
    bool isNear(const Transformation &other, double comparison_threshold) const;

    /** Calculates the result of the manifold-plus operation @f$ \boxplus @f$.
     *
     * @param omega @f$ \omega \in \mathbb{R}^6 @f$, a vector in the tangent
     * space of @f$ T @f$
     * @return The result, @f$ T = T \boxplus \omega @f$
     * @note the @f$ \boxplus @f$ operator uses the **left** composition with
     * the exponential map.
     * @f[
     * \boxplus: \mathbb{SE}(3) \times \mathbb{R}^6 \mapsto \mathbb{SE}(3) \\
     * T \boxplus \omega = exp(\omega)T
     * @f]
     */
    template <typename VType>
    Transformation &manifoldPlus(const Eigen::MatrixBase<VType> &omega);

    /** Calculates the result of the manifold-minus operation @f$ \boxminus @f$.
     *
     * @param T @f$ T \in \mathbb{SE}3 @f$, a transformation
     * @return The result, @f$ \omega = T_t \boxminus T @f$
     * @note the @f$ \boxminus @f$ operator is implemented as follows:
     * @f[
     * \boxminus: \mathbb{SE}(3) \times \mathbb{SE}(3) \mapsto \mathbb{R}^6  \\
     * T_t \boxminus T = log(T_t T^{-1})
     * @f]
     * where @f$ T_t @f$ corresponds to the rotation of **this** object.
     */
    template <typename Other, bool OtherApprox>
    Vec6 manifoldMinus(const Transformation<Other, OtherApprox> &R) const;

    template <typename Other, bool OtherApprox, typename OtherDerived>
    void manifoldMinus(const Transformation<Other, OtherApprox> &R, Eigen::MatrixBase<OtherDerived> &retval) const;

    /** Same as manifoldMinus, except also computes the Jacobians with respect
     * to the left and right arguments of manifoldMinus.  For manifold minus,
     * @f$ \omega = T_t \boxminus T @f$.
     *
     * @param[in] T @f$ T \in \mathbb{SE}3 @f$, a transformation
     * @param[out] J_left The Jacobian of manifoldMinus wrt the left argument,
     * @f$ T_t @f$, which is also **this** transformation.
     * @param[out] J_right The Jacobian of manifoldMinus wrt the right argument,
     * @f$ T @f$, which is also the input transformation.
     * @return The result, @f$ \omega = T_t \boxminus T @f$
     */
    template<typename otherStorage, bool otherApprox>
    Vec6 manifoldMinusAndJacobian(const Transformation<otherStorage, otherApprox> &T, Mat6 *J_left = nullptr, Mat6 *J_right = nullptr) const;

    template <typename Other, typename OutT>
    void compose(const Other &T_Right, OutT &T_out) const;

    /** Composes two transformations and computes the Jacobians wrt the left
     * and right transformations:
     * @f$ T_{out} = T_{left}*T_{right} @f$  Note that **this** transformation
     * corresponds with @f$ T_{left} @f$ and the input transformation is @f$
     * T_{right}@f$.
     * @param[in] T_right @f$ T_{right} \in \mathbb{SE}3 @f$, the input
     * transformation.
     * @param[out] J_left The Jacobian of compose wrt the left argument,
     * @f$ T_{left} @f$ , which is also **this** transformation.
     * @param[out] J_right The Jacobian of compose wrt the right argument,
     * @f$ T_{right} @f$, which is also the input transformation.
     * @return The resulting composition @f$ T_{out} @f$.
     */
    template <typename Other, bool approx = approximate>
    Transformation<T_str, approximate> composeAndJacobian(const Transformation<Other, approx> &T_right,
                                                          Mat6 &J_left,
                                                          Mat6 &J_right) const;

    /** Compute the inverse of **this** transformation and computes the Jacobian
     * of the inverse mapping wrt **this** transformation.
     * @param[out] J_transformation The Jacobian of the inverse mapping wrt **this**
     * transformation.
     * @return The inverse of **this** transformation.
     */
    Transformation inverseAndJacobian(Mat6 &J_transformation) const;

    /** @return the corresponding 3x3 rotation matrix */
    Mat3 getRotationMatrix() const;

    /** @return the 3x1 translation vector */
    Vec3 getTranslation() const;

    /** @return the 4x4 transformation matrix */
    Mat4 getMatrix() const;

    /** Implements transformation composition. */
    template <typename Other, bool OtherApprox>
    Transformation<Mat34, approximate> operator*(const Transformation<Other, OtherApprox> &T) const;

    /** Overload operator for manifold - */
    Vec6 operator-(const Transformation &T) const;
};
}

#include "wave/geometry_og/impl/transformation_impl.hpp"

#endif  // WAVE_TRANSFORMATION_HPP