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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.
// Copyright (c) 2013-2015 Adam Wulkiewicz, Lodz, Poland.

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

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_DISJOINT_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_DISJOINT_HPP

#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/algorithms/comparable_distance.hpp>

#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/geometry/extensions/nsphere/views/center_view.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace disjoint
{

// Arvo's algorithm implemented
// TODO - implement the method mentioned in the article below and compare performance
// "On Faster Sphere-Box Overlap Testing" - Larsson, T.; Akeine-Moller, T.; Lengyel, E.
template
<
    typename Box, typename NSphere,
    std::size_t Dimension, std::size_t DimensionCount
>
struct box_nsphere_comparable_distance_cartesian
{
    typedef typename geometry::select_most_precise
        <
            typename coordinate_type<Box>::type,
            typename coordinate_type<NSphere>::type
        >::type coordinate_type;

    typedef typename geometry::default_distance_result
        <
            Box,
            typename point_type<NSphere>::type
        >::type result_type;

    static inline result_type apply(Box const& box, NSphere const& nsphere)
    {
        result_type r = 0;

        if( get<Dimension>(nsphere) < get<min_corner, Dimension>(box) )
        {
            coordinate_type tmp = get<min_corner, Dimension>(box) - get<Dimension>(nsphere);
            r = tmp*tmp;
        }
        else if( get<max_corner, Dimension>(box) < get<Dimension>(nsphere) )
        {
            coordinate_type tmp = get<Dimension>(nsphere) - get<max_corner, Dimension>(box);
            r = tmp*tmp;
        }

        return r + box_nsphere_comparable_distance_cartesian<
                        Box, NSphere, Dimension + 1, DimensionCount
                    >::apply(box, nsphere);
    }
};

template <typename Box, typename NSphere, std::size_t DimensionCount>
struct box_nsphere_comparable_distance_cartesian<Box, NSphere, DimensionCount, DimensionCount>
{
    static inline int apply(Box const& , NSphere const& )
    {
        return 0;
    }
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Point, typename NSphere, std::size_t DimensionCount, bool Reverse>
struct disjoint<Point, NSphere, DimensionCount, point_tag, nsphere_tag, Reverse>
{
    static inline bool apply(Point const& p, NSphere const& s)
    {
        typedef typename coordinate_system<Point>::type p_cs;
        typedef typename coordinate_system<NSphere>::type s_cs;
        static const bool check_cs = ::boost::is_same<p_cs, cs::cartesian>::value
                                  && ::boost::is_same<s_cs, cs::cartesian>::value;
        BOOST_MPL_ASSERT_MSG(check_cs,
                             NOT_IMPLEMENTED_FOR_THOSE_COORDINATE_SYSTEMS,
                             (p_cs, s_cs));

        typename radius_type<NSphere>::type const r = get_radius<0>(s);
        center_view<const NSphere> const c(s);

        return r * r < geometry::comparable_distance(p, c);
    }
};

template <typename NSphere, typename Box, std::size_t DimensionCount, bool Reverse>
struct disjoint<NSphere, Box, DimensionCount, nsphere_tag, box_tag, Reverse>
{
    static inline bool apply(NSphere const& s, Box const& b)
    {
        typedef typename coordinate_system<Box>::type b_cs;
        typedef typename coordinate_system<NSphere>::type s_cs;
        static const bool check_cs = ::boost::is_same<b_cs, cs::cartesian>::value
                                  && ::boost::is_same<s_cs, cs::cartesian>::value;
        BOOST_MPL_ASSERT_MSG(check_cs,
                             NOT_IMPLEMENTED_FOR_THOSE_COORDINATE_SYSTEMS,
                             (b_cs, s_cs));

        typename radius_type<NSphere>::type const r = get_radius<0>(s);

        return r * r < geometry::detail::disjoint::box_nsphere_comparable_distance_cartesian
                           <
                               Box, NSphere, 0, DimensionCount
                           >::apply(b, s);
    }
};

template <typename NSphere1, typename NSphere2, std::size_t DimensionCount, bool Reverse>
struct disjoint<NSphere1, NSphere2, DimensionCount, nsphere_tag, nsphere_tag, Reverse>
{
    static inline bool apply(NSphere1 const& s1, NSphere2 const& s2)
    {
        typedef typename coordinate_system<NSphere1>::type s1_cs;
        typedef typename coordinate_system<NSphere2>::type s2_cs;
        static const bool check_cs = ::boost::is_same<s1_cs, cs::cartesian>::value
                                  && ::boost::is_same<s2_cs, cs::cartesian>::value;
        BOOST_MPL_ASSERT_MSG(check_cs,
                             NOT_IMPLEMENTED_FOR_THOSE_COORDINATE_SYSTEMS,
                             (s1_cs, s2_cs));

        /*return get_radius<0>(s1) + get_radius<0>(s2)
               <   ::sqrt(geometry::comparable_distance(center_view<NSphere>(s1), center_view<NSphere>(s2)));*/

        typename radius_type<NSphere1>::type const r1 = get_radius<0>(s1);
        typename radius_type<NSphere2>::type const r2 = get_radius<0>(s2);
        center_view<NSphere1 const> const c1(s1);
        center_view<NSphere2 const> const c2(s2);

        return r1 * r1 + 2 * r1 * r2 + r2 * r2
                < geometry::comparable_distance(c1, c2);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_DISJOINT_HPP


/*
 [auto_generated]
 libs/numeric/odeint/examples/molecular_dynamics_cells.cpp

 [begin_description]
 Molecular dynamics example with cells.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

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

#include <boost/numeric/odeint.hpp>

#include <cstddef>
#include <vector>
#include <cmath>
#include <algorithm>
#include <tuple>
#include <iostream>
#include <random>

#include <boost/range/algorithm/for_each.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/range/algorithm/unique_copy.hpp>
#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/iterator/zip_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/permutation_iterator.hpp>
#include <boost/iterator/counting_iterator.hpp>

#include "point_type.hpp"

struct local_force
{
    double m_gamma;        // friction
    local_force( double gamma = 0.0 ) : m_gamma( gamma ) { }
    template< typename Point >
    Point operator()( Point& x , Point& v ) const
    {
        return - m_gamma * v;
    }
};

struct lennard_jones
{
    double m_sigma;
    double m_eps;
    lennard_jones( double sigma = 1.0 , double eps = 0.1 ) : m_sigma( sigma ) , m_eps( eps ) { }
    double operator()( double r ) const
    {
        double c = m_sigma / r;
        double c3 = c * c * c;
        double c6 = c3 * c3;
        return 4.0 * m_eps * ( -12.0 * c6 * c6 / r + 6.0 * c6 / r );
    }
};

template< typename F >
struct conservative_interaction
{
    F m_f;
    conservative_interaction( F const &f = F() ) : m_f( f ) { }
    template< typename Point >
    Point operator()( Point const& x1 , Point const& x2 ) const
    {
        Point diff = x1 - x2;
        double r = abs( diff );
        double f = m_f( r );
        return - diff / r * f;
    }
};

template< typename F >
conservative_interaction< F > make_conservative_interaction( F const &f )
{
    return conservative_interaction< F >( f );
}

// force = interaction( x1 , x2 )
// force = local_force( x , v )
template< typename LocalForce , typename Interaction >
class md_system_bs
{
public:

    typedef std::vector< double > vector_type;
    typedef point< double , 2 > point_type;
    typedef point< int , 2 > index_type;
    typedef std::vector< point_type > point_vector;
    typedef std::vector< index_type > index_vector;
    typedef std::vector< size_t > hash_vector;
    typedef LocalForce local_force_type;
    typedef Interaction interaction_type;

    struct params
    {
        size_t n;
        size_t n_cell_x , n_cell_y , n_cells;
        double x_max , y_max , cell_size;
        double eps , sigma;    // interaction strength, interaction radius
        interaction_type interaction;
        local_force_type local_force;
    };

    struct cell_functor
    {
        params const &m_p;

        cell_functor( params const& p ) : m_p( p ) { }

        template< typename Tuple >
        void operator()( Tuple const& t ) const
        {
            auto point = boost::get< 0 >( t );
            size_t i1 = size_t( point[0] / m_p.cell_size ) , i2 = size_t( point[1] / m_p.cell_size );
            boost::get< 1 >( t ) = index_type( i1 , i2 );
            boost::get< 2 >( t ) = hash_func( boost::get< 1 >( t ) , m_p );
        }
    };

    struct transform_functor
    {
        typedef size_t argument_type;
        typedef size_t result_type;
        hash_vector const* m_index;
        transform_functor( hash_vector const& index ) : m_index( &index ) { }
        size_t operator()( size_t i ) const { return (*m_index)[i]; }
    };

    struct interaction_functor
    {
        hash_vector const &m_cells_begin;
        hash_vector const &m_cells_end;
        hash_vector const &m_order;
        point_vector const &m_x;
        point_vector const &m_v;
        params const &m_p;
        size_t m_ncellx , m_ncelly;

        interaction_functor( hash_vector const& cells_begin , hash_vector const& cells_end , hash_vector pos_order ,
                            point_vector const&x , point_vector const& v , params const &p )
        : m_cells_begin( cells_begin ) , m_cells_end( cells_end ) , m_order( pos_order ) , m_x( x ) , m_v( v ) ,
        m_p( p ) { }

        template< typename Tuple >
        void operator()( Tuple const &t ) const
        {
            point_type x = periodic_bc( boost::get< 0 >( t ) , m_p ) , v = boost::get< 1 >( t );
            index_type index = boost::get< 3 >( t );
            size_t pos_hash = boost::get< 4 >( t );

            point_type a = m_p.local_force( x , v );

            for( int i=-1 ; i<=1 ; ++i )
            {
                for( int j=-1 ; j<=1 ; ++j )
                {
                    index_type cell_index = index + index_type( i , j );
                    size_t cell_hash = hash_func( cell_index , m_p );
                    for( size_t ii = m_cells_begin[ cell_hash ] ; ii < m_cells_end[ cell_hash ] ; ++ii )
                    {
                        if( m_order[ ii ] == pos_hash ) continue;
                        point_type x2 = periodic_bc( m_x[ m_order[ ii ] ] , m_p );

                        if( cell_index[0] >= m_p.n_cell_x ) x2[0] += m_p.x_max;
                        if( cell_index[0] < 0 ) x2[0] -= m_p.x_max;
                        if( cell_index[1] >= m_p.n_cell_y ) x2[1] += m_p.y_max;
                        if( cell_index[1] < 0 ) x2[1] -= m_p.y_max;

                        a += m_p.interaction( x , x2 );
                    }
                }
            }
            boost::get< 2 >( t ) = a;
        }
    };

    md_system_bs( size_t n ,
                  local_force_type const& local_force = local_force_type() ,
                  interaction_type const& interaction = interaction_type() ,
                  double xmax = 100.0 , double ymax = 100.0 , double cell_size = 2.0 )
    : m_p()
    {
        m_p.n = n;
        m_p.x_max = xmax;
        m_p.y_max = ymax;
        m_p.interaction = interaction;
        m_p.local_force = local_force;
        m_p.n_cell_x = size_t( xmax / cell_size );
        m_p.n_cell_y = size_t( ymax / cell_size );
        m_p.n_cells = m_p.n_cell_x * m_p.n_cell_y;
        m_p.cell_size = cell_size;
    }

    void init_point_vector( point_vector &x ) const { x.resize( m_p.n ); }

    void operator()( point_vector const& x , point_vector const& v , point_vector &a , double t ) const
    {
        // init
        hash_vector pos_hash( m_p.n , 0 );
        index_vector pos_index( m_p.n );
        hash_vector pos_order( m_p.n , 0 );
        hash_vector cells_begin( m_p.n_cells ) , cells_end( m_p.n_cells ) , cell_order( m_p.n_cells );

        boost::iota( pos_order , 0 );
        boost::iota( cell_order , 0 );

        // calculate grid hash
        // calcHash( m_dGridParticleHash, m_dGridParticleIndex, dPos, m_numParticles);
        std::for_each(
            boost::make_zip_iterator( boost::make_tuple( x.begin() , pos_index.begin() , pos_hash.begin() ) ) ,
            boost::make_zip_iterator( boost::make_tuple( x.end() , pos_index.end() , pos_hash.end() ) ) ,
            cell_functor( m_p ) );

//         // sort particles based on hash
//         // sortParticles(m_dGridParticleHash, m_dGridParticleIndex, m_numParticles);
        boost::sort( pos_order , [&]( size_t i1 , size_t i2 ) -> bool {
            return pos_hash[i1] < pos_hash[i2]; } );

        // reorder particle arrays into sorted order and find start and end of each cell
        std::for_each( cell_order.begin() , cell_order.end() , [&]( size_t i ) {
            auto pos_begin = boost::make_transform_iterator( pos_order.begin() , transform_functor( pos_hash ) );
            auto pos_end = boost::make_transform_iterator( pos_order.end() , transform_functor( pos_hash ) );
            cells_begin[ i ] = std::distance( pos_begin , std::lower_bound( pos_begin , pos_end , i ) );
            cells_end[ i ] = std::distance( pos_begin , std::upper_bound( pos_begin , pos_end , i ) );
        } );

        std::for_each(
            boost::make_zip_iterator( boost::make_tuple(
                x.begin() ,
                v.begin() ,
                a.begin() ,
                pos_index.begin() ,
                boost::counting_iterator< size_t >( 0 )
            ) ) ,
            boost::make_zip_iterator( boost::make_tuple(
                x.end() ,
                v.end() ,
                a.end() ,
                pos_index.end() ,
                boost::counting_iterator< size_t >( m_p.n )
            ) ) ,
            interaction_functor( cells_begin , cells_end , pos_order , x , v , m_p ) );
    }

    void bc( point_vector &x )
    {
        for( size_t i=0 ; i<m_p.n ; ++i )
        {
            x[i][0] = periodic_bc( x[ i ][0] , m_p.x_max );
            x[i][1] = periodic_bc( x[ i ][1] , m_p.y_max );
        }
    }

    static inline double periodic_bc( double x , double xmax )
    {
        double tmp = x - xmax * int( x / xmax );
        return tmp >= 0.0 ? tmp : tmp + xmax;
    }

    static inline point_type periodic_bc( point_type const& x , params const& p )
    {
        return point_type( periodic_bc( x[0] , p.x_max ) , periodic_bc( x[1] , p.y_max ) );
    }

    static inline int check_interval( int i , int max )
    {
        int tmp = i % max;
        return tmp >= 0 ? tmp : tmp + max;
    }

    static inline size_t hash_func( index_type index , params const & p )
    {
        size_t i1 = check_interval( index[0] , p.n_cell_x );
        size_t i2 = check_interval( index[1] , p.n_cell_y );
        return i1 * p.n_cell_y + i2;
    }

    params m_p;
};

template< typename LocalForce , typename Interaction >
md_system_bs< LocalForce , Interaction > make_md_system_bs( size_t n , LocalForce const &f1 , Interaction const &f2 ,
    double xmax = 100.0 , double ymax = 100.0 , double cell_size = 2.0 )
{
    return md_system_bs< LocalForce , Interaction >( n , f1 , f2 , xmax , ymax , cell_size );
}

using namespace boost::numeric::odeint;

int main( int argc , char *argv[] )
{
    const size_t n1 = 32;
    const size_t n2 = 32;
    const size_t n = n1 * n2;
    auto sys = make_md_system_bs( n , local_force() , make_conservative_interaction( lennard_jones() ) , 100.0 , 100.0 , 2.0 );
    typedef decltype( sys ) system_type;
    typedef system_type::point_vector point_vector;

    std::mt19937 rng;
    std::normal_distribution<> dist( 0.0 , 1.0 );

    point_vector x , v;
    sys.init_point_vector( x );
    sys.init_point_vector( v );

    for( size_t i=0 ; i<n1 ; ++i )
    {
        for( size_t j=0 ; j<n2 ; ++j )
        {
            size_t index = i * n2 + j;
            x[index][0] = 10.0 + i * 2.0 ;
            x[index][1] = 10.0 + j * 2.0 ;
            v[index][0] = dist( rng ) ;
            v[index][1] = dist( rng ) ;
        }
    }

    velocity_verlet< point_vector > stepper;
    const double dt = 0.025;
    double t = 0.0;
    // std::cout << "set term x11" << endl;
    for( size_t oi=0 ; oi<10000 ; ++oi )
    {
        for( size_t ii=0 ; ii<50 ; ++ii,t+=dt )
            stepper.do_step( sys , std::make_pair( std::ref( x ) , std::ref( v ) ) , t , dt );
        sys.bc( x );

        std::cout << "set size square" << "\n";
        std::cout << "unset key" << "\n";
        std::cout << "p [0:" << sys.m_p.x_max << "][0:" << sys.m_p.y_max << "] '-' pt 7 ps 0.5" << "\n";
        for( size_t i=0 ; i<n ; ++i )
            std::cout << x[i][0] << " " << x[i][1] << " " << v[i][0] << " " << v[i][1] << "\n";
        std::cout << "e" << std::endl;
    }

    return 0;
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file gaussian_filter_linear_vs_x_test_suite.hpp
 * \date July 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>
#include "../typecast.hpp"

#include <Eigen/Dense>

#include <cmath>
#include <iostream>

#include <fl/util/math/linear_algebra.hpp>
#include <fl/filter/filter_interface.hpp>

#include <fl/filter/gaussian/gaussian_filter_linear.hpp>
#include <fl/model/transition/linear_transition.hpp>
#include <fl/model/sensor/linear_gaussian_sensor.hpp>
#include <fl/model/sensor/linear_decorrelated_gaussian_sensor.hpp>

enum : signed int
{
    DecorrelatedGaussianModel,
    GaussianModel
};

static constexpr double epsilon = 0.1;

template <typename TestType>
class GaussianFilterLinearVsXTest
    : public ::testing::Test
{
protected:
    typedef typename TestType::Parameter Configuration;

    enum: signed int
    {
        StateDim = Configuration::StateDim,
        InputDim = Configuration::InputDim,
        ObsrvDim = Configuration::ObsrvDim,

        StateSize = fl::TestSize<StateDim, TestType>::Value,
        InputSize = fl::TestSize<InputDim, TestType>::Value,
        ObsrvSize = fl::TestSize<ObsrvDim, TestType>::Value
    };

    enum ModelSetup
    {
        Random,
        Identity
    };

    typedef Eigen::Matrix<fl::Real, StateSize, 1> State;
    typedef Eigen::Matrix<fl::Real, InputSize, 1> Input;
    typedef Eigen::Matrix<fl::Real, ObsrvSize, 1> Obsrv;

    typedef fl::IntegerTypeMap<
                fl::IntegerTypePair<
                    DecorrelatedGaussianModel,
                    fl::LinearDecorrelatedGaussianSensor<Obsrv, State>
                >,
                fl::IntegerTypePair<
                    GaussianModel,
                    fl::LinearGaussianSensor<Obsrv, State>
                >
            > SensorMap;

    typedef fl::LinearTransition<State, State, Input> LinearTransition;

    typedef typename SensorMap::template Select<
                Configuration::SelectedModel
            >::Type LinearSensor;

    typedef fl::GaussianFilter<
                LinearTransition, LinearSensor
            > KalmanFilter;

    typedef typename Configuration::template FilterDefinition<
                LinearTransition,
                LinearSensor
            > FilterDefinition;

    typedef typename FilterDefinition::Type Filter;

    GaussianFilterLinearVsXTest()
        : predict_steps_(200),
          predict_update_steps_(30)
    { }

    KalmanFilter create_kalman_filter() const
    {
        return KalmanFilter(
                LinearTransition(StateDim, InputDim),
                LinearSensor(ObsrvDim, StateDim));
    }

    Filter create_filter() const
    {
        return Configuration::create_filter(
                LinearTransition(StateDim, InputDim),
                LinearSensor(ObsrvDim, StateDim));
    }

    void setup_models(
        KalmanFilter& kalman_filter, Filter& other_filter, ModelSetup setup)
    {
        auto A = kalman_filter.transition().create_dynamics_matrix();
        auto B = kalman_filter.transition().create_input_matrix();
        auto Q = kalman_filter.transition().create_noise_matrix();

        auto H = kalman_filter.sensor().create_sensor_matrix();
        auto R = kalman_filter.sensor().create_noise_matrix();

        Q.setZero();
        R.setZero();
        B.setZero();

        switch (setup)
        {
        case Random:
            A.diagonal().setRandom();
            H.diagonal().setRandom();
            Q.diagonal().setRandom(); Q *= Q.transpose().eval();
            R.diagonal().setRandom(); R *= R.transpose().eval();
            break;

        case Identity:
            A.setIdentity();
            H.setIdentity();
            Q.setIdentity();
            R.setIdentity();
            break;
        }

        kalman_filter.transition().dynamics_matrix(A);
        kalman_filter.transition().input_matrix(B);
        kalman_filter.transition().noise_matrix(Q);
        kalman_filter.sensor().sensor_matrix(H);
        kalman_filter.sensor().noise_matrix(R);

        other_filter.transition().dynamics_matrix(A);
        other_filter.transition().input_matrix(B);
        other_filter.transition().noise_matrix(Q);
        other_filter.sensor().sensor_matrix(H);
        other_filter.sensor().noise_matrix(R);
    }

    State zero_state() { return State::Zero(StateDim); }
    Input zero_input() { return Input::Zero(InputDim); }
    Obsrv zero_obsrv() { return Obsrv::Zero(ObsrvDim); }

    State rand_state() { return State::Random(StateDim); }
    Input rand_input() { return Input::Random(InputDim); }
    Obsrv rand_obsrv() { return Obsrv::Random(ObsrvDim); }

protected:
    int predict_steps_;
    int predict_update_steps_;
};

TYPED_TEST_CASE_P(GaussianFilterLinearVsXTest);

//TYPED_TEST_P(GaussianFilterLinearVsXTest, predict)
//{
//    typedef TestFixture This;

//    auto other_filter = This::create_filter();
//    auto kalman_filter = This::create_kalman_filter();

//    This::setup_models(kalman_filter, other_filter, This::Random);

//    auto belief_other = other_filter.create_belief();
//    auto belief_kf = kalman_filter.create_belief();

//    for (int i = 0; i < This::predict_steps_; ++i)
//    {
//        other_filter.predict(belief_other, This::zero_input(), belief_other);
//        kalman_filter.predict(belief_kf, This::zero_input(), belief_kf);

//        if (!fl::are_similar(belief_other.mean(), belief_kf.mean(), epsilon))
//        {
//            std::cout << "i = " << i << std::endl;
//            PV(belief_kf.mean());
//            PV(belief_other.mean());
//        }

//        if (!fl::are_similar(belief_other.covariance(), belief_kf.covariance(), epsilon))
//        {
//            std::cout << "i = " << i << std::endl;
//            PV(belief_kf.covariance());
//            PV(belief_other.covariance());
//        }

//        ASSERT_TRUE(
//            fl::are_similar(belief_other.mean(), belief_kf.mean(), epsilon));

//        ASSERT_TRUE(
//            fl::are_similar(belief_other.covariance(), belief_kf.covariance(), epsilon));
//    }

//    PV(belief_kf.mean());
//    PV(belief_other.mean());
//    PV(belief_kf.covariance());
//    PV(belief_other.covariance());
//}

TYPED_TEST_P(GaussianFilterLinearVsXTest, predict_and_update)
{
    typedef TestFixture This;

    auto other_filter = This::create_filter();
    auto kalman_filter = This::create_kalman_filter();

    This::setup_models(kalman_filter, other_filter, This::Random);

    auto belief_other = other_filter.create_belief();
    auto belief_kf = kalman_filter.create_belief();

    for (int i = 0; i < This::predict_update_steps_; ++i)
    {
        auto y = This::rand_obsrv();

        kalman_filter.predict(belief_kf, This::zero_input(), belief_kf);
        other_filter.predict(belief_other, This::zero_input(), belief_other);

//        if (!fl::are_similar(belief_other.mean(), belief_kf.mean(), epsilon))
//        {
//            std::cout << "predict i = " << i << std::endl;
//            PV(belief_kf.mean());
//            PV(belief_other.mean());
//        }

//        if (!fl::are_similar(belief_other.covariance(), belief_kf.covariance(), epsilon))
//        {
//            std::cout << "predict i = " << i << std::endl;
//            PV(belief_kf.covariance());
//            PV(belief_other.covariance());
//        }

        kalman_filter.update(belief_kf, y, belief_kf);
        other_filter.update(belief_other, y, belief_other);

//        if (!fl::are_similar(belief_other.mean(), belief_kf.mean(), epsilon))
//        {
//            std::cout << "update i = " << i << std::endl;
//            PV(belief_kf.mean());
//            PV(belief_other.mean());
//        }

//        if (!fl::are_similar(belief_other.covariance(), belief_kf.covariance(), epsilon))
//        {
//            std::cout << "update i = " << i << std::endl;
//            PV(belief_kf.covariance());
//            PV(belief_other.covariance());
//        }

        ASSERT_TRUE(
            fl::are_similar(belief_other.mean(), belief_kf.mean(), epsilon));

        ASSERT_TRUE(
            fl::are_similar(belief_other.covariance(), belief_kf.covariance(), epsilon));
    }

//    PV(belief_kf.mean());
//    PV(belief_other.mean());

//    PV(belief_kf.covariance());
//    PV(belief_other.covariance());

//    std::cout << other_filter.name() << std::endl;
}

REGISTER_TYPED_TEST_CASE_P(GaussianFilterLinearVsXTest,
                           predict_and_update);


#include <iostream>
#include <vector>
#include <bitset>
#include <random>
#include <Eigen/Dense>
#include "utils.h"
#include "SC.h"

//using Eigen::MatrixXd;

std::vector< std::vector<double> > SC_true_abs_err;
std::vector< std::vector<double> > SC_LFSR_abs_err;
std::vector< std::vector<double> > SC_LD_abs_err;

std::vector<double> SC_true_RMSE;
std::vector<double> SC_LFSR_RMSE;
std::vector<double> SC_LD_RMSE;

void init()
{
    //fill the vector till position x

    int size = 32;

    SC_true_abs_err.reserve(size);
    SC_LFSR_abs_err.reserve(size);
    SC_LD_abs_err.reserve(size);

    SC_true_RMSE.reserve(size);
    SC_LFSR_RMSE.reserve(size);
    SC_LD_RMSE.reserve(size);

    std::vector<double> tmp(1);

    for(int i = 0; i < size; i++)
    {
        SC_true_abs_err.push_back(tmp);
        SC_LFSR_abs_err.push_back(tmp);
        SC_LD_abs_err.push_back(tmp);

        SC_true_RMSE.push_back(0);
        SC_LFSR_RMSE.push_back(0);
        SC_LD_RMSE.push_back(0);
    }
}

void Test_utils_compare()
{
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> dis(-1.0, 1.0);

    for(int i = 0; i < 1000; i++)
    {
        double tmp1 = dis(gen);
        double tmp2 = dis(gen);

        Complement a(16, tmp1);
        Complement b(16, tmp2);

        bool flag_true = tmp1 > tmp2 ? 1 : 0;
        bool flag_compare = compare(a, b);

        if(flag_true != flag_compare)
        {
            std::cout << flag_true << ' ' << flag_compare << std::endl;
            std::cout << tmp1 << ' ' << tmp2 << std::endl;
            compare(a, b, true);
            return;
        }
    }

    printf("unit test utils compare() done, all green\n");

}

void Test_SC_err(std::string RNG_type, int data_bits, int scaling_factor, int num)
{
    //return abs error and RMSE(Root Mean Squared Error) of different type of SN

    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> dis(-8.0, 8.0);

    SC sc("bipolar", RNG_type, data_bits, scaling_factor);

    //printf("%d start\n", data_bits);

    std::vector<double> tmp_abs;
    std::vector<double> tmp_RMSE;

    tmp_abs.reserve(num);
    tmp_RMSE.reserve(num);

    for(int i = 0; i < num; i++)
    {
        double a = dis(gen);
        double b = dis(gen);

        double ans = sc.SC_Mul(a, b);

        double abs_err = abs(ans - a * b);
        tmp_abs.push_back(abs_err);
        tmp_RMSE.push_back(abs_err * abs_err);
    }

    //printf("%d one done\n", data_bits);

    if(RNG_type == "true")
    {
        SC_true_abs_err[data_bits] = tmp_abs;
        SC_true_RMSE[data_bits] = Std_Dev(tmp_RMSE);
    }

    if(RNG_type == "LFSR")
    {
        SC_LFSR_abs_err[data_bits] = tmp_abs;
        SC_LFSR_RMSE[data_bits] = Std_Dev(tmp_RMSE);
    }

    if(RNG_type == "LD")
    {
        SC_LD_abs_err[data_bits] = tmp_abs;
        SC_LD_RMSE[data_bits] = Std_Dev(tmp_RMSE);
    }
}

void Test_LFSR()
{
    LFSR lfsr(8);

    for(int i = 0; i < 10; i++)
        std::cout << lfsr.next() << std::endl;

}

void Test_SC_LFSR()
{
    SC sc("bipolar", "LFSR", 16, 3);
}

void Test()
{
    SC sc("bipolar", "LFSR", 3, 3);

    double a = -4;
    double b = 4;

    double ans = sc.SC_Mul(a, b);

    //printf("%lf\n", ans);
}

void write(int x, int y)
{
    //write results of data_bits x to data_bits y to file for storing

    //true random
    for(int i = 2; i <= 16; i++)
        printf("%lf ", SC_true_RMSE[i]);

    printf("\n");

    //LFSR
    for(int i = 2; i <= 16; i++)
        printf("%lf ", SC_LFSR_RMSE[i]);

    printf("\n");

    //LD
    for(int i = 2; i <= 16; i++)
        printf("%lf ", SC_LD_RMSE[i]);

    printf("\n");
}

int main()
{
    init();

    int trials = 10000;

    for(int i = 2; i <= 16; i++)
    {
        Test_SC_err("true", i, 3, trials);
        Test_SC_err("LFSR", i, 3, trials);
        Test_SC_err("LD", i, 3, trials);
    }

    write(2, 16);

    //fwrite abs err
    fwrite_vec("../data/results/true_abs_err_10k.out", SC_true_abs_err);

    fwrite_vec("../data/results/LFSR_abs_err_10k.out", SC_LFSR_abs_err);

    fwrite_vec("../data/results/Halton_abs_err_10k.out", SC_LD_abs_err);

    //fwrite RMSE
    fwrite_vec("../data/results/true_RMSE_10k.out", SC_true_RMSE);

    fwrite_vec("../data/results/LFSR_RMSE_10k.out", SC_LFSR_RMSE);

    fwrite_vec("../data/results/Halton_RMSE_10k.out", SC_LD_RMSE);

    //Test();

    //Test_SC_LFSR();

    //Test_LFSR();

    /*LD ld(3, 5); //ld(base, sequence_length)

    for(int i = 0; i < 30; i++)
    {
        //ld.show_status();
        //ld.update_status();
        //ld.next();
        std::cout << ld.next() << std::endl;
    }
    */

    return 0;

}


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

using namespace std;
using namespace Eigen;

void print_image(VectorXd image, int num_rows, int num_cols);
void print_image_values(VectorXd image, int num_rows, int num_cols);
vector<int> get_labels(string path);
vector<VectorXd> get_images(string path);
vector<VectorXd> get_output_vectors(vector<int> labels);


//---------------------------------------------------------------------------//
//!
//! \file   tstGaussKronrodIntegrator.cpp
//! \author Luke Kersting
//! \brief  Gauss-Kronrod quadrature integrator unit tests.
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// Trilinos Includes
#include <Teuchos_UnitTestHarness.hpp>
#include <Teuchos_Array.hpp>

// FRENSIE Includes
#include "Utility_GaussKronrodIntegrator.hpp"

//---------------------------------------------------------------------------//
// Testing Functors
//---------------------------------------------------------------------------//
struct X2Functor
{
  double operator()( const double x ) const
  {
    if( x >= 0.0 && x <= 1.0 )
      return x*x;
    else
      return 0.0;
  }

  static double getIntegratedValue()
  {
    return 1.0/3.0;
  }

  static double getLowerIntegratedValue()
  {
    return 1.0/24.0;
  }

  static double getUpperIntegratedValue()
  {
    return 7.0/24.0;
  }
};

struct X3Functor
{
  double operator()( const double x ) const
  {
    if( x >= 0.0 && x <= 1.0 )
      return x*x*x;
    else
      return 0.0;
  }

  static double getIntegratedValue()
  {
    return 0.25;
  }

  static double getLowerIntegratedValue()
  {
    return 1.0/64.0;
  }

  static double getUpperIntegratedValue()
  {
    return 15.0/64.0;
  }
};

double exp_neg_x( const double x )
{
  return exp( -x );
}

double exp_neg_abs_x( const double x, const double a )
{
  return exp( -a*fabs(x) );
}

double inv_sqrt_abs_x( const double x )
{
  return 1/sqrt(fabs(x));
}

//---------------------------------------------------------------------------//
// Testing Structs.
//---------------------------------------------------------------------------//
class TestGaussKronrodIntegrator : public Utility::GaussKronrodIntegrator
{
public:
  TestGaussKronrodIntegrator( const double relative_error_tol )
    : Utility::GaussKronrodIntegrator( relative_error_tol )
  { /* ... */ }

  ~TestGaussKronrodIntegrator()
  { /* ... */ }

  // Allow public access to the GaussKronrodIntegrator protected member functions
  using Utility::GaussKronrodIntegrator::calculateQuadratureIntegrandValuesAtAbscissa;
  using Utility::GaussKronrodIntegrator::bisectAndIntegrateBinInterval;
  using Utility::GaussKronrodIntegrator::rescaleAbsoluteError;
  using Utility::GaussKronrodIntegrator::subintervalTooSmall;
  using Utility::GaussKronrodIntegrator::checkRoundoffError;
  using Utility::GaussKronrodIntegrator::sortBins;
  using Utility::GaussKronrodIntegrator::getWynnEpsilonAlgorithmExtrapolation;
};

//---------------------------------------------------------------------------//
// Instantiation macros.
//---------------------------------------------------------------------------//
#define UNIT_TEST_INSTANTIATION( type, name ) \
  TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT( type, name, X2Functor ) \
  TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT( type, name, X3Functor )

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that functions can be integrated over [0,1]
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( GaussKronrodIntegrator,
				                   integrateWithPointRule,
                                   Functor )
{
  Utility::GaussKronrodIntegrator gk_integrator( 1e-12 );

  double absolute_error, result_abs, result_asc, test_result, tol;
  long double result;

  Functor functor_instance;

  gk_integrator.integrateWithPointRule<15>( functor_instance,
                                        0.0,
                                        1.0,
                                        result,
                                        absolute_error,
                                        result_abs,
                                        result_asc );

  tol = absolute_error/result;
  test_result = result;

  TEST_FLOATING_EQUALITY( Functor::getIntegratedValue(), test_result, tol );

  gk_integrator.integrateWithPointRule<21>( functor_instance,
                                        0.0,
                                        1.0,
                                        result,
                                        absolute_error,
                                        result_abs,
                                        result_asc );

  tol = absolute_error/result;
  test_result = result;

  TEST_FLOATING_EQUALITY( Functor::getIntegratedValue(), test_result, tol );

  gk_integrator.integrateWithPointRule<31>( functor_instance,
                                        0.0,
                                        1.0,
                                        result,
                                        absolute_error,
                                        result_abs,
                                        result_asc );

  tol = absolute_error/result;
  test_result = result;

  TEST_FLOATING_EQUALITY( Functor::getIntegratedValue(), test_result, tol );

  gk_integrator.integrateWithPointRule<41>( functor_instance,
                                        0.0,
                                        1.0,
                                        result,
                                        absolute_error,
                                        result_abs,
                                        result_asc );

  tol = absolute_error/result;
  test_result = result;

  TEST_FLOATING_EQUALITY( Functor::getIntegratedValue(), test_result, tol );

  gk_integrator.integrateWithPointRule<51>( functor_instance,
                                        0.0,
                                        1.0,
                                        result,
                                        absolute_error,
                                        result_abs,
                                        result_asc );

  tol = absolute_error/result;
  test_result = result;

  TEST_FLOATING_EQUALITY( Functor::getIntegratedValue(), test_result, tol );

  gk_integrator.integrateWithPointRule<61>( functor_instance,
                                        0.0,
                                        1.0,
                                        result,
                                        absolute_error,
                                        result_abs,
                                        result_asc );

  tol = absolute_error/result;
  test_result = result;

  TEST_FLOATING_EQUALITY( Functor::getIntegratedValue(), test_result, tol );
}

UNIT_TEST_INSTANTIATION( GaussKronrodIntegrator, integrateWithPointRule );

//---------------------------------------------------------------------------//
// Check that quadrature integrand values can be evaulated at abscissa
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( GaussKronrodIntegrator,
				                   calculateQuadratureIntegrandValuesAtAbscissa,
                                   Functor )
{
  TestGaussKronrodIntegrator test_integrator( 1e-12 );

  double half_length = 0.5;
  double midpoint = 0.5;
  double abscissa = 0.5;

  double integrand_value_lower, integrand_value_upper ;

  Functor functor_instance;

  test_integrator.calculateQuadratureIntegrandValuesAtAbscissa(
                functor_instance,
                abscissa,
                half_length,
                midpoint,
                integrand_value_lower,
                integrand_value_upper );

  double tol = 1e-12;

  TEST_FLOATING_EQUALITY( functor_instance( 0.25 ), integrand_value_lower, tol );
  TEST_FLOATING_EQUALITY( functor_instance( 0.75 ), integrand_value_upper, tol );
}

UNIT_TEST_INSTANTIATION( GaussKronrodIntegrator, calculateQuadratureIntegrandValuesAtAbscissa );

//---------------------------------------------------------------------------//
// Check that quadrature integrand can be bisected and integrated
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( GaussKronrodIntegrator,
				                   bisectAndIntegrateBinInterval,
                                   Functor )
{
  TestGaussKronrodIntegrator test_integrator( 1e-12 );

  Utility::BinTraits bin, bin_1, bin_2;

  double bin_1_asc, bin_2_asc, tol_1, tol_2;

  bin.lower_limit = 0.0;
  bin.upper_limit = 1.0;

  Functor functor_instance;

  test_integrator.bisectAndIntegrateBinInterval<15>(
                functor_instance,
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc );

  tol_1 = bin_1.error/bin_1.result;
  tol_2 = bin_2.error/bin_2.result;

  TEST_FLOATING_EQUALITY( Functor::getLowerIntegratedValue(),
                          static_cast<double>( bin_1.result ),
                          tol_1 );
  TEST_FLOATING_EQUALITY( Functor::getUpperIntegratedValue(),
                          static_cast<double>( bin_2.result ),
                          tol_2 );

  TEST_EQUALITY( bin_1.lower_limit, bin.lower_limit );
  TEST_FLOATING_EQUALITY( bin_1.upper_limit, 0.5, 1e-15);
  TEST_FLOATING_EQUALITY( bin_2.lower_limit, 0.5, 1e-15 );
  TEST_EQUALITY( bin_2.upper_limit, bin.upper_limit );

  test_integrator.bisectAndIntegrateBinInterval<21>(
                functor_instance,
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc );

  tol_1 = bin_1.error/bin_1.result;
  tol_2 = bin_2.error/bin_2.result;

  TEST_FLOATING_EQUALITY( Functor::getLowerIntegratedValue(),
                          static_cast<double>( bin_1.result ),
                          tol_1 );
  TEST_FLOATING_EQUALITY( Functor::getUpperIntegratedValue(),
                          static_cast<double>( bin_2.result ),
                          tol_2 );

  test_integrator.bisectAndIntegrateBinInterval<31>(
                functor_instance,
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc );

  tol_1 = bin_1.error/bin_1.result;
  tol_2 = bin_2.error/bin_2.result;

  TEST_FLOATING_EQUALITY( Functor::getLowerIntegratedValue(),
                          static_cast<double>( bin_1.result ),
                          tol_1 );
  TEST_FLOATING_EQUALITY( Functor::getUpperIntegratedValue(),
                          static_cast<double>( bin_2.result ),
                          tol_2 );

  test_integrator.bisectAndIntegrateBinInterval<41>(
                functor_instance,
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc );

  tol_1 = bin_1.error/bin_1.result;
  tol_2 = bin_2.error/bin_2.result;

  TEST_FLOATING_EQUALITY( Functor::getLowerIntegratedValue(),
                          static_cast<double>( bin_1.result ),
                          tol_1 );
  TEST_FLOATING_EQUALITY( Functor::getUpperIntegratedValue(),
                          static_cast<double>( bin_2.result ),
                          tol_2 );

  test_integrator.bisectAndIntegrateBinInterval<51>(
                functor_instance,
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc );

  tol_1 = bin_1.error/bin_1.result;
  tol_2 = bin_2.error/bin_2.result;

  TEST_FLOATING_EQUALITY( Functor::getLowerIntegratedValue(),
                          static_cast<double>( bin_1.result ),
                          tol_1 );
  TEST_FLOATING_EQUALITY( Functor::getUpperIntegratedValue(),
                          static_cast<double>( bin_2.result ),
                          tol_2 );

  test_integrator.bisectAndIntegrateBinInterval<61>(
                functor_instance,
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc );

  tol_1 = bin_1.error/bin_1.result;
  tol_2 = bin_2.error/bin_2.result;

  TEST_FLOATING_EQUALITY( Functor::getLowerIntegratedValue(),
                          static_cast<double>( bin_1.result ),
                          tol_1 );
  TEST_FLOATING_EQUALITY( Functor::getUpperIntegratedValue(),
                          static_cast<double>( bin_2.result ),
                          tol_2 );
}

UNIT_TEST_INSTANTIATION( GaussKronrodIntegrator, bisectAndIntegrateBinInterval );

//---------------------------------------------------------------------------//
// Check that the error can be rescaled
TEUCHOS_UNIT_TEST( GaussKronrodIntegrator,
                   rescaleAbsoluteError )
{
  TestGaussKronrodIntegrator test_integrator( 1e-12 );

  double absolute_error = 0.0;
  double result_abs = 0.0;
  double result_asc = 0.0;
  double tol = 1e-12;
  double limit = std::numeric_limits<double>::min() / ( 50.0 *
                   std::numeric_limits<double>::epsilon() );

  absolute_error = limit/2.0;

  test_integrator.rescaleAbsoluteError(
                absolute_error,
                result_abs,
                result_asc );

  TEST_FLOATING_EQUALITY( limit/2.0, absolute_error, tol );

  absolute_error = 1.0;
  result_asc = 2.0;

  test_integrator.rescaleAbsoluteError(
                absolute_error,
                result_abs,
                result_asc );

  TEST_FLOATING_EQUALITY( 2.0, absolute_error, tol );

  absolute_error = 1.0;
  result_asc = 800.0;

  test_integrator.rescaleAbsoluteError(
                absolute_error,
                result_abs,
                result_asc );

  TEST_FLOATING_EQUALITY( 100.0, absolute_error, tol );

  absolute_error = 50.0*std::numeric_limits<double>::epsilon();
  result_asc = 0.0;
  result_abs = 2.0;
  double min_error = 50.0*std::numeric_limits<double>::epsilon() * result_abs;

  test_integrator.rescaleAbsoluteError(
                absolute_error,
                result_abs,
                result_asc );

  TEST_FLOATING_EQUALITY( min_error, absolute_error, tol );
}

//---------------------------------------------------------------------------//
// Check the roundoff error
TEUCHOS_UNIT_TEST( GaussKronrodIntegrator,
                   checkRoundoffError )
{
  TestGaussKronrodIntegrator test_integrator( 1e-12 );

  Utility::BinTraits bin, bin_1, bin_2;
  int round_off_1 = 0;
  int round_off_2 = 0;
  int number_of_interactions = 0;
  double error_12 = 0.0, bin_1_asc = 0.0, bin_2_asc = 0.0;
  double tol = 1e-12;

  bin.result = 9.9999;
  bin_1.result = 5.0;
  bin_2.result = 5.0;

  bin.error = 1.0;
  bin_1.error = 0.5;
  bin_2.error = 0.49;

  bin_1_asc = bin_1.error;
  bin_2_asc = bin_2.error;

  test_integrator.checkRoundoffError(
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc,
                round_off_1,
                round_off_2,
                number_of_interactions );

  TEST_EQUALITY_CONST( 0, round_off_1 );
  TEST_EQUALITY_CONST( 0, round_off_2 );

  bin_1_asc = 0.0;
  bin_2_asc = 0.0;

  test_integrator.checkRoundoffError(
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc,
                round_off_1,
                round_off_2,
                number_of_interactions );

  TEST_EQUALITY_CONST( 1, round_off_1 );
  TEST_EQUALITY_CONST( 0, round_off_2 );

  bin_2.error = 0.501;
  number_of_interactions = 10;

  test_integrator.checkRoundoffError(
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc,
                round_off_1,
                round_off_2,
                number_of_interactions );

  TEST_EQUALITY_CONST( 2, round_off_1 );
  TEST_EQUALITY_CONST( 1, round_off_2 );

  bin.result = 9.9;

  test_integrator.checkRoundoffError(
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc,
                round_off_1,
                round_off_2,
                number_of_interactions );

  TEST_EQUALITY_CONST( 2, round_off_1 );
  TEST_EQUALITY_CONST( 2, round_off_2 );
}

//---------------------------------------------------------------------------//
// Check the roundoff error
TEUCHOS_UNIT_TEST( GaussKronrodIntegrator,
                   checkRoundoffError2 )
{
  TestGaussKronrodIntegrator test_integrator( 1e-12 );

  Utility::ExtrpolatedBinTraits bin, bin_1, bin_2;
  int round_off_1 = 0;
  int round_off_2 = 0;
  int round_off_3 = 0;
  int number_of_interactions = 0;
  bool extrapolate = false;
  double error_12 = 0.0, bin_1_asc = 0.0, bin_2_asc = 0.0;
  double tol = 1e-12;

  bin.result = 9.9999;
  bin_1.result = 5.0;
  bin_2.result = 5.0;

  bin.error = 1.0;
  bin_1.error = 0.5;
  bin_2.error = 0.49;

  bin_1_asc = bin_1.error;
  bin_2_asc = bin_2.error;

  test_integrator.checkRoundoffError(
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc,
                round_off_1,
                round_off_2,
                round_off_3,
                extrapolate,
                number_of_interactions );

  TEST_EQUALITY_CONST( 0, round_off_1 );
  TEST_EQUALITY_CONST( 0, round_off_2 );
  TEST_EQUALITY_CONST( 0, round_off_3 );

  bin_1_asc = 0.0;
  bin_2_asc = 0.0;

  test_integrator.checkRoundoffError(
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc,
                round_off_1,
                round_off_2,
                round_off_3,
                extrapolate,
                number_of_interactions );

  TEST_EQUALITY_CONST( 1, round_off_1 );
  TEST_EQUALITY_CONST( 0, round_off_2 );
  TEST_EQUALITY_CONST( 0, round_off_3 );

  extrapolate = true;

  test_integrator.checkRoundoffError(
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc,
                round_off_1,
                round_off_2,
                round_off_3,
                extrapolate,
                number_of_interactions );

  TEST_EQUALITY_CONST( 1, round_off_1 );
  TEST_EQUALITY_CONST( 1, round_off_2 );
  TEST_EQUALITY_CONST( 0, round_off_3 );

  bin_2.error = 0.501;
  number_of_interactions = 10;

  test_integrator.checkRoundoffError(
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc,
                round_off_1,
                round_off_2,
                round_off_3,
                extrapolate,
                number_of_interactions );

  TEST_EQUALITY_CONST( 1, round_off_1 );
  TEST_EQUALITY_CONST( 2, round_off_2 );
  TEST_EQUALITY_CONST( 1, round_off_3 );

  bin.result = 9.9;

  test_integrator.checkRoundoffError(
                bin,
                bin_1,
                bin_2,
                bin_1_asc,
                bin_2_asc,
                round_off_1,
                round_off_2,
                round_off_3,
                extrapolate,
                number_of_interactions );

  TEST_EQUALITY_CONST( 1, round_off_1 );
  TEST_EQUALITY_CONST( 2, round_off_2 );
  TEST_EQUALITY_CONST( 2, round_off_3 );
}

//---------------------------------------------------------------------------//
// Check that the error list can be sorted
TEUCHOS_UNIT_TEST( GaussKronrodIntegrator,
                   sortBins )
{
  TestGaussKronrodIntegrator test_integrator( 1e-12 );

  Utility::ExtrpolatedBinTraits bin, bin_1, bin_2;

  int nr_max = 0;
  int number_of_intervals = 3;

  // Set up bin order array
  Teuchos::Array<int> bin_order(3);
  bin_order[0] = 0;
  bin_order[1] = 1;
  bin_order[2] = 2;

  // Set bin array
  Utility::BinArray bin_array(1000);
  bin.error = 10.0;
  bin_array[0] = bin;
  bin.error = 8.0;
  bin_array[1] = bin;
  bin.error = 1.0;
  bin_array[2] = bin;

  // Set bin_1 and bin_2
  bin_1.error = 5.0;
  bin_2.error = 2.0;

  test_integrator.sortBins(
                bin_order,
                bin_array,
                bin_1,
                bin_2,
                number_of_intervals,
                nr_max );

  TEST_EQUALITY_CONST( 1, bin_order[0] );
  TEST_EQUALITY_CONST( 0, bin_order[1] );
  TEST_EQUALITY_CONST( 3, bin_order[2] );
  TEST_EQUALITY_CONST( 2, bin_order[3] );
  TEST_EQUALITY_CONST( 0, nr_max );

  // Test with nr_max != 0
  nr_max = 1;
  number_of_intervals = 3;

  // Set up bin order array
  bin_order.resize(3);
  bin_order[0] = 0;
  bin_order[1] = 1;
  bin_order[2] = 2;

  // Set bin array
  bin.error = 10.0;
  bin_array[0] = bin;
  bin.error = 8.0;
  bin_array[1] = bin;
  bin.error = 1.0;
  bin_array[2] = bin;

  // Set bin_1 and bin_2
  bin_1.error = 11.0;
  bin_2.error = 2.0;

  test_integrator.sortBins(
                bin_order,
                bin_array,
                bin_1,
                bin_2,
                number_of_intervals,
                nr_max );

  TEST_EQUALITY_CONST( 1, bin_order[0] );
  TEST_EQUALITY_CONST( 0, bin_order[1] );
  TEST_EQUALITY_CONST( 3, bin_order[2] );
  TEST_EQUALITY_CONST( 2, bin_order[3] );
  TEST_EQUALITY_CONST( 0, nr_max );

  // Test 3
  nr_max = 0;
  number_of_intervals = 3;

  // Set up bin order array
  bin_order.resize(3);
  bin_order[0] = 1;
  bin_order[1] = 0;
  bin_order[2] = 2;

  bin_array.clear();

  // Set bin array
  bin.error = 0.673651;
  bin_array[0] = bin;
  bin.error = 1.90537;
  bin_array[1] = bin;
  bin.error = 6.50354e-15;
  bin_array[2] = bin;

  // Set bin_1 and bin_2
  bin_1.error = 0.673652;
  bin_2.error = 6.50353e-15;

  test_integrator.sortBins(
                bin_order,
                bin_array,
                bin_1,
                bin_2,
                number_of_intervals,
                nr_max );

  TEST_EQUALITY_CONST( 1, bin_order[0] );
  TEST_EQUALITY_CONST( 0, bin_order[1] );
  TEST_EQUALITY_CONST( 2, bin_order[2] );
  TEST_EQUALITY_CONST( 3, bin_order[3] );
  TEST_EQUALITY_CONST( 0, nr_max );

  // Test 4
  nr_max = 3;
  number_of_intervals = 6;

  // Set up bin order array
  bin_order.resize(6);
  bin_order[0] = 3;
  bin_order[1] = 4;
  bin_order[2] = 0;
  bin_order[3] = 2;
  bin_order[4] = 1;
  bin_order[5] = 5;

  bin_array.clear();

  // Set bin array
  bin.error = 4.0;
  bin_array[0] = bin;
  bin.error = 2.0;
  bin_array[1] = bin;
  bin.error = 3.0;
  bin_array[2] = bin;
  bin.error = 6.0;
  bin_array[3] = bin;
  bin.error = 5.0;
  bin_array[4] = bin;
  bin.error = 1.0;
  bin_array[5] = bin;

  // Set bin_1 and bin_2
  bin_1.error = 2.5;
  bin_2.error = 0.5;

  test_integrator.sortBins(
                bin_order,
                bin_array,
                bin_1,
                bin_2,
                number_of_intervals,
                nr_max );

  TEST_EQUALITY_CONST( 3, bin_order[0] );
  TEST_EQUALITY_CONST( 4, bin_order[1] );
  TEST_EQUALITY_CONST( 0, bin_order[2] );
  TEST_EQUALITY_CONST( 2, bin_order[3] );
  TEST_EQUALITY_CONST( 1, bin_order[4] );
  TEST_EQUALITY_CONST( 5, bin_order[5] );
  TEST_EQUALITY_CONST( 6, bin_order[6] );
  TEST_EQUALITY_CONST( 3, nr_max );

  // Test 5
  nr_max = 3;
  number_of_intervals = 6;

  // Set up bin order array
  bin_order.resize(6);
  bin_order[0] = 3;
  bin_order[1] = 4;
  bin_order[2] = 0;
  bin_order[3] = 2;
  bin_order[4] = 1;
  bin_order[5] = 5;

  bin_array.clear();

  // Set bin array
  bin.error = 4.0;
  bin_array[0] = bin;
  bin.error = 2.0;
  bin_array[1] = bin;
  bin.error = 3.0;
  bin_array[2] = bin;
  bin.error = 6.0;
  bin_array[3] = bin;
  bin.error = 5.0;
  bin_array[4] = bin;
  bin.error = 1.0;
  bin_array[5] = bin;

  // Set bin_1 and bin_2
  bin_1.error = 4.5;
  bin_2.error = 0.5;

  test_integrator.sortBins(
                bin_order,
                bin_array,
                bin_1,
                bin_2,
                number_of_intervals,
                nr_max );

  TEST_EQUALITY_CONST( 3, bin_order[0] );
  TEST_EQUALITY_CONST( 4, bin_order[1] );
  TEST_EQUALITY_CONST( 2, bin_order[2] );
  TEST_EQUALITY_CONST( 0, bin_order[3] );
  TEST_EQUALITY_CONST( 1, bin_order[4] );
  TEST_EQUALITY_CONST( 5, bin_order[5] );
  TEST_EQUALITY_CONST( 6, bin_order[6] );
  TEST_EQUALITY_CONST( 2, nr_max );
}

//---------------------------------------------------------------------------//
// Check that the Wynn Epsilon-Algorithm extrapolated value can be calculated
TEUCHOS_UNIT_TEST( GaussKronrodIntegrator,
                   getWynnEpsilonAlgorithmExtrapolation )
{
  TestGaussKronrodIntegrator test_integrator( 1e-12 );

  Teuchos::Array<double> bin_extrapolated_result(52);
  Teuchos::Array<double> last_three_results(3);
  double extrapolated_result, extrapolated_error;
  int number_of_extrapolated_intervals, number_of_extrapolated_calls;
  double tol = 1e-16;
  number_of_extrapolated_calls = 0;

  // test 1
  number_of_extrapolated_intervals = 2;
  bin_extrapolated_result[0] = 3.93505142975913369L;
  bin_extrapolated_result[1] = 3.95407442555431254L;
  bin_extrapolated_result[2] = 3.96752571487956640L;

  test_integrator.getWynnEpsilonAlgorithmExtrapolation(
                bin_extrapolated_result,
                last_three_results,
                extrapolated_result,
                extrapolated_error,
                number_of_extrapolated_intervals,
                number_of_extrapolated_calls );

  TEST_EQUALITY_CONST( number_of_extrapolated_intervals, 2 );
  TEST_EQUALITY_CONST( number_of_extrapolated_calls, 1 );
  TEST_FLOATING_EQUALITY( extrapolated_error,
                          std::numeric_limits<double>::max(),
                          tol );
  TEST_FLOATING_EQUALITY( extrapolated_result,
                          3.99999999999999645,
                          tol );
  TEST_FLOATING_EQUALITY( last_three_results[0],
                          3.99999999999999645,
                          tol );
  TEST_FLOATING_EQUALITY( last_three_results[1],
                          0.0,
                          tol );
  TEST_FLOATING_EQUALITY( last_three_results[2],
                          0.0,
                          tol );

  // test 2
  number_of_extrapolated_intervals = 3;
  bin_extrapolated_result[0] = 3.99999999999999645L;
  bin_extrapolated_result[1] = 3.95407442555431254L;
  bin_extrapolated_result[2] = 3.96752571487956640L;
  bin_extrapolated_result[3] = 3.97703721277715605L;
  bin_extrapolated_result[4] = 3.96752571487956640L;

  test_integrator.getWynnEpsilonAlgorithmExtrapolation(
                bin_extrapolated_result,
                last_three_results,
                extrapolated_result,
                extrapolated_error,
                number_of_extrapolated_intervals,
                number_of_extrapolated_calls );

  TEST_EQUALITY_CONST( number_of_extrapolated_intervals, 3 );
  TEST_EQUALITY_CONST( number_of_extrapolated_calls, 2 );
  TEST_FLOATING_EQUALITY( extrapolated_error,
                          std::numeric_limits<double>::max(),
                          tol );
  TEST_FLOATING_EQUALITY( extrapolated_result,
                          4.00000000000000355,
                          tol );
  TEST_FLOATING_EQUALITY( last_three_results[0],
                          3.99999999999999645,
                          tol );
  TEST_FLOATING_EQUALITY( last_three_results[1],
                          4.00000000000000355,
                          tol );
  TEST_FLOATING_EQUALITY( last_three_results[2],
                          0.0,
                          tol );

  // test 3
  number_of_extrapolated_intervals = 4;
  bin_extrapolated_result[0] = 3.99999999999999645L;
  bin_extrapolated_result[1] = 4.00000000000000355L;
  bin_extrapolated_result[2] = 3.96752571487956640L;
  bin_extrapolated_result[3] = 3.97703721277715605L;
  bin_extrapolated_result[4] = 3.98376285743978320L;
  bin_extrapolated_result[5] = 3.97703721277715605L;

  test_integrator.getWynnEpsilonAlgorithmExtrapolation(
                bin_extrapolated_result,
                last_three_results,
                extrapolated_result,
                extrapolated_error,
                number_of_extrapolated_intervals,
                number_of_extrapolated_calls );

  TEST_EQUALITY_CONST( number_of_extrapolated_intervals, 4 );
  TEST_EQUALITY_CONST( number_of_extrapolated_calls, 3 );
  TEST_FLOATING_EQUALITY( extrapolated_error,
                          std::numeric_limits<double>::max(),
                          tol );
  TEST_FLOATING_EQUALITY( extrapolated_result,
                          4.00000000000000089,
                          tol );
  TEST_FLOATING_EQUALITY( last_three_results[0],
                          3.99999999999999645,
                          tol );
  TEST_FLOATING_EQUALITY( last_three_results[1],
                          4.00000000000000355,
                          tol );
  TEST_FLOATING_EQUALITY( last_three_results[2],
                          4.00000000000000089,
                          tol );

  // test 4
  number_of_extrapolated_intervals = 5;
  bin_extrapolated_result[0] = 4.00000000000000089L;
  bin_extrapolated_result[1] = 4.00000000000000355L;
  bin_extrapolated_result[2] = 3.99999999999999911L;
  bin_extrapolated_result[3] = 3.97703721277715605L;
  bin_extrapolated_result[4] = 3.98376285743978320L;
  bin_extrapolated_result[5] = 3.98851860638857758L;
  bin_extrapolated_result[6] = 3.98376285743978320L;

  test_integrator.getWynnEpsilonAlgorithmExtrapolation(
                bin_extrapolated_result,
                last_three_results,
                extrapolated_result,
                extrapolated_error,
                number_of_extrapolated_intervals,
                number_of_extrapolated_calls );

  TEST_EQUALITY_CONST( number_of_extrapolated_intervals, 5 );
  TEST_EQUALITY_CONST( number_of_extrapolated_calls, 4 );
  TEST_FLOATING_EQUALITY( extrapolated_error,
                          5.68434188608080149e-14,
                          tol );
  TEST_FLOATING_EQUALITY( extrapolated_result,
                          3.99999999999998135,
                          tol );
  TEST_FLOATING_EQUALITY( last_three_results[0],
                          4.00000000000000355,
                          tol );
  TEST_FLOATING_EQUALITY( last_three_results[1],
                          4.00000000000000089,
                          tol );
  TEST_FLOATING_EQUALITY( last_three_results[2],
                          3.99999999999998135,
                          tol );

}

//---------------------------------------------------------------------------//
// Check that functions can be integrated over [0,1] adaptively
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( GaussKronrodIntegrator,
				   integrateAdaptively,
				   Functor )
{
  Utility::GaussKronrodIntegrator gk_integrator( 1e-12 );

  double result, absolute_error, tol;

  Functor functor_instance;

  // Test the 15-point rule
  gk_integrator.integrateAdaptively<15>( functor_instance,
				  0.0,
				  1.0,
				  result,
				  absolute_error );

  tol = absolute_error/result;

  TEST_FLOATING_EQUALITY( Functor::getIntegratedValue(), result, tol );

  // Test the 21-point rule
  gk_integrator.integrateAdaptively<21>( functor_instance,
				  0.0,
				  1.0,
				  result,
				  absolute_error );

  tol = absolute_error/result;

  TEST_FLOATING_EQUALITY( Functor::getIntegratedValue(), result, tol );

  // Test the 31-point rule
  gk_integrator.integrateAdaptively<31>( functor_instance,
				  0.0,
				  1.0,
				  result,
				  absolute_error );

  tol = absolute_error/result;

  TEST_FLOATING_EQUALITY( Functor::getIntegratedValue(), result, tol );

  // Test the 41-point rule
  gk_integrator.integrateAdaptively<41>( functor_instance,
				  0.0,
				  1.0,
				  result,
				  absolute_error );

  tol = absolute_error/result;

  TEST_FLOATING_EQUALITY( Functor::getIntegratedValue(), result, tol );

  // Test the 51-point rule
  gk_integrator.integrateAdaptively<51>( functor_instance,
				  0.0,
				  1.0,
				  result,
				  absolute_error );

  tol = absolute_error/result;

  TEST_FLOATING_EQUALITY( Functor::getIntegratedValue(), result, tol );

  // Test the 61-point rule
  gk_integrator.integrateAdaptively<61>( functor_instance,
				  0.0,
				  1.0,
				  result,
				  absolute_error );

  tol = absolute_error/result;

  TEST_FLOATING_EQUALITY( Functor::getIntegratedValue(), result, tol );
}

UNIT_TEST_INSTANTIATION( GaussKronrodIntegrator, integrateAdaptively );

//---------------------------------------------------------------------------//
// Check that a function with integrable singularities can be integrated
TEUCHOS_UNIT_TEST( GaussKronrodIntegrator,
		   integrateAdaptivelyWynnEpsilon )
{
  boost::function<double (double x)> function_wrapper = inv_sqrt_abs_x;

  Teuchos::Array<double> points_of_interest( 3 );
  points_of_interest[0] = -1.0;
  points_of_interest[1] = 0.0; // integrable singularity
  points_of_interest[2] = 1.0;

  Utility::GaussKronrodIntegrator gk_int( 1e-12, 0.0, 100000 );

  double result, absolute_error;

  gk_int.integrateAdaptivelyWynnEpsilon( function_wrapper,
					 points_of_interest(),
					 result,
					 absolute_error );

  double tol = absolute_error/result;

  TEST_FLOATING_EQUALITY( result, 4.0, tol );
}

//---------------------------------------------------------------------------//
// Check that a function with no singularities can be integrated using Wynn Epsilon
TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( GaussKronrodIntegrator,
                                   integrateAdaptivelyWynnEpsilon_no_singularities,
                                   Functor )
{
  Functor functor_instance;

  Teuchos::Array<double> points_of_interest( 3 );
  points_of_interest[0] = 0.0;
  points_of_interest[1] = 0.5;
  points_of_interest[2] = 1.0;

  Utility::GaussKronrodIntegrator gkq_set( 1e-12, 0.0, 100000 );

  double result, absolute_error;

  gkq_set.integrateAdaptivelyWynnEpsilon(
            functor_instance,
    		points_of_interest(),
			result,
			absolute_error );

  double tol = absolute_error/result;

  TEST_FLOATING_EQUALITY( Functor::getIntegratedValue(), result, tol );
}

UNIT_TEST_INSTANTIATION( GaussKronrodIntegrator, integrateAdaptivelyWynnEpsilon_no_singularities );

//---------------------------------------------------------------------------//
// end tstGaussKronrodIntegrator.cpp
//---------------------------------------------------------------------------//


#include <iostream>
#include <vector>
#include <alglib/graph/undirected_graph.h>

using namespace std;
using namespace alglib::graph;

int main() {

    undirected_graph<string, int> G;

    string c1 = "Bikaner";
    string c2 = "Bangalore";
    string c3 = "Jaipur";
    string c4 = "Hanumangarh";
    string c5 = "Jodhpur";

    G.add_vertex(c2);
    G.add_vertex(c1);
    G.add_vertex(c3);
    G.add_vertex(c4);
    G.add_vertex(c5);

    G.add_edge(c1, c2, 100);
    G.add_edge(c5, c1, 250);

    cout << "No. of edges: " << G.num_edges() << endl;

    cout << "Cities adjacent to Bikaner: ";
    for(auto it = G.avbegin(c1); it != G.avend(c1); it++)
        cout << *it << "\t";
    cout << endl;

    undirected_graph<int> G1;

    G1.add_vertex(2);
    G1.add_vertex(10);
    G1.add_vertex(11);
    G1.add_vertex(6);

    G1.add_edge(3, 10);
    G1.add_edge(10, 11);
    G1.add_edge(6, 10);

    cout << "Numbers adjacent to 10: ";
    for(auto it = G1.avbegin(10); it != G1.avend(10); it++)
        cout << *it << "\t";
    cout << endl;
}


#include <hpx/hpx_init.hpp>
#include <hpx/util/high_resolution_timer.hpp>
#include <hpx/include/compute.hpp>
#include <hpx/include/parallel_transform.hpp>

#include <boost/program_options.hpp>

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

#define CALC_TYPE double

void print_tile(std::vector<CALC_TYPE> A, std::string id, std::size_t row, std::size_t col, std::size_t N)
{
   for(int i = 0; i < N; ++i) {
      for(int j = 0; j < N; ++j) {
         std::ostringstream os;
         os << id << " " << row * N + i << " " << col * N + j << " " << A[i * N + j] << std::endl;
         std::cout << os.str();
      }
   }
}

std::vector<CALC_TYPE> gen_tile(std::size_t row, std::size_t col, std::size_t N, std::size_t T)
{
   std::vector<CALC_TYPE> v;
   v.resize(N * N);
   std::srand(row * T + col);
   for(int i = 0; i < N; ++i) {
      for(int j = 0; j < N; ++j) {
         v[i * N + j] = (double) (std::rand() % 10000 - 5000) / 1000;
      }
   }
   return v;
}

//return inv
std::vector<CALC_TYPE> inversion(hpx::shared_future<std::vector<CALC_TYPE>> ft_A,
                                 std::size_t N)
{
   auto A = ft_A.get();
   double tmp;
   std::vector<CALC_TYPE> v;
   v.resize(N * N);

   for(int i = 0; i < N; ++i) {
      for(int j = 0; j < N; ++j) {
         v[i * N + j] = 0;
      }
      v[i * N + i] = 1;
   }
   for(int k = 0; k < N; ++k) {
      tmp = A[k * N + k];
      for(int j = 0; j < N; ++j) {
         v[k * N + j] /= tmp;
         A[k * N + j] /= tmp;
      }

      // can be parallalized
      for(int i = 0; i < k; ++i) {
         tmp = A[i * N + k];
         for(int j = 0; j < N; ++j) {
            v[i * N + j] -= tmp * v[k * N + j];
            A[i * N + j] -= tmp * A[k * N + j];
         }
      }
      for(int i = k + 1; i < N; ++i) {
         tmp = A[i * N + k];
         for(int j = 0; j < N; ++j) {
            v[i * N + j] -= tmp * v[k * N + j];
            A[i * N + j] -= tmp * A[k * N + j];
         }
      }
   }
   return v;
}

//A = A * B
std::vector<CALC_TYPE> pmm(hpx::shared_future<std::vector<CALC_TYPE>> ft_A,
                           hpx::shared_future<std::vector<CALC_TYPE>> ft_B,
                           std::size_t N)
{
   std::vector<CALC_TYPE> v;
   v.resize(N * N);
   auto A = ft_A.get();
   auto B = ft_B.get();

   // i and j can be parallalized
   for(int i = 0; i < N; ++i) {
      for(int j = 0; j < N; ++j) {
         v[i * N + j] = 0;
         for(int k = 0; k < N; ++k) {
            v[i * N + j] += A[i * N + k] * B[k * N + j];
         }
      }
   }
   return v;
}

//C = C - A * B
std::vector<CALC_TYPE> pmm_d(hpx::shared_future<std::vector<CALC_TYPE>> ft_A,
                             hpx::shared_future<std::vector<CALC_TYPE>> ft_B,
                             hpx::shared_future<std::vector<CALC_TYPE>> ft_C,
                             std::size_t N)
{
   auto A = ft_A.get();
   auto B = ft_B.get();
   auto C = ft_C.get();

   // i and j can be parallalized
   for(int i = 0; i < N; ++i) {
      for(int j = 0; j < N; ++j) {
         for(int k = 0; k < N; ++k) {
            C[i * N + j] -= A[i * N + k] * B[k * N + j];
         }
      }
   }
   return C;
}

void lu_tiled(std::vector<hpx::shared_future<std::vector<CALC_TYPE>>> &ft_tiles, std::size_t N, std::size_t T)
{
    std::vector<hpx::shared_future<std::vector<CALC_TYPE>>> ft_inv;
    ft_inv.resize(T);

    for (std::size_t k = 0; k < T - 1; ++k)
    {
       ft_inv[k] = hpx::dataflow(&inversion, ft_tiles[k * T + k], N);
       for (std::size_t i = k + 1; i < T; ++i)
       {
          ft_tiles[i * T + k] = hpx::dataflow(&pmm, ft_tiles[i * T + k], ft_inv[k], N);
          for (std::size_t j = k + 1; j < T; ++j)
          {
             ft_tiles[i * T + j] = hpx::dataflow(&pmm_d, ft_tiles[i * T + k], ft_tiles[k * T + j], ft_tiles[i * T + j], N);
          }
       }
    }

}

int hpx_main(boost::program_options::variables_map& vm)
{
    std::size_t N = vm["N"].as<std::size_t>();
    std::size_t T = vm["T"].as<std::size_t>();
    std::string out = vm["out"].as<std::string>();

    hpx::util::high_resolution_timer t;

    std::vector<hpx::shared_future<std::vector<CALC_TYPE>>> A_tiles;
    A_tiles.resize(T * T);

    for (std::size_t i = 0; i < T; ++i)
    {
       for (std::size_t j = 0; j < T; ++j)
       {
          A_tiles[i * T + j] = hpx::dataflow(&gen_tile, i, j, N, T);
       }
    }

    if (out == "debug")
    {
       for (std::size_t i = 0; i < T; ++i)
       {
          for (std::size_t j = 0; j < T; ++j)
          {
             print_tile(A_tiles[i * T + j].get(), "a", i, j, N);
          }
       }
    }

    lu_tiled(A_tiles, N, T);

    if (out == "debug")
    {
       for (std::size_t i = 0; i < T; ++i)
       {
          for (std::size_t j = 0; j < T; ++j)
          {
             print_tile(A_tiles[i * T + j].get(), "lu", i, j, N);
          }
       }
    }

    double elapsed = t.elapsed();
    std::cout << "Elapsed " << elapsed << " s\n";
    return hpx::finalize();
}

int main(int argc, char* argv[])
{
    using namespace boost::program_options;

    options_description desc_commandline;
    desc_commandline.add_options()
        ("N", value<std::size_t>()->default_value(10),
         "Dimension of each Tile (N*N elements per tile)")
        ("T", value<std::size_t>()->default_value(10),
         "Number of Tiles in each dimension (T*T tiles)")
        ("out", value<std::string>()->default_value("no"),
         "(debug) => print matrices in coo format")
    ;

    return hpx::init(desc_commandline, argc, argv);
}


#ifndef TEST_UNIT_MATH_FWD_MAT_VECTORIZE_EXPECT_FWD_MATRIX_VALUE_HPP
#define TEST_UNIT_MATH_FWD_MAT_VECTORIZE_EXPECT_FWD_MATRIX_VALUE_HPP

#include <stan/math/fwd/mat.hpp>
#include <math/fwd/mat/vectorize/build_fwd_matrix.hpp>
#include <math/fwd/mat/vectorize/expect_val_deriv_eq.hpp>
#include <Eigen/Dense>
#include <vector>

template <typename F, typename T>
void expect_fwd_matrix_value() {
  using stan::math::fvar;
  using std::vector;
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> matrix_t;

  int num_inputs = F::valid_inputs().size();
  int num_cols = 3;
  matrix_t template_m(num_inputs, num_cols);

  for (int i = 0; i < template_m.size(); ++i) {
    matrix_t a = build_fwd_matrix<F>(template_m, i);
    matrix_t fa = F::template apply<matrix_t>(a);
    EXPECT_EQ(a.size(), fa.size());
    expect_val_deriv_eq(F::apply_base(a(i)), fa(i));
  }

  size_t vector_matrix_size = 2;
  for (size_t i = 0; i < vector_matrix_size; ++i) {
    for (int j = 0; j < template_m.size(); ++j) {
      vector<matrix_t> b;
      for (size_t k = 0; k < vector_matrix_size; ++k)
        if (k == i)
          b.push_back(build_fwd_matrix<F>(template_m, j));
        else
          b.push_back(build_fwd_matrix<F>(template_m));
      vector<matrix_t> fb = F::template apply<vector<matrix_t> >(b);
      EXPECT_EQ(b.size(), fb.size());
      EXPECT_EQ(b[i].size(), fb[i].size());
      expect_val_deriv_eq(F::apply_base(b[i](j)), fb[i](j));
    }
  }

  int seed_i = num_inputs + 1;
  matrix_t a = build_fwd_matrix<F>(template_m, seed_i);
  matrix_t fab = F::template apply<matrix_t>(a.block(1, 1, 1, 1));
  expect_val_deriv_eq(F::apply_base(a(1, 1)), fab(0, 0));
}

#endif


/*
 * test_bresenham.cpp
 *
 * Test the Graph Library for Autonomous and Dynamic Systems
 *
 * author:  Cyril Robin <cyril.robin@laas.fr>
 * created: 2013-09-13
 * license: BSD
 */
#define BOOST_TEST_MODULE const_string test
#include <boost/test/included/unit_test.hpp>

#include "gladys/bresenham.hpp"

using namespace gladys;

BOOST_AUTO_TEST_SUITE( bresenham )

BOOST_AUTO_TEST_CASE( test_bresenham )
{
    // two arbitrary points
    gladys::point_xy_t s {  1, 1 } ;
    gladys::point_xy_t t { 11, 5 } ;

    // ascending line
    gladys::points_t l = gladys::bresenham( s, t );

    BOOST_TEST_MESSAGE( "line 1 size = " << l.size() );
    BOOST_CHECK_EQUAL( l.size() , 11 );

    // descending line
    gladys::points_t m = gladys::bresenham( t, s );
    BOOST_TEST_MESSAGE( "line 2 size = " << m.size() );
    BOOST_CHECK_EQUAL( m.size() , 11 );

    // m is the reverse of l
    bool b = true;
    for ( unsigned int i = 0 ; ( b && ( i < 11 )) ; i++ )
        b = ( l[i][0] == m[10-i][0] && l[i][1] == m[10-i][1] ) ;
    BOOST_TEST_MESSAGE( "Check order " );
    BOOST_CHECK_EQUAL( b , true );

    // Check one specific point
    b =  ( l[4][0] == 5 && l[4][1] == 3 ) ;
    BOOST_TEST_MESSAGE( "Check the coordinate of some specific point" );
    BOOST_CHECK_EQUAL( b , true );
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef STAN_MATH_PRIM_MAT_PROB_MATRIX_NORMAL_PREC_RNG_HPP
#define STAN_MATH_PRIM_MAT_PROB_MATRIX_NORMAL_PREC_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/mat/err/check_pos_semidefinite.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/err/check_size_match.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup multivar_dists
 * Sample from the the matrix normal distribution for the given Mu,
 * Sigma and D where Sigma and D are given as precision matrices, not
 * covariance matrices.
 *
 * @param Mu The mean matrix.
 * @param Sigma The mxm inverse covariance matrix (i.e., the precision
 * matrix) of the rows of y.
 * @param D The nxn inverse covariance matrix (i.e., the precision
 * matrix) of the columns of y.
 * @param rng Pseudo-random number generator.
 * @return A sample from the distribution, of type Matrix<double,
 * Dynamic, Dynamic>.
 * @throw std::invalid_argument if Sigma or D are not square.
 * @throw std::domain_error if Sigma or D are not symmetric, not
 * semi-positive definite, or if they contain infinities or NaNs.
 * @tparam RNG Type of pseudo-random number generator.
 */
template <class RNG>
inline Eigen::MatrixXd matrix_normal_prec_rng(const Eigen::MatrixXd &Mu,
                                              const Eigen::MatrixXd &Sigma,
                                              const Eigen::MatrixXd &D,
                                              RNG &rng) {
  using boost::normal_distribution;
  using boost::variate_generator;

  static const char *function = "matrix_normal_prec_rng";

  check_positive(function, "Sigma rows", Sigma.rows());
  check_finite(function, "Sigma", Sigma);
  check_symmetric(function, "Sigma", Sigma);

  check_positive(function, "D rows", D.rows());
  check_finite(function, "D", D);
  check_symmetric(function, "D", D);

  check_size_match(function, "Rows of location parameter", Mu.rows(),
                   "Rows of Sigma", Sigma.rows());
  check_size_match(function, "Columns of location parameter", Mu.cols(),
                   "Rows of D", D.rows());

  check_finite(function, "Location parameter", Mu);

  Eigen::LDLT<Eigen::MatrixXd> Sigma_ldlt(Sigma);
  // Sigma = PS^T LS DS LS^T PS
  // PS a permutation matrix.
  // LS lower triangular with unit diagonal.
  // DS diagonal.
  Eigen::LDLT<Eigen::MatrixXd> D_ldlt(D);
  // D = PD^T LD DD LD^T PD

  check_pos_semidefinite(function, "Sigma", Sigma_ldlt);
  check_pos_semidefinite(function, "D", D_ldlt);

  // If
  // C ~ N[0, I, I]
  // Then
  // A C B ~ N[0, A A^T, B^T B]
  // So to get
  // Y - Mu ~ N[0, Sigma^(-1), D^(-1)]
  // We need to do
  // Y - Mu = Q^T^(-1) C R^(-1)
  // Where Q^T^(-1) and R^(-1) are such that
  // Q^(-1) Q^(-1)^T = Sigma^(-1)
  // R^(-1)^T R^(-1) = D^(-1)
  // We choose:
  // Q^(-1)^T = PS^T LS^T^(-1) sqrt[DS]^(-1)
  // R^(-1) = sqrt[DD]^(-1) LD^(-1) PD
  // And therefore
  // Y - Mu = (PS^T LS^T^(-1) sqrt[DS]^(-1)) C (sqrt[DD]^(-1) LD^(-1) PD)

  int m = Sigma.rows();
  int n = D.rows();

  variate_generator<RNG &, normal_distribution<>> std_normal_rng(
      rng, normal_distribution<>(0, 1));

  // X = sqrt[DS]^(-1) C sqrt[DD]^(-1)
  // X ~ N[0, DS, DD]
  Eigen::MatrixXd X(m, n);
  Eigen::VectorXd row_stddev
      = Sigma_ldlt.vectorD().array().inverse().sqrt().matrix();
  Eigen::VectorXd col_stddev
      = D_ldlt.vectorD().array().inverse().sqrt().matrix();
  for (int row = 0; row < m; ++row) {
    for (int col = 0; col < n; ++col) {
      double stddev = row_stddev(row) * col_stddev(col);
      // C(row, col) = std_normal_rng();
      X(row, col) = stddev * std_normal_rng();
    }
  }

  // Y - Mu = PS^T (LS^T^(-1) X LD^(-1)) PD
  // Y' = LS^T^(-1) X LD^(-1)
  // Y' = LS^T.solve(X) LD^(-1)
  // Y' = (LD^(-1)^T (LS^T.solve(X))^T)^T
  // Y' = (LD^T.solve((LS^T.solve(X))^T))^T
  // Y = Mu + PS^T Y' PD
  Eigen::MatrixXd Y = Mu
                      + (Sigma_ldlt.transpositionsP().transpose()
                         * (D_ldlt.matrixU().solve(
                                (Sigma_ldlt.matrixU().solve(X)).transpose()))
                               .transpose()
                         * D_ldlt.transpositionsP());

  return Y;
}
}  // namespace math
}  // namespace stan
#endif


// Boost.Geometry

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

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

#include <geometry_test_common.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/srs/epsg.hpp>
#include <boost/geometry/srs/transformation.hpp>

#include "check_geometry.hpp"

template <typename T>
void test_issue_657()
{
    using namespace boost::geometry;
    using namespace boost::geometry::model;
    using namespace boost::geometry::srs;

    typedef model::point<T, 2, bg::cs::cartesian> point_car;
    typedef model::point<T, 2, cs::geographic<bg::degree> > point_geo;

    transformation<> tr1((bg::srs::epsg(4326)),
                         (bg::srs::proj4(
                             "+proj=merc +a=6378137 +b=6378137 +lat_ts=0.0 +lon_0=0.0 +x_0=0.0 "
                             "+y_0=0 +k=1.0 +units=m +nadgrids=@null +wktext  +no_defs")));
    transformation<> tr2((bg::srs::epsg(4326)),
                         (bg::srs::epsg(3785)));
    transformation<bg::srs::static_epsg<4326>,
                   bg::srs::static_epsg<3785> > tr3;

    point_geo pt(-114.7399212, 36.0160698);
    point_car pt_out(-12772789.6016, 4302832.77709);
    point_car pt_out1, pt_out2, pt_out3;

    tr1.forward(pt, pt_out1);
    tr2.forward(pt, pt_out2);
    tr3.forward(pt, pt_out3);

    test::check_geometry(pt_out1, pt_out, 0.001);
    test::check_geometry(pt_out2, pt_out, 0.001);
    test::check_geometry(pt_out3, pt_out, 0.001);
}

int test_main(int, char*[])
{
    test_issue_657<double>();

    return 0;
}


#include <iostream>
#include "Sensor.h"
#include "Thruster.h"
#include "Body.h"

#include <Eigen/Eigen/Dense>

using namespace Eigen;

int main(){
	Body bod(Vector3d(0,0,0),Vector3d(0,0.5,0.1),2,1);
	for(int i = 0; i < 100; i++){
		bod.Update(0.01);
		std::cout<<bod.getPos()<<std::endl;
	}
}


#pragma once

#include <CCD/Rational.hpp>
#include <Eigen/Core>
#include <array>
#include <vector>

#include <cfenv>

namespace ccd {

typedef Eigen::Matrix<Rational, 3, 1, Eigen::ColMajor | Eigen::DontAlign>
    Vector3r;
typedef Eigen::Matrix<double, 3, 1> Vector3d;
static const int COPLANAR = -1;
static const int INTERSECTED = 1;
static const int NOT_INTERSECTED1 = 2;
static const int NOT_INTERSECTED2 = 3;
static const Vector3r ORIGIN = Vector3r(0, 0, 0);

static const int BI_DEGE_PLANE = 1;
static const int BI_DEGE_XOR_02 = 2;
static const int BI_DEGE_XOR_13 = 3;

class bilinear {
public:
    // v0, v1 are vertices of one triangle, v2, v3 are the vertices of another
    // one.
    bilinear(
        const Vector3r& v0,
        const Vector3r& v1,
        const Vector3r& v2,
        const Vector3r& v3);
    bool is_degenerated;
    std::vector<std::array<int, 3>> facets;
    std::array<int, 2> phi_f = { { 2, 2 } };
    std::array<Vector3r, 4> v;
};
template <typename V1, typename V2> Vector3r cross(const V1& v1, const V2& v2)
{
    Vector3r res;
    res[0] = v1[1] * v2[2] - v1[2] * v2[1];
    res[1] = v1[2] * v2[0] - v1[0] * v2[2];
    res[2] = v1[0] * v2[1] - v1[1] * v2[0];

    return res;
}

Vector3r sum(const Vector3r& a, const Vector3r& b);
Rational func_g(
    const Vector3r& x,
    const std::array<Vector3r, 4>& corners,
    const std::array<int, 3>& indices);

Rational phi(const Vector3r x, const std::array<Vector3r, 4>& corners);

void get_tet_phi(bilinear& bl);
// bool XOR(const bool a, const bool b)
//{
//    if (a && b)
//        return false;
//    if (!a && !b)
//        return false;
//    return true;
//}

// accept 0,1,2,3 as inputs
bool int_seg_XOR(const int a, const int b);

// accept -1,0,1,2,3 as inputs
int int_ray_XOR(const int a, const int b);
template <typename V> void print(const V& v)
{
    std::cout << v[0] << " " << v[1] << " " << v[2] << std::endl;
}

void write(const Vector3d& v, std::ostream& out);
Vector3d read(std::istream& in);

int orient3d(
    const Vector3r& a, const Vector3r& b, const Vector3r& c, const Vector3r& d);
int orient2d(
    const Vector3r& a, const Vector3r& b, const Vector3r& c, const int axis);

bool segment_segment_intersection(
    const Vector3r& s0,
    const Vector3r& e0,
    const Vector3r& s1,
    const Vector3r& e1);
// 0 not intersected, 1 intersected, 2 s0 on segment
// can deal with degenerated cases
int ray_segment_intersection(
    const Vector3r& s0,
    const Vector3r& dir0,
    const Vector3r& s1,
    const Vector3r& e1);

// this function can also tell us if they are parallel and overlapped
// and also tell us if the parallel case has seg-seg overlapping:

bool same_point(const Vector3r& p1, const Vector3r& p2);
Vector3r tri_norm(const Vector3r& t0, const Vector3r& t1, const Vector3r& t2);

template <typename T>
static bool orient3D_LPI_prefilter_multiprecision(
    const T& px,
    const T& py,
    const T& pz,
    const T& qx,
    const T& qy,
    const T& qz,
    const T& rx,
    const T& ry,
    const T& rz,
    const T& sx,
    const T& sy,
    const T& sz,
    const T& tx,
    const T& ty,
    const T& tz,
    T& a11,
    T& a12,
    T& a13,
    T& d,
    const std::function<int(T)>& checker)
{

    a11 = (px - qx);
    a12 = (py - qy);
    a13 = (pz - qz);
    T a21(sx - rx);
    T a22(sy - ry);
    T a23(sz - rz);
    T a31(tx - rx);
    T a32(ty - ry);
    T a33(tz - rz);
    T a2233((a22 * a33) - (a23 * a32));
    T a2133((a21 * a33) - (a23 * a31));
    T a2132((a21 * a32) - (a22 * a31));
    d = (((a11 * a2233) - (a12 * a2133)) + (a13 * a2132));
    int flag1 = checker(d);
    if (flag1 == -2 || flag1 == 0) {
        return false; // not enough precision
    }
    T px_rx(px - rx);
    T py_ry(py - ry);
    T pz_rz(pz - rz);

    T n((((py_ry)*a2133) - ((px_rx)*a2233)) - ((pz_rz)*a2132));

    a11 = a11 * n;
    a12 = a12 * n;
    a13 = a13 * n;
    return true;
}

template <typename T>
static bool orient3D_TPI_prefilter_multiprecision(
    const T& ov1x,
    const T& ov1y,
    const T& ov1z,
    const T& ov2x,
    const T& ov2y,
    const T& ov2z,
    const T& ov3x,
    const T& ov3y,
    const T& ov3z,
    const T& ow1x,
    const T& ow1y,
    const T& ow1z,
    const T& ow2x,
    const T& ow2y,
    const T& ow2z,
    const T& ow3x,
    const T& ow3y,
    const T& ow3z,
    const T& ou1x,
    const T& ou1y,
    const T& ou1z,
    const T& ou2x,
    const T& ou2y,
    const T& ou2z,
    const T& ou3x,
    const T& ou3y,
    const T& ou3z,
    T& d,
    T& n1,
    T& n2,
    T& n3,
    const std::function<int(T)>& checker)
{
    ::feclearexcept(FE_UNDERFLOW | FE_OVERFLOW | FE_INVALID);

    T v3x(ov3x - ov2x);
    T v3y(ov3y - ov2y);
    T v3z(ov3z - ov2z);
    T v2x(ov2x - ov1x);
    T v2y(ov2y - ov1y);
    T v2z(ov2z - ov1z);
    T w3x(ow3x - ow2x);
    T w3y(ow3y - ow2y);
    T w3z(ow3z - ow2z);
    T w2x(ow2x - ow1x);
    T w2y(ow2y - ow1y);
    T w2z(ow2z - ow1z);
    T u3x(ou3x - ou2x);
    T u3y(ou3y - ou2y);
    T u3z(ou3z - ou2z);
    T u2x(ou2x - ou1x);
    T u2y(ou2y - ou1y);
    T u2z(ou2z - ou1z);

    T nvx(v2y * v3z - v2z * v3y);
    T nvy(v3x * v2z - v3z * v2x);
    T nvz(v2x * v3y - v2y * v3x);

    T nwx(w2y * w3z - w2z * w3y);
    T nwy(w3x * w2z - w3z * w2x);
    T nwz(w2x * w3y - w2y * w3x);

    T nux(u2y * u3z - u2z * u3y);
    T nuy(u3x * u2z - u3z * u2x);
    T nuz(u2x * u3y - u2y * u3x);

    T nwyuz(nwy * nuz - nwz * nuy);
    T nwxuz(nwx * nuz - nwz * nux);
    T nwxuy(nwx * nuy - nwy * nux);

    T nvyuz(nvy * nuz - nvz * nuy);
    T nvxuz(nvx * nuz - nvz * nux);
    T nvxuy(nvx * nuy - nvy * nux);

    T nvywz(nvy * nwz - nvz * nwy);
    T nvxwz(nvx * nwz - nvz * nwx);
    T nvxwy(nvx * nwy - nvy * nwx);

    d = (nvx * nwyuz - nvy * nwxuz + nvz * nwxuy);

    int flag1 = checker(d);
    if (flag1 == -2 || flag1 == 0) {
        return false; // not enough precision
    }

    T p1(nvx * ov1x + nvy * ov1y + nvz * ov1z);
    T p2(nwx * ow1x + nwy * ow1y + nwz * ow1z);
    T p3(nux * ou1x + nuy * ou1y + nuz * ou1z);

    n1 = p1 * nwyuz - p2 * nvyuz + p3 * nvywz;
    n2 = p2 * nvxuz - p3 * nvxwz - p1 * nwxuz;
    n3 = p3 * nvxwy - p2 * nvxuy + p1 * nwxuy;
    return true;
}

template <typename T>
static int orient3D_LPI_postfilter_multiprecision(
    const T& a11,
    const T& a12,
    const T& a13,
    const T& d,
    const T& px,
    const T& py,
    const T& pz,
    const T& ax,
    const T& ay,
    const T& az,
    const T& bx,
    const T& by,
    const T& bz,
    const T& cx,
    const T& cy,
    const T& cz,
    const std::function<int(T)>& checker)
{

    T px_cx(px - cx);
    T py_cy(py - cy);
    T pz_cz(pz - cz);

    T d11((d * px_cx) + (a11));
    T d21(ax - cx);
    T d31(bx - cx);
    T d12((d * py_cy) + (a12));
    T d22(ay - cy);
    T d32(by - cy);
    T d13((d * pz_cz) + (a13));
    T d23(az - cz);
    T d33(bz - cz);

    T d2233(d22 * d33);
    T d2332(d23 * d32);
    T d2133(d21 * d33);
    T d2331(d23 * d31);
    T d2132(d21 * d32);
    T d2231(d22 * d31);

    T det(
        d11 * (d2233 - d2332) - d12 * (d2133 - d2331) + d13 * (d2132 - d2231));

    int flag2 = checker(det);
    if (flag2 == -2) {
        return 100; // not enough precision, only happens when using floating
                    // points
    }
    if (flag2 == 1) {
        if (d > 0) {
            return 1;
        }
        if (d < 0) {
            return -1;
        }
    }
    if (flag2 == -1) {
        if (d > 0) {
            return -1;
        }
        if (d < 0) {
            return 1;
        }
    }
    return 0;
}

template <typename T>
static int orient3D_TPI_postfilter_multiprecision(
    const T& d,
    const T& n1,
    const T& n2,
    const T& n3,
    const T& q1x,
    const T& q1y,
    const T& q1z,
    const T& q2x,
    const T& q2y,
    const T& q2z,
    const T& q3x,
    const T& q3y,
    const T& q3z,
    const std::function<int(T)>& checker)
{
    ::feclearexcept(FE_UNDERFLOW | FE_OVERFLOW | FE_INVALID);

    T dq3x(d * q3x);
    T dq3y(d * q3y);
    T dq3z(d * q3z);

    T a11(n1 - dq3x);
    T a12(n2 - dq3y);
    T a13(n3 - dq3z);
    T a21(q1x - q3x);
    T a22(q1y - q3y);
    T a23(q1z - q3z);
    T a31(q2x - q3x);
    T a32(q2y - q3y);
    T a33(q2z - q3z);

    T det(
        a11 * (a22 * a33 - a23 * a32) - a12 * (a21 * a33 - a23 * a31)
        + a13 * (a21 * a32 - a22 * a31));

    int flag2 = checker(det);
    if (flag2 == -2) {
        return 100; // not enough precision
    }
    if (flag2 == 1) {
        if (d > 0) {
            return 1;
        }
        if (d < 0) {
            return -1;
        }
    }
    if (flag2 == -1) {
        if (d > 0) {
            return -1;
        }
        if (d < 0) {
            return 1;
        }
    }
    return 0;
}
static const std::function<int(Rational)> check_rational = [](Rational v) {
    if (v.get_sign() > 0)
        return 1;
    if (v.get_sign() < 0)
        return -1;
    return 0;
};

// already know lpi exist;
// 0 not intersected, 1 intersect open triangle, 2 shoot on edge, 3 shoot on
// edge t2-t3
int is_line_cut_triangle(
    const Vector3r& e0,
    const Vector3r& e1,
    const Vector3r& t1,
    const Vector3r& t2,
    const Vector3r& t3,
    const bool halfopen,
    const Vector3r& norm);
int line_triangle_inter_return_t(
    const Vector3r& e0,
    const Vector3r& e1,
    const Vector3r& t1,
    const Vector3r& t2,
    const Vector3r& t3,
    Rational& t);
// if a line (going across pt, pt+dir) intersects triangle
// triangle is not degenerated
// 0 not intersected, 1 intersected, 3 intersected t2-t3 edge
int line_triangle_intersection(
    const Vector3r& pt,
    const Vector3r& dir,
    const Vector3r& t1,
    const Vector3r& t2,
    const Vector3r& t3,
    const bool halfopen);
// we check if triangle intersect segment,
// this function is used in cube edge--prism tri and cube edge--bilinear tri
// if halfopen= true, can tell us if intersect the edge t2-t3
// 0 not intersected, 1 intersected, 2 intersect edge, 3 intersect t2-t3 edge
int segment_triangle_intersection(
    const Vector3r& e0,
    const Vector3r& e1,
    const Vector3r& t1,
    const Vector3r& t2,
    const Vector3r& t3,
    const bool halfopen);
// 0 no intersection, 1 intersect, 2 point on triangle, 3 point or ray go to on
// t2-t3 edge, -1 shoot on border
int ray_triangle_intersection(
    const Vector3r& pt,
    const Vector3r& dir,
    const Vector3r& t1,
    const Vector3r& t2,
    const Vector3r& t3,
    const bool halfopen);
} // namespace ccd