// 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;
}


#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;

}


#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


#include "perlin_noise.h"

#include <array>
#include <boost/random.hpp>
#include <vector>

#include "composite_data_buffer.h"

namespace noises {
namespace nodes
{
    struct PerlinNoiseData
    {
        PerlinNoiseData() { }

        std::unique_ptr<boost::random::mt19937> rng;

        std::vector<signed char> vectors;

        int dimensions;
    };

    PerlinNoise::PerlinNoise()
    {
        seed_socket_ = &inputs().add("Seed", SocketType::uniform);
        seed_socket_->set_accepts(ConnectionDataType::value<long, 1>());

        // Just a vector2 to start out with
        points_socket_ = &inputs().add("Points", SocketType::attribute);
        points_socket_->set_accepts(ConnectionDataType::value<float, 2>());
        points_socket_->set_accepts(ConnectionDataType::value<float, 3>());
        points_socket_->set_accepts(ConnectionDataType::value<float, 4>());
        points_socket_->set_accepts(ConnectionDataType::value<float, 5>());
        points_socket_->set_accepts(ConnectionDataType::value<float, 6>());

        output_socket_ = &outputs().add("Output", ConnectionDataType::value<float, 1>(), SocketType::attribute);
    }

    std::string PerlinNoise::node_name() const
    {
        return "Perlin Noise";
    }

    void PerlinNoise::execute_uniforms(const CompositeDataBuffer &input, DataBuffer &output) const
    {
        long seed = input.get_uniform<long, 1>(*seed_socket_);

        std::shared_ptr<PerlinNoiseData> data(new PerlinNoiseData);

        data->rng.reset(new boost::random::mt19937);
        data->rng->seed(seed);

        int dimensions = points_socket_->connection()->get().data_type().dimensions();
        load_hypercube_edge_vectors(*data, dimensions);

        utils::shuffle_group(data->vectors, *data->rng, dimensions);

        output.set_scratch(0, std::move(data));
    }

    void PerlinNoise::load_hypercube_edge_vectors(PerlinNoiseData &data, int dimensions) const
    {
        switch(dimensions)
        {
            case 0:
                throw std::logic_error("Cannot have 0 dimension noise.");
            case 1:
                throw std::logic_error("Cannot have 1 dimension perlin noise.");
            case 2:
                data.vectors = HypercubeEdges<2>::edge_vectors_flattened();
                break;
            case 3:
                data.vectors = HypercubeEdges<3>::edge_vectors_flattened();
                break;
            case 4:
                data.vectors = HypercubeEdges<4>::edge_vectors_flattened();
                break;
            case 5:
                data.vectors = HypercubeEdges<5>::edge_vectors_flattened();
                break;
            case 6:
                data.vectors = HypercubeEdges<6>::edge_vectors_flattened();
        }

        data.dimensions = dimensions;
    }

    void PerlinNoise::execute_attributes(const CompositeDataBuffer &input, DataBuffer &output, DataBuffer::size_type index) const
    {
        PerlinNoiseData& data = *output.get_scratch_ref<std::shared_ptr<PerlinNoiseData>>(0);
        int dimensions = data.dimensions;

        const float* attribute_point = get_input_point_attribute(index, input, dimensions);

        std::vector<int> dimension_starts(dimensions);
        std::vector<std::vector<int>> points;
        std::vector<int> gradient_indexes;

        int num_gradients = data.vectors.size() / dimensions;

        // Get floor(x), floor(y), floor(z) etc
        for(int i = 0; i < dimensions; i++)
        {
            dimension_starts[i] = std::floor(attribute_point[i]);
        }

        // Get all points in the hypercube surrounding the input point
        get_point_permutations(dimension_starts, dimensions, std::vector<int>(), points);

        // Get all rng gradients for each point. Hash x/y/z to get index into gradients array
        for(unsigned int i = 0; i < points.size(); i++)
        {
            std::vector<int>& point = points[i];
            unsigned int sum = 0;
            for(int j = 0; j < dimensions; j++)
            {
                static const unsigned int primes[] { 1699, 2237, 2671, 3571, 1949, 2221, 3469, 3083 };
                sum += point[j] * point[j] * primes[j]; //Hopefully this has enough entropy and random enough results
            }

            gradient_indexes.push_back(sum % num_gradients);
        }

        // Because of how the get_point_permutations algorithm works, adjacent verticies are in pairs recursively
        // There will always be an even number of points (2^n)
        std::vector<float> out(points.size());

        // vector from the first hypercube vertex to the input point (main diagonal) i.e. uv
        std::vector<float> uvs(dimensions);

        for(int i = 0; i < dimensions; i++)
        {
            uvs[i] = attribute_point[i] - points[0][i];
        }

        // Number of vertexes for an n-dimensional hypercube is 2^n
        int num_points = std::pow(2, dimensions);

        // Find dot product for every vertex.
        // Multiply the vector from the hypercube vertex to the input point, and the gradient at the hypercube vertex
        for(int i = 0; i < num_points; i++)
        {
            std::vector<int>& point = points[i];
            int gradient_index = gradient_indexes[i] * dimensions;

            // In theory, the dot products will be -1 to 1
            float dot = 0;

            for(int j = 0; j < dimensions; j++)
            {
                // gradient value (for this dimension) at hypercube vertex
                signed char gradient_u = data.vectors[gradient_index + j];

                // input point (in this dimension)
                float attr_p = attribute_point[j];

                // vector (for this dimension) from hypercube vertex to input point
                float u = attr_p - point[j];

                dot += u * gradient_u;
            }

            out[i] = dot;
        }

        // Collapse the dimensions down
        int current_dimension = dimensions;
        for(; num_points > 1; num_points /= 2)
        {
            // Take the dot products two at a time and interpolate between then
            int half_num = num_points / 2;
            for(int i = 0; i < half_num; i++)
            {
                int index = i * 2;

                float x_1 = out[index];
                float x_2 = out[index + 1];

                float u = uvs[current_dimension - 1];
                float interpolated = 6 * std::pow(u, 5) - (15 * std::pow(u, 4)) + (10 * std::pow(u, 3));

                out[i] = (x_1 * (1.0f - interpolated)) + (x_2 * interpolated);
            }

            current_dimension--;
        }

        output.set_attribute<float, 1>(*output_socket_, index, &out[0]);
    }

    const float* PerlinNoise::get_input_point_attribute(DataBuffer::size_type index, const CompositeDataBuffer& input, int dimensions) const
    {
        switch(dimensions)
        {
            case 2:
                return &input.get_attribute<float, 2>(*points_socket_, index);
            case 3:
                return &input.get_attribute<float, 3>(*points_socket_, index);
            case 4:
                return &input.get_attribute<float, 4>(*points_socket_, index);
            case 5:
                return &input.get_attribute<float, 5>(*points_socket_, index);
            case 6:
                return &input.get_attribute<float, 7>(*points_socket_, index);
        }
        throw std::logic_error("Not supported");
    }

    void PerlinNoise::get_point_permutations(std::vector<int>& dimension_starts, int dimensions, std::vector<int> point, std::vector<std::vector<int>>& points)
    {
        int dimension = point.size();
        if(dimension == dimensions)
        {
            points.push_back(point);
        }
        else
        {
            int start = dimension_starts[dimension];

            point.push_back(start);
            get_point_permutations(dimension_starts, dimensions, point, points);

            point[dimension] = start + 1;
            get_point_permutations(dimension_starts, dimensions, point, points);
        }
    }
} }


#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    typedef std::complex<double>      cdouble;
    const unsigned                    xd= 2, yd= 5, n= xd * yd;
    compressed2D<cdouble>             A(n, n);
    mat::laplacian_setup(A, xd, yd);

    // Fill imaginary part of the matrix
    A*= cdouble(1, -1);
    std::cout << "A is\n" << with_format(A, 7, 1) << "\n";

    std::cout << "trace(A) is " << trace(A) << "\n\n";
    std::cout << "conj(A) is\n" << with_format(mtl::mat::conj(A), 7, 1) << "\n"; // ADL issue on g++ 4.4
    std::cout << "trans(A) is\n" << with_format(trans(A), 7, 1) << "\n";
    std::cout << "hermitian(A) is\n" << with_format(hermitian(A), 7, 1) << "\n";

    return 0;
}


#include "smmap/gurobi_solvers.h"
#include <gurobi_c++.h>
#include <iostream>
#include <mutex>
#include <Eigen/Eigenvalues>

using namespace Eigen;

static std::mutex gurobi_env_construct_mtx;

GRBQuadExpr normSquared(const std::vector<GRBLinExpr>& exprs)
{
    GRBQuadExpr vector_norm_squared = 0;

    // TODO: replace with a single call to addTerms?
    for (size_t expr_ind = 0; expr_ind < exprs.size(); expr_ind++)
    {
        vector_norm_squared += exprs[expr_ind] * exprs[expr_ind];
    }

    return vector_norm_squared;
}

GRBQuadExpr normSquared(const std::vector<GRBLinExpr>& exprs, const VectorXd& weights)
{
    assert(exprs.size() == (size_t)weights.rows());
    GRBQuadExpr vector_norm_squared = 0;

    // TODO: replace with a single call to addTerms?
    for (size_t expr_ind = 0; expr_ind < exprs.size(); expr_ind++)
    {
        vector_norm_squared += weights((ssize_t)expr_ind) * exprs[expr_ind] * exprs[expr_ind];
    }

    return vector_norm_squared;
}

GRBQuadExpr normSquared(GRBVar* vars, const ssize_t num_vars)
{
    GRBQuadExpr vector_norm_squared = 0;

    // TODO: replace with a single call to addTerms?
    for (ssize_t var_ind = 0; var_ind < num_vars; var_ind++)
    {
        vector_norm_squared += vars[var_ind] * vars[var_ind];
    }

    return vector_norm_squared;
}

std::vector<GRBLinExpr> buildVectorOfExperssions(const MatrixXd& A, GRBVar* vars, const VectorXd& b)
{
    const ssize_t num_expr = A.rows();
    const ssize_t num_vars = A.cols();
    std::vector<GRBLinExpr> exprs(num_expr, 0);

    for (ssize_t expr_ind = 0; expr_ind < num_expr; expr_ind++)
    {
        for (ssize_t var_ind = 0; var_ind < num_vars; var_ind++)
        {
            exprs[expr_ind] += A(expr_ind, var_ind) * vars[var_ind];
        }
        exprs[expr_ind] -= b(expr_ind);
    }

    return exprs;
}

VectorXd smmap::minSquaredNorm(const MatrixXd& A, const VectorXd& b, const double max_x_norm)
{
    VectorXd x;
    GRBVar* vars = nullptr;
    try
    {
        const ssize_t num_vars = A.cols();
        const std::vector<double> lb(num_vars, -max_x_norm);
        const std::vector<double> ub(num_vars, max_x_norm);

        // TODO: Find a way to put a scoped lock here
        gurobi_env_construct_mtx.lock();
        GRBEnv env;
        gurobi_env_construct_mtx.unlock();

        env.set(GRB_IntParam_OutputFlag, 0);
        GRBModel model(env);
        vars = model.addVars(lb.data(), ub.data(), nullptr, nullptr, nullptr, (int)num_vars);
        model.update();

        model.addQConstr(normSquared(vars, num_vars), GRB_LESS_EQUAL, max_x_norm * max_x_norm);
        model.setObjective(normSquared(buildVectorOfExperssions(A, vars, b)), GRB_MINIMIZE);
        model.update();
        model.optimize();

        if (model.get(GRB_IntAttr_Status) == GRB_OPTIMAL)
        {
            x.resize(num_vars);
            for (ssize_t var_ind = 0; var_ind < num_vars; var_ind++)
            {
                x(var_ind) = vars[var_ind].get(GRB_DoubleAttr_X);
            }
        }
        else
        {
            std::cout << "Status: " << model.get(GRB_IntAttr_Status) << std::endl;
            exit(-1);
        }
    }
    catch(GRBException e)
    {
        std::cout << "Error code = " << e.getErrorCode() << std::endl;
        std::cout << e.getMessage() << std::endl;
    }
    catch(...)
    {
        std::cout << "Exception during optimization" << std::endl;
    }

    delete[] vars;
    return x;
}

VectorXd smmap::minSquaredNorm(const MatrixXd& A, const VectorXd& b, const double max_x_norm, const VectorXd& weights)
{
    VectorXd x;
    GRBVar* vars = nullptr;
    try
    {
        const ssize_t num_vars = A.cols();
        const std::vector<double> lb(num_vars, -max_x_norm);
        const std::vector<double> ub(num_vars, max_x_norm);

        // TODO: Find a way to put a scoped lock here
        gurobi_env_construct_mtx.lock();
        GRBEnv env;
        gurobi_env_construct_mtx.unlock();

        env.set(GRB_IntParam_OutputFlag, 0);
        GRBModel model(env);
        vars = model.addVars(lb.data(), ub.data(), nullptr, nullptr, nullptr, (int)num_vars);
        model.update();

        model.addQConstr(normSquared(vars, num_vars), GRB_LESS_EQUAL, max_x_norm * max_x_norm);

        GRBQuadExpr objective_fn = normSquared(buildVectorOfExperssions(A, vars, b), weights);
        // Check if we need to add anything extra to the main diagonal.
        const VectorXd eigenvalues = (A.transpose() * weights.asDiagonal() * A).selfadjointView<Upper>().eigenvalues();
        if ((eigenvalues.array() < 1.1e-4).any())
        {
            const std::vector<double> diagonal(num_vars, 1.1e-4 - eigenvalues.minCoeff());
            objective_fn.addTerms(diagonal.data(), vars, vars, (int)num_vars);
        }
        model.setObjective(objective_fn, GRB_MINIMIZE);

        model.update();
        model.optimize();

        if (model.get(GRB_IntAttr_Status) == GRB_OPTIMAL)
        {
            x.resize(num_vars);
            for (ssize_t var_ind = 0; var_ind < num_vars; var_ind++)
            {
                x(var_ind) = vars[var_ind].get(GRB_DoubleAttr_X);
            }
        }
        else
        {
            std::cout << "Status: " << model.get(GRB_IntAttr_Status) << std::endl;
            exit(-1);
        }
    }
    catch(GRBException e)
    {
        std::cout << "Error code = " << e.getErrorCode() << std::endl;
        std::cout << e.getMessage() << std::endl;
    }
    catch(...)
    {
        std::cout << "Exception during optimization" << std::endl;
    }

    delete[] vars;
    return x;
}

Eigen::VectorXd smmap::minSquaredNormSE3VelocityConstraints(const Eigen::MatrixXd& A, const Eigen::VectorXd& b, const double max_se3_velocity, const Eigen::VectorXd& weights)
{
    VectorXd x;
    GRBVar* vars = nullptr;
    try
    {
        const ssize_t num_vars = A.cols();
        assert(num_vars % 6 == 0);

        const std::vector<double> lb(num_vars, -max_se3_velocity);
        const std::vector<double> ub(num_vars, max_se3_velocity);

        // TODO: Find a way to put a scoped lock here
        gurobi_env_construct_mtx.lock();
        GRBEnv env;
        gurobi_env_construct_mtx.unlock();

        env.set(GRB_IntParam_OutputFlag, 0);
        GRBModel model(env);
        vars = model.addVars(lb.data(), ub.data(), nullptr, nullptr, nullptr, (int)num_vars);
        model.update();

        // Add the SE3 velocity constraints
        for (int i = 0; i < num_vars / 6; i++)
        {
            model.addQConstr(normSquared(&vars[i * 6], 6), GRB_LESS_EQUAL, max_se3_velocity * max_se3_velocity);
        }

        GRBQuadExpr objective_fn = normSquared(buildVectorOfExperssions(A, vars, b), weights);
        // Check if we need to add anything extra to the main diagonal.
        const VectorXd eigenvalues = (A.transpose() * weights.asDiagonal() * A).selfadjointView<Upper>().eigenvalues();
        if ((eigenvalues.array() < 1.1e-4).any())
        {
            std::vector<double> diagonal(num_vars, 1.1e-4 - eigenvalues.minCoeff());
            objective_fn.addTerms(diagonal.data(), vars, vars, (int)num_vars);
        }
        model.setObjective(objective_fn, GRB_MINIMIZE);

        model.update();
        model.optimize();

        if (model.get(GRB_IntAttr_Status) == GRB_OPTIMAL)
        {
            x.resize(num_vars);
            for (ssize_t var_ind = 0; var_ind < num_vars; var_ind++)
            {
                x(var_ind) = vars[var_ind].get(GRB_DoubleAttr_X);
            }
        }
        else
        {
            std::cout << "Status: " << model.get(GRB_IntAttr_Status) << std::endl;
            exit(-1);
        }
    }
    catch(GRBException e)
    {
        std::cout << "Error code = " << e.getErrorCode() << std::endl;
        std::cout << e.getMessage() << std::endl;
    }
    catch(...)
    {
        std::cout << "Exception during optimization" << std::endl;
    }

    delete[] vars;
    return x;
}


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef NORMAL_FITNESS_METRIC_HPP_
#define NORMAL_FITNESS_METRIC_HPP_

#include <boost/math/constants/constants.hpp>
#include <vector>
#include <cmath>

#include "clotho/fitness/ifitness.hpp"

extern const std::string NORM_NAME;

/**
 * univariate normal distribution function
 *
 * Note: for k-dimensional trait vector the fitness is based on the phenotype of
 * only the first trait
 */
class normal_fitness_metric : public ifitness {
public:
    typedef double      real_type;
    typedef real_type   result_type;

    normal_fitness_metric( real_type mu = 0., real_type sigma = 1. );

    result_type operator()( double x );
    result_type operator()( float x ) { return operator()( (double) x ); }

    result_type operator()( double x, real_type mu, real_type sigma );
    result_type operator()( float x, real_type mu, real_type sigma ) { return operator()( (double) x, mu, sigma); }

    inline result_type operator()( const std::vector< double > & multi_variate ) {
        return ((multi_variate.empty()) ? operator()( 0. ) : operator()( multi_variate.front() ));
    }

    inline result_type operator()( const std::vector< float > & multi_variate ) {
        return ((multi_variate.empty()) ? operator()( 0. ) : operator()( multi_variate.front() ));
    }

    inline result_type operator()( const std::vector< double > & multi_variate, real_type mu, real_type sigma ) {
        return ((multi_variate.empty()) ? operator()( 0., mu, sigma ) : operator()( multi_variate.front(), mu, sigma ));
    }

    inline result_type operator()( const std::vector< float > & multi_variate, real_type mu, real_type sigma ) {
        return ((multi_variate.empty()) ? operator()( 0., mu, sigma ) : operator()( multi_variate.front(), mu, sigma ));
    }

    inline result_type operator()( double * first, double * last ) {
        return (first == last) ? operator()( 0. ) : operator()( *first );
    }

    inline result_type operator()( float * first, float * last ) {
        return (first == last) ? operator()( 0. ) : operator()( *first );
    }

    const std::string name() const;

    void log( std::ostream & out ) const;

    virtual ~normal_fitness_metric();

protected:
    real_type m_mean;
    real_type m_sigma;

    real_type _coeff, _denom;
};

#endif  // NORMAL_FITNESS_METRIC_HPP_


#include <stdio.h>
#include <omp.h>

#include <iostream>

#include <Eigen/Dense>
#include <Parareal/core.h>
#include <Parareal/backward_euler.h>

#include "rhs_linear1d.h"

typedef Eigen::VectorXd Evec;
typedef Eigen::MatrixXd Emat;

int main(int argc, char **argv)
{
  ode_system ode;
  ode.dimension = 1; ode.t_init = 0; ode.t_final = 1;
  ode.y0 = Evec(1); ode.y0(0) = 1;
  ode.f = std::function<int(double, Evec&, Evec&)>(&linear1d);
  ode.J = std::function<int(double, Evec&, Emat&)>(&linear1d_jac);

  time_stepper solver;

  // Backward Euler
  solver.dt = 0.1;
  solver.F = std::function<int(ode_system&, double, Evec &)>(&backward_euler);
  solver.F_allt = std::function<int(ode_system&, double, Emat &)>(&backward_euler_allt);

  int steps = ode.num_steps(solver.dt);
  double tt = 0.0;

  // Parareal Solver
  Emat yf(steps, ode.dimension);
  tt = omp_get_wtime();
  solver.integrate_allt(ode, yf);
  tt = omp_get_wtime() - tt;

  std::cout << yf << std::endl;
  printf("Time taken %f\n", tt);

  return 0;
}


#include <iostream>
#include <vector>
#include <unordered_map>
#include <fstream>
#include <boost/functional/hash.hpp>
#include <algorithm>
#include <utility>
#include <cassert>
#include <chrono>
#include <iomanip>

using namespace std;
using namespace std::chrono;

#define point pair<double, double>
#define inter pair<bool, point>
const double doublePrecision = 1e-6;
const double delta = 1.0;
const double upperDelta = 1.0;

static void print(point u) {
	cerr << setprecision(15) << u.first << " " << u.second << endl;
}

// Read pair of doubles.
static vector<point> readBinary(char* fileName, int limitSize) {
	ifstream input(fileName, ios::binary);
    auto pos = input.tellg();
    input.seekg( 0, ios::end );
    auto size=input.tellg()-pos;
    input.seekg(0,ios::beg);
/*
	size >>= 1;
	cerr <<"size = " << size << endl;
	int sizeToRead = limitSize == -1 ? size : (limitSize < size ? limitSize : size);
	vector<point> coord(sizeToRead);
	cerr << "sizeToRead = " << sizeToRead << endl;
	for (unsigned index = 0; index < sizeToRead; ++index) {
		double x, y;
		input.read(reinterpret_cast<char*>(&x), sizeof(double));
		input.read(reinterpret_cast<char*>(&y), sizeof(double));
		long double x0 = static_cast<long double>(x);
		long double y0 = static_cast<long double>(y);
		coord[index] = make_pair(x0, y0 + delta);
		if (index < 10)
			print(coord[index]);
	}
	return coord;
*/
    unsigned elements = size / sizeof(point);
    if (limitSize != -1)
    	elements = limitSize < elements ? limitSize : elements;
    vector<point> cdf;
    cdf.resize(elements);
    input.read(reinterpret_cast<char*>(cdf.data()), elements * sizeof(point));
    return cdf;
}

// Write the sample points in a binary file.
static void writeBinary(char* fileName, vector<point>& samplePoints) {
	ofstream output(fileName, ios::binary);
	for (point p: samplePoints) {
		double x = static_cast<double>(p.first);
		double y = static_cast<double>(p.second - delta);
		output.write(reinterpret_cast<char*>(&x), sizeof(double));
		output.write(reinterpret_cast<char*>(&y), sizeof(double));
	}
}

// Read from a .txt file.
static vector<point> readTxt(char* fileName) {
	ifstream input(fileName);
	vector<point> coord;
	double x, y;
	while (input >> x >> y) {
		long double x0 = static_cast<long double>(x);
		long double y0 = static_cast<long double>(y);
		coord.push_back(make_pair(x0, y0 + delta));
	}
	return coord;
}

// Write into a .txt file
static void writeTxt(char* fileName, vector<point>& samplePoints) {
	ofstream output(fileName);
	for (point p: samplePoints)
		output << p.first << " " << (p.second - delta) << "\n";
}

// Computes the position of the triple (u, v, w) in respect to pi.
static int angle(bool counterclockwise, point u, point v, point w) {
	double ret = ((v.first - u.first) * (w.second - u.second) - (v.second - u.second) * (w.first - u.first));
	return counterclockwise ? ret : -ret;
}

static double myabs(long double x) {
	return (x < -doublePrecision) ? -x : x;
}

// Intersection of the lines (u1, u2) and (v1, v2).
static inter intersection(point u1, point u2, point v1, point v2) {
	double a1 = u2.second - u1.second, b1 = u1.first - u2.first, c1 = a1 * u1.first + b1 * u1.second;
	double a2 = v2.second - v1.second, b2 = v1.first - v2.first, c2 = a2 * v1.first + b2 * v1.second;
	double det = a1 * b2 - a2 * b1;

	// compute with long double precision.
	if (abs(det) < doublePrecision) {
		cerr << "ba e determinantul zero!!! Ia sa vedem, totusi" << endl;
		print(u1);
		print(u2);
		print(v1);
		print(v2);
		return make_pair(false, u1);
	}
	return make_pair(true, make_pair((c1 * b2 - c2 * b1) / det, (a1 * c2 - a2 * c1) / det));
	/*if ((min(v1.first, v2.first) <= ret.first) && (ret.first <= max(v1.first, v2.first)) && (min(v1.second, v2.second) <= ret.second) && (ret.second <= max(v1.second, v2.second))) {
		return ret;
	} else {
		print(ret);
		print(u1);
		print(u2);
		print(v1);
		print(v2);
		//cerr << u1.firs << " "  << u2 << " " << v1 << " " << v2 << endl;
		assert(0);
	}*/
}

double globalEpsForChange;
double minusEpsForChange;

point change(bool sign, vector<point>& coord, unsigned index) {
    return sign ? make_pair(coord[index - 1].first, coord[index - 1].second * (1 + globalEpsForChange) + delta + upperDelta)
                : make_pair(coord[index - 1].first, coord[index - 1].second + delta);
}

// Computes the samples points.
vector<point> samplePointsOfApproximateFunction(vector<point>& coord, double eps) {
	unsigned n = coord.size();

    globalEpsForChange = eps;
    minusEpsForChange = 1e-2;

	cerr << "size of coord in funciton = " << n << endl;

	point *windowBound = new point[2], *leftBound = new point[2], *rightBound = new point[2];

	// lists of neighbours.
	unordered_map<point, point, boost::hash<point>> *rightList = new unordered_map<point, point, boost::hash<point>>[2];
	unordered_map<point, point, boost::hash<point>> *leftList = new unordered_map<point, point, boost::hash<point>>[2];

	//map<point, point> *rightList = new map<point, point>[2];
	//map<point, point> *leftList = new map<point, point>[2];

	cerr << "before alloc" << endl;

	vector<point> samplePoints;

	cerr << "wwas???" << endl;

	// Maybe we should reverse the order in points.
	/*for (unsigned index = 1; index <= n; ++index) {
		points[index][0] = make_pair(coord[index - 1].first, coord[index - 1].seconc * (1 - eps) + delta);
		points[index][1] = make_pair(coord[index - 1].first, coord[index - 1].second * (1 + eps) + delta + upperDelta);
	}*/

	cerr << "Umwandeln" << endl;

	for (int sign = 1; sign >= 0; --sign) {
        point p1 = change(sign, coord, 1), p2 = change(sign, coord, 2);

		windowBound[sign] = p1;//points[1][sign];
		leftBound[sign] = p1;//points[1][sign];
		rightBound[sign] = p1;//points[1][sign];

		rightList[sign][p1] = p2;//points[2][sign];
		leftList[sign][p2] = p1;//points[1][sign];
	}

	for (unsigned index = 3; index < n; ++index) {
		//cerr << "now at index = " << index << endl;
		bool nextWindow = false;
		// Updating convex hulls
		for (int sign = 1; sign >= 0; --sign) {
            point indexPoint = change(sign, coord, index);
			point currPoint = change(sign, coord, index - 1);

			while ((currPoint != windowBound[sign]) && (angle(sign, indexPoint, currPoint, leftList[sign][currPoint]) > doublePrecision))
				currPoint = leftList[sign][currPoint];
			rightList[sign][currPoint] = indexPoint; //[index][sign];
			leftList[sign][indexPoint] = currPoint;
		}

		for (int sign = 1; sign >= 0; --sign) {
			int star = sign, diamond = !sign;
            point indexPointStar = change(star, coord, index);
            point indexPointDiamond = change(diamond, coord, index);
            point lastIndexPoint = change(star, coord, index - 1);

			if ((!nextWindow) && (angle(star, indexPointStar, leftBound[star], rightBound[diamond]) < -doublePrecision)) {
				// Get new sample point.
				//cerr << "found sth!" << endl;
				inter value = intersection(leftBound[star], rightBound[diamond], windowBound[star], windowBound[diamond]);
				if (value.first == true) {
					samplePoints.push_back(value.second);
				} else {
					cerr << "For one sample point the determinant is 0!" << endl;
				}
				//cerr << "it's not him" << endl;

				// Update the window.
				windowBound[diamond] = rightBound[diamond];
				value = intersection(leftBound[star], rightBound[diamond], lastIndexPoint, indexPointStar);
				if (value.first == true)
					windowBound[star] = value.second;
				else
					assert(0);

				// Update the lists.
				rightList[star][windowBound[star]] = indexPointStar;
				leftList[star][indexPointStar] = windowBound[star];

				// Update rightBound
				rightBound[star] = indexPointStar;
				rightBound[diamond] = indexPointDiamond;

				// Update leftBound
				leftBound[star] = windowBound[star];
				leftBound[diamond] = windowBound[diamond];

				while (angle(diamond, leftBound[diamond], rightBound[star], rightList[diamond][leftBound[diamond]]) < -doublePrecision)
					leftBound[diamond] = rightList[diamond][leftBound[diamond]];
				nextWindow = true;
			}
		}

		// Updating the supports and separating lines.
		if (!nextWindow) {
			for (int sign = 1; sign >= 0; --sign) {
				int star = sign, diamond = !sign;
                point indexPoint = change(star, coord, index);

				if (angle(star, indexPoint, leftBound[diamond], rightBound[star]) < -doublePrecision) {
					rightBound[star] = indexPoint;
					while (angle(star, indexPoint, leftBound[diamond], rightList[diamond][leftBound[diamond]]) < -doublePrecision)
						leftBound[diamond] = rightList[diamond][leftBound[diamond]];
				}
			}
		}
	}

	// Compute the two last approximated points.
	// Quite weird here!
	cerr << "by now" << samplePoints.size() << endl;
	// I'm still confused if there is need for a condition here.
	cerr << "n-ter Eintrag = " << endl;
	//print(points[n][1]);
	//print(points[n][0]);
	cerr << "leftBound + rightBound + windowBound" << endl;
	for (int sign = 1; sign >= 0; --sign) {
		print(leftBound[sign]);
		print(rightBound[sign]);
		print(windowBound[sign]);
	}

	inter value = intersection(leftBound[0], rightBound[1], windowBound[1], windowBound[0]);
	if (value.first == true)
		samplePoints.push_back(value.second);
	else
		cerr << "Penultimul punct nu exista!" << endl;

	value = intersection(leftBound[0], rightBound[1], change(1, coord, n), change(0, coord, n));
	if (value.first == true)
		samplePoints.push_back(value.second);
	else
		cerr << "Ultimul punct nu exista!" << endl;
	return samplePoints;
}

static double interpolate(vector<point>& spline, double pos, bool dump) {
   if (pos <= spline.front().first)
      return spline.front().second;
   if (pos >= spline.back().first)
      return spline.back().second;

	auto iter=lower_bound(spline.begin(),spline.end(),pos,[](const point& a,double b) { return a.first<b; });

   if (dump) cerr << "(" << (iter-1)->first << "," << (iter-1)->second << ") - " << pos << " - (" << (iter+0)->first << "," << (iter+0)->second << ")" << endl;

   if (iter->first == pos)
      return iter->second;

   double dx = iter->first - (iter - 1)->first;
   double dy = iter->second - (iter - 1)->second;

   double ofs = pos - (iter - 1)->first;
   return (iter - 1)->second + ofs * (dy / dx);
}

/*
static void convertBack(vector<point>& func) {
	for (auto& e: func)
		e.second -= delta;
}
*/

static vector<point> compressFunc(vector<point>& func, unsigned desiredSize)
// Compress to the desired size
{
	// Relax a bit to speed up compression
	unsigned maxSize = desiredSize + (desiredSize / 100), minSize = desiredSize - (desiredSize / 100);

	cerr << maxSize << " and min = " << minSize << endl;

	// Fits?
	if (func.size()<=maxSize)
		return func;

	// No, binary search
	long long capacity = 1e9, left = 1, right = capacity;
	double mul = 1.0 / capacity;
	//long double last = 0;
	while (left < right) {
		long long middle=(left+right)/2;

		double epsilon = mul * middle;
				cerr << "------------- Binary search for " << epsilon << endl;

		//if (abs(epsilon - last) < doublePrecision)return

		if (myabs(epsilon - 1) < doublePrecision) {
			return samplePointsOfApproximateFunction(func, epsilon);
		}
		if (myabs(epsilon - capacity) < doublePrecision) {
			return samplePointsOfApproximateFunction(func, epsilon);
		}

		vector<point> candidate = samplePointsOfApproximateFunction(func, epsilon);

		cerr << "done sample" << endl;
		cerr << "------------- Binary search for " << epsilon << " has " << candidate.size() << endl;

		cerr << candidate.size() << " " << minSize << " " << maxSize << endl;

		if (candidate.size() < minSize) {
	 		right = middle;
		} else if (candidate.size()>maxSize) {
	 		left = middle;
		} else {
			cerr << "Found already in binary search = " << middle << " and epsilon " << mul * middle << endl;
			//convertBack(candidate);
	 		return candidate;
		}
	}

	cerr << "At the end is left = " << left << " and so epsilon " << (mul * left) << endl;

   // Final call, this is the best we could get
   vector<point> candidate = samplePointsOfApproximateFunction(func, mul * left);
   //convertBack(candidate);
   return candidate;
}

int mainn(int argc, char** argv) {
	if (argc != 7) {
		cout << "Usage: ./a.out inputVariant(1 -> binary, 0 -> txt) inputFileName outputVariant(1 -> binary, 0 -> txt) outputFileName howManySupportingPoints howManyToRead\n";
		return 1;
	}
	int inputVariant = atoi(argv[1]);
	char* inputFileName = argv[2];
	int outputVariant = atoi(argv[3]);
	char* outputFileName = argv[4];
	int countSupportingPoints = atoi(argv[5]);
	int countToRead = atoi(argv[6]);

	cerr << "Begin of reading" << endl;
	auto readingStart = high_resolution_clock::now();

	vector<point> coord;
	if (inputVariant == 1)
		coord = readBinary(inputFileName, countToRead);
	else
		coord = readTxt(inputFileName);

	auto readingStop = high_resolution_clock::now();
	cerr << "Reading done!" << endl;
	/*
	if (useSort) {
		cerr << "Begin of sort" << endl;
		cerr << "wieder" << coord.size() << endl;
		auto startSort = high_resolution_clock::now();

		sort(coord.begin(), coord.end());

		auto sortStop = high_resolution_clock::now();
		cerr << "Sort done!" << endl;

		cerr << "wieder" << coord.size() << endl;

		//writeBinary(outputFileName, coord);
		//return 0;
	}
	*/
	cerr << "test" << coord[0].first << " " << coord[0].second << endl;
	cerr << "test" << coord[1].first << " " << coord[1].second << endl;

	cerr << "Begin of the algorithm" << endl;
	auto startAlg = high_resolution_clock::now();

	cerr << "count = " << countSupportingPoints << endl;
/*
	unsigned n = coord.size();
	points = new point*[n + 1];
	for (unsigned index = 1; index <= n; ++index) {
		points[index] = new point[2];
	}
*/
	//vector<point> samplePoints = compressFunc(coord, countSupportingPoints);
	vector<point> samplePoints = samplePointsOfApproximateFunction(coord, 1e-15);

	cerr << "End of the algorithm" << endl;
	auto algStop = high_resolution_clock::now();

	cerr << "Begin of writing" << endl;
	auto startWriting = high_resolution_clock::now();
/*
	if (outputVariant == 1)
		writeBinary(outputFileName, samplePoints);
	else
		writeTxt(outputFileName, samplePoints);

	auto writingDone = high_resolution_clock::now();
*/
	if (samplePoints.size() == 0) {
		cerr << "alles falsch" << endl;
		return 0;
	}
	  double sumError = 0, maxError = 0;
	  for (auto e: coord) {
          // TODO: I have put here a + delta! Don't forget that!
	     double estimate = interpolate(samplePoints, e.first, false);
	     double real = e.second + delta;
	     double diff = estimate-real;
	     if (diff<0)
	     	diff=-diff;
	     if (diff>maxError)
	        maxError=diff;
	     sumError+=diff;
	  }
	  cerr << samplePoints.size() << " " << maxError << " " << (sumError / coord.size()) << endl;

	//auto duration = duration_cast<milliseconds>(writingDone - readingStart);
	//cerr << "Total time is = " << duration.count() << endl;
	return 0;
}


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_triangulation.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>

#include "theia/solvers/estimator.h"
#include "theia/solvers/ransac.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/sfm/types.h"

namespace theia {

namespace {
// The pixel observation and projection matrix needed in order to triangulate a
// 3D point.
struct PointObservation {
  Matrix3x4d projection_matrix;
  Eigen::Vector2d feature;
};

// Returns true if the point is in front of the camera and false if the point is
// behind the camera.
bool IsPointInFrontOfCamera(const Matrix3x4d& projection_matrix,
                            const Eigen::Vector4d& point) {
  return point.dot(projection_matrix.row(2)) > 0;
}

class TriangulationEstimator
    : public Estimator<PointObservation, Eigen::Vector4d> {
 public:
  TriangulationEstimator() {}

  double SampleSize() const { return 2; }

  // Triangulates the 3D point from 2 observations.
  bool EstimateModel(const std::vector<PointObservation>& observations,
                     std::vector<Eigen::Vector4d>* triangulated_points) const {
    // TODO(cmsweeney): We do not check the angle between the two views at the
    // moment. This requires the ray direction of each feature meaning we would
    // have to either decompose the projection matrix or pass in the ray
    // direction as part of the Point Observation. RANSAC should be good enough
    // at filtering out these bad solutions so we ignore this for now.
    triangulated_points->resize(1);
    if (!Triangulate(observations[0].projection_matrix,
                     observations[1].projection_matrix,
                     observations[0].feature,
                     observations[1].feature,
                     &triangulated_points->at(0))) {
      return false;
    }
    // Only return true if the point is in front of both cameras and the
    // triangulation was a success.
    return IsPointInFrontOfCamera(observations[0].projection_matrix,
                                  triangulated_points->at(0)) &&
        IsPointInFrontOfCamera(observations[1].projection_matrix,
                               triangulated_points->at(0));
  }

  double Error(const PointObservation& observation,
               const Eigen::Vector4d& triangulated_point) const {
    if (!IsPointInFrontOfCamera(observation.projection_matrix,
                                triangulated_point)) {
      return std::numeric_limits<double>::max();
    }

    const Eigen::Vector2d reprojection =
        (observation.projection_matrix * triangulated_point).hnormalized();
    return (observation.feature - reprojection).squaredNorm();
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(TriangulationEstimator);
};

}  // namespace

bool EstimateTriangulation(const RansacParameters& ransac_params,
                           const std::vector<Matrix3x4d>& projection_matrices,
                           const std::vector<Eigen::Vector2d>& features,
                           Eigen::Vector4d* triangulated_point,
                           RansacSummary* summary) {
  if (projection_matrices.size() < 2) {
    return false;
  }

  // Create point correspondences.
  std::vector<PointObservation> point_observations(
      projection_matrices.size());
  for (int i = 0; i < point_observations.size(); i++) {
    point_observations[i].projection_matrix = projection_matrices[i];
    point_observations[i].feature = features[i];
  }

  // RANSAC triangulation.
  TriangulationEstimator triangulation_estimator;
  Ransac<TriangulationEstimator> ransac(ransac_params,
                                        triangulation_estimator);
  CHECK(ransac.Initialize());
  if (!ransac.Estimate(point_observations, triangulated_point, summary)) {
    return false;
  }

  return true;
}

}  // namespace theia


/*
 * Filename: similarity.cpp
 *
 * Copyright 2020 Tecnalia
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "similarity.h"

#include <Eigen/LU>

#include <numeric>

namespace manipulability_metrics
{
double volumeIntersection(const Ellipsoid& desired_ellipsoid, const Eigen::Matrix<double, 6, Eigen::Dynamic>& jacobian)
{
  // Force evaluation by casting to avoid multiple (lazy) computations when iterating
  auto jjti = static_cast<Eigen::Matrix<double, 6, 6>>((jacobian * jacobian.transpose()).inverse());

  return std::accumulate(cbegin(desired_ellipsoid), cend(desired_ellipsoid), 1.0, [&](auto volume, const auto& ax) {
    double nu = 1.0 / sqrt(ax.unit.transpose() * jjti * ax.unit);
    return volume * std::min(nu, ax.len);
  });
}

double inverseShapeDiscrepancy(const Ellipsoid& desired_ellipsoid,
                               const Eigen::Matrix<double, 6, Eigen::Dynamic>& jacobian)
{
  // Force evaluation by casting to avoid multiple (lazy) computations when iterating
  auto jjti = static_cast<Eigen::Matrix<double, 6, 6>>((jacobian * jacobian.transpose()).inverse());

  return 1.0 / std::accumulate(cbegin(desired_ellipsoid), cend(desired_ellipsoid),
                               std::numeric_limits<double>::epsilon(), [&](auto sq_diff, const auto& ax) {
                                 double nu = 1.0 / sqrt(ax.unit.transpose() * jjti * ax.unit);
                                 return sq_diff + pow(nu - ax.len, 2.0);
                               });
}

double dualVolumeIntersection(const Ellipsoid& desired_ellipsoid,
                              const Eigen::Matrix<double, 6, Eigen::Dynamic>& left_jacobian,
                              const Eigen::Matrix<double, 6, Eigen::Dynamic>& right_jacobian)
{
  const auto left_jjti =
      static_cast<Eigen::Matrix<double, 6, 6>>((left_jacobian * left_jacobian.transpose()).inverse());
  const auto right_jjti =
      static_cast<Eigen::Matrix<double, 6, 6>>((right_jacobian * right_jacobian.transpose()).inverse());

  double volume = 1.0;
  for (auto i = 0; i < 6; ++i)
  {
    double left_nu = 1.0 / sqrt(desired_ellipsoid[i].unit.transpose() * left_jjti * desired_ellipsoid[i].unit);
    double right_nu = 1.0 / sqrt(desired_ellipsoid[i].unit.transpose() * right_jjti * desired_ellipsoid[i].unit);
    volume *= std::min({ desired_ellipsoid[i].len, left_nu, right_nu });
  }

  return volume;
}

double dualInverseShapeDiscrepancy(const Ellipsoid& desired_ellipsoid,
                                   const Eigen::Matrix<double, 6, Eigen::Dynamic>& left_jacobian,
                                   const Eigen::Matrix<double, 6, Eigen::Dynamic>& right_jacobian)
{
  const auto left_jjti =
      static_cast<Eigen::Matrix<double, 6, 6>>((left_jacobian * left_jacobian.transpose()).inverse());
  const auto right_jjti =
      static_cast<Eigen::Matrix<double, 6, 6>>((right_jacobian * right_jacobian.transpose()).inverse());

  double sq_diff = 0.0;
  for (auto i = 0; i < 6; ++i)
  {
    double left_nu = 1.0 / sqrt(desired_ellipsoid[i].unit.transpose() * left_jjti * desired_ellipsoid[i].unit);
    double right_nu = 1.0 / sqrt(desired_ellipsoid[i].unit.transpose() * right_jjti * desired_ellipsoid[i].unit);
    sq_diff += pow(desired_ellipsoid[i].len - std::min(left_nu, right_nu), 2.0);
  }

  return 1.0 / (sq_diff + std::numeric_limits<double>::epsilon());
}
}  // namespace manipulability_metrics


#include "misc.h"

#include <stdlib.h>
#include <cmath>
#include <random>

#include "../molecules/bead.h"
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

double getDist(Bead* b1, Bead* b2, double box_l[], int npbc) {
  double dist = 0;
  for (int i = 0; i < 3; i++) {
    double di = b1->GetCrd(1, i) - b2->GetCrd(1,i);
    if (i < npbc) {
      // Periodic boundary conditions
      di -= box_l[i] * round(di / box_l[i]);
    }
    dist += (di * di);
  }

  return sqrt(dist);

}

double getDist(Bead& b1, Bead& b2, double box_l[], int npbc) {
  double dist = 0;
  for (int i = 0; i < 3; i++) {
    double di = b1.GetCrd(1, i) - b2.GetCrd(1,i);
    if (i < npbc) {
      // Periodic boundary conditions
      di -= box_l[i] * round(di / box_l[i]);
    }
    dist += (di * di);
  }

  return sqrt(dist);

}

void GetDistVector(Bead& b1, Bead& b2, double box_l[], int npbc,
                   double (&dist)[3]) {
  // Vector pointing from Bead1 to Bead2.
  for (int i = 0; i < 3; i++) {
    double di = b2.GetCrd(1, i) - b1.GetCrd(1, i);

    // Periodic boundary conditions.
    if (i < npbc) {
      di -= box_l[i] * round(di / box_l[i]);
    }
    dist[i] = di;
  }

}

void GetDistVectorConsistent(Bead& b1, Bead& b2, double box_l[], int npbc,
                             double (&dist)[3]) {

  double xyz1[3] = {b1.GetCrd(1, 0), b1.GetCrd(1, 1), b1.GetCrd(1, 2)};
  double xyz2[3] = {b2.GetCrd(1, 0), b2.GetCrd(1, 1), b2.GetCrd(1, 2)};
  for (int i = 0; i < 3; i++) {
    xyz1[i] -= box_l[i] * floor(xyz1[i] / box_l[i]);
    xyz2[i] -= box_l[i] * floor(xyz2[i] / box_l[i]);
    dist[i] = xyz2[i] - xyz1[i];
  }

}

void GetDistVectorC(Bead& b1, Bead& b2, double box_l[], int npbc,
                    double (&dist)[3]) {
  // Vector pointing from Bead1 to Bead2.
  for (int i = 0; i < 3; i++) {
    double di = b2.GetCrd(0, i) - b1.GetCrd(0, i);

    // Periodic boundary conditions.
    if (i < npbc) {
      di -= box_l[i] * round(di / box_l[i]);
    }
    // Finding the shortest distance between periodic images.
    if (abs(di) > box_l[i]/2.0) {
      if (di < 0) {
        di += box_l[i];
      }
      else {
        di -= box_l[i];
      }
    }
    dist[i] = di;
  }

}

void randSphere(double vec[], mt19937& rand_gen) {
  double rand_square = 2;
  double r1, r2;
  while (rand_square > 1) {
    // Random numbers between -1 and 1.
    r1 = 1 - 2 * ((double)rand_gen() / rand_gen.max());
    r2 = 1 - 2 * ((double)rand_gen() / rand_gen.max());
    rand_square = r1 * r1 + r2 * r2;
  }
  double ranh = 2 * sqrt(1 - rand_square);
  vec[0] = r1 * ranh;
  vec[1] = r2 * ranh;
  vec[2] = (1 - 2*rand_square);

}

double gasdev(double mean, double stdev, mt19937& ranGen) {
  static int iset = 0;
  static double gset;
  double fac, rsq, v1, v2;
  if (iset == 0) {
    do {
      v1=2.0*((double)ranGen() / ranGen.max()) - 1.0;
      v2=2.0*((double)ranGen() / ranGen.max()) - 1.0;
      rsq = v1*v1 + v2*v2;
    } while (rsq >= 1.0 || rsq == 0.0 );
      fac = sqrt(-2.0 * log(rsq)/rsq);

      gset = v1*fac;
      iset = 1;
      return v2*fac * stdev + mean;
  }
  else {
    iset = 0;
    return gset * stdev + mean;
  }

  return 0;

}

string YesOrNo(bool input) {
  if (input) {
    return "yes";
  }
  else {
    return "no";
  }

}

int factorial(int n) {
  if (n != 1) {
     return n*factorial(n-1);
  }
  else {
    return 1;
  }

}

/** This is a simple least square fit procedure. */
double Interpolate(double * bin_pos, double * f, int len, double sigma) {
  /*                     INDEX 2
     X = | bin1_pos 1 |  B = | b1 |  Y = | g(bin1_pos) | d
         | bin2_pos 1 |      | b2 |      | g(bin2_pos) | i     INDEX 1
         |    ...     |                  |     ...     | m e
   */
  MatrixXf X(len, 2);
  VectorXf Y(len);
  for (int i = 0; i < len; i++) {
    X(i, 0) = bin_pos[i];
    X(i, 1) = 1.0;
    Y(i) = f[i];
  }
  VectorXf B = (X.transpose() * X).ldlt().solve(X.transpose() * Y);

  /*
  cout << sigma << '\t' << sigma * B(0) + B(1) << endl;
  for (int i = 0; i < len; i++)
  cout << bin_pos[i] << '\t' << f[i] << endl;
  cout << endl;
  */

  return sigma * B(0) + B(1);

}

double Interpolate2(double * bin_pos, double * f, int len, double sigma) {
  MatrixXf X(len, 2);
  VectorXf Y(len);
  for (int i = 0; i < len; i++) {
    X(i, 0) = bin_pos[i];
    X(i, 1) = 1.0;
    Y(i) = f[i];
  }
  VectorXf B = (X.transpose() * X).ldlt().solve(X.transpose() * Y);
  return bin_pos[len-1] * B(0) + B(1);

}