/////////////////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2021 Andreas Milton Maniotis.
//
// Email: andreas.maniotis@gmail.com
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
/////////////////////////////////////////////////////////////////////////////////////////////

#include "aml/list.hpp"
#include "aml/term_algebra.hpp"

#include <type_traits>

#include <iostream>
#include <boost/core/demangle.hpp>

namespace test::list
{
    template<auto n>
    struct number
    {
        static constexpr auto eval() { return n; }
    };

    void test_head_and_tail()
    {
        using aml::_;

        using list_t = aml::head_and_tail<int, int*, int**, int***>;

        static_assert( std::is_same< list_t::head, int>::value );
        static_assert( std::is_same< aml::power< list_t::tail, _<2> >::return_, aml::head_and_tail<int**, int***> >::value);

        static_assert( std::is_same< aml::head<int, int*, int**>, int >::value );

        static_assert( std::is_same< aml::at< _<0>, int>, int >::value);

        static_assert( std::is_same< aml::at< _<3>, int, int*, int**, int***, int**** >, int***>::value );
        static_assert( std::is_same< aml::at< _<3>, int, int*, int**, int***>, int*** >::value );

        using t3 = aml::list<int*, int**, int***>;

        static_assert( std::is_same< typename t3::head, aml::list_head<t3> >::value );
        static_assert( std::is_same< aml::list_head<t3>, int* >::value );

        static_assert( std::is_same< typename t3::tail, aml::list_tail<t3> >::value );
        static_assert( std::is_same< aml::list_tail<t3>, aml::list<int**, int***> >::value);

        static_assert( std::is_same< typename t3::init, aml::list_init<t3> >::value );
        static_assert( std::is_same< aml::list_init<t3>, aml::list<int*, int**> >::value );

        static_assert( std::is_same< t3::last, aml::list_last<t3> >::value );
        static_assert( std::is_same< aml::list_last<t3>, int*** >::value );
    }

    template< typename... > struct F {};

    void test_apply()
    {
        using f0 = aml::list<>::apply<F>;
        using f1 = aml::list<int*>::apply<F>;
        using f2 = aml::list<int*, int**>::apply<F>;

        using f0_ = aml::list_apply< aml::list<>, aml::function<F> >;
        using f1_ = aml::list_apply< aml::list<int*>, aml::function<F> >;
        using f2_ = aml::list_apply< aml::list<int*, int**>, aml::function<F> >;

        using g0 = aml::list<>::pointwise_apply<F>;
        using g1 = aml::list<int*>::pointwise_apply<F>;
        using g2 = aml::list<int*, int**>::pointwise_apply<F>;

        using g0_ = aml::list_pointwise_apply< aml::list<>, aml::function<F> >;
        using g1_ = aml::list_pointwise_apply< aml::list<int*>, aml::function<F> >;
        using g2_ = aml::list_pointwise_apply< aml::list<int*, int**>, aml::function<F> >;

        static_assert(std::is_same<f0, f0_>::value);
        static_assert(std::is_same<f1, f1_>::value);
        static_assert(std::is_same<f2, f2_>::value);

        static_assert(std::is_same<g0, g0_>::value);
        static_assert(std::is_same<g1, g1_>::value);
        static_assert(std::is_same<g2, g2_>::value);

        static_assert(std::is_same< g0, aml::list<> >::value);
        static_assert(std::is_same< g1, aml::list<F<int*>> >::value);
        static_assert(std::is_same< g2, aml::list<F<int*>, F<int**> > >::value);
    }

    void test_drop_and_take()
    {
        using aml::_;

        using t3 = aml::list<int*, int**, int***>;

        static_assert( std::is_same< t3::drop< _<0> >, t3 >::value );

        static_assert( std::is_same< t3::drop< _<1> >, aml::list<int**, int***> >::value );
        static_assert( std::is_same< t3::drop< _<2> >, aml::list<       int***> >::value );
        static_assert( std::is_same< t3::drop< _<3> >, aml::list<             > >::value );

        static_assert( std::is_same< aml::list<>::take< _<0> >, aml::list<> >::value );

        static_assert( std::is_same< t3::take< _<1> >, aml::list< int*                > >::value );
        static_assert( std::is_same< t3::take< _<2> >, aml::list< int*, int**         > >::value );
        static_assert( std::is_same< t3::take< _<3> >, aml::list< int*, int**, int*** > >::value );
    }

    void test_cons()
    {
        using aml::_;

        using l0 = aml::list<>;
        using l0_a = l0::cons<>;
        using l0_b = l0::rcons<>;

        using l1 = l0::cons< _<1>, _<2>, _<3> >;
        using l1_a = l0::rcons<_<1>, _<2>, _<3> >;

        using l2 = l1::cons<>;
        using l2_a = l1::rcons<>;

        using l3 = l1::rcons< _<4>, _<5> >;
        using l4 = l1::cons< _<-1>, _< 0> >;

        static_assert( std::is_same<l0, l0_a>::value );
        static_assert( std::is_same<l0, l0_b>::value );
        static_assert( std::is_same<l1, l1_a>::value );

        static_assert( std::is_same<l2, l2_a>::value );
        static_assert( std::is_same<l3, aml::list< _<1>, _<2>, _<3>, _<4>, _<5> > >::value );

        static_assert( std::is_same<l4, aml::list< _<-1>, _<-0>,  _<1>, _<2>, _<3> > >::value );

        using l5 = aml::list_cons< aml::list< _<1>, _<2>, _<3> >  >;
        using l6 = aml::list_rcons< aml::list< _<1>, _<2>, _<3> > >;

        using l7 = aml::list_rcons< aml::list< _<1>, _<2>, _<3> >, _<4>, _<5>  >;
        using l8 = aml::list_cons< aml::list< _<1>, _<2>, _<3> >, _<-1>, _<0>  >;

        static_assert( std::is_same< l5, aml::list< _<1>, _<2>, _<3> > >::value );
        static_assert( std::is_same< l5, l6 >::value );

        static_assert( std::is_same< l7, aml::list< _<1>, _<2>, _<3>, _<4>, _<5> > >::value );
        static_assert( std::is_same< l8, aml::list< _<-1>, _<0>, _<1>, _<2>, _<3> > >::value );
    }

    void test_reverse()
    {
        using aml::_;

        using l1 = aml::list<>;
        using l2 = aml::list<>::reverse;
        using l3 = aml::list_reverse< aml::list<> >;

        using l4 = aml::list< _<1> >;
        using l5 = aml::list< _<1> >::reverse;
        using l6 = aml::list_reverse< l4 >;

        using l7 = aml::list< _<1>, _<2> >;
        using l8 = l7::reverse;
        using l9 = aml::list_reverse< l7 >;

        static_assert( std::is_same< l1, l2 >::value );
        static_assert( std::is_same< l1, l3 >::value );

        static_assert( std::is_same< l4, l5 >::value );
        static_assert( std::is_same< l4, l6 >::value );

        static_assert( std::is_same< l8, l9 >::value );

        static_assert( std::is_same< l8::reverse, l7 >::value );
    }

    template<typename List, typename X>
    using G = typename List::template rcons<  aml::num< -X::eval() >  >;

    void test_fold_left()
    {
        using aml::_;

        using l0  =  aml::list<>;
        using l1  =  aml::list< _<1> >;
        using l2  =  aml::list< _<1>, _<2> >;
        using l4  =  aml::list< _<1>, _<2>, _<3>, _<4> >;

        using f0  =  l0::lfold_with< G, aml::list<> >;
        using r0  =  aml::list< >;

        static_assert( std::is_same< f0, r0 >::value );

        using f1  =  l1::lfold_with< G, aml::list<> >;
        using r1  =  aml::list< _<-1> >;

        static_assert( std::is_same< f1, r1 >::value );

        using f2  =  l2::lfold_with< G, aml::list<> >;
        using r2  =  aml::list< _<-1>, _<-2> >;

        static_assert( std::is_same< f2, r2 >::value );

        using f4  =  l4::lfold_with< G, aml::list<> >;
        using r4  =  aml::list< _<-1>, _<-2>, _<-3>, _<-4> >;

        static_assert( std::is_same< f4, r4 >::value );
    }

    template<typename X, typename List>
    using H  =  typename List::template cons<  aml::num< -X::eval() >  >;

    void test_fold_right()
    {
        using aml::_;

        using l0  =  aml::list<>;
        using l1  =  aml::list< _<1> >;
        using l2  =  aml::list< _<1>, _<2> >;
        using l4  =  aml::list< _<1>, _<2>, _<3>, _<4> >;

        using f0  =  l0::rfold_with< H, aml::list<> >;
        using r0  =  aml::list< >;

        static_assert( std::is_same< f0, r0 >::value );

        using f1  =  l1::rfold_with< H, aml::list<> >;
        using r1  =  aml::list< _<-1> >;

        static_assert( std::is_same< f1, r1 >::value );

        using f2  =  l2::rfold_with< H, aml::list<> >;
        using r2  =  aml::list< _<-1>, _<-2> >;

        static_assert( std::is_same< f2, r2 >::value );

        using f4  =  l4::rfold_with< H, aml::list<> >;
        using r4  =  aml::list< _<-1>, _<-2>, _<-3>, _<-4> >;

        static_assert( std::is_same< f4, r4 >::value );
    }

    void test_scan_left()
    {
        using aml::_;

        using l0  =  aml::list<>;
        using l1  =  aml::list< _<1> >;
        using l2  =  aml::list< _<1>, _<2> >;
        using l4  =  aml::list< _<1>, _<2>, _<3>, _<4> >;

        using f0  =  l0::lscan_with< G, aml::list<> >;
        using r0  =  aml::list< aml::list<> >;

        static_assert( std::is_same< f0, r0 >::value );

        using f1  =  l1::lscan_with< G, aml::list<> >;
        using r1  =  aml::list<  aml::list<>,  aml::list< _<-1> >  >;

        static_assert( std::is_same< f1, r1 >::value );

        using f2  =  l2::lscan_with< G, aml::list<> >;
        using r2  =  aml::list< aml::list<>, aml::list< _<-1> >, aml::list< _<-1>, _<-2> > >;

        static_assert( std::is_same< f2, r2 >::value );

        using f4  =  l4::lscan_with< G, aml::list<> >;
        using r4  =  aml::list<  aml::list<>,
                                 aml::list< _<-1> >,
                                 aml::list< _<-1>, _<-2> >,
                                 aml::list< _<-1>, _<-2>, _<-3> >,
                                 aml::list< _<-1>, _<-2>, _<-3>, _<-4> >  >;

        static_assert( std::is_same< f4, r4 >::value );
    }

    void test_scan_right()
    {
        using aml::_;

        using l0  =  aml::list<>;
        using l1  =  aml::list< _<1> >;
        using l2  =  aml::list< _<1>, _<2> >;
        using l4  =  aml::list< _<1>, _<2>, _<3>, _<4> >;

        using f0  =  l0::rscan_with< H, aml::list<> >;
        using r0  =  aml::list< aml::list<> >;

        static_assert( std::is_same< f0, r0 >::value );

        using f1  =  l1::rscan_with< H, aml::list<> >;
        using r1  =  aml::list<  aml::list<>,  aml::list< _<-1> >  >::reverse;

        static_assert( std::is_same< f1, r1 >::value );

        using f2  =  l2::rscan_with< H, aml::list<> >;
        using r2  =  aml::list<  aml::list< _<-1>, _<-2> >,
                                 aml::list< _<-2> > ,
                                 aml::list<>  >;

        static_assert( std::is_same< f2, r2 >::value );

        using f4  =  l4::rscan_with< H, aml::list<> >;
        using r4  =  aml::list<  aml::list<  _<-1>, _<-2>, _<-3>, _<-4>  >,
                                 aml::list<         _<-2>, _<-3>, _<-4>  >,
                                 aml::list<                _<-3>, _<-4>  >,
                                 aml::list<                       _<-4>  >,
                                 aml::list<                              >   >;

        static_assert( std::is_same< f4, r4 >::value );

    }

    template<typename B, typename A>
    using multiply = aml::list< aml::num< B::eval() * A::eval() >, aml::num< -A::eval() > >;

    void test_map_accum_left()
    {

        using aml::_;

        using macl_0  =  aml::list<>::map_accum_left_with<multiply, _<1> >;

        static_assert( std::is_same<macl_0, aml::list< _<1>, aml::list<>  > >::value );

        using macl_1  =  aml::list< _<1> >::map_accum_left_with< multiply, _<1> >;

        static_assert( std::is_same<macl_1, aml::list< _<1>, aml::list<_<-1> >  > >::value );

        using macl_2  =  aml::list< _<1>, _<2> >::map_accum_left_with< multiply, _<1> >;

        static_assert( std::is_same<macl_2, aml::list< _<2>, aml::list<_<-1>, _<-2> >  > >::value );

        using macl_3  =  aml::list< _<1>, _<2>, _<3>  >::map_accum_left_with< multiply, _<1> >;

        static_assert( std::is_same<macl_3, aml::list< _<6>, aml::list<_<-1>, _<-2>, _<-3> >  > >::value );
    }

    void test_map_accum_right()
    {

        using aml::_;

        using macr_0  =  aml::list<>::map_accum_right_with<multiply, _<1> >;

        static_assert( std::is_same<macr_0, aml::list< _<1>, aml::list<>  > >::value );

        using macr_1  =  aml::list< _<1> >::map_accum_right_with< multiply, _<1> >;

        static_assert( std::is_same<macr_1, aml::list< _<1>, aml::list<_<-1> >  > >::value );

        using macr_2  =  aml::list< _<1>, _<2> >::map_accum_right_with< multiply, _<1> >;

        static_assert( std::is_same<macr_2, aml::list< _<2>, aml::list<_<-2>, _<-1> >  > >::value );

        using macr_3  =  aml::list< _<1>, _<2>, _<3>  >::map_accum_right_with< multiply, _<1> >;

        static_assert( std::is_same<macr_3, aml::list< _<6>, aml::list<_<-3>, _<-2>, _<-1> >  > >::value );
    }

    template<typename N>
    using is_even = aml::bool_< (N::eval() & 1) == 0 >;

    void test_partition()
    {
        using aml::_;

        using l0    =  aml::list<>::partition_with<is_even>;
        using l1_a  =  aml::list< _<1> >::partition_with<is_even>;
        using l1_b  =  aml::list< _<2> >::partition_with<is_even>;
        using l2_a  =  aml::list< _<1>, _<2> >::partition_with<is_even>;
        using l2_b  =  aml::list< _<1>, _<3> >::partition_with<is_even>;
        using l2_c  =  aml::list< _<1>, _<4> >::partition_with<is_even>;
        using l2_d  =  aml::list< _<2>, _<4> >::partition_with<is_even>;

        using l4    =  aml::list< _<1>, _<2>, _<4>, _<3> >::partition_with<is_even>;

        using a0    =  aml::list<>;
        using r0    =  aml::list<>;

        using a1_a  =  aml::list<>;
        using r1_a  =  aml::list< _<1> >;

        using a1_b  =  aml::list< _<2> >;
        using r1_b  =  aml::list<>;

        using a2_a  =  aml::list< _<2> >;
        using r2_a  =  aml::list< _<1> >;

        using a2_b  =  aml::list<>;
        using r2_b  =  aml::list< _<1>, _<3> >;

        using a2_c  =  aml::list< _<4> >;
        using r2_c  =  aml::list< _<1> >;

        using a2_d  =  aml::list< _<2>, _<4> >;
        using r2_d  =  aml::list< >;

        static_assert( std::is_same< a0, l0::accepted >::value);
        static_assert( std::is_same< r0, l0::rejected >::value);

        static_assert( std::is_same< a1_a, l1_a::accepted >::value);
        static_assert( std::is_same< r1_a, l1_a::rejected >::value);

        static_assert( std::is_same< a1_b, l1_b::accepted >::value);
        static_assert( std::is_same< r1_b, l1_b::rejected >::value);

        static_assert( std::is_same< a2_a, l2_a::accepted >::value);
        static_assert( std::is_same< r2_a, l2_a::rejected >::value);

        static_assert( std::is_same< a2_b, l2_b::accepted >::value);
        static_assert( std::is_same< r2_b, l2_b::rejected >::value);

        static_assert( std::is_same< a2_c, l2_c::accepted >::value);
        static_assert( std::is_same< r2_c, l2_c::rejected >::value);

        static_assert( std::is_same< a2_d, l2_d::accepted >::value);
        static_assert( std::is_same< r2_d, l2_d::rejected >::value);

        static_assert( std::is_same< l4::accepted, aml::list< _<2>, _<4> > >::value);
        static_assert( std::is_same< l4::rejected, aml::list< _<1>, _<3> > >::value);

    }

    void test_split_by_first_occurence_of()
    {
        using aml::_;

        using s0    =  aml::list<>::split_by_first_occurence_of< is_even >;
        using s1_a  =  aml::list< _<2> >::split_by_first_occurence_of< is_even >;
        using s1_r  =  aml::list< _<1> >::split_by_first_occurence_of< is_even >;

        using s2    =  aml::list< _<1>, _<2> >::split_by_first_occurence_of<is_even>;
        using s3    =  aml::list< _<2>, _<1> >::split_by_first_occurence_of<is_even>;

        using s4    =  aml::list< _<1>, _<3>, _<2>, _<5> >::split_by_first_occurence_of<is_even>;

        static_assert( std::is_same<s0::prefix, aml::list<> >::value );
        static_assert( std::is_same<s0::suffix, aml::list<> >::value );

        static_assert( std::is_same< s1_a::prefix, aml::list<> >::value );
        static_assert( std::is_same< s1_a::suffix, aml::list<_<2>> >::value );

        static_assert( std::is_same< s1_r::prefix, aml::list<_<1>> >::value );
        static_assert( std::is_same< s1_r::suffix, aml::list< > >::value );

        static_assert( std::is_same< s2::prefix, aml::list< _<1> > >::value );
        static_assert( std::is_same< s2::suffix, aml::list< _<2> > >::value );

        static_assert( std::is_same< s3::prefix, aml::list<> >::value );
        static_assert( std::is_same< s3::suffix, aml::list< _<2>, _<1> > >::value );

        static_assert( std::is_same< s4::prefix, aml::list< _<1>, _<3> > >::value );
        static_assert( std::is_same< s4::suffix, aml::list< _<2>, _<5> > >::value );
    }

    void test_take_and_drop_while()
    {
        using aml::_;

        using l0 = aml::list<>;
        using l1 = aml::list< _<1> >;
        using l2 = aml::list< _<0> >;
        using l3 = aml::list< _<0>, _<1> >;
        using l4 = aml::list< _<1>, _<0> >;
        using l5 = aml::list< _<0>, _<2>, _<4>, _<3>, _<6> >;

        using t0  = l0::take_while< is_even >;
        using t1  = l1::take_while< is_even >;
        using t2  = l2::take_while< is_even >;
        using t3  = l3::take_while< is_even >;
        using t4  = l4::take_while< is_even >;
        using t5  = l5::take_while< is_even >;

        using d0  = l0::drop_while< is_even >;
        using d1  = l1::drop_while< is_even >;
        using d2  = l2::drop_while< is_even >;
        using d3  = l3::drop_while< is_even >;
        using d4  = l4::drop_while< is_even >;
        using d5  = l5::drop_while< is_even >;

        static_assert( std::is_same< t0, aml::list<> >::value );
        static_assert( std::is_same< t1, aml::list<> >::value );
        static_assert( std::is_same< t2, aml::list<_<0>>>::value );
        static_assert( std::is_same< t3, aml::list<_<0>> >::value );
        static_assert( std::is_same< t4, aml::list< > >::value );
        static_assert( std::is_same< t5, aml::list< _<0>, _<2>, _<4> > >::value );

        static_assert( std::is_same< d0, aml::list<> >::value );
        static_assert( std::is_same< d1, aml::list<_<1>> >::value );
        static_assert( std::is_same< d2, aml::list<> >::value );
        static_assert( std::is_same< d3, aml::list<_<1>> >::value );
        static_assert( std::is_same< d4, aml::list< _<1>, _<0> > >::value );
        static_assert( std::is_same< d5, aml::list< _<3>, _<6> > >::value );
    }

    //    template< typename X, typename Y, typename... >
    //    template<typename X, typename Y, typename...>
    //    using less = aml::bool_< (X::eval() < Y::eval()) >;

    template<typename X, typename Y>
    using less  = aml::bool_< (X::eval() < Y::eval()) >;

    template<typename... X>
    using size_less  =  aml::bool_< (sizeof( aml::at< aml::_<0>, X...>) < sizeof( aml::at< aml::_<1>, X...>)) >;

    void test_sort()
    {

        using aml::_;

        using unsorted_5 = aml::list< _<3>, _<2>, _<1>, _<4>, _<0> >;

        using sorted_5 = unsorted_5::sort_with<less>;
        static_assert( std::is_same< sorted_5, aml::list< _<0>, _<1>, _<2>, _<3>, _<4> > >::value );

        static_assert( std::is_same< aml::list<>::sort_with<less>, aml::list<> >::value );

        // test stability
        using list  =  aml::list< unsigned char, unsigned int, int, signed char, unsigned int, int, unsigned int>;

        using expected_sorting  =  aml::list< unsigned char, signed char,
                                              unsigned int, int, unsigned int, int, unsigned int >;

        using acquired_sorting = list::sort_with<size_less>;

        static_assert( std::is_same< acquired_sorting, expected_sorting >::value );

        static_assert( std::is_same< aml::list_sort<list, aml::function<size_less> >, expected_sorting>::value );

    }

}

#include <iostream>
#include <string>

int main()
{

    void (*test_set[])() =
    {
        test::list::test_head_and_tail,
        test::list::test_apply,
        test::list::test_drop_and_take,
        test::list::test_cons,
        test::list::test_reverse,
        test::list::test_fold_left,
        test::list::test_fold_right,
        test::list::test_scan_left,
        test::list::test_scan_right,
        test::list::test_map_accum_left,
        test::list::test_map_accum_right,
        test::list::test_partition,
        test::list::test_split_by_first_occurence_of,
        test::list::test_take_and_drop_while,
        test::list::test_sort

    };

    for ( auto test : test_set )
        test();

    std::cout << __FILE__ << ": " << sizeof(test_set)/sizeof(test_set[0])  << " tests passed." << std::endl;

}


#include <cstdlib>
#include <iostream>
#include <memory>
#include <random>
#include <algorithm>
#include <utility>
#include <string>
#include <sstream>
#include <boost/asio.hpp>

using namespace std;
using boost::asio::ip::tcp;

namespace std {
    template<typename T>
    std::string to_string(const T &n) {
        std::ostringstream stm;
        stm << n;
        return stm.str();
    }
}

class session : public std::enable_shared_from_this<session> {
public:
    session(tcp::socket socket) : socket_(move(socket)), mt(rd()), dist(1, 49) {
    }

    void start() {
        generateLotteryNumbers();
        do_write();
    }

private:
    void generateLotteryNumbers() {
        numbers.empty();
        while (numbers.size() < 6) {
            int candidate = dist(mt);
            if (find(numbers.begin(), numbers.end(), candidate) == numbers.end())
                numbers.push_back(candidate);
        }
        sort(numbers.begin(), numbers.end());
        _buffer.empty();
        for (auto x: numbers) {
            _buffer.append(std::to_string(x)).append(",");
        }
        _buffer.erase(_buffer.begin() + _buffer.size() - 1);
        _buffer.append("\n");
    }

    void do_write() {
        auto self(shared_from_this());
        boost::asio::async_write(socket_, boost::asio::buffer(_buffer.c_str(), _buffer.size()),
                                 [this, self](boost::system::error_code ec, std::size_t) {
                                 });
    }

    tcp::socket socket_;
    enum {
        max_length = 1024
    };
    char data_[max_length];
    string _buffer;
    random_device rd;
    mt19937 mt;
    uniform_int_distribution<int> dist;
    vector<int> numbers;
};

class server {
public:
    server(boost::asio::io_service &io_service, short port)
            : acceptor_(io_service, tcp::endpoint(tcp::v4(), port)),
              socket_(io_service) {
        do_accept();
    }

private:
    void do_accept() {
        acceptor_.async_accept(socket_,
                               [this](boost::system::error_code ec) {
                                   if (!ec) {
                                       std::make_shared<session>(std::move(socket_))->start();
                                   }

                                   do_accept();
                               });
    }

    tcp::acceptor acceptor_;
    tcp::socket socket_;
};

int main(int argc, char *argv[]) {
    try {
        boost::asio::io_service io_service;
        server s(io_service, std::atoi("7001"));
        cerr << "Server is running at port 7001" << endl;
        io_service.run();
    }
    catch (std::exception &e) {
        std::cerr << "Exception: " << e.what() << endl;
    }

    return 0;
}


/*
 * Copyright (C) 2015, Nils Moehrle, Michael Waechter
 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
 */

#include <numeric>

#include <mve/image_color.h>
#include <acc/bvh_tree.h>
#include <Eigen/Core>
#include <Eigen/LU>

#include "util.h"
#include "histogram.h"
#include "texturing.h"
#include "sparse_table.h"
#include "progress_counter.h"

typedef acc::BVHTree<unsigned int, math::Vec3f> BVHTree;

TEX_NAMESPACE_BEGIN

/**
 * Dampens the quality of all views in which the face's projection
 * has a much different color than in the majority of views.
 * Returns whether the outlier removal was successfull.
 *
 * @param infos contains information about one face seen from several views
 * @param settings runtime configuration.
 */
bool
photometric_outlier_detection(std::vector<FaceProjectionInfo> * infos, Settings const & settings) {
    if (infos->size() == 0) return true;

    /* Configuration variables. */

    double const gauss_rejection_threshold = 6e-3;

    /* If all covariances drop below this we stop outlier detection. */
    double const minimal_covariance = 5e-4;

    int const outlier_detection_iterations = 10;
    int const minimal_num_inliers = 4;

    float outlier_removal_factor = std::numeric_limits<float>::signaling_NaN();
    switch (settings.outlier_removal) {
        case OUTLIER_REMOVAL_NONE: return true;
        case OUTLIER_REMOVAL_GAUSS_CLAMPING:
            outlier_removal_factor = 1.0f;
        break;
        case OUTLIER_REMOVAL_GAUSS_DAMPING:
            outlier_removal_factor = 0.2f;
        break;
    }

    Eigen::MatrixX3d inliers(infos->size(), 3);
    std::vector<std::uint32_t> is_inlier(infos->size(), 1);
    for (std::size_t row = 0; row < infos->size(); ++row) {
        inliers.row(row) = mve_to_eigen(infos->at(row).mean_color).cast<double>();
    }

    Eigen::RowVector3d var_mean;
    Eigen::Matrix3d covariance;
    Eigen::Matrix3d covariance_inv;

    for (int i = 0; i < outlier_detection_iterations; ++i) {

        if (inliers.rows() < minimal_num_inliers) {
            return false;
        }

        /* Calculate the inliers' mean color and color covariance. */
        var_mean = inliers.colwise().mean();
        Eigen::MatrixX3d centered = inliers.rowwise() - var_mean;
        covariance = (centered.adjoint() * centered) / double(inliers.rows() - 1);

        /* If all covariances are very small we stop outlier detection
         * and only keep the inliers (set quality of outliers to zero). */
        if (covariance.array().abs().maxCoeff() < minimal_covariance) {
            for (std::size_t row = 0; row < infos->size(); ++row) {
                if (!is_inlier[row]) infos->at(row).quality = 0.0f;
            }
            return true;
        }

        /* Invert the covariance. FullPivLU is not the fastest way but
         * it gives feedback about numerical stability during inversion. */
        Eigen::FullPivLU<Eigen::Matrix3d> lu(covariance);
        if (!lu.isInvertible()) {
            return false;
        }
        covariance_inv = lu.inverse();

        /* Compute new number of inliers (all views with a gauss value above a threshold). */
        for (std::size_t row = 0; row < infos->size(); ++row) {
            Eigen::RowVector3d color = mve_to_eigen(infos->at(row).mean_color).cast<double>();
            double gauss_value = multi_gauss_unnormalized<double,3>(color, var_mean, covariance_inv);
            is_inlier[row] = (gauss_value >= gauss_rejection_threshold ? 1 : 0);
        }
        /* Resize Eigen matrix accordingly and fill with new inliers. */
        inliers.resize(std::accumulate(is_inlier.begin(), is_inlier.end(), 0), Eigen::NoChange);
        for (std::size_t row = 0, inlier_row = 0; row < infos->size(); ++row) {
            if (is_inlier[row]) {
                inliers.row(inlier_row++) = mve_to_eigen(infos->at(row).mean_color).cast<double>();
            }
        }
    }

    covariance_inv *= outlier_removal_factor;
    for (FaceProjectionInfo & info : *infos) {
        Eigen::RowVector3d color = mve_to_eigen(info.mean_color).cast<double>();
        double gauss_value = multi_gauss_unnormalized<double,3>(color, var_mean, covariance_inv);
        assert(0.0 <= gauss_value && gauss_value <= 1.0);
        switch(settings.outlier_removal) {
            case OUTLIER_REMOVAL_NONE: return true;
            case OUTLIER_REMOVAL_GAUSS_DAMPING:
                info.quality *= gauss_value;
            break;
            case OUTLIER_REMOVAL_GAUSS_CLAMPING:
                if (gauss_value < gauss_rejection_threshold) info.quality = 0.0f;
            break;
        }
    }
    return true;
}

void
calculate_face_projection_infos(mve::TriangleMesh::ConstPtr mesh,
    std::vector<TextureView> * texture_views, Settings const & settings,
    FaceProjectionInfos * face_projection_infos) {

    std::vector<unsigned int> const & faces = mesh->get_faces();
    std::vector<math::Vec3f> const & vertices = mesh->get_vertices();
    mve::TriangleMesh::NormalList const & face_normals = mesh->get_face_normals();

    std::size_t const num_views = texture_views->size();

    util::WallTimer timer;
    std::cout << "\tBuilding BVH from " << faces.size() / 3 << " faces... " << std::flush;
    BVHTree bvh_tree(faces, vertices);
    std::cout << "done. (Took: " << timer.get_elapsed() << " ms)" << std::endl;

    ProgressCounter view_counter("\tCalculating face qualities", num_views);
    #pragma omp parallel
    {
        std::vector<std::pair<std::size_t, FaceProjectionInfo> > projected_face_view_infos;

        #pragma omp for schedule(dynamic)
        for (std::int32_t j = 0; j < static_cast<std::uint16_t>(num_views); ++j) {
            view_counter.progress<SIMPLE>();

            TextureView * texture_view = &texture_views->at(j);
            texture_view->load_image();
            texture_view->generate_validity_mask();

            if (settings.data_term == DATA_TERM_GMI) {
                texture_view->generate_gradient_magnitude();
                texture_view->erode_validity_mask();
            }

            math::Vec3f const & view_pos = texture_view->get_pos();
            math::Vec3f const & viewing_direction = texture_view->get_viewing_direction();

            for (std::size_t i = 0; i < faces.size(); i += 3) {
                std::size_t face_id = i / 3;

                math::Vec3f const & v1 = vertices[faces[i]];
                math::Vec3f const & v2 = vertices[faces[i + 1]];
                math::Vec3f const & v3 = vertices[faces[i + 2]];
                math::Vec3f const & face_normal = face_normals[face_id];
                math::Vec3f const face_center = (v1 + v2 + v3) / 3.0f;

                /* Check visibility and compute quality */

                math::Vec3f view_to_face_vec = (face_center - view_pos).normalized();
                math::Vec3f face_to_view_vec = (view_pos - face_center).normalized();

                /* Backface and basic frustum culling */
                float viewing_angle = face_to_view_vec.dot(face_normal);
                if (viewing_angle < 0.0f || viewing_direction.dot(view_to_face_vec) < 0.0f)
                    continue;

                float cutoffAngle = settings.geometric_visibility_test ? 75.0f : 90.0f;
                if (std::acos(viewing_angle) > MATH_DEG2RAD(cutoffAngle))
                    continue;

                /* Projects into the valid part of the TextureView? */
                if (!texture_view->inside(v1, v2, v3))
                    continue;

                if (settings.geometric_visibility_test) {
                    /* Viewing rays do not collide? */
                    bool visible = true;
                    math::Vec3f const * samples[] = {&v1, &v2, &v3};
                    // TODO: random monte carlo samples...

                    for (std::size_t k = 0; k < sizeof(samples) / sizeof(samples[0]); ++k) {
                        BVHTree::Ray ray;
                        ray.origin = *samples[k];
                        ray.dir = view_pos - ray.origin;
                        ray.tmax = ray.dir.norm();
                        ray.tmin = ray.tmax * 0.0001f;
                        ray.dir.normalize();

                        BVHTree::Hit hit;
                        if (bvh_tree.intersect(ray, &hit)) {
                            visible = false;
                            break;
                        }
                    }
                    if (!visible) continue;
                }

                FaceProjectionInfo info = {j, 0.0f, math::Vec3f(0.0f, 0.0f, 0.0f)};

                /* Calculate quality. */
                texture_view->get_face_info(v1, v2, v3, &info, settings);

                if (info.quality == 0.0) continue;

                /* Change color space. */
                mve::image::color_rgb_to_ycbcr(*(info.mean_color));

                std::pair<std::size_t, FaceProjectionInfo> pair(face_id, info);
                projected_face_view_infos.push_back(pair);
            }

            texture_view->release_image();
            texture_view->release_validity_mask();
            if (settings.data_term == DATA_TERM_GMI) {
                texture_view->release_gradient_magnitude();
            }
            view_counter.inc();
        }

        //std::sort(projected_face_view_infos.begin(), projected_face_view_infos.end());

        #pragma omp critical
        {
            for (std::size_t i = projected_face_view_infos.size(); 0 < i; --i) {
                std::size_t face_id = projected_face_view_infos[i - 1].first;
                FaceProjectionInfo const & info = projected_face_view_infos[i - 1].second;
                face_projection_infos->at(face_id).push_back(info);
            }
            projected_face_view_infos.clear();
        }
    }
}

void
postprocess_face_infos(Settings const & settings,
        FaceProjectionInfos * face_projection_infos,
        DataCosts * data_costs) {

    ProgressCounter face_counter("\tPostprocessing face infos",
        face_projection_infos->size());
    #pragma omp parallel for schedule(dynamic)
    for (std::int64_t i = 0; i < face_projection_infos->size(); ++i) {
        face_counter.progress<SIMPLE>();

        std::vector<FaceProjectionInfo> & infos = face_projection_infos->at(i);
        if (settings.outlier_removal != OUTLIER_REMOVAL_NONE) {
            photometric_outlier_detection(&infos, settings);

            infos.erase(std::remove_if(infos.begin(), infos.end(),
                [](FaceProjectionInfo const & info) -> bool {return info.quality == 0.0f;}),
                infos.end());
        }
        std::sort(infos.begin(), infos.end());

        face_counter.inc();
    }

    /* Determine the function for the normlization. */
    float max_quality = 0.0f;
    for (std::size_t i = 0; i < face_projection_infos->size(); ++i)
        for (FaceProjectionInfo const & info : face_projection_infos->at(i))
            max_quality = std::max(max_quality, info.quality);

    Histogram hist_qualities(0.0f, max_quality, 10000);
    for (std::size_t i = 0; i < face_projection_infos->size(); ++i)
        for (FaceProjectionInfo const & info : face_projection_infos->at(i))
            hist_qualities.add_value(info.quality);

    float percentile = hist_qualities.get_approx_percentile(0.995f);

    /* Calculate the costs. */
    for (std::uint32_t i = 0; i < face_projection_infos->size(); ++i) {
        for (FaceProjectionInfo const & info : face_projection_infos->at(i)) {

            /* Clamp to percentile and normalize. */
            float normalized_quality = std::min(1.0f, info.quality / percentile);
            float data_cost = (1.0f - normalized_quality);
            data_costs->set_value(i, info.view_id, data_cost);
        }

        /* Ensure that all memory is freeed. */
        face_projection_infos->at(i) = std::vector<FaceProjectionInfo>();
    }

    std::cout << "\tMaximum quality of a face within an image: " << max_quality << std::endl;
    std::cout << "\tClamping qualities to " << percentile << " within normalization." << std::endl;
}

void
calculate_data_costs(mve::TriangleMesh::ConstPtr mesh, std::vector<TextureView> * texture_views,
    Settings const & settings, DataCosts * data_costs) {

    std::size_t const num_faces = mesh->get_faces().size() / 3;
    std::size_t const num_views = texture_views->size();

    if (num_faces > std::numeric_limits<std::uint32_t>::max())
        throw std::runtime_error("Exeeded maximal number of faces");
    if (num_views > std::numeric_limits<std::uint16_t>::max())
        throw std::runtime_error("Exeeded maximal number of views");

    FaceProjectionInfos face_projection_infos(num_faces);
    calculate_face_projection_infos(mesh, texture_views, settings, &face_projection_infos);
    postprocess_face_infos(settings, &face_projection_infos, data_costs);
}

TEX_NAMESPACE_END


// An example showing TEASER++ registration with the Stanford bunny model
// std
#include <iostream>

// teaser dependencies
#include <Eigen/Core>
#include <teaser/registration.h>

// visualization
#include "polyscope/polyscope.h"
#include "polyscope/point_cloud.h"

// IO
#include "IO/readPLY.hpp"

// Macro constants for generating noise and outliers
#define NOISE_BOUND 0.05
#define N_OUTLIERS 1700
#define OUTLIER_TRANSLATION_LB 5
#define OUTLIER_TRANSLATION_UB 10

int main() {

    Eigen::MatrixXd src_V, trg_V;
    Eigen::MatrixXi F;

    readPLY("../data/1000point_model.ply", src_V, F);
    readPLY("../data/1000point_scene.ply", trg_V, F);

    std::cout << "Size of src_V: [" << src_V.rows() << ", " << src_V.cols() << "]\n";
    std::cout << "Size of trg_V: [" << trg_V.rows() << ", " << trg_V.cols() << "]\n";

    // polyscope init
    polyscope::init();

    // plot before registration
    polyscope::registerPointCloud("source", src_V.transpose());
    polyscope::registerPointCloud("target", trg_V.transpose());
    polyscope::show();

    // Run TEASER++ registration
    // Prepare solver parameters
    teaser::RobustRegistrationSolver::Params params;
    params.noise_bound = 0.0337;
    params.cbar2 = 1;
    params.estimate_scaling = false;
    params.rotation_max_iterations = 100;
    params.rotation_gnc_factor = 1.4;
    params.rotation_estimation_algorithm =
        teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS; // GNC_TLS or FGR
    params.rotation_cost_threshold = 0.005;

    // Solve with TEASER++
    teaser::RobustRegistrationSolver solver(params);
    solver.solve(src_V, trg_V);

    auto solution = solver.getSolution();

    // extract solution
    double scale = solution.scale;
    Eigen::MatrixXd rot = solution.rotation;
    Eigen::VectorXd tra = solution.translation;

    // perform rotation and translation
    src_V = rot  * src_V;
    src_V = src_V.colwise() + tra;

    // plot after registration
    polyscope::registerPointCloud("source", src_V.transpose());
    polyscope::registerPointCloud("target", trg_V.transpose());
    polyscope::show();

}


//
// $Id$
//
//
// Original author: Darren Kessner <darren@proteowizard.org>
//
// Copyright 2006 Louis Warschaw Prostate Cancer Center
//   Cedars Sinai Medical Center, Los Angeles, California  90048
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#include "LinearSolver.hpp"
#include "pwiz/utility/misc/unit.hpp"
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include "pwiz/utility/misc/Std.hpp"
#include <cstring>

using namespace pwiz::util;
using namespace pwiz::math;

namespace ublas = boost::numeric::ublas;

ostream* os_ = 0;

void testDouble()
{
    if (os_) *os_ << "testDouble()\n";

    LinearSolver<> solver;

    ublas::matrix<double> A(2,2);
    A(0,0) = 1; A(0,1) = 2;
    A(1,0) = 3; A(1,1) = 4;

    ublas::vector<double> y(2);
    y(0) = 5;
    y(1) = 11;

    ublas::vector<double> x = solver.solve(A, y);

    if (os_) *os_ << "A: " << A << endl;
    if (os_) *os_ << "y: " << y << endl;
    if (os_) *os_ << "x: " << x << endl;

    unit_assert(x(0) == 1.);
    unit_assert(x(1) == 2.);
}

void testComplex()
{
    if (os_) *os_ << "testComplex()\n";

    LinearSolver<> solver;

    ublas::matrix< complex<double> > A(2,2);
    A(0,0) = 1; A(0,1) = 2;
    A(1,0) = 3; A(1,1) = 4;

    ublas::vector< complex<double> > y(2);
    y(0) = 5;
    y(1) = 11;

    ublas::vector< complex<double> > x = solver.solve(A, y);

    if (os_) *os_ << "A: " << A << endl;
    if (os_) *os_ << "y: " << y << endl;
    if (os_) *os_ << "x: " << x << endl;

    unit_assert(x(0) == 1.);
    unit_assert(x(1) == 2.);
}

void testDoubleQR()
{
    if (os_) *os_ << "testDoubleQR()\n";

    LinearSolver<LinearSolverType_QR> solver;

    ublas::matrix<double> A(2,2);
    A(0,0) = 1.; A(0,1) = 2.;
    A(1,0) = 3.; A(1,1) = 4.;

    ublas::vector<double> y(2);
    y(0) = 5.;
    y(1) = 11.;

    ublas::vector<double> x = solver.solve(A, y);

    if (os_) *os_ << "A: " << A << endl;
    if (os_) *os_ << "y: " << y << endl;
    if (os_) *os_ << "x: " << x << endl;

    if (os_) *os_ << x(0) << " - 1. = " << x(0) - 1. << endl;

    unit_assert_equal(x(0), 1., 1e-14);
    unit_assert_equal(x(1), 2., 1e-14);
}

/*
void testComplexQR()
{
    if (os_) *os_ << "testComplex()\n";

    LinearSolver<LinearSolverType_QR> solver;

    ublas::matrix< complex<double> > A(2,2);
    A(0,0) = 1; A(0,1) = 2;
    A(1,0) = 3; A(1,1) = 4;

    ublas::vector< complex<double> > y(2);
    y(0) = 5;
    y(1) = 11;

    ublas::vector< complex<double> > x = solver.solve(A, y);

    if (os_) *os_ << "A: " << A << endl;
    if (os_) *os_ << "y: " << y << endl;
    if (os_) *os_ << "x: " << x << endl;

    unit_assert(x(0) == 1.);
    unit_assert(x(1) == 2.);
}
*/

void testSparse()
{
    if (os_) *os_ << "testSparse()\n";

    LinearSolver<> solver;

    ublas::mapped_matrix<double> A(2,2,4);
    A(0,0) = 1.; A(0,1) = 2.;
    A(1,0) = 3.; A(1,1) = 4.;

    ublas::vector<double> y(2);
    y(0) = 5.;
    y(1) = 11.;

    ublas::vector<double> x = solver.solve(A, y);

    if (os_) *os_ << "A: " << A << endl;
    if (os_) *os_ << "y: " << y << endl;
    if (os_) *os_ << "x: " << x << endl;

    unit_assert_equal(x(0), 1., 1e-14);
    unit_assert_equal(x(1), 2., 1e-14);
}

/*
void testSparseComplex()
{
    if (os_) *os_ << "testSparseComplex()\n";

    LinearSolver<> solver;

    ublas::mapped_matrix< complex<double> > A(2,2,4);
    A(0,0) = 1.; A(0,1) = 2.;
    A(1,0) = 3.; A(1,1) = 4.;

    ublas::vector< complex<double> > y(2);
    y(0) = 5.;
    y(1) = 11.;

    ublas::vector< complex<double> > x = solver.solve(A, y);

    if (os_) *os_ << "A: " << A << endl;
    if (os_) *os_ << "y: " << y << endl;
    if (os_) *os_ << "x: " << x << endl;

    unit_assert(norm(x(0)-1.) < 1e-14);
    unit_assert(norm(x(1)-2.) < 1e-14);
}
*/

void testBanded()
{
    if (os_) *os_ << "testBanded()\n";

    LinearSolver<> solver;

    ublas::banded_matrix<double> A(2,2,1,1);
    A(0,0) = 1.; A(0,1) = 2.;
    A(1,0) = 3.; A(1,1) = 4.;

    ublas::vector<double> y(2);
    y(0) = 5.;
    y(1) = 11.;

    ublas::vector<double> x = solver.solve(A, y);

    if (os_) *os_ << "A: " << A << endl;
    if (os_) *os_ << "y: " << y << endl;
    if (os_) *os_ << "x: " << x << endl;

    unit_assert_equal(x(0), 1., 1e-14);
    unit_assert_equal(x(1), 2., 1e-14);
}

void testBandedComplex()
{
    if (os_) *os_ << "testBandedComplex()\n";

    LinearSolver<> solver;

    ublas::banded_matrix< complex<double> > A(2,2,1,1);
    A(0,0) = 1.; A(0,1) = 2.;
    A(1,0) = 3.; A(1,1) = 4.;

    ublas::vector< complex<double> > y(2);
    y(0) = 5.;
    y(1) = 11.;

    ublas::vector< complex<double> > x = solver.solve(A, y);

    if (os_) *os_ << "A: " << A << endl;
    if (os_) *os_ << "y: " << y << endl;
    if (os_) *os_ << "x: " << x << endl;

    unit_assert(norm(x(0)-1.) < 1e-14);
    unit_assert(norm(x(1)-2.) < 1e-14);
}

int main(int argc, char* argv[])
{
    TEST_PROLOG(argc, argv)

    try
    {
        if (argc>1 && !strcmp(argv[1],"-v")) os_ = &cout;
        if (os_) *os_ << "LinearSolverTest\n";

        testDouble();
        testComplex();
        testDoubleQR();
        //testComplexQR();
        testSparse();
        //testSparseComplex(); // lu_factorize doesn't like mapped_matrix<complex>
        testBanded();
        //testBandedComplex(); // FIXME: GCC 4.2 doesn't like this test with link=shared
    }
    catch (exception& e)
    {
        TEST_FAILED(e.what())
    }
    catch (...)
    {
        TEST_FAILED("Caught unknown exception.")
    }

    TEST_EPILOG
}


#include <stdexcept>
#include <utility>
#include <boost/cstdint.hpp>
#include <boost/bind.hpp>
#include <boost/unordered_set.hpp>
#include <luabind/luabind.hpp>
#include <luabind/iterator_policy.hpp>
#include "a_star.h"
#include "bresenham_supercover.h"
#include "heightfield.h"

namespace math
{

template<class T>
heightfield<T>::heightfield(matrix<4,4> const &tf, size_t nrows, size_t ncols):
    nrows_(nrows),
    ncols_(ncols)
{
	set_local_to_world(tf);

	heights_.resize(nrows * ncols);

	local_aabb_.lo.set(0, -1, 0);
	local_aabb_.hi.set(math::scalar(ncols_ - 1), 1, math::scalar(nrows_ - 1));
}

template<class T>
heightfield<T>::~heightfield()
{
}

template<class T>
scalar heightfield<T>::y_under(vec<3> const &position, scalar default_value)
{
	vec<3> local_position = position * world_to_local_;

	int col = (int) local_position.x;
	int row = (int) local_position.z;

	if (col < 0 || row < 0 || col >= (int) ncols_ - 1 || row >= (int) nrows_ - 1) return default_value;

	scalar v1 = local_y_at(col + 0, row + 0);
	scalar v2 = local_y_at(col + 1, row + 0);
	scalar v3 = local_y_at(col + 0, row + 1);
	scalar v4 = local_y_at(col + 1, row + 1);

	scalar k1 = local_position.x - col;
	scalar k2 = local_position.z - col;

	scalar v12 = v1 * (1 - k1) + v2 * k1;
	scalar v34 = v3 * (1 - k1) + v4 * k1;

	return v12 * (1 - k2) + v34 * k2;
}

template<class T>
scalar heightfield<T>::max_y_in_cell(int col, int row, scalar default_value) const
{
	if (col < 0 || row < 0 || col >= (int) ncols_ - 1 || row >= (int) nrows_ - 1)
	{
		return default_value;
	}

	scalar result = (scalar) local_y_at(col + 0, row + 0);
	result = std::max(result, (scalar) local_y_at(col + 0, row + 1));
	result = std::max(result, (scalar) local_y_at(col + 1, row + 0));
	result = std::max(result, (scalar) local_y_at(col + 1, row + 1));

	return result;
}

template<class T>
scalar heightfield<T>::max_y_in_cell(vec<3> const &position, scalar default_value) const
{
	vec<3> local_position = position * world_to_local_;

	int col = (int) local_position.x;
	int row = (int) local_position.z;

	return max_y_in_cell(col, row, default_value);
}

template<class T>
void heightfield<T>::set_local_to_world(math::matrix<4, 4> const &tf)
{
	local_to_world_ = tf;
    local_to_world_.inverse(world_to_local_);
}

template<class T> typename heightfield<T>::cell_t
heightfield<T>::world_position_to_cell(vec<3> const &position) const
{
	vec<3> p = position * world_to_local_;
	return cell_t(int(p.x), int(p.z));
}

template<class T>
vec<3> heightfield<T>::cell_to_world_position(cell_t const &cell) const
{
	return vec<3>((scalar) cell.x + 0.5f, 0, (scalar) cell.y + 0.5f) * local_to_world_;
}

template<class T>
contact_info<3> heightfield<T>::trace(ray<3> const &r, scalar max_distance) const
{
	vec<3> local_p0 = r.r0 * world_to_local_;
	vec<3> local_p1 = (r.r0 + normalize(r.a) * max_distance) * world_to_local_;

	ray<3> local_ray(local_p0, local_p1 - local_p0);

	contact_info<3> result;
	result.happened = false;

	scalar local_ray_t0, local_ray_t1;

	if (!local_aabb_.trace(local_ray, &local_ray_t0, &local_ray_t1)) return result;

	vec<3> local_p0_adjusted = local_ray.apply(local_ray_t0);
	vec<3> local_p1_adjusted = local_ray.apply(local_ray_t1);

	bresenham_supercover<> cells_iterator(cell_t(int (local_p0_adjusted.x), int(local_p0_adjusted.z)),
		cell_t(int(local_p1_adjusted.x), int(local_p1_adjusted.z)));

	while (true)
	{
		cell_t cell;
		bool flag;

		boost::tie(cell, flag) = cells_iterator.get();
		if (flag) break;

		if (cell.x == ncols_ - 1) --cell.x;
		if (cell.y == nrows_ - 1) --cell.y;

		vec<3> v1 = local_vertex_at(cell.x + 0, cell.y + 0);
		vec<3> v2 = local_vertex_at(cell.x + 0, cell.y + 1);
		vec<3> v3 = local_vertex_at(cell.x + 1, cell.y + 1);
		vec<3> v4 = local_vertex_at(cell.x + 1, cell.y + 0);

		triangle<3> t1(v1, v2, v3);
		triangle<3> t2(v1, v3, v4);

		bool trace_h1, trace_h2;
        scalar trace_t1, trace_t2;
		vec<3> trace_p1, trace_p2;

		trace_h1 = t1.trace(local_ray, trace_t1, trace_p1);
		trace_h2 = t2.trace(local_ray, trace_t2, trace_p2);

		if (trace_h1 || trace_h2)
        {
            contact_info<3> ci;

            ci.happened = true;
            ci.penetrated = false;

            if (trace_h1 && ((trace_h2 && trace_t1 < trace_t2) || trace_h2 == false))
            {
                ci.position = trace_p1;
                ci.normal = t1.get_normal();
                ci.time = trace_t1;
            }
            else
            {
                ci.position = trace_p2;
                ci.normal = t2.get_normal();
                ci.time = trace_t2;
            }

			if (result.worse_than(ci))
			{
				result = ci;
			}
        }
    }

	result.position = result.position * local_to_world_;

	return result;
}

template<class T>
void heightfield<T>::load_from_raw_buffer(value_t const *heights)
{
    heights_.assign(heights, heights + nsamples());

	post_load();
}

template<class T>
void heightfield<T>::resize(size_t ncols, size_t nrows)
{
	std::vector<value_t> heights;
	heights.resize(nrows * ncols);

	for (size_t i = 0, ni = std::min(nrows, nrows_); i < ni; ++i)
	{
		for (size_t j = 0, nj = std::min(ncols, ncols_); j < nj; ++j)
		{
			heights[i * ncols + j] = heights_[i * ncols_ + j];
		}
	}

	heights_.swap(heights);
	ncols_ = ncols;
	nrows_ = nrows;

	post_load();
}

namespace
{
template<class T>
class scorer_closure {
public:
	typedef typename heightfield<T>::cell_t cell_t;

	scorer_closure(heightfield<T> const *hf, scalar min_value, cell_t goal, std::vector<cell_t> const &obstacles):
		hf_(hf),
		min_value_(min_value),
		goal_(goal)
	{
		obstacles_.insert(obstacles.begin(), obstacles.end());
	}

	scalar get_score(cell_t const &cell) const
	{
		if (cell.x < 0) return -1;
		if (cell.y < 0) return -1;
		if (cell.x >= (int) hf_->nrows()) return -1;
		if (cell.y >= (int) hf_->ncols()) return -1;

		typename obstacles_t::const_iterator it = obstacles_.find(cell);
		if (it != obstacles_.end()) return -1;

		scalar delta = (scalar) (cell - goal_).length_sq();

		if (hf_->max_y_in_cell(cell.x, cell.y) < min_value_)
		{
			return delta;
		}
		else
		{
			return -1;
		}
	}

private:
	struct cell_hash {
		std::size_t operator ()(cell_t const &cell) const {
			return (cell.x * 1011) ^ cell.y;
		}
	};

	struct cell_eq {
		std::size_t operator ()(cell_t const &lhs, cell_t const &rhs) const {
			return lhs == rhs;
		}
	};

	typedef boost::unordered_set<cell_t, cell_hash, cell_eq> obstacles_t;

	heightfield<T> const *hf_;
	scalar min_value_;
	cell_t goal_;
	obstacles_t obstacles_;
};
}

template<class T> std::vector<typename heightfield<T>::cell_t>
heightfield<T>::build_path(vec<3> const &from, vec<3> const &to, value_t min_value, bool allow_best_heuristic_point,
	std::vector<cell_t> const &obstacles) const
{
	cell_t start = world_position_to_cell(from);
	cell_t goal = world_position_to_cell(to);

	scorer_closure<T> scorer(this, min_value, goal, obstacles);
	a_star pathfinder(start, goal, boost::bind(&scorer_closure<T>::get_score, &scorer, _1));

	pathfinder.calculate_path();

	return pathfinder.build_path(allow_best_heuristic_point);
}

template<class T>
void heightfield<T>::post_load()
{
	local_aabb_.null();

	for (typename std::vector<value_t>::const_iterator it = heights_.begin();
		 it != heights_.end(); ++it)
	{
		if (local_aabb_.lo.y > *it) local_aabb_.lo.y = *it;
		if (local_aabb_.hi.y < *it) local_aabb_.hi.y = *it;
	}

	local_aabb_.lo.y -= 1;
	local_aabb_.hi.y += 1;

	local_aabb_.lo.x = 0;
	local_aabb_.lo.z = 0;

	local_aabb_.hi.x = scalar(ncols_ - 1);
	local_aabb_.hi.z = scalar(nrows_ - 1);
}

template<class T>
void heightfield<T>::bind(lua_State *L, char const *name)
{
	using namespace luabind;

	module(L, "math")
	[
		class_<heightfield>(name)
		.def(constructor<matrix<4,4> const &, size_t, size_t>())
		.def("y_under", &heightfield::y_under)
		.def("max_y_in_cell", (scalar (heightfield::*)(vec<3> const &, scalar) const) &heightfield::max_y_in_cell)
		.def("ncols", &heightfield::ncols)
		.def("nrows", &heightfield::nrows)
		.def("set_local_to_world", &heightfield::set_local_to_world)
		.def("get_local_aabb", &heightfield::get_local_aabb)
		.def("world_position_to_cell", &heightfield::world_position_to_cell)
		.def("cell_to_world_position", &heightfield::cell_to_world_position)
		.def("build_path", &heightfield::build_path)
		.def("trace", &heightfield::trace)
		.def("resize", &heightfield::resize)
	];
}

template class heightfield<boost::uint8_t>;
template class heightfield<boost::uint16_t>;
template class heightfield<scalar>;

void bind_heightfield(lua_State *L)
{
	heightfield<boost::uint8_t>::bind(L, "heightfield_u8");
	heightfield<boost::uint16_t>::bind(L, "heightfield_u16");
	heightfield<scalar>::bind(L, "heightfield_s");
}

}


#include "MP2.hpp"
#include "RHF.hpp"
#include <armadillo>

namespace willow {
namespace qcmol {

arma::mat MP2::cphf(const arma::mat &l_ai, const arma::vec &ao_tei,
                    const bool l_print) {

  const double epsilon = 1.0e-8;
  // arma::vec v_p_ai (nvir*nocc, arma::fill::zeros);
  // arma::mat pm_ai  (v_p_aj.memptr(), nvir, nocc, false);

  arma::mat Cvir = Cmat.submat(0, ivir1, nbf - 1, ivir2);
  arma::mat Cocc = Cmat.submat(0, 0, nbf - 1, iocc2);

  arma::vec v_Pm(nvir * nocc, arma::fill::zeros);
  arma::mat Pm(v_Pm.memptr(), nvir, nocc, false);

  for (auto moi = 0; moi < nocc; moi++) {
    for (auto am = 0; am < nvir; am++) {
      auto moa = ivir1 + am;
      Pm(am, moi) = l_ai(am, moi) / (eval(moa) - eval(moi));
    }
  }

  std::vector<arma::vec> Pm_list;
  Pm_list.push_back(v_Pm);

  arma::vec v_APm(nvir * nocc, arma::fill::zeros);
  arma::mat APm(v_APm.memptr(), nvir, nocc, false);
  std::vector<arma::vec> APm_list;

  // arma::mat Dm  (nbf,  nbf,  arma::fill::zeros);
  // arma::mat Gm  (nbf,  nbf,  arma::fill::zeros);

  arma::vec P_sum_old(nvir * nocc, arma::fill::zeros);
  arma::vec P_sum_new(nvir * nocc, arma::fill::zeros);

  const int maxiter = 30;

  for (auto iter = 0; iter < maxiter; ++iter) {

    const int ndim = iter + 1;

    // Compute A*P[i-1] (call AP matrix)
    v_Pm = Pm_list[iter];

    arma::mat Dm = Cvir * Pm * Cocc.t();
    arma::mat Gm = g_matrix(ao_tei, Dm);
    APm = 4.0 * Cvir.t() * Gm * Cocc;

    for (auto moi = 0; moi < nocc; moi++) {
      for (auto am = 0; am < nvir; am++) {
        auto moa = ivir1 + am;
        double tmpval = APm(am, moi) / (eval(moi) - eval(moa));
        APm(am, moi) = tmpval;
      }
    }

    APm_list.push_back(v_APm);

    // compute alpha
    // Solve the linear system of equations C*X = B
    // where C is matrix, X and B are vector
    // put result (x) into array alpha
    arma::vec alpha(ndim, arma::fill::zeros);
    {
      arma::vec norm(ndim, arma::fill::zeros);

      for (auto i = 0; i < ndim; i++) {
        norm(i) = arma::norm(Pm_list[i]);
      }

      // Construct matrix Cm
      arma::mat Cm(ndim, ndim, arma::fill::zeros);

      for (auto j = 0; j < ndim; j++) {
        for (auto i = 0; i < ndim; i++) {

          double tmp1 = -arma::dot(Pm_list[i], APm_list[j]);

          if (i == j) {
            tmp1 += arma::dot(Pm_list[i], Pm_list[i]);
          }

          Cm(i, j) = tmp1 / (norm(i) * norm(j));
        }
      }

      arma::vec Bv(ndim, arma::fill::zeros);
      Bv(0) = norm(0);

      arma::vec Xv = Cm.i() * Bv;

      for (auto i = 0; i < ndim; i++) {
        alpha(i) = Xv(i) / norm(i);
      }
    }

    // P_sum_new = alpha[0]*P[0] +
    P_sum_new.zeros();
    for (auto j = 0; j < ndim; j++) {
      P_sum_new += alpha(j) * Pm_list[j];
    }

    // Test for convergence
    // (based on RMS (P2aj_new - P2aj_old)
    // and max abs. val. of element

    double tmpval = 0.0;
    double maxabs = 0.0;
    for (auto j = 0; j < nocc * nvir; j++) {
      double tmpval1 = P_sum_new(j) - P_sum_old(j);
      double tmpval2 = tmpval1 * tmpval1;
      tmpval += tmpval2;
      if (tmpval2 > maxabs)
        maxabs = tmpval2;
    }

    if (std::sqrt(tmpval / (nocc * nvir)) < epsilon &&
        std::sqrt(maxabs) < epsilon)
      break;

    // Put P_sum_new into P_sum_old

    P_sum_old = P_sum_new;

    // Compute projection of A*P[i-1] on P[0], ..., P[i-1]

    arma::vec projctn(nocc * nvir, arma::fill::zeros);

    for (auto j = 0; j < iter + 1; j++) {
      double dot_prod = arma::dot(Pm_list[j], Pm_list[j]);
      double coef = arma::dot(Pm_list[j], APm) / dot_prod;

      projctn += coef * Pm_list[j];
    }

    Pm_list.push_back(v_APm - projctn);

    // Test for convergence (based on norm (Pm[i]))

    double rmsd = arma::norm(Pm_list[iter + 1]) / sqrt(nocc * nvir);
    // double rmsd = arma::norm (p_ai - p_ai_old);

    if (l_print)
      std::cout << "Iter CPHF " << iter << "   " << rmsd << std::endl;
    if (rmsd < epsilon)
      break;
  }

  // Converged vector is in P_sum_new

  arma::mat p_ai(P_sum_new.memptr(), nvir, nocc, true);

  return p_ai;
}

/*
arma:mat MP2::cphf_old ()
{
  double epsilon = 1.0e-8;

  for (auto moi = 0; moi < nocc; moi++) {
    for (auto am = 0; am < navir; am++) {
      auto moa = ivir1 + am;
      p_ai(am,moi) = l_ai(am,moi)/(eval(moa) - eval(moi));
    }
  }

  // Solve the CPHF equations (iteratively, with DIIS like method)
  int ii = 0;
  int niter = 0;
  const int maxiter = 30;
  for (auto iter = 0; iter != maxiter; ++iter) {

    arma::mat p_ai_old = p_ai;
    // Compute A*P[ii-1]
    // Dm(nbf,nbf) = (nbf,navir)*(navir,nocc)*(nbf,nocc)
    arma::mat Dm = Cvir*p_ai_old*Cocc.t();
    arma::mat Gm = g_matrix (ints.TEI, Dm);

    //  APm(navir,nocc) =
    arma::mat APm = 2.0*Cvir.t()*Gm*Cocc;

    for (auto moi = 0; moi < nocc; moi++) {
      for (auto am = 0; am < navir; am++) {
        auto moa = ivir1 + am;
        p_ai (am,moi) = (l_ai(am,moi) - APm(am,moi))/(eval(moa) - eval(moi));
      }
    }
    p_ai = 0.6*p_ai + 0.4*p_ai_old;

    double rmsd = arma::norm (p_ai - p_ai_old);

    std::cout << "Iter CPHF " << iter << "   " << rmsd << std::endl;
    if (rmsd < 1.0e-7) break;
  }

  std::cout << "p_ai \n";

  return p_ai;
}
*/

} // namespace qcmol
} // namespace willow


//
// Copyright (c) 2020 INRIA
//

#include <pinocchio/math/matrix.hpp>

#include <boost/variant.hpp> // to avoid C99 warnings

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_isNormalized)
{
  srand(0);

  using namespace pinocchio;
  typedef Eigen::Matrix<double,Eigen::Dynamic,1> Vector;

  const int max_size = 1000;
#ifdef NDEBUG
  const int max_test = 1e6;
#else
  const int max_test = 1e2;
#endif
  for(int i = 0; i < max_test; ++i)
  {
    const Eigen::DenseIndex size = rand() % max_size + 1; // random vector size
    Vector vec;
    vec = Vector::Random(size) + Vector::Constant(size,2.);
    BOOST_CHECK(!isNormalized(vec));

    vec.normalize();
    BOOST_CHECK(isNormalized(vec));

    // Specific check for the Zero vector
    BOOST_CHECK(!isNormalized(Vector(Vector::Zero(size))));
  }
}

BOOST_AUTO_TEST_SUITE_END()


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

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

#ifndef BOOST_GEOMETRY_TEST_DIFFERENCE_HPP
#define BOOST_GEOMETRY_TEST_DIFFERENCE_HPP

#include <fstream>
#include <iomanip>

#include <geometry_test_common.hpp>

#include <boost/core/ignore_unused.hpp>
#include <boost/foreach.hpp>

#include <boost/range/algorithm/copy.hpp>

#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/difference.hpp>
#include <boost/geometry/algorithms/sym_difference.hpp>

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/algorithms/num_points.hpp>

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

#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/multi_linestring.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

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

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

#if defined(TEST_WITH_SVG)
#  define BOOST_GEOMETRY_DEBUG_SEGMENT_IDENTIFIER
#  define BOOST_GEOMETRY_DEBUG_IDENTIFIER
#  include <boost/geometry/io/svg/svg_mapper.hpp>
#  include <boost/geometry/algorithms/detail/overlay/debug_turn_info.hpp>
#endif

template <typename Output, typename G1, typename G2>
void difference_output(std::string const& caseid, G1 const& g1, G2 const& g2, Output const& output)
{
    boost::ignore_unused(caseid, g1, g2, output);

#if defined(TEST_WITH_SVG)
    {
        typedef typename bg::coordinate_type<G1>::type coordinate_type;
        typedef typename bg::point_type<G1>::type point_type;

        std::ostringstream filename;
        filename << "difference_"
            << caseid << "_"
            << string_from_type<coordinate_type>::name()
#if defined(BOOST_GEOMETRY_NO_ROBUSTNESS)
            << "_no_rob"
#endif
            << ".svg";

        std::ofstream svg(filename.str().c_str());

        bg::svg_mapper<point_type> mapper(svg, 500, 500);

        mapper.add(g1);
        mapper.add(g2);

        mapper.map(g1, "fill-opacity:0.3;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:3");
        mapper.map(g2, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:3");

        for (typename Output::const_iterator it = output.begin(); it != output.end(); ++it)
        {
            mapper.map(*it,
                //sym ? "fill-opacity:0.2;stroke-opacity:0.4;fill:rgb(255,255,0);stroke:rgb(255,0,255);stroke-width:8" :
                "fill-opacity:0.2;stroke-opacity:0.4;fill:rgb(255,0,0);stroke:rgb(255,0,255);stroke-width:8");
        }
    }
#endif
}

template <typename OutputType, typename G1, typename G2>
void test_difference(std::string const& caseid, G1 const& g1, G2 const& g2,
        int expected_count, int expected_point_count,
        double expected_area,
        double percentage = 0.0001,
        bool sym = false)
{
    typedef typename bg::coordinate_type<G1>::type coordinate_type;
    boost::ignore_unused<coordinate_type>();

    std::vector<OutputType> clip;

    if (sym)
    {
        bg::sym_difference(g1, g2, clip);
    }
    else
    {
        bg::difference(g1, g2, clip);
    }

    typename bg::default_area_result<G1>::type area = 0;
    std::size_t n = 0;
    for (typename std::vector<OutputType>::iterator it = clip.begin();
            it != clip.end();
            ++it)
    {
        if (expected_point_count >= 0)
        {
            n += bg::num_points(*it);
        }

        area += bg::area(*it);
    }

    difference_output(caseid, g1, g2, clip);

#ifndef BOOST_GEOMETRY_DEBUG_ASSEMBLE
    {
        // Test inserter functionality
        // Test if inserter returns output-iterator (using Boost.Range copy)
        typedef typename bg::point_type<G1>::type point_type;
        typedef typename bg::rescale_policy_type<point_type>::type
            rescale_policy_type;

        rescale_policy_type rescale_policy
                = bg::get_rescale_policy<rescale_policy_type>(g1, g2);

        std::vector<OutputType> inserted, array_with_one_empty_geometry;
        array_with_one_empty_geometry.push_back(OutputType());
        if (sym)
        {
            boost::copy(array_with_one_empty_geometry,
                bg::detail::sym_difference::sym_difference_insert<OutputType>
                    (g1, g2, rescale_policy, std::back_inserter(inserted)));
        }
        else
        {
            boost::copy(array_with_one_empty_geometry,
                bg::detail::difference::difference_insert<OutputType>(
                    g1, g2, rescale_policy, std::back_inserter(inserted)));
        }

        BOOST_CHECK_EQUAL(boost::size(clip), boost::size(inserted) - 1);
    }
#endif

#if ! defined(BOOST_GEOMETRY_NO_BOOST_TEST)
    if (expected_point_count >= 0)
    {
        BOOST_CHECK_MESSAGE(bg::math::abs(int(n) - expected_point_count) < 3,
                "difference: " << caseid
                << " #points expected: " << expected_point_count
                << " detected: " << n
                << " type: " << (type_for_assert_message<G1, G2>())
                );
    }

    if (expected_count >= 0)
    {
        BOOST_CHECK_MESSAGE(int(clip.size()) == expected_count,
                "difference: " << caseid
                << " #outputs expected: " << expected_count
                << " detected: " << clip.size()
                << " type: " << (type_for_assert_message<G1, G2>())
                );
    }

    BOOST_CHECK_CLOSE(area, expected_area, percentage);
#endif

}

#ifdef BOOST_GEOMETRY_CHECK_WITH_POSTGIS
static int counter = 0;
#endif

template <typename OutputType, typename G1, typename G2>
void test_one(std::string const& caseid,
        std::string const& wkt1, std::string const& wkt2,
        int expected_count1,
        int expected_point_count1,
        double expected_area1,
        int expected_count2,
        int expected_point_count2,
        double expected_area2,
        int expected_count_s,
        int expected_point_count_s,
        double expected_area_s,
        double percentage = 0.0001)
{
#ifdef BOOST_GEOMETRY_CHECK_WITH_SQLSERVER
    std::cout
        << "-- " << caseid << std::endl
        << "with qu as (" << std::endl
        << "select geometry::STGeomFromText('" << wkt1 << "',0) as p," << std::endl
        << "geometry::STGeomFromText('" << wkt2 << "',0) as q)" << std::endl
        << "select " << std::endl
        << " p.STDifference(q).STNumGeometries() as cnt1,p.STDifference(q).STNumPoints() as pcnt1,p.STDifference(q).STArea() as area1," << std::endl
        << " q.STDifference(p).STNumGeometries() as cnt2,q.STDifference(p).STNumPoints() as pcnt2,q.STDifference(p).STArea() as area2," << std::endl
        << " p.STDifference(q) as d1,q.STDifference(p) as d2 from qu" << std::endl << std::endl;
#endif

    G1 g1;
    bg::read_wkt(wkt1, g1);

    G2 g2;
    bg::read_wkt(wkt2, g2);

    bg::correct(g1);
    bg::correct(g2);

    test_difference<OutputType>(caseid + "_a", g1, g2,
        expected_count1, expected_point_count1,
        expected_area1, percentage);
#ifdef BOOST_GEOMETRY_DEBUG_ASSEMBLE
    return;
#endif
    test_difference<OutputType>(caseid + "_b", g2, g1,
        expected_count2, expected_point_count2,
        expected_area2, percentage);
    test_difference<OutputType>(caseid + "_s", g1, g2,
        expected_count_s,
        expected_point_count_s,
        expected_area_s,
        percentage, true);

#ifdef BOOST_GEOMETRY_CHECK_WITH_POSTGIS
    std::cout
        << (counter > 0 ? "union " : "")
        << "select " << counter++
        << ", '" << caseid << "' as caseid"
        << ", ST_NumPoints(ST_Difference(ST_GeomFromText('" << wkt1 << "'), "
        << "      ST_GeomFromText('" << wkt2 << "'))) "
        << ", ST_NumGeometries(ST_Difference(ST_GeomFromText('" << wkt1 << "'), "
        << "      ST_GeomFromText('" << wkt2 << "'))) "
        << ", ST_Area(ST_Difference(ST_GeomFromText('" << wkt1 << "'), "
        << "      ST_GeomFromText('" << wkt2 << "'))) "
        //<< ", " << expected_area1 << " as expected_area_a"
        //<< ", " << expected_count1 << " as expected_count_a"
        << ", ST_NumPoints(ST_Difference(ST_GeomFromText('" << wkt2 << "'), "
        << "      ST_GeomFromText('" << wkt1 << "'))) "
        << ", ST_NumGeometries(ST_Difference(ST_GeomFromText('" << wkt2 << "'), "
        << "      ST_GeomFromText('" << wkt1 << "'))) "
        << ", ST_Area(ST_Difference(ST_GeomFromText('" << wkt2 << "'), "
        << "      ST_GeomFromText('" << wkt1 << "'))) "
        //<< ", " << expected_area2 << " as expected_area_b"
        //<< ", " << expected_count2 << " as expected_count_b"
        << ", ST_NumPoints(ST_SymDifference(ST_GeomFromText('" << wkt1 << "'), "
        << "      ST_GeomFromText('" << wkt2 << "'))) "
        << ", ST_NumGeometries(ST_SymDifference(ST_GeomFromText('" << wkt1 << "'), "
        << "      ST_GeomFromText('" << wkt2 << "'))) "
        << ", ST_Area(ST_SymDifference(ST_GeomFromText('" << wkt1 << "'), "
        << "      ST_GeomFromText('" << wkt2 << "'))) "
        //<< ", " << expected_area1 + expected_area2 << " as expected_area_s"
        //<< ", " << expected_count1 + expected_count2 << " as expected_count_s"
        << std::endl;
#endif

}

template <typename OutputType, typename G1, typename G2>
void test_one(std::string const& caseid,
        std::string const& wkt1, std::string const& wkt2,
        int expected_count1,
        int expected_point_count1,
        double expected_area1,
        int expected_count2,
        int expected_point_count2,
        double expected_area2,
        double percentage = 0.0001)
{
    test_one<OutputType, G1, G2>(caseid, wkt1, wkt2,
        expected_count1, expected_point_count1, expected_area1,
        expected_count2, expected_point_count2, expected_area2,
        expected_count1 + expected_count2,
        expected_point_count1 >= 0 && expected_point_count2 >= 0
            ? (expected_point_count1 + expected_point_count2) : -1,
        expected_area1 + expected_area2,
        percentage);
}

template <typename OutputType, typename G1, typename G2>
void test_one_lp(std::string const& caseid,
        std::string const& wkt1, std::string const& wkt2,
        std::size_t expected_count,
        int expected_point_count,
        double expected_length)
{
    G1 g1;
    bg::read_wkt(wkt1, g1);

    G2 g2;
    bg::read_wkt(wkt2, g2);

    bg::correct(g1);

    std::vector<OutputType> pieces;
    bg::difference(g1, g2, pieces);

    typename bg::default_length_result<G1>::type length = 0;
    std::size_t n = 0;
    std::size_t piece_count = 0;
    for (typename std::vector<OutputType>::iterator it = pieces.begin();
            it != pieces.end();
            ++it)
    {
        if (expected_point_count >= 0)
        {
            n += bg::num_points(*it);
        }
        piece_count++;
        length += bg::length(*it);
    }

    BOOST_CHECK_MESSAGE(piece_count == expected_count,
            "difference: " << caseid
            << " #outputs expected: " << expected_count
            << " detected: " << pieces.size()
            );

    if (expected_point_count >= 0)
    {
        BOOST_CHECK_EQUAL(n, std::size_t(expected_point_count));
    }

    BOOST_CHECK_CLOSE(length, expected_length, 0.001);

    std::string lp = "lp_";
    difference_output(lp + caseid, g1, g2, pieces);
}

#endif


// Copyright Paul A. Bristow 2016, 2017, 2018.
// Copyright John Maddock 2016.

// Use, modification and distribution are 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)

// test_lambert_w_integrals.cpp
//! \brief quadrature tests that cover the whole range of the Lambert W0 function.

#include <boost/config.hpp>   // for BOOST_MSVC definition etc.
#include <boost/version.hpp>   // for BOOST_MSVC versions.

#ifdef BOOST_HAS_FLOAT128

// Boost macros
#define BOOST_TEST_MAIN
#define BOOST_LIB_DIAGNOSTIC "on" // Report library file details.
#include <boost/test/included/unit_test.hpp> // Boost.Test
// #include <boost/test/unit_test.hpp> // Boost.Test
#include <boost/test/floating_point_comparison.hpp>

#include <boost/array.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/type_traits/is_constructible.hpp>

#include <boost/multiprecision/float128.hpp>

#include <boost/math/special_functions/fpclassify.hpp> // isnan, ifinite.
#include <boost/math/special_functions/next.hpp> // float_next, float_prior
using boost::math::float_next;
using boost::math::float_prior;
#include <boost/math/special_functions/ulp.hpp>  // ulp

#include <boost/math/tools/test_value.hpp>  // for create_test_value and macro BOOST_MATH_TEST_VALUE.
#include <boost/math/policies/policy.hpp>
using boost::math::policies::digits2;
using boost::math::policies::digits10;
#include <boost/math/special_functions/lambert_w.hpp> // For Lambert W lambert_w function.
using boost::math::lambert_wm1;
using boost::math::lambert_w0;

#include <limits>
#include <cmath>
#include <typeinfo>
#include <iostream>
#include <type_traits>
#include <exception>

std::string show_versions(void);

// Added code and test for Integral of the Lambert W function: by Nick Thompson.
// https://en.wikipedia.org/wiki/Lambert_W_function#Definite_integrals

#include <boost/math/constants/constants.hpp> // for integral tests.
#include <boost/math/quadrature/tanh_sinh.hpp> // for integral tests.
#include <boost/math/quadrature/exp_sinh.hpp> // for integral tests.

  using boost::math::policies::policy;
  using boost::math::policies::make_policy;

// using statements needed for changing error handling policy.
using boost::math::policies::evaluation_error;
using boost::math::policies::domain_error;
using boost::math::policies::overflow_error;
using boost::math::policies::ignore_error;
using boost::math::policies::throw_on_error;

typedef policy<
  domain_error<throw_on_error>,
  overflow_error<ignore_error>
> no_throw_policy;

// Assumes that function has a throw policy, for example:
//    NOT lambert_w0<T>(1 / (x * x), no_throw_policy());
// Error in function boost::math::quadrature::exp_sinh<double>::integrate:
// The exp_sinh quadrature evaluated your function at a singular point and resulted in inf.
// Please ensure your function evaluates to a finite number of its entire domain.
template <typename T>
T debug_integration_proc(T x)
{
   T result; // warning C4701: potentially uninitialized local variable 'result' used
  // T result = 0 ; // But result may not be assigned below?
  try
  {
   // Assign function call to result in here...
    if (x <= sqrt(boost::math::tools::min_value<T>()) )
    {
      result = 0;
    }
    else
    {
      result = lambert_w0<T>(1 / (x * x));
    }
   // result = lambert_w0<T>(1 / (x * x), no_throw_policy());  // Bad idea, less helpful diagnostic message is:
    // Error in function boost::math::quadrature::exp_sinh<double>::integrate:
    // The exp_sinh quadrature evaluated your function at a singular point and resulted in inf.
    // Please ensure your function evaluates to a finite number of its entire domain.

  } // try
  catch (const std::exception& e)
  {
    std::cout << "Exception " << e.what() << std::endl;
    // set breakpoint here:
    std::cout << "Unexpected exception thrown in integration code at abscissa (x): " << x << "." << std::endl;
    if (!std::isfinite(result))
    {
      // set breakpoint here:
      std::cout << "Unexpected non-finite result in integration code at abscissa (x): " << x << "." << std::endl;
    }
    if (std::isnan(result))
    {
      // set breakpoint here:
      std::cout << "Unexpected non-finite result in integration code at abscissa (x): " << x << "." << std::endl;
    }
  } // catch
  return result;
} // T debug_integration_proc(T x)

template<class Real>
void test_integrals()
{
  // Integral of the Lambert W function:
  // https://en.wikipedia.org/wiki/Lambert_W_function
  using boost::math::quadrature::tanh_sinh;
  using boost::math::quadrature::exp_sinh;
  // file:///I:/modular-boost/libs/math/doc/html/math_toolkit/quadrature/double_exponential/de_tanh_sinh.html
  using std::sqrt;

  std::cout << "Integration of type " << typeid(Real).name()  << std::endl;

  Real tol = std::numeric_limits<Real>::epsilon();
  { //  // Integrate for function lambert_W0(z);
    tanh_sinh<Real> ts;
    Real a = 0;
    Real b = boost::math::constants::e<Real>();
    auto f = [](Real z)->Real
    {
      return lambert_w0<Real>(z);
    };
    Real z = ts.integrate(f, a, b); // OK without any decltype(f)
    BOOST_CHECK_CLOSE_FRACTION(z, boost::math::constants::e<Real>() - 1, tol);
  }
  {
    // Integrate for function lambert_W0(z/(z sqrt(z)).
    exp_sinh<Real> es;
    auto f = [](Real z)->Real
    {
      return lambert_w0<Real>(z)/(z * sqrt(z));
    };
    Real z = es.integrate(f); // OK
    BOOST_CHECK_CLOSE_FRACTION(z, 2 * boost::math::constants::root_two_pi<Real>(), tol);
  }
  {
    // Integrate for function lambert_W0(1/z^2).
    exp_sinh<Real> es;
    //const Real sqrt_min = sqrt(boost::math::tools::min_value<Real>()); // 1.08420217e-19 fo 32-bit float.
    // error C3493: 'sqrt_min' cannot be implicitly captured because no default capture mode has been specified
    auto f = [](Real z)->Real
    {
      if (z <= sqrt(boost::math::tools::min_value<Real>()) )
      { // Too small would underflow z * z and divide by zero to overflow 1/z^2 for lambert_w0 z parameter.
        return static_cast<Real>(0);
      }
      else
      {
        return lambert_w0<Real>(1 / (z * z)); // warning C4756: overflow in constant arithmetic, even though cannot happen.
      }
    };
    Real z = es.integrate(f);
    BOOST_CHECK_CLOSE_FRACTION(z, boost::math::constants::root_two_pi<Real>(), tol);
  }
} // template<class Real> void test_integrals()

BOOST_AUTO_TEST_CASE( integrals )
{
  std::cout << "Macro BOOST_MATH_LAMBERT_W0_INTEGRALS is defined." << std::endl;
  BOOST_TEST_MESSAGE("\nTest Lambert W0 integrals.");
  try
  {
  // using statements needed to change precision policy.
  using boost::math::policies::policy;
  using boost::math::policies::make_policy;
  using boost::math::policies::precision;
  using boost::math::policies::digits2;
  using boost::math::policies::digits10;

  // using statements needed for changing error handling policy.
  using boost::math::policies::evaluation_error;
  using boost::math::policies::domain_error;
  using boost::math::policies::overflow_error;
  using boost::math::policies::ignore_error;
  using boost::math::policies::throw_on_error;

  typedef policy<
    domain_error<throw_on_error>,
    overflow_error<ignore_error>
  > no_throw_policy;

  /*
  // Experiment with better diagnostics.
  typedef float Real;

  Real inf = std::numeric_limits<Real>::infinity();
  Real max = (std::numeric_limits<Real>::max)();
  std::cout.precision(std::numeric_limits<Real>::max_digits10);
  //std::cout << "lambert_w0(inf) = " << lambert_w0(inf) << std::endl; // lambert_w0(inf) = 1.79769e+308
  std::cout << "lambert_w0(inf, throw_policy()) = " << lambert_w0(inf, no_throw_policy()) << std::endl; // inf
  std::cout << "lambert_w0(max) = " << lambert_w0(max) << std::endl; // lambert_w0(max) = 703.227
  //std::cout << lambert_w0(inf) << std::endl; // inf - will throw.
  std::cout << "lambert_w0(0) = " << lambert_w0(0.) << std::endl; // 0
  std::cout << "lambert_w0(std::numeric_limits<Real>::denorm_min()) = " << lambert_w0(std::numeric_limits<Real>::denorm_min()) << std::endl; // 4.94066e-324
  std::cout << "lambert_w0(std::numeric_limits<Real>::min()) = " << lambert_w0((std::numeric_limits<Real>::min)()) << std::endl; // 2.22507e-308

  // Approximate the largest lambert_w you can get for type T?
  float max_w_f = boost::math::lambert_w_detail::lambert_w0_approx((std::numeric_limits<float>::max)()); // Corless equation 4.19, page 349, and Chapeau-Blondeau equation 20, page 2162.
  std::cout << "w max_f " << max_w_f << std::endl; // 84.2879
  Real max_w = boost::math::lambert_w_detail::lambert_w0_approx((std::numeric_limits<Real>::max)()); // Corless equation 4.19, page 349, and Chapeau-Blondeau equation 20, page 2162.
  std::cout << "w max " << max_w << std::endl; // 703.227

  std::cout << "lambert_w0(7.2416706213544837e-163) = " << lambert_w0(7.2416706213544837e-163) << std::endl; //
  std::cout << "test integral 1/z^2" << std::endl;
  std::cout << "ULP = " << boost::math::ulp(1., policy<digits2<> >()) << std::endl; // ULP = 2.2204460492503131e-16
  std::cout << "ULP = " << boost::math::ulp(1e-10, policy<digits2<> >()) << std::endl; // ULP = 2.2204460492503131e-16
  std::cout << "ULP = " << boost::math::ulp(1., policy<digits2<11> >()) << std::endl; // ULP = 2.2204460492503131e-16
  std::cout << "epsilon =  " << std::numeric_limits<Real>::epsilon() << std::endl; //
  std::cout << "sqrt(max) =  " << sqrt(boost::math::tools::max_value<float>() ) << std::endl; // sqrt(max) =  1.8446742974197924e+19
  std::cout << "sqrt(min) =  " << sqrt(boost::math::tools::min_value<float>() ) << std::endl; // sqrt(min) =  1.0842021724855044e-19

// Demo debug version.
Real tol = std::numeric_limits<Real>::epsilon();
Real x;
{
  using boost::math::quadrature::exp_sinh;
  exp_sinh<Real> es;
  // Function to be integrated, lambert_w0(1/z^2).

    //auto f = [](Real z)->Real
    //{ // Naive - no protection against underflow and subsequent divide by zero.
    //  return lambert_w0<Real>(1 / (z * z));
    //};
    // Diagnostic is:
    // Error in function boost::math::lambert_w0<Real>: Expected a finite value but got inf

    auto f = [](Real z)->Real
    { // Debug with diagnostics for underflow and subsequent divide by zero and other bad things.
      return debug_integration_proc(z);
    };
    // Exception Error in function boost::math::lambert_w0<double>: Expected a finite value but got inf.

    // Unexpected exception thrown in integration code at abscissa: 7.2416706213544837e-163.
    // Unexpected exception thrown in integration code at abscissa (x): 3.478765835953569e-23.
    x = es.integrate(f);
    std::cout << "es.integrate(f) = " << x << std::endl;
    BOOST_CHECK_CLOSE_FRACTION(x, boost::math::constants::root_two_pi<Real>(), tol);
    // root_two_pi<double = 2.506628274631000502
  }
    */

  test_integrals<boost::multiprecision::float128>();
  }
  catch (std::exception& ex)
  {
    std::cout << ex.what() << std::endl;
  }
}

#else

int main() { return 0; }

#endif


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

#include <boost/make_shared.hpp>
#include <boost/bind.hpp>

#include "Tudat/Mathematics/Statistics/randomVariableGenerator.h"
#include "Tudat/Mathematics/Statistics/boostProbabilityDistributions.h"

namespace tudat
{

namespace statistics
{

//! Function to create a random number generating function from a continuous univariate distribution implemented in boost
std::function< double( ) > createBoostContinuousRandomVariableGeneratorFunction(
        const ContinuousBoostStatisticalDistributions boostDistribution,
        const std::vector< double >& parameters,
        const double seed )
{
    return std::bind( &RandomVariableGenerator< double >::getRandomVariableValue,
                        createBoostContinuousRandomVariableGenerator( boostDistribution, parameters, seed ) );
}

//! Function to create a random number generator from a continuous univariate distribution implemented in boost
std::shared_ptr< RandomVariableGenerator< double > > createBoostContinuousRandomVariableGenerator(
        const ContinuousBoostStatisticalDistributions boostDistribution,
        const std::vector< double >& parameters,
        const double seed )
{
    return std::make_shared< ContinuousRandomVariableGenerator >(
                createBoostRandomVariable( boostDistribution, parameters ), seed );
}

}

}


//
// Copyright (c) 2019-2020 INRIA
//

#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
#include "pinocchio/algorithm/contact-dynamics.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

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

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

using namespace Eigen;
using namespace pinocchio;

BOOST_AUTO_TEST_CASE ( test_FD_with_contact_cst_gamma )
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model), data_check(model);

  VectorXd q = VectorXd::Ones(model.nq);
  normalize(model,q);

  computeJointJacobians(model, data, q);

  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Random(model.nv);

  const std::string RF = "rleg6_joint";
  const Model::JointIndex RF_id = model.getJointId(RF);
//  const std::string LF = "lleg6_joint";
//  const Model::JointIndex LF_id = model.getJointId(LF);

  Data::Matrix6x J_RF(6,model.nv); J_RF.setZero();
  getJointJacobian(model, data, RF_id, LOCAL, J_RF);
  Motion::Vector6 gamma_RF; gamma_RF.setZero();
  forwardKinematics(model,data,q,v,VectorXd::Zero(model.nv));
  gamma_RF += data.a[RF_id].toVector(); // Jdot * qdot

  forwardDynamics(model, data, q, v, tau, J_RF, gamma_RF);
  VectorXd ddq_ref = data.ddq;
  Force::Vector6 contact_force_ref = data.lambda_c;

  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext((size_t)model.njoints,Force::Zero());
  fext[RF_id] = ForceRef<Force::Vector6>(contact_force_ref);

  // check call to RNEA
  rnea(model,data_check,q,v,ddq_ref,fext);

  BOOST_CHECK(data_check.tau.isApprox(tau));
  forwardKinematics(model,data_check,q,VectorXd::Zero(model.nv),ddq_ref);
  BOOST_CHECK(data_check.a[RF_id].toVector().isApprox(-gamma_RF));

  Data data_fd(model);
  VectorXd q_plus(model.nq);
  VectorXd v_eps(model.nv); v_eps.setZero();
  VectorXd v_plus(v);
  VectorXd tau_plus(tau);
  const double eps = 1e-8;

  // check: dddq_dtau and dlambda_dtau
  MatrixXd dddq_dtau(model.nv,model.nv);
  Data::Matrix6x dlambda_dtau(6,model.nv);

  for(int k = 0; k < model.nv; ++k)
  {
    tau_plus[k] += eps;
    forwardDynamics(model, data_fd, q, v, tau_plus, J_RF, gamma_RF);

    const Data::TangentVectorType & ddq_plus = data_fd.ddq;
    Force::Vector6 contact_force_plus = data_fd.lambda_c;

    dddq_dtau.col(k) = (ddq_plus - ddq_ref)/eps;
    dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref)/eps;

    tau_plus[k] -= eps;
  }

  MatrixXd A(model.nv+6,model.nv+6);
  data.M.transpose().triangularView<Eigen::Upper>() = data.M.triangularView<Eigen::Upper>();
  A.topLeftCorner(model.nv,model.nv) = data.M;
  A.bottomLeftCorner(6, model.nv) = J_RF;
  A.topRightCorner(model.nv, 6) = J_RF.transpose();
  A.bottomRightCorner(6,6).setZero();

  MatrixXd Ainv = A.inverse();
  BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau,std::sqrt(eps)));
  BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau,std::sqrt(eps)));

  // check: dddq_dv and dlambda_dv
  MatrixXd dddq_dv(model.nv,model.nv);
  Data::Matrix6x dlambda_dv(6,model.nv);

  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += eps;
    forwardDynamics(model, data_fd, q, v_plus, tau, J_RF, gamma_RF);

    const Data::TangentVectorType & ddq_plus = data_fd.ddq;
    Force::Vector6 contact_force_plus = data_fd.lambda_c;

    dddq_dv.col(k) = (ddq_plus - ddq_ref)/eps;
    dlambda_dv.col(k) = (contact_force_plus - contact_force_ref)/eps;

    v_plus[k] -= eps;
  }

  computeRNEADerivatives(model,data_check,q,v,VectorXd::Zero(model.nv));
  MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv;
  MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv;

  BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv,std::sqrt(eps)));
  BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv,std::sqrt(eps)));

  MatrixXd dddq_dq(model.nv,model.nv);
  Data::Matrix6x dlambda_dq(6,model.nv);

  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] = eps;
    q_plus = integrate(model,q,v_eps);
    computeJointJacobians(model, data_fd, q_plus);
    getJointJacobian(model, data_fd, RF_id, LOCAL, J_RF);
    forwardDynamics(model, data_fd, q_plus, v, tau, J_RF, gamma_RF);

    const Data::TangentVectorType & ddq_plus = data_fd.ddq;
    Force::Vector6 contact_force_plus = data_fd.lambda_c;

    dddq_dq.col(k) = (ddq_plus - ddq_ref)/eps;
    dlambda_dq.col(k) = (contact_force_plus - contact_force_ref)/eps;

    v_eps[k] = 0.;
  }

  computeRNEADerivatives(model,data_check,q,v,ddq_ref,fext);
  Data::Matrix6x v_partial_dq(6,model.nv), a_partial_dq(6,model.nv), a_partial_dv(6,model.nv), a_partial_da(6,model.nv);
  v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero();
  Data data_kin(model);
  computeForwardKinematicsDerivatives(model,data_kin,q,VectorXd::Zero(model.nv),ddq_ref);
  getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);

  MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq;
  dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq;

  MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq;
  dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq;

  BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps)));
  BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps)));

}

template<typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
VectorXd contactDynamics(const Model & model, Data & data,
                         const Eigen::MatrixBase<ConfigVectorType> & q,
                         const Eigen::MatrixBase<TangentVectorType1> & v,
                         const Eigen::MatrixBase<TangentVectorType2> & tau,
                         const Model::JointIndex id)
{
  computeJointJacobians(model, data, q);
  Data::Matrix6x J(6,model.nv); J.setZero();

  getJointJacobian(model, data, id, LOCAL, J);
  Motion::Vector6 gamma;
  forwardKinematics(model, data, q, v, VectorXd::Zero(model.nv));
  gamma = data.a[id].toVector();

  forwardDynamics(model, data, q, v, tau, J, gamma);
  VectorXd res(VectorXd::Zero(model.nv+6));

  res.head(model.nv) = data.ddq;
  res.tail(6) = data.lambda_c;

  return res;
}

BOOST_AUTO_TEST_CASE ( test_FD_with_contact_varying_gamma )
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model), data_check(model);

  VectorXd q = VectorXd::Ones(model.nq);
  normalize(model,q);

  VectorXd v = VectorXd::Random(model.nv);
  VectorXd tau = VectorXd::Random(model.nv);

  const std::string RF = "rleg6_joint";
  const Model::JointIndex RF_id = model.getJointId(RF);

  Data::Matrix6x J_RF(6,model.nv); J_RF.setZero();
  computeJointJacobians(model, data, q);
  getJointJacobian(model, data, RF_id, LOCAL, J_RF);
  Motion::Vector6 gamma_RF; gamma_RF.setZero();

  VectorXd x_ref = contactDynamics(model,data,q,v,tau,RF_id);
  VectorXd ddq_ref = x_ref.head(model.nv);
  Force::Vector6 contact_force_ref = x_ref.tail(6);

  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext((size_t)model.njoints,Force::Zero());
  fext[RF_id] = ForceRef<Force::Vector6>(contact_force_ref);

  // check call to RNEA
  rnea(model,data_check,q,v,ddq_ref,fext);

  BOOST_CHECK(data_check.tau.isApprox(tau));
  forwardKinematics(model,data_check,q,v,ddq_ref);
  BOOST_CHECK(data_check.a[RF_id].toVector().isZero());

  Data data_fd(model);
  VectorXd q_plus(model.nq);
  VectorXd v_eps(model.nv); v_eps.setZero();
  VectorXd v_plus(v);
  VectorXd tau_plus(tau);
  VectorXd x_plus(model.nv + 6);
  const double eps = 1e-8;

  // check: dddq_dtau and dlambda_dtau
  MatrixXd dddq_dtau(model.nv,model.nv);
  Data::Matrix6x dlambda_dtau(6,model.nv);

  for(int k = 0; k < model.nv; ++k)
  {
    tau_plus[k] += eps;
    x_plus = contactDynamics(model,data,q,v,tau_plus,RF_id);

    const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
    Force::Vector6 contact_force_plus = x_plus.tail(6);

    dddq_dtau.col(k) = (ddq_plus - ddq_ref)/eps;
    dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref)/eps;

    tau_plus[k] -= eps;
  }

  MatrixXd A(model.nv+6,model.nv+6);
  data.M.transpose().triangularView<Eigen::Upper>() = data.M.triangularView<Eigen::Upper>();
  A.topLeftCorner(model.nv,model.nv) = data.M;
  A.bottomLeftCorner(6, model.nv) = J_RF;
  A.topRightCorner(model.nv, 6) = J_RF.transpose();
  A.bottomRightCorner(6,6).setZero();

  MatrixXd Ainv = A.inverse();
  BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau,std::sqrt(eps)));
  BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau,std::sqrt(eps)));

  // check: dddq_dv and dlambda_dv
  MatrixXd dddq_dv(model.nv,model.nv);
  Data::Matrix6x dlambda_dv(6,model.nv);

  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += eps;
    x_plus = contactDynamics(model,data,q,v_plus,tau,RF_id);

    const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
    Force::Vector6 contact_force_plus = x_plus.tail(6);

    dddq_dv.col(k) = (ddq_plus - ddq_ref)/eps;
    dlambda_dv.col(k) = (contact_force_plus - contact_force_ref)/eps;

    v_plus[k] -= eps;
  }

  computeRNEADerivatives(model,data_check,q,v,VectorXd::Zero(model.nv));
  Data::Matrix6x v_partial_dq(6,model.nv), a_partial_dq(6,model.nv), a_partial_dv(6,model.nv), a_partial_da(6,model.nv);
  v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero();
  Data data_kin(model);
  computeForwardKinematicsDerivatives(model,data_kin,q,v,VectorXd::Zero(model.nv));
  getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);

  MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv;
  dddq_dv_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dv;
  MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv;
  dlambda_dv_anal -= Ainv.bottomRows(6).rightCols(6) * a_partial_dv;

  BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv,std::sqrt(eps)));
  BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv,std::sqrt(eps)));

  MatrixXd dddq_dq(model.nv,model.nv);
  Data::Matrix6x dlambda_dq(6,model.nv);

  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] = eps;
    q_plus = integrate(model,q,v_eps);

    x_plus = contactDynamics(model,data,q_plus,v,tau,RF_id);

    const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
    Force::Vector6 contact_force_plus = x_plus.tail(6);

    dddq_dq.col(k) = (ddq_plus - ddq_ref)/eps;
    dlambda_dq.col(k) = (contact_force_plus - contact_force_ref)/eps;

    v_eps[k] = 0.;
  }

  computeRNEADerivatives(model,data_check,q,v,ddq_ref,fext);
  v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero();
  computeForwardKinematicsDerivatives(model,data_kin,q,v,ddq_ref);
  getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);

  MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq;
  dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq;

  BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps)));

  MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq;
  dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq;

  BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps)));

}

BOOST_AUTO_TEST_SUITE_END ()


#include <boost/math/distributions/pareto.hpp>


#pragma once

#include <array>
#include <Eigen/Dense>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Boolean_set_operations_2.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/Polygon_with_holes_2.h>
#include <CGAL/Aff_transformation_2.h>

#include <object_model_msgs/msg/object_model.hpp>

using state_t = std::array<float, object_model_msgs::msg::Track::STATE_SIZE>;
using state_squared_t = std::array<float, object_model_msgs::msg::Track::STATE_SIZE*object_model_msgs::msg::Track::STATE_SIZE>;

using state_vector_t = Eigen::Matrix<float, object_model_msgs::msg::Track::STATE_SIZE, 1>;
using state_covariance_matrix_t = Eigen::Matrix<float, object_model_msgs::msg::Track::STATE_SIZE, object_model_msgs::msg::Track::STATE_SIZE>;
using measurement_noise_matrix_t = Eigen::Matrix<float, object_model_msgs::msg::Track::STATE_SIZE, object_model_msgs::msg::Track::STATE_SIZE>;
using process_noise_matrix_t = Eigen::Matrix<float, object_model_msgs::msg::Track::STATE_SIZE, object_model_msgs::msg::Track::STATE_SIZE>;

using capable_vector_t = std::array<bool, object_model_msgs::msg::Track::STATE_SIZE>;

// EKF - Temporal Alignment
constexpr int ctra_size_t = 6;
using ctra_vector_t = Eigen::Matrix<float, ctra_size_t, 1>;
using ctra_matrix_t = Eigen::Matrix<float, ctra_size_t, ctra_size_t>;
using ctra_array_t = std::array<float, 6>;
using ctra_squared_t = std::array<float, ctra_size_t*ctra_size_t>;

// CGAL
using Kernel = CGAL::Exact_predicates_exact_constructions_kernel;
using Polygon = CGAL::Polygon_2<Kernel>;
using Point = CGAL::Point_2<Kernel>;
using PolygonWithHoles = CGAL::Polygon_with_holes_2<Kernel>;
using Transformation = CGAL::Aff_transformation_2<Kernel>;