#pragma once
#ifndef VECTORMATH_NEW_H
#define VECTORMATH_NEW_H

#include <vector>
#include <memory>

#include <Eigen/Core>

#include <data/Spin_System.hpp>
#include <engine/Vectormath_Defines.hpp>

namespace Engine
{
    namespace Vectormath
    {
        /////////////////////////////////////////////////////////////////
        //////// Single Vector Math

        // Rotate a vector around an axis by a certain degree (Implemented with Rodrigue's formula)
        void rotate(const Vector3 & v, const Vector3 & axis, const scalar & angle, Vector3 & v_out);
        void rotate( const vectorfield & v, const vectorfield & axis, const scalarfield & angle,
                     vectorfield & v_out );

        // Decompose a vector into numbers of translations in a basis
        Vector3 decompose(const Vector3 & v, const std::vector<Vector3> & basis);

        /////////////////////////////////////////////////////////////////
        //////// Translating across the lattice

        inline int idx_from_translations(const intfield & n_cells, const int n_cell_atoms, const std::array<int, 3> & translations)
        {
            int Na = n_cells[0];
            int Nb = n_cells[1];
            int Nc = n_cells[2];
            int N = n_cell_atoms;

            int da = translations[0];
            int db = translations[1];
            int dc = translations[2];

            return da*N + db*N*Na + dc*N*Na*Nb;
        }

        #ifndef USE_CUDA

        inline int idx_from_translations(const intfield & n_cells, const int n_cell_atoms, const std::array<int, 3> & translations_i, const std::array<int, 3> translations)
        {
            int Na = n_cells[0];
            int Nb = n_cells[1];
            int Nc = n_cells[2];
            int N = n_cell_atoms;

            int da = translations_i[0] + translations[0];
            int db = translations_i[1] + translations[1];
            int dc = translations_i[2] + translations[2];

            if (translations[0] < 0)
                da += N*Na;
            if (translations[1] < 0)
                db += N*Na*Nb;
            if (translations[2] < 0)
                dc += N*Na*Nb*Nc;

            int idx = (da%Na)*N + (db%Nb)*N*Na + (dc%Nc)*N*Na*Nb;

            return idx;
        }

        inline bool boundary_conditions_fulfilled(const intfield & n_cells, const intfield & boundary_conditions, const std::array<int, 3> & translations_i, const std::array<int, 3> & translations_j)
        {
            int da = translations_i[0] + translations_j[0];
            int db = translations_i[1] + translations_j[1];
            int dc = translations_i[2] + translations_j[2];
            return ((boundary_conditions[0] || (0 <= da && da < n_cells[0])) &&
                    (boundary_conditions[1] || (0 <= db && db < n_cells[1])) &&
                    (boundary_conditions[2] || (0 <= dc && dc < n_cells[2])));
        }

        #endif
        #ifdef USE_CUDA

        inline int idx_from_translations(const intfield & n_cells, const int n_cell_atoms, const std::array<int, 3> & translations_i, const int translations[3])
        {
            int Na = n_cells[0];
            int Nb = n_cells[1];
            int Nc = n_cells[2];
            int N = n_cell_atoms;

            int da = translations_i[0] + translations[0];
            int db = translations_i[1] + translations[1];
            int dc = translations_i[2] + translations[2];

            if (translations[0] < 0)
                da += N*Na;
            if (translations[1] < 0)
                db += N*Na*Nb;
            if (translations[2] < 0)
                dc += N*Na*Nb*Nc;

            int idx = (da%Na)*N + (db%Nb)*N*Na + (dc%Nc)*N*Na*Nb;

            return idx;
        }

        inline bool boundary_conditions_fulfilled(const intfield & n_cells, const intfield & boundary_conditions, const std::array<int, 3> & translations_i, const int translations_j[3])
        {
            int da = translations_i[0] + translations_j[0];
            int db = translations_i[1] + translations_j[1];
            int dc = translations_i[2] + translations_j[2];
            return ((boundary_conditions[0] || (0 <= da && da < n_cells[0])) &&
                    (boundary_conditions[1] || (0 <= db && db < n_cells[1])) &&
                    (boundary_conditions[2] || (0 <= dc && dc < n_cells[2])));
        }

        __inline__ __device__ bool cu_check_atom_type(int atom_type)
        {
            #ifdef SPIRIT_ENABLE_DEFECTS
                // If defects are enabled we check for
                //		vacancies (type < 0)
                if (atom_type >= 0) return true;
                else return false;
            #else
                // Else we just return true
                return true;
            #endif
        }

        __inline__ __device__ bool cu_check_atom_type(int atom_type, int reference_type)
        {
            #ifdef SPIRIT_ENABLE_DEFECTS
                // If defects are enabled we do a check if
                //		atom types match.
                if (atom_type == reference_type) return true;
                else return false;
            #else
                // Else we just return true
                return true;
            #endif
        }

        // Calculates, for a spin i, a pair spin's index j.
        // This function takes into account boundary conditions and atom types and returns `-1` if any condition is not met.
        __inline__ __device__ int cu_idx_from_pair(int ispin, const int * boundary_conditions, const int * n_cells, int N, const int * atom_types, const Pair & pair, bool invert=false)
        {
            // Invalid index if atom type of spin i is not correct
            if ( pair.i != ispin%N || !cu_check_atom_type(atom_types[ispin]) )
                return -1;

            // Number of cells
            auto& Na = n_cells[0];
            auto& Nb = n_cells[1];
            auto& Nc = n_cells[2];

            // Invalid index if translations reach out over the lattice bounds
            if (std::abs(pair.translations[0]) > Na ||
                std::abs(pair.translations[1]) > Nb ||
                std::abs(pair.translations[2]) > Nc )
                return -1;

            // Translations (cell) of spin i
            int nic = ispin / (N*Na*Nb);
            int nib = (ispin - nic*N*Na*Nb) / (N*Na);
            int nia = ispin - nic*N*Na*Nb - nib*N*Na;

            // Translations (cell) of spin j (possibly outside of non-periodical domain)
            int pm = 1;
            if (invert)
                pm = -1;
            int nja = nia + pm*pair.translations[0];
            int njb = nib + pm*pair.translations[1];
            int njc = nic + pm*pair.translations[2];

            // Check boundary conditions: a
            if ( boundary_conditions[0] || (0 <= nja && nja < Na) )
            {
                // Boundary conditions fulfilled
                // Find the translations of spin j within the non-periodical domain
                if (nja < 0)
                    nja += Na;
                // Calculate the correct index
                if (nja >= Na)
                    nja -= Na;
            }
            else
            {
                // Boundary conditions not fulfilled
                return -1;
            }

            // Check boundary conditions: b
            if ( boundary_conditions[1] || (0 <= njb && njb < Nb) )
            {
                // Boundary conditions fulfilled
                // Find the translations of spin j within the non-periodical domain
                if (njb < 0)
                    njb += Nb;
                // Calculate the correct index
                if (njb >= Nb)
                    njb -= Nb;
            }
            else
            {
                // Boundary conditions not fulfilled
                return -1;
            }

            // Check boundary conditions: c
            if ( boundary_conditions[2] || (0 <= njc && njc < Nc) )
            {
                // Boundary conditions fulfilled
                // Find the translations of spin j within the non-periodical domain
                if (njc < 0)
                    njc += Nc;
                // Calculate the correct index
                if (njc >= Nc)
                    njc -= Nc;
            }
            else
            {
                // Boundary conditions not fulfilled
                return -1;
            }

            // Calculate the index of spin j according to it's translations
            int jspin = pair.j + (nja)*N + (njb)*N*Na + (njc)*N*Na*Nb;

            // Invalid index if atom type of spin j is not correct
            if ( pair.j != jspin%N || !cu_check_atom_type(atom_types[jspin]) )
                return -1;

            // Return a valid index
            return jspin;
        }

        #endif

        inline std::array<int, 3> translations_from_idx(const intfield & n_cells, const int n_cell_atoms, int idx)
        {
            std::array<int, 3> ret;
            int Na = n_cells[0];
            int Nb = n_cells[1];
            int Nc = n_cells[2];
            int N = n_cell_atoms;

            ret[2] = idx / (N*Na*Nb);
            ret[1] = (idx - ret[2] * N*Na*Nb) / (N*Na);
            ret[0] = (idx - ret[2] * N*Na*Nb - ret[1] * N*Na) / N;
            return ret;
        }

        // Check atom types
        inline bool check_atom_type(int atom_type)
        {
            #ifdef SPIRIT_ENABLE_DEFECTS
                // If defects are enabled we check for
                //		vacancies (type < 0)
                if (atom_type >= 0) return true;
                else return false;
            #else
                // Else we just return true
                return true;
            #endif
        }
        inline bool check_atom_type(int atom_type, int reference_type)
        {
            #ifdef SPIRIT_ENABLE_DEFECTS
                // If defects are enabled we do a check if
                //		atom types match.
                if (atom_type == reference_type) return true;
                else return false;
            #else
                // Else we just return true
                return true;
            #endif
        }

        // Calculates, for a spin i, a pair spin's index j.
        // This function takes into account boundary conditions and atom types and returns `-1` if any condition is not met.
        inline int idx_from_pair(int ispin, const intfield & boundary_conditions, const intfield & n_cells, int N, const intfield & atom_types, const Pair & pair, bool invert=false)
        {
            // Invalid index if atom type of spin i is not correct
            if ( pair.i != ispin%N || !check_atom_type(atom_types[ispin]) )
                return -1;

            // Number of cells
            auto& Na = n_cells[0];
            auto& Nb = n_cells[1];
            auto& Nc = n_cells[2];

            // Invalid index if translations reach out over the lattice bounds
            if (std::abs(pair.translations[0]) > Na ||
                std::abs(pair.translations[1]) > Nb ||
                std::abs(pair.translations[2]) > Nc )
                return -1;

            // Translations (cell) of spin i
            int nic = ispin / (N*Na*Nb);
            int nib = (ispin - nic*N*Na*Nb) / (N*Na);
            int nia = (ispin - nic*N*Na*Nb - nib*N*Na) / N;

            int pm = 1;
            if (invert)
                pm = -1;
            // Translations (cell) of spin j (possibly outside of non-periodical domain)
            int nja = nia + pm*pair.translations[0];
            int njb = nib + pm*pair.translations[1];
            int njc = nic + pm*pair.translations[2];

            // Check boundary conditions: a
            if ( boundary_conditions[0] || (0 <= nja && nja < Na) )
            {
                // Boundary conditions fulfilled
                // Find the translations of spin j within the non-periodical domain
                if (nja < 0)
                    nja += Na;
                // Calculate the correct index
                if (nja >= Na)
                    nja -= Na;
            }
            else
            {
                // Boundary conditions not fulfilled
                return -1;
            }

            // Check boundary conditions: b
            if ( boundary_conditions[1] || (0 <= njb && njb < Nb) )
            {
                // Boundary conditions fulfilled
                // Find the translations of spin j within the non-periodical domain
                if (njb < 0)
                    njb += Nb;
                // Calculate the correct index
                if (njb >= Nb)
                    njb -= Nb;
            }
            else
            {
                // Boundary conditions not fulfilled
                return -1;
            }

            // Check boundary conditions: c
            if ( boundary_conditions[2] || (0 <= njc && njc < Nc) )
            {
                // Boundary conditions fulfilled
                // Find the translations of spin j within the non-periodical domain
                if (njc < 0)
                    njc += Nc;
                // Calculate the correct index
                if (njc >= Nc)
                    njc -= Nc;
            }
            else
            {
                // Boundary conditions not fulfilled
                return -1;
            }

            // Calculate the index of spin j according to it's translations
            int jspin = pair.j + (nja)*N + (njb)*N*Na + (njc)*N*Na*Nb;

            // Invalid index if atom type of spin j is not correct
            if ( !check_atom_type(atom_types[jspin]) )
                return -1;

            // Return a valid index
            return jspin;
        }

        /////////////////////////////////////////////////////////////////
        //////// Vectorfield Math - special stuff

        // Build an array of spin positions and atom types. TODO: find a better name for this function
        void Build_Spins(vectorfield & positions, intfield & atom_types,
                         const std::vector<Vector3> & cell_atoms, const intfield & cell_atom_types,
                         const std::vector<Vector3> & translation_vectors, const intfield & n_cells);
        // Calculate the mean of a vectorfield
        std::array<scalar, 3> Magnetization(const vectorfield & vf);
        // Calculate the topological charge inside a vectorfield
        scalar TopologicalCharge(const vectorfield & vf, const vectorfield & vf_pos, const std::vector<std::array<int, 3>> & triangulation);

        // Utility function for the SIB Solver - maybe create a MathUtil namespace?
        void transform(const vectorfield & spins, const vectorfield & force, vectorfield & out);

        void get_random_vector(std::uniform_real_distribution<scalar> & distribution, std::mt19937 & prng, Vector3 & vec);
        void get_random_vectorfield(std::mt19937 & prng, vectorfield & xi);
        void get_random_vector_unitsphere(std::uniform_real_distribution<scalar> & distribution, std::mt19937 & prng, Vector3 & vec);
        void get_random_vectorfield_unitsphere(std::mt19937 & prng, vectorfield & xi);

        // Calculate a gradient scalar distribution according to a starting value, direction and inclination
        void get_gradient_distribution(const Data::Geometry & geometry, Vector3 gradient_direction, scalar gradient_start, scalar gradient_inclination, scalarfield & distribution, scalar range_min, scalar range_max);

        // Calculate the spatial gradient of a vectorfield in a certain direction.
        //      This requires to know the underlying geometry, as well as the boundary conditions.
        void directional_gradient(const vectorfield & vf, const Data::Geometry & geometry, const intfield & boundary_conditions, const Vector3 & direction, vectorfield & gradient);

        /////////////////////////////////////////////////////////////////
        //////// Vectormath-like operations

        // sets sf := s
        // sf is a scalarfield
        // s is a scalar
        void fill(scalarfield & sf, scalar s);

        // TODO: Add the test
        void fill(scalarfield & sf, scalar s, const intfield & mask);

        // Scale a scalarfield by a given value
        void scale(scalarfield & sf, scalar s);

        // Add a scalar to all entries of a scalarfield
        void add(scalarfield & sf, scalar s);

        // Sum over a scalarfield
        scalar sum(const scalarfield & sf);

        // Calculate the mean of a scalarfield
        scalar mean(const scalarfield & sf);

        // Cut off all values to remain in a certain range
        void set_range(scalarfield & sf, scalar sf_min, scalar sf_max);

        // sets vf := v
        // vf is a vectorfield
        // v is a vector
        void fill(vectorfield & vf, const Vector3 & v);
        void fill(vectorfield & vf, const Vector3 & v, const intfield & mask);

        // Normalize the vectors of a vectorfield
        void normalize_vectors(vectorfield & vf);

        // Get the norm of a vectorfield
        void norm( const vectorfield & vf, scalarfield & norm );

        // Pair of Minimum and Maximum of any component of any vector of a vectorfield
        std::pair<scalar, scalar> minmax_component(const vectorfield & v1);

        // Maximum absolute component of a vectorfield
        scalar max_abs_component(const vectorfield & vf);

        // Scale a vectorfield by a given value
        void scale(vectorfield & vf, const scalar & sc);

        // Sum over a vectorfield
        Vector3 sum(const vectorfield & vf);

        // Calculate the mean of a vectorfield
        Vector3 mean(const vectorfield & vf);

        // divide two scalarfields
        void divide( const scalarfield & numerator, const scalarfield & denominator, scalarfield & out );

        // TODO: move this function to manifold??
        // computes the inner product of two vectorfields v1 and v2
        scalar dot(const vectorfield & vf1, const vectorfield & vf2);

        // computes the inner products of vectors in v1 and v2
        // v1 and v2 are vectorfields
        void dot(const vectorfield & vf1, const vectorfield & vf2, scalarfield & out);

        // TODO: find a more appropriate name
        // computes the product of scalars in sf1 and sf2
        // sf1 and sf2 are vectorfields
        void dot(const scalarfield & sf1, const scalarfield & sf2, scalarfield & out);

        // computes the vector (cross) products of vectors in v1 and v2
        // v1 and v2 are vector fields
        void cross(const vectorfield & vf1, const vectorfield & vf2, vectorfield & out);

        // out[i] += c*a
        void add_c_a(const scalar & c, const Vector3 & a, vectorfield & out);
        // out[i] += c*a[i]
		void add_c_a(const scalar & c, const vectorfield & vf, vectorfield & out);
		void add_c_a(const scalar & c, const vectorfield & vf, vectorfield & out, const intfield & mask);
        // out[i] += c[i]*a[i]
        void add_c_a( const scalarfield & c, const vectorfield & vf, vectorfield & out );

        // out[i] = c*a
        void set_c_a(const scalar & c, const Vector3 & a, vectorfield & out);
        void set_c_a(const scalar & c, const Vector3 & a, vectorfield & out, const intfield & mask);
        // out[i] = c*a[i]
        void set_c_a(const scalar & c, const vectorfield & vf, vectorfield & out);
        void set_c_a(const scalar & c, const vectorfield & vf, vectorfield & out, const intfield & mask);
        // out[i] = c[i]*a[i]
        void set_c_a( const scalarfield & sf, const vectorfield & vf, vectorfield & out );

        // out[i] += c * a*b[i]
        void add_c_dot(const scalar & c, const Vector3 & a, const vectorfield & b, scalarfield & out);
        // out[i] += c * a[i]*b[i]
        void add_c_dot(const scalar & c, const vectorfield & a, const vectorfield & b, scalarfield & out);

        // out[i] = c * a*b[i]
        void set_c_dot(const scalar & c, const Vector3 & a, const vectorfield & b, scalarfield & out);
        // out[i] = c * a[i]*b[i]
        void set_c_dot(const scalar & c, const vectorfield & a, const vectorfield & b, scalarfield & out);

        // out[i] += c * a x b[i]
        void add_c_cross(const scalar & c, const Vector3 & a, const vectorfield & b, vectorfield & out);
        // out[i] += c * a[i] x b[i]
        void add_c_cross(const scalar & c, const vectorfield & a, const vectorfield & b, vectorfield & out);
        // out[i] += c[i] * a[i] x b[i]
        void add_c_cross(const scalarfield & c, const vectorfield & a, const vectorfield & b, vectorfield & out);

        // out[i] = c * a x b[i]
        void set_c_cross(const scalar & c, const Vector3 & a, const vectorfield & b, vectorfield & out);
        // out[i] = c * a[i] x b[i]
        void set_c_cross(const scalar & c, const vectorfield & a, const vectorfield & b, vectorfield & out);

    }
}

#endif


#include <frovedis.hpp>
#include <frovedis/matrix/rowmajor_matrix.hpp>
#include <frovedis/ml/nb/naive_bayes.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    std::string m_matf = "./input/m_mat";
    std::string m_lblf = "./input/m_lbl";
    std::string b_matf = "./input/b_mat";
    std::string b_lblf = "./input/b_lbl";

    auto b_mat = colmajor_matrix<double>(make_rowmajor_matrix_load<double>(b_matf));
    auto b_lbl = make_dvector_loadline<double>(b_lblf);
    auto m1 = bernoulli_nb<double,colmajor_matrix<double>,colmajor_matrix_local<double>>(b_mat,b_lbl);
    auto b_tmat = make_rowmajor_matrix_local_load<double>(b_matf);
    auto pd1 = m1.predict(b_tmat);
    //for(auto& e: pd1) std::cout << e << " "; std::cout << std::endl;

    auto m_mat = colmajor_matrix<double>(make_rowmajor_matrix_load<double>(m_matf));
    auto m_lbl = make_dvector_loadline<double>(m_lblf);
    auto m2 = multinomial_nb<double,colmajor_matrix<double>,colmajor_matrix_local<double>>(m_mat,m_lbl);
    auto m_tmat = make_rowmajor_matrix_local_load<double>(m_matf);
    auto pd2 = m2.predict(m_tmat);
    //for(auto& e: pd2) std::cout << e << " "; std::cout << std::endl;

    // confirming correctness of result
    std::vector<double> expected = {1,0,1,1,1,0,1,1};
    BOOST_CHECK (pd1 == expected && pd2 == expected);
}


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <complex>
#include <cmath>

using namespace boost::numeric::ublas ;

template<class E1, class E2>
struct component_prod{
	typedef E1			argument_type1 ;
	typedef E2			argument_type2 ;
	typedef E1			result_type ;

	static
	result_type apply(argument_type1 t1, argument_type2 t2) {
		return element_prod( t1, t2 ) ;
	}
};

template<class E1, class E2>
typename vector_binary_traits<E1, E2, component_prod<typename E1::value_type,
	typename E2::value_type> >::result_type
layer_prod( const vector_expression<E1>& e1, const vector_expression<E2>& e2 ) {
    typedef typename vector_binary_traits<E1, E2, component_prod<
    	typename E1::value_type, typename E2::value_type> >::expression_type expression_type ;
    return expression_type( e1(), e2() ) ;
}

template<class E>
struct matrix_inverse {
	typedef E		argument_type ;
	typedef argument_type	result_type ;
	typedef typename E::value_type	value_type ;

	static
	result_type apply(argument_type t) {
		argument_type tmp(t) ;
		value_type d = t(0,0)*t(1,1) - t(0,1)*t(1,0) ;
		tmp(0,0) = t(1,1) / d ;
		tmp(1,0) = -t(1,0) / d ;
		tmp(0,1) = -t(0,1) / d ;
		tmp(1,1) = t(0,0) / d ;
		return tmp ;
	}
};

template<class E>
typename vector_unary_traits<E, matrix_inverse<typename E::value_type> >::result_type
inverse( const vector_expression<E>& e ) {
	typedef typename vector_unary_traits<E, matrix_inverse<typename E::value_type> >::expression_type
		expression_type ;
	return expression_type( e() ) ;
}

/** ========== VECTOR UNARY SPECIAL ========== **/
template<class E, std::size_t I1, std::size_t I2>
struct nested_index {
	typedef E			argument_type ;
	typedef typename E::value_type		result_type ;

	static
	result_type apply(argument_type t) {
		return t(I1,I2) ;
	}
};

template<class E1, class E2>
struct nested_plus_assign {
	typedef E1		argument_type1 ;
	typedef E2		argument_type2 ;
	typedef argument_type1	result_type ;
	typedef typename E1::size_type	size_type ;

	static
	result_type apply(argument_type1 t1, argument_type2 t2) {
		size_type size( t2.size() ) ;
		for(size_type i=0; i<size; ++i)
			t1 += t2(i) ;
		return t1 ;
	}

};

template<class E>
struct component_determinant{
	typedef E			argument_type ;
	typedef typename E::value_type			result_type ;

	static
	result_type apply(argument_type t) {
		return t(0,0)*t(1,1) - t(0,1)*t(1,0) ;
	}
};

template<class E, class F>
class vector_unary_special :
	public vector_expression<vector_unary_special<E,F> >
{
	public:
		typedef F	functor_type ;
		typedef E	expression_type ;
		typedef typename E::size_type	size_type ;
		typedef typename E::value_type::value_type	value_type ;
		typedef value_type	const_reference ;
		typedef typename E::iterator		iterator ;
		typedef typename E::const_iterator			const_iterator ;

		explicit
		vector_unary_special( const expression_type& e ) : __e(e) {}

		const expression_type& expression() const {
			return __e ;
		}

		const size_type size() const {
			return expression().size() ;
		}

		const_reference operator() (size_type i) const {
			return functor_type::apply( __e(i) ) ;
		}

		const_iterator begin() const {
			return expression().begin() ;
		}

		const_iterator end() const {
			return expression().end() ;
		}

	private:
		expression_type	__e ;
};

template<class E, class F>
struct vector_unary_special_traits {
	typedef vector_unary_special<E,F>	result_type ;
	typedef result_type		expression_type ;
};

template<class E>
typename vector_unary_special_traits<E,	component_determinant<typename E::value_type> >::result_type
layer_determinant( const vector_expression<E>& e ) {
    typedef typename vector_unary_special_traits<E, component_determinant<
    	typename E::value_type> >::expression_type expression_type ;
    return expression_type( e() ) ;
}

template<template<class E1, class E2>class F, class V, class E>
void nested_vector_assign( V& v, const vector_expression<E>& e) {
	typedef F<V,E>		functor_type ;
    typedef typename V::size_type size_type ;
    v = functor_type::apply( v, e () ) ;
}

template<class E, std::size_t I1, std::size_t I2>
typename vector_unary_special_traits<E, nested_index<typename E::value_type, I1, I2> >::result_type
nested_access( const vector_expression<E>& e ) {
	typedef typename vector_unary_special_traits<E, nested_index<typename E::value_type,
		I1, I2> >::expression_type	expression_type ;
	return expression_type( e() ) ;
}

template<class T>
struct scalar_abs:
    public scalar_real_unary_functor<T> {
    typedef typename scalar_real_unary_functor<T>::argument_type
		 argument_type;
    typedef typename scalar_real_unary_functor<T>::result_type
		 result_type;

    static inline result_type apply(argument_type t) {
        return type_traits<result_type>::abs(t);
    }
};

/**
 * Magnitude of a complex vector.
 */
template<class E> BOOST_UBLAS_INLINE
    typename vector_unary_traits<E,
    scalar_abs<typename E::value_type> >::result_type
abs(const vector_expression<E> &e) {
    typedef typename vector_unary_traits<E,
    scalar_abs<typename E::value_type> >::expression_type
        expression_type;
    return expression_type( e() );
}

int main() {

	typedef std::complex<double>	complex_num ;

	bool full_test = false ;

	vector<complex_num> vc(3) ;
	vc(0) = complex_num(5, 10) ;
	vc(1) = complex_num(-3, 2) ;
	vc(2) = complex_num(1, -1) ;

	std::cout << "vc:" << vc << std::endl ;
//	std::cout << "abs(vc):" << abs(vc) << std::endl ;

	if( full_test ) {
		vector<double> v1(4) ;
		vector<double> v2(4) ;
		vector<double> v3(4) ;
		vector<double> v4(4) ;
		for(unsigned i=0; i<v1.size(); ++i) {
			v1(i) = (i+1)*1 ;
			v2(i) = (i+1)*2 ;
			v3(i) = (i+1)*6 ;
			v4(i) = (i+1)*4 ;
		}
		vector<vector<double> > vv1(2) ;
		vv1(0) = v1 ;
		vv1(1) = v2 ;
		vector<vector<double> > vv2(2) ;
		vv2(0) = v3 ;
		vv2(1) = v4 ;
		vector<vector<double> > v_diff = layer_prod(vv2, vv1) ;

		std::cout << "vv1: " << vv1 << std::endl ;
		std::cout << "vv2: " << vv2 << std::endl ;
		std::cout << "v_diff: " << v_diff << std::endl ;
		vector<double> vtest (4,0.0) ;
		nested_vector_assign<nested_plus_assign>(vtest, v_diff) ;
		std::cout << "vtest: " << vtest << std::endl ;

		matrix<double> m1(2,2) ;
		matrix<double> m2(2,2) ;
		matrix<double> m3(2,2) ;
		matrix<double> m4(2,2) ;
		for(unsigned i=0; i<m1.size1(); ++i) {
			for(unsigned j=0; j<m1.size2(); ++j) {
				m1(i,j) = (i+1)*1 + j * 1 ;
				m2(i,j) = (i+1)*6 + j * 2 ;
				m3(i,j) = (i+1)*5 + j * 3 ;
				m4(i,j) = (i+1)*4 + j * 4 ;
			}
		}
		vector<matrix<double> > vm1(2) ;
		vm1(0) = m1 ;
		vm1(1) = m2 ;
		vector<matrix<double> > vm2(2) ;
		vm2(0) = m3 ;
		vm2(1) = m4 ;
		vector<matrix<double> > m_diff = vm1 - vm2 ;

		std::cout << "vm1: " << vm1 << std::endl ;
		std::cout << "vm2: " << vm2 << std::endl ;
		std::cout << "m_diff: " << m_diff << std::endl ;

		vector<matrix<double> > test_inverse = inverse(m_diff) ;
		std::cout << "test_inverse:" << test_inverse << std::endl ;

		vector<double> test_access = nested_access<vector<matrix<double> >,1,1>(m_diff) ;
		std::cout << "test_access:" << test_access << std::endl ;

		vector<double> det = layer_determinant(m_diff) ;
		std::cout << "det: " << det << std::endl ;

		double result = prod( m1, m2 )(0,0) ;
		std::cout << "result: " << result << std::endl ;
	}
}


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_DIMENSION_REDUCTION_H_
#define TURI_DIMENSION_REDUCTION_H_
#include <unity/lib/toolkit_class_macros.hpp>
#include <unity/toolkits/feature_engineering/transformer_base.hpp>
#include <Eigen/Core>
#include <export.hpp>

namespace turi {
namespace sdk_model {
namespace feature_engineering {

typedef Eigen::MatrixXd dense_matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> dense_vector;

/**
 * Create a random projection matrix if the dimenions were already set in the
 * constructor, in which case no data is needed.
 *
 * The Gaussian random projection is Y = (1 / \sqrt(k)) X * R, where:
 *   - X is the original data (n x d)
 *   - R is the projection matrix (d x k)
 *   - Y is the output data (n x k)
 *   - k is the embedding dimension, i.e. `embedding_dim`.
 *
 * See Achlioptas (2003) and Li, Hastie, and Church (2006) for details. Our
 * naming convention is to call d the "ambient dimension" and k the "embedded
 * dimension".
 * -----------------------------------------------------------------------------
 *
 * The private members of a random_projection instance are:
 *
 * unprocessed_features:
 *   Column names before feature validation and preprocessing, particularly
 *   whether the names are to be included or excluded.
 *
 * feature_columns:
 *   Feature column names after validation and pre-processing. These are the
 *   actual columns that we will work with.
 *
 * original_dimension
 *   Dimension of the data input to the `transform` function, as determined by
 *   data passed to the `fit` function.
 *
 * projection_matrix:
 *   Where the rubber meets the road. This is post-multiplied by the data to
 *   produce the output data.
 *
 * fitted:
 *   Indicates if the model has been fitted yet.
 *
 * exclude:
 *   Indicates if the `unprocessed_features` should be included or excluded.
 * -----------------------------------------------------------------------------
 *
 * Several items are added to the model's state (in addition to the options
 * defined in `init_options`) so they will be visible to the Python user. More
 * information about these can be found in the Python documentation. The items
 * are:
 *
 * - original_dimension: dimension of the input data, unpacked.
 * - features: list of column names to project.
 * - excluded_features: list of column names to exclude.
 * - random_seed: seed for generating the projection matrix.
 */
class EXPORT random_projection : public transformer_base {

  static constexpr size_t RANDOM_PROJECTION_VERSION = 0;

  flexible_type unprocessed_features;
  std::vector<std::string> feature_columns;
  std::map<std::string, flex_type_enum> feature_types;

  size_t original_dimension = 0;
  std::shared_ptr<dense_matrix> projection_matrix;

  bool fitted = false;
  bool exclude = false;

 public:

  virtual inline ~random_projection() {}

  /**
   * Define the options manager and set the initial options.
   */
  void init_options(const std::map<std::string, flexible_type>& user_opts) override;

  /**
   * Get the version number for a `random_projection` object.
   */
  size_t get_version() const override;

  /**
   * Save a `random_projection` object using Turi's oarc.
   */
  void save_impl(oarchive& iarc) const override;

  /**
   * Load a `random_projection` object using Turi's iarc.
   */
  void load_version(iarchive & iarc, size_t version) override;

  /**
   * Initialize the transformer. This is the primary entry points for C++ users.
   */
  void init_transformer(const std::map<std::string, flexible_type>& user_opts) override;

  /**
   * Fit the random projection. There is no real logic to write here, right?
   */
  void fit(gl_sframe data) override;

  /**
   * Transform data into a low-dimensional space.
   */
  gl_sframe transform(gl_sframe data) override;

  /**
   * Fit and transform the given data. Intended as an optimization because
   * fit and transform are usually always called together. The default
   * implementaiton calls fit and then transform.
   *
   * \param data
   */
  gl_sframe fit_transform(gl_sframe data) {
    data.materialize();
    fit(data);
    return transform(data);
  }

  BEGIN_CLASS_MEMBER_REGISTRATION("_RandomProjection")
  REGISTER_CLASS_MEMBER_FUNCTION(random_projection::init_transformer, "user_opts")
  REGISTER_CLASS_MEMBER_FUNCTION(random_projection::fit, "data")
  REGISTER_CLASS_MEMBER_FUNCTION(random_projection::transform, "data")
  REGISTER_CLASS_MEMBER_FUNCTION(random_projection::fit_transform, "data")
  REGISTER_CLASS_MEMBER_FUNCTION(random_projection::get_current_options);
  REGISTER_CLASS_MEMBER_FUNCTION(random_projection::list_fields);
  REGISTER_NAMED_CLASS_MEMBER_FUNCTION("_get_default_options",
                                       random_projection::get_default_options);
  REGISTER_NAMED_CLASS_MEMBER_FUNCTION("get",
                                       random_projection::get_value_from_state,
                                       "key");

  END_CLASS_MEMBER_REGISTRATION
};

} //namespace feature_engineering
} //namespace sdk_model
} // namespace turi
#endif


#include "MyModel.h"
#include "RNG.h"
#include "Utils.h"
#include "Data.h"
//#include "MultiSite2.h"
#include <cmath>
#include <fstream>
#include <chrono>
#include <typeinfo>  //for 'typeid' to work

//#include "HODLR_Tree.hpp"
// #include <Eigen/Core>
#include "celerite/celerite.h"

using namespace std;
using namespace Eigen;
using namespace DNest4;

// get the instance for the full dataset
//DataSet& full = DataSet::getRef("full");

#define DONEW false
#define DOCEL true

#define trend true
#define GP true

MyModel::MyModel()
:objects(5, 10, false, MyConditionalPrior())
,mu(Data::get_instance().get_t().size())
,C(Data::get_instance().get_t().size(), Data::get_instance().get_t().size())
{
    //setupHODLR();
    // celerite::solver::BandSolver<double> solver;
}

/*void MyModel::setupHODLR()
{
    const vector<double>& t = full.get_t();

    kernel = new QPkernel(t);
    kernel->set_hyperpars(1., 1., 1., 1.);  // not sure if this is needed

    A = new HODLR_Tree<QPkernel>(kernel, full.N, 150);
}*/

void MyModel::from_prior(RNG& rng)
{
    objects.from_prior(rng);
    objects.consolidate_diff();

    double ymin, ymax, tmin, tmax;
    tmin = Data::get_instance().get_t_min();
    tmax = Data::get_instance().get_t_max();
    ymin = Data::get_instance().get_y_min();
    ymax = Data::get_instance().get_y_max();

    background = ymin + (ymax - ymin)*rng.rand();

    #if trend
        // double max_slope = Data::get_instance().get_max_slope();
        // slope = -1E-4 + 2E-4*rng.rand();
        // quad = -1E-4 + 2E-4*rng.rand();
        double topslope = abs(ymax-ymin) / (tmax - tmin);
        slope = -topslope + 2.*topslope*rng.rand();
        double topquad = abs(ymax-ymin) / ((tmax - tmin)*(tmax - tmin));
        quad = -topquad + 2.*topquad*rng.rand();
    #endif

    // centered at 1 (data in m/s)
    /*extra_sigma = exp(tan(M_PI*(0.97*rng.rand() - 0.485)));*/
    // centered at 0.001 (data in km/s)
    extra_sigma = exp(-6.908 + tan(M_PI*(0.97*rng.rand() - 0.485)));

    //eta5 = exp(tan(M_PI*(0.97*rng.rand() - 0.485)));

    #if GP
        // Log-uniform prior from 10^(-1) to 50 m/s
        //eta1 = exp(log(1E-1) + log(5E2)*rng.rand());
        // Log-uniform prior from 10^(-5) to 0.05 km/s
        eta1 = exp(log(1E-5) + log(1E-1)*rng.rand());

        // Log-uniform prior
        eta2 = exp(log(1E-6) + log(1E6)*rng.rand());

        // or uniform prior between 10 and 40 days
        eta3 = 15. + 35.*rng.rand();

        // Log-uniform prior from 10^(-1) to 10 (fraction of eta3)
        // Log-uniform prior from 10^(-1) to 2 (fraction of eta3)
        // eta4 = 1.;
        exp(log(1E-5) + log(1E5)*rng.rand());
        // eta4 = rng.rand();
    #endif

    calculate_mu();

    #if GP
        calculate_C();
    #endif
}

void MyModel::calculate_C()
{

    // Get the data
    const vector<double>& t = Data::get_instance().get_t();
    const vector<double>& sig = Data::get_instance().get_sig();

    #if DONEW
        //auto begin = std::chrono::high_resolution_clock::now();  // start timing

        kernel->set_hyperpars(eta1, eta2, eta3, eta4);
        cout << eta1 << "   " << eta2 << "   " << eta3 << "   " << eta4 << "   ";
        VectorXd yvar(t.size());
        for (int i = 0; i < t.size(); ++i)
            yvar(i) = eta1*eta1 + sig[i] * sig[i] + extra_sigma * extra_sigma;

        //auto begin = std::chrono::high_resolution_clock::now();  // start timing
        A->assemble_Matrix(yvar, 1e-14, 's');
        //auto end = std::chrono::high_resolution_clock::now();
        //cout << "assembling up took " << std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count() << " ns" << std::endl;
        A->compute_Factor();

        //auto end = std::chrono::high_resolution_clock::now();

        //ofstream timerfile;
        //timerfile.open("timings.txt", std::ios_base::app);
        //timerfile.setf(ios::fixed,ios::floatfield);
        //timerfile << t.size() << '\t' << std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count() << " ns" << std::endl;

    #elif DOCEL
        // celerite!
        // auto begin1 = std::chrono::high_resolution_clock::now();  // start timing

        /*
        This implements the kernel in Eq (61) of Foreman-Mackey et al. (2017)
        The kernel has parameters a, b, c and P
        corresponding to an amplitude, factor, decay timescale and period.
        */

        VectorXd alpha_real(1),
                 beta_real(1),
                 alpha_complex_real(1),
                 alpha_complex_imag(1),
                 beta_complex_real(1),
                 beta_complex_imag(1);

        a = eta1;
        b = eta4;
        P = eta3;
        c = eta2;

        alpha_real << a*(1.+b)/(2.+b);
        beta_real << c;
        alpha_complex_real << a/(2.+b);
        alpha_complex_imag << 0.;
        beta_complex_real << c;
        beta_complex_imag << 2.*M_PI / P;

        VectorXd yvar(t.size()), tt(t.size());
        for (int i = 0; i < t.size(); ++i){
            yvar(i) = sig[i] * sig[i] + extra_sigma * extra_sigma;
            tt(i) = t[i];
        }

        solver.compute(
            alpha_real, beta_real,
            alpha_complex_real, alpha_complex_imag,
            beta_complex_real, beta_complex_imag,
            tt, yvar  // Note: this is the measurement _variance_
        );

        // auto end1 = std::chrono::high_resolution_clock::now();
        // cout << "new GP: " << std::chrono::duration_cast<std::chrono::nanoseconds>(end1-begin1).count() << " ns" << std::endl;

    #else

        int N = Data::get_instance().get_t().size();
        // auto begin = std::chrono::high_resolution_clock::now();  // start timing

        for(size_t i=0; i<N; i++)
        {
            for(size_t j=i; j<N; j++)
            {
                //C(i, j) = eta1*eta1*exp(-0.5*pow((t[i] - t[j])/eta2, 2) );
                C(i, j) = eta1*eta1*exp(-0.5*pow((t[i] - t[j])/eta2, 2)
                           -2.0*pow(sin(M_PI*(t[i] - t[j])/eta3)/eta4, 2) );

                if(i==j)
                    C(i, j) += sig[i]*sig[i] + extra_sigma*extra_sigma; //+ eta5*t[i]*t[i];
                else
                    C(j, i) = C(i, j);
            }
        }

        // auto end = std::chrono::high_resolution_clock::now();
        // cout << "old GP: " << std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count() << " ns" << "\t"; // << std::endl;

        //ofstream timerfile;
        //timerfile.open("timings.txt", std::ios_base::app);
        //timerfile.setf(ios::fixed,ios::floatfield);
        //timerfile << t.size() << '\t' << std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count() << " ns" << std::endl;

    #endif
}

void MyModel::calculate_mu()
{
    // Get the times from the data
    const vector<double>& t = Data::get_instance().get_t();

    // Update or from scratch?
    bool update = (objects.get_added().size() < objects.get_components().size()) &&
            (staleness <= 10);

    // Get the components
    const vector< vector<double> >& components = (update)?(objects.get_added()):
                (objects.get_components());
    // at this point, components has:
    //  if updating: only the added planets' parameters
    //  if from scratch: all the planets' parameters

    // Zero the signal
    if(!update) // not updating, means recalculate everything
    {
        mu.assign(mu.size(), background);
        staleness = 0;
        #if trend
            for(size_t i=0; i<t.size(); i++)
                mu[i] += slope*(t[i] - t[0]) + quad*(t[i] - t[0])*(t[i] - t[0]);

            // cout << slope << "\t" << quad << endl;
        #endif
    }
    else // just updating (adding) planets
        staleness++;

    //auto begin = std::chrono::high_resolution_clock::now();  // start timing

    double P, K, phi, ecc, viewing_angle, f, v, ti;
    for(size_t j=0; j<components.size(); j++)
    {
        P = exp(components[j][0]);
        K = components[j][1];
        phi = components[j][2];
        ecc = components[j][3];
        viewing_angle = components[j][4];

        for(size_t i=0; i<t.size(); i++)
        {
            ti = t[i];
            f = true_anomaly(ti, P, ecc, t[0]-(P*phi)/(2.*M_PI));
            v = K*(cos(f+viewing_angle) + ecc*cos(viewing_angle));
            mu[i] += v;
        }
    }

    // cout << background << endl;

    //auto end = std::chrono::high_resolution_clock::now();
    //ofstream timerfile;
    //timerfile.open("timings.txt", std::ios_base::app);
    //timerfile.setf(ios::fixed,ios::floatfield);
    //timerfile << components.size() << '\t' << ecc << '\t' << viewing_angle << '\t' << std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count() << " ns" << std::endl;

    //cout<<Ea<<endl;
}

double MyModel::perturb(RNG& rng)
{
    double logH = 0.;

    if(rng.rand() <= 0.5)
    {
        logH += objects.perturb(rng);
        objects.consolidate_diff();
        calculate_mu();
    }

    #if GP
    else if(rng.rand() <= 0.5)
    {
        if(rng.rand() <= 0.25)
        {
            eta1 = log(eta1);
            eta1 += log(1E4)*rng.randh(); // range of prior support
            wrap(eta1, log(1E-5), log(1E-1)); // wrap around inside prior
            eta1 = exp(eta1);
        }
        else if(rng.rand() <= 0.33330)
        {
            eta2 = log(eta2);
            eta2 += log(1E12)*rng.randh(); // range of prior support
            wrap(eta2, log(1E-6), log(1E6)); // wrap around inside prior
            eta2 = exp(eta2);
        }
        else if(rng.rand() <= 0.5)
        {
            eta3 += 35.*rng.randh(); // range of prior support
            wrap(eta3, 15., 50.); // wrap around inside prior
        }
        else
        {
            // eta4 = 1.0;

            eta4 = log(eta4);
            eta4 += log(1E10)*rng.randh(); // range of prior support
            wrap(eta4, log(1E-5), log(1E5)); // wrap around inside prior
            eta4 = exp(eta4);

            // eta4 += rng.randh();
            // wrap(eta4, 0., 1.);
        }

        calculate_C();

    }
    #endif // GP

    else if(rng.rand() <= 0.5)
    {
        // data in km/s
        extra_sigma = log(extra_sigma);
        extra_sigma = (atan(extra_sigma + 6.908)/M_PI + 0.485)/0.97;
        extra_sigma += rng.randh();
        wrap(extra_sigma, 0., 1.);
        extra_sigma = -6.908 + tan(M_PI*(0.97*extra_sigma - 0.485));
        extra_sigma = exp(extra_sigma);

        #if GP
            calculate_C();
        #endif
    }
    else
    {

    #if trend

        // Get the times from the data
        const vector<double>& t = Data::get_instance().get_t();

        for(size_t i=0; i<mu.size(); i++)
            mu[i] = mu[i] - background - slope*(t[i]-t[0]) - quad*(t[i]-t[0])*(t[i]-t[0]);

        double ymin, ymax, tmin, tmax;
        tmin = Data::get_instance().get_t_min();
        tmax = Data::get_instance().get_t_max();
        ymin = Data::get_instance().get_y_min();
        ymax = Data::get_instance().get_y_max();
        double topslope = abs(ymax-ymin) / (tmax - tmin);
        double topquad = abs(ymax-ymin) / ((tmax - tmin)*(tmax - tmin));

        // propose new offset
        background += (ymax - ymin)*rng.randh();
        wrap(background, ymin, ymax);

        // propose new slope and quad
        slope += 2.*topslope*rng.randh();
        wrap(slope, -topslope, topslope);
        quad += 2.*topquad*rng.randh();
        wrap(quad, -topquad, topquad);
        // slope += 2E-6*rng.randh();
        // wrap(slope, -3E-6, -1E-6);
        // quad += 2E-9*rng.randh();
        // wrap(quad, 8E-9, 1E-8);

        // add it back again
        for(size_t i=0; i<mu.size(); i++)
            mu[i] = mu[i] + background + slope*(t[i]-t[0]) + quad*(t[i]-t[0])*(t[i]-t[0]);

    #else
        for(size_t i=0; i<mu.size(); i++)
            mu[i] -= background;

        double ymin, ymax;
        ymin = Data::get_instance().get_y_min();
        ymax = Data::get_instance().get_y_max();

        background += (ymax - ymin)*rng.randh();
        wrap(background, ymin, ymax);

        for(size_t i=0; i<mu.size(); i++)
            mu[i] += background;
    #endif

    }

    return logH;
}

double MyModel::log_likelihood() const
{
    int N = Data::get_instance().get_y().size();

    /** The following code calculates the log likelihood in the case of a GP model */

    // Get the data
    const vector<double>& y = Data::get_instance().get_y();

    //auto begin = std::chrono::high_resolution_clock::now();  // start timing
    #if GP

    #if DONEW
        // Set up the kernel.
        //auto begin = std::chrono::high_resolution_clock::now();  // start timing
        //QPkernel kernel;
        //kernel->set_hyperpars(eta1, eta2, eta3, eta4);
        //auto end = std::chrono::high_resolution_clock::now();
        //cout << "set kernel took " << std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count() << " ns" << std::endl;

        // Setting things up
        //auto begin = std::chrono::high_resolution_clock::now();  // start timing

        //auto end = std::chrono::high_resolution_clock::now();
        //cout << "setting up took " << std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count() << " ns" << std::endl;

        MatrixXd b(y.size(), 1), x;
        //VectorXd yvar(t.size());
        for (int i = 0; i < y.size(); ++i) {
            //yvar(i) = eta1*eta1 + sig[i] * sig[i] + extra_sigma * extra_sigma;
            b(i, 0) = y[i] - mu[i];
        }

        //auto begin = std::chrono::high_resolution_clock::now();  // start timing
        A->solve(b, x);
        double determinant;
        A->compute_Determinant(determinant);
        //auto end = std::chrono::high_resolution_clock::now();
        //cout << "solve and determinant took " << std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count() << " ns" << std::endl;

        //cout << logDeterminant << "   " << determinant << endl;
        //assert (logDeterminant == determinant);
        double exponent2 = 0.;
        for(int i = 0; i < y.size(); ++i)
            exponent2 += b(i,0)*x(i);

        double logL = -0.5*y.size()*log(2*M_PI)
                        - 0.5*determinant - 0.5*exponent2;

        //cout << logL << endl;
        //cout << logL << "   " << logL2 << endl;
        //assert (logL == logL2);

    #else
        // residual vector (observed y minus model y)
        VectorXd residual(y.size());
        for(size_t i=0; i<y.size(); i++)
            residual(i) = y[i] - mu[i];

        #if DOCEL
            // logDeterminant = solver.log_determinant();
            // VectorXd solution = solver.solve(residual);

            double logL = -0.5 * (solver.dot_solve(residual) +
                                  solver.log_determinant() +
                                  y.size()*log(2*M_PI));
        #else
            // perform the cholesky decomposition of C
            Eigen::LLT<Eigen::MatrixXd> cholesky = C.llt();
            // get the lower triangular matrix L
            MatrixXd L = cholesky.matrixL();

            double logDeterminant = 0.;
            for(size_t i=0; i<y.size(); i++)
                logDeterminant += 2.*log(L(i,i));

            VectorXd solution = cholesky.solve(residual);

            // y*solution
            double exponent = 0.;
            for(size_t i=0; i<y.size(); i++)
                exponent += residual(i)*solution(i);

            double logL = -0.5*y.size()*log(2*M_PI)
                            - 0.5*logDeterminant - 0.5*exponent;
        #endif

        // cout << "old GP log_det: " << logDeterminant << "\t"; // << std::endl;
        // cout << "new GP log_det: " << solver.log_determinant() << std::endl;

        // calculate C^-1*(y-mu)
        // auto begin = std::chrono::high_resolution_clock::now();  // start timing
        // auto end = std::chrono::high_resolution_clock::now();
        // cout << "solve took " << std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count() << " ns \t";// <<  std::endl;

        // auto begin1 = std::chrono::high_resolution_clock::now();  // start timing
        // auto end1 = std::chrono::high_resolution_clock::now();
        // cout << "solve took " << std::chrono::duration_cast<std::chrono::nanoseconds>(end1-begin1).count() << " ns" << std::endl;
    #endif

    //auto end = std::chrono::high_resolution_clock::now();
    ////cout << "Likelihood took " << std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count() << " ns" << std::endl;

    //ofstream timerfile;
    //timerfile.open("timings_logL.txt", std::ios_base::app);
    //timerfile.setf(ios::fixed,ios::floatfield);
    //timerfile << y.size() << '\t' << std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count() << " ns" << std::endl;

    // cout << "finished log_likelihood!" << endl;
    // return logL;

    #else

    /** The following code calculates the log likelihood in the case of a t-Student model without correlated noise*/
    //  for(size_t i=0; i<y.size(); i++)
    //  {
    //      var = sig[i]*sig[i] + extra_sigma*extra_sigma;
    //      logL += gsl_sf_lngamma(0.5*(nu + 1.)) - gsl_sf_lngamma(0.5*nu)
    //          - 0.5*log(M_PI*nu) - 0.5*log(var)
    //          - 0.5*(nu + 1.)*log(1. + pow(y[i] - mu[i], 2)/var/nu);
    //  }

    /** The following code calculates the log likelihood in the case of a Gaussian likelihood*/
    const vector<double>& sig = Data::get_instance().get_sig();

    double halflog2pi = 0.5*log(2.*M_PI);
    double logL = 0.;
    double var;
    for(size_t i=0; i<y.size(); i++)
    {
        var = sig[i]*sig[i] + extra_sigma*extra_sigma;
        logL += - halflog2pi - 0.5*log(var)
                - 0.5*(pow(y[i] - mu[i], 2)/var);
    }

    #endif // GP

    if(std::isnan(logL) || std::isinf(logL))
        logL = -1E300;
    return logL;
}

void MyModel::print(std::ostream& out) const
{
    // output presision
    out.setf(ios::fixed,ios::floatfield);
    out.precision(8);

    //out<<extra_sigma<<'\t'<<eta1<<'\t'<<eta2<<'\t'<<eta3<<'\t'<<eta4<<'\t'<<eta5<<'\t';
    //out<<extra_sigma<<'\t'<<eta1<<'\t'<<eta2<<'\t';
    // out<<extra_sigma<<'\t'<<eta1<<'\t'<<eta2<<'\t'<<eta3<<'\t'<<eta4<<'\t';

    out<<extra_sigma<<'\t';

    #if GP
        out<<eta1<<'\t'<<eta2<<'\t'<<eta3<<'\t'<<eta4<<'\t';
    #endif
    #if trend
        out<<slope<<'\t'<<quad*1E8<<'\t';
    #endif

    objects.print(out); out<<' '<<staleness<<' ';
    out<<background<<' ';
}

string MyModel::description() const
{
    #if #GP
        #if trend
            return string("extra_sigma   eta1   eta2   eta3   eta4  slope   quad   objects.print   staleness   background");
        #else
            return string("extra_sigma   eta1   eta2   eta3   eta4  objects.print   staleness   background");
        #endif
    #else
        return string("extra_sigma   objects.print   staleness   background");
    #endif
    //return string("extra_sigma  eta1    eta2    objects.print   staleness   background");
    //return string("extra_sigma  eta1    eta2    eta3    eta4    eta5    offsets    objects.print   staleness   background");
}

/**
    Calculates the eccentric anomaly at time t by solving Kepler's equation.
    See "A Practical Method for Solving the Kepler Equation", Marc A. Murison, 2006

    @param t the time at which to calculate the eccentric anomaly.
    @param period the orbital period of the planet
    @param ecc the eccentricity of the orbit
    @param t_peri time of periastron passage
    @return eccentric anomaly.
*/
double MyModel::ecc_anomaly(double t, double period, double ecc, double time_peri)
{
    double tol;
    if (ecc < 0.8) tol = 1e-14;
    else tol = 1e-13;

    double n = 2.*M_PI/period;  // mean motion
    double M = n*(t - time_peri);  // mean anomaly
    double Mnorm = fmod(M, 2.*M_PI);
    double E0 = keplerstart3(ecc, Mnorm);
    double dE = tol + 1;
    double E;
    int count = 0;
    while (dE > tol)
    {
        E = E0 - eps3(ecc, Mnorm, E0);
        dE = abs(E-E0);
        E0 = E;
        count++;
        // failed to converge, this only happens for nearly parabolic orbits
        if (count == 100) break;
    }
    return E;
}

/**
    Provides a starting value to solve Kepler's equation.
    See "A Practical Method for Solving the Kepler Equation", Marc A. Murison, 2006

    @param e the eccentricity of the orbit
    @param M mean anomaly (in radians)
    @return starting value for the eccentric anomaly.
*/
double MyModel::keplerstart3(double e, double M)
{
    double t34 = e*e;
    double t35 = e*t34;
    double t33 = cos(M);
    return M + (-0.5*t35 + e + (t34 + 1.5*t33*t35)*t33)*sin(M);
}

/**
    An iteration (correction) method to solve Kepler's equation.
    See "A Practical Method for Solving the Kepler Equation", Marc A. Murison, 2006

    @param e the eccentricity of the orbit
    @param M mean anomaly (in radians)
    @param x starting value for the eccentric anomaly
    @return corrected value for the eccentric anomaly
*/
double MyModel::eps3(double e, double M, double x)
{
    double t1 = cos(x);
    double t2 = -1 + e*t1;
    double t3 = sin(x);
    double t4 = e*t3;
    double t5 = -x + t4 + M;
    double t6 = t5/(0.5*t5*t4/t2+t2);

    return t5/((0.5*t3 - 1/6*t1*t6)*e*t6+t2);
}

/**
    Calculates the true anomaly at time t.
    See Eq. 2.6 of The Exoplanet Handbook, Perryman 2010

    @param t the time at which to calculate the true anomaly.
    @param period the orbital period of the planet
    @param ecc the eccentricity of the orbit
    @param t_peri time of periastron passage
    @return true anomaly.
*/
double MyModel::true_anomaly(double t, double period, double ecc, double t_peri)
{
    double E = ecc_anomaly(t, period, ecc, t_peri);
    double f = acos( (cos(E)-ecc)/( 1-ecc*cos(E) ) );
    //acos gives the principal values ie [0:PI]
    //when E goes above PI we need another condition
    if(E>M_PI)
      f=2*M_PI-f;

    return f;
}


// Copyright Aleksey Gurtovoy 2000-2004
//
// 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)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id$
// $Date$
// $Revision$

#include <boost/mpl/fold.hpp>
#include <boost/mpl/reverse_fold.hpp>
//#include <boost/mpl/vector.hpp>
#include <boost/mpl/list.hpp>
#include <boost/mpl/list_c.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/push_front.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/size.hpp>
#include <boost/type_traits/is_float.hpp>

#include <boost/mpl/aux_/test.hpp>

MPL_TEST_CASE()
{
    typedef list<long,float,short,double,float,long,long double> types;
    typedef fold<
          types
        , int_<0>
        , if_< boost::is_float<_2>,next<_1>,_1 >
        >::type number_of_floats;

    MPL_ASSERT_RELATION( number_of_floats::value, ==, 4 );
}

MPL_TEST_CASE()
{
    typedef list_c<int,5,-1,0,-7,-2,0,-5,4> numbers;
    typedef list_c<int,-1,-7,-2,-5> negatives;
    typedef reverse_fold<
          numbers
        , list_c<int>
        , if_< less< _2,int_<0> >, push_front<_1,_2>, _1 >
        >::type result;

    MPL_ASSERT(( equal< result,negatives,equal_to<_1,_2> > ));
}


#include "Common/Common.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Demos/Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "PositionBasedElasticRodsModel.h"
#include "PositionBasedElasticRodsConstraints.h"
#include "PositionBasedElasticRodsTSC.h"
#include <iostream>
#include "Demos/Utils/Timing.h"

#define _USE_MATH_DEFINES
#include "math.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;

void timeStep ();
void buildModel ();
void createRod();
void render ();
void reset();
void cleanup();
void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end);

void TW_CALL setRestDarbouxX(const void *value, void *clientData);
void TW_CALL setRestDarbouxY(const void *value, void *clientData);
void TW_CALL setRestDarbouxZ(const void *value, void *clientData);

void TW_CALL getRestDarbouxX(void *value, void *clientData);
void TW_CALL getRestDarbouxY(void *value, void *clientData);
void TW_CALL getRestDarbouxZ(void *value, void *clientData);

void TW_CALL getBendingAndTwistingStiffnessX(void *value, void *clientData);
void TW_CALL getBendingAndTwistingStiffnessY(void *value, void *clientData);
void TW_CALL getBendingAndTwistingStiffnessZ(void *value, void *clientData);

void TW_CALL setBendingAndTwistingStiffnessX(const void *value, void *clientData);
void TW_CALL setBendingAndTwistingStiffnessY(const void *value, void *clientData);
void TW_CALL setBendingAndTwistingStiffnessZ(const void *value, void *clientData);

void TW_CALL setTimeStep(const void *value, void *clientData);
void TW_CALL getTimeStep(void *value, void *clientData);
void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData);
void TW_CALL getVelocityUpdateMethod(void *value, void *clientData);

PositionBasedElasticRodsModel model;
PositionBasedElasticRodsTSC sim;

const int numberOfPoints = 32;
bool doPause = false;
std::vector<unsigned int> selectedParticles;
Vector3r oldMousePos;

// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS

	// OpenGL
	MiniGL::init (argc, argv, 1024, 768, 0, 0, "Elastic rod demo");
	MiniGL::initLights ();
	MiniGL::setClientIdleFunc (50, timeStep);
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setSelectionFunc(selection);

	buildModel ();

	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3r (5.0, -10.0, 30.0), Vector3r (5.0, 0.0, 0.0));

	TwAddVarRW(MiniGL::getTweakBar(), "Pause", TW_TYPE_BOOLCPP, &doPause, " label='Pause' group=Simulation key=SPACE ");
	TwAddVarCB(MiniGL::getTweakBar(), "TimeStepSize", TW_TYPE_REAL, setTimeStep, getTimeStep, &model, " label='Time step size'  min=0.0 max = 0.1 step=0.001 precision=4 group=Simulation ");
	TwType enumType = TwDefineEnum("VelocityUpdateMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "VelocityUpdateMethod", enumType, setVelocityUpdateMethod, getVelocityUpdateMethod, &sim, " label='Velocity update method' enum='0 {First Order Update}, 1 {Second Order Update}' group=Simulation ");

	TwAddVarCB(MiniGL::getTweakBar(), "RestDarbouxX", TW_TYPE_REAL, setRestDarbouxX, getRestDarbouxX, &model, " label='Rest Darboux X'  min=-1.0 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	TwAddVarCB(MiniGL::getTweakBar(), "RestDarbouxY", TW_TYPE_REAL, setRestDarbouxY, getRestDarbouxY, &model, " label='Rest Darboux Y'  min=-1.0 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	TwAddVarCB(MiniGL::getTweakBar(), "RestDarbouxZ", TW_TYPE_REAL, setRestDarbouxZ, getRestDarbouxZ, &model, " label='Rest Darboux Z'  min=-1.0 max = 1.0 step=0.01 precision=2 group=BendTwist ");

	TwAddVarCB(MiniGL::getTweakBar(), "BendingAndTwistingStiffnessX", TW_TYPE_REAL, setBendingAndTwistingStiffnessX, getBendingAndTwistingStiffnessX, &model, " label='Bending X stiffness'  min=0.01 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	TwAddVarCB(MiniGL::getTweakBar(), "BendingAndTwistingStiffnessY", TW_TYPE_REAL, setBendingAndTwistingStiffnessY, getBendingAndTwistingStiffnessY, &model, " label='Bending Y stiffness'  min=0.01 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	TwAddVarCB(MiniGL::getTweakBar(), "BendingAndTwistingStiffnessZ", TW_TYPE_REAL, setBendingAndTwistingStiffnessZ, getBendingAndTwistingStiffnessZ, &model, " label='Twisting stiffness'  min=0.01 max = 1.0 step=0.01 precision=2 group=BendTwist ");

	glutMainLoop ();

	cleanup ();

	Timing::printAverageTimes();

	return 0;
}

void cleanup()
{
	delete TimeManager::getCurrent();
}

void reset()
{
	Timing::printAverageTimes();
	Timing::reset();

	model.reset();
	sim.reset();
	TimeManager::getCurrent()->setTime(0.0);
}

void mouseMove(int x, int y)
{
	Vector3r mousePos;
	MiniGL::unproject(x, y, mousePos);
	const Vector3r diff = mousePos - oldMousePos;

	TimeManager *tm = TimeManager::getCurrent();
	const Real h = tm->getTimeStepSize();

	ParticleData &pd = model.getParticles();
	for (unsigned int j = 0; j < selectedParticles.size(); j++)
	{
		pd.getVelocity(selectedParticles[j]) += 5.0*diff / h;
	}
	oldMousePos = mousePos;
}

void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end)
{
	std::vector<unsigned int> hits;
	selectedParticles.clear();
	ParticleData &pd = model.getParticles();
	Selection::selectRect(start, end, &pd.getPosition(0), &pd.getPosition(pd.size() - 1), selectedParticles);
	if (selectedParticles.size() > 0)
		MiniGL::setMouseMoveFunc(GLUT_MIDDLE_BUTTON, mouseMove);
	else
		MiniGL::setMouseMoveFunc(-1, NULL);

	MiniGL::unproject(end[0], end[1], oldMousePos);
}

void buildModel()
{
	TimeManager::getCurrent()->setTimeStepSize(0.002f);
	model.setBendingAndTwistingStiffness(Vector3r(0.5, 0.5, 0.5));

	sim.setDamping(0.001f);

	createRod();
}

void timeStep ()
{
	if (doPause)
		return;

	// Simulation code
	for (unsigned int i = 0; i < 8; i++)
		sim.step(model);
}

void render()
{
	MiniGL::coordinateSystem();

	// Draw sim model

	ParticleData &particles = model.getParticles();
	ParticleData &ghostParticles = model.getGhostParticles();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();

	float selectionColor[4] = { 0.8f, 0.0f, 0.0f, 1 };
	float pointColor[4] = { 0.1f, 0.2f, 0.6f, 1 };
	float ghostPointColor[4] = { 0.1f, 0.1f, 0.1f, 0.5f };
	float edgeColor[4] = { 0.0f, 0.6f, 0.2f, 1 };

	float red[4] = { 0.8f, 0.0f, 0.0f, 1 };
	const ParticleData &pd = model.getParticles();
	for (unsigned int j = 0; j < selectedParticles.size(); j++)
	{
		MiniGL::drawSphere(pd.getPosition(selectedParticles[j]), 0.21f, red);
	}

	for (unsigned int i = 0; i < numberOfPoints; i++)
	{
		MiniGL::drawSphere(particles.getPosition(i), 0.2f, pointColor);
	}

	for (unsigned int i = 0; i < numberOfPoints-1; i++)
	{
		MiniGL::drawSphere(ghostParticles.getPosition(i), 0.1f, ghostPointColor);
		MiniGL::drawVector(particles.getPosition(i), particles.getPosition(i + 1), 0.2f, edgeColor);
	}

	MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
}

/** Create the elastic rod model
*/
void createRod()
{
	ParticleData &particles = model.getParticles();
	ParticleData &ghostParticles = model.getGhostParticles();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();

	//centreline points
	for (unsigned int i = 0; i < numberOfPoints; i++)
	{
		particles.addVertex(Vector3r((Real)i*1.0, 0.0, 0.0));
	}

	//edge ghost points
	for (unsigned int i = 0; i < numberOfPoints-1; i++)
	{
		ghostParticles.addVertex(Vector3r((Real)i*1.0 + 0.5, 1.0, 0.0));
	}

	//lock two first particles and first ghost point
	particles.setMass(0, 0.0f);
	particles.setMass(1, 0.0f);
	ghostParticles.setMass(0, 0.0f);

	for (unsigned int i = 0; i < numberOfPoints - 1; i++)
	{
		model.addDistanceConstraint(i, i + 1);
		model.addPerpendiculaBisectorConstraint(i, i + 1, i);
		model.addGhostPointEdgeDistanceConstraint(i, i + 1, i);

		if (i < numberOfPoints - 2)
		{
			//  Single rod element:
			//      D   E		//ghost points
			//		|	|
			//  --A---B---C--	// rod points
			int pA = i;
			int pB = i + 1;
			int pC = i + 2;
			int pD = i;
			int pE = i + 1;
			model.addDarbouxVectorConstraint(pA, pB, pC, pD, pE);
		}
	}
}

void TW_CALL setRestDarbouxX(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[0] = *(const Real *)(value);
}

void TW_CALL setRestDarbouxY(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[1] = *(const Real *)(value);
}

void TW_CALL setRestDarbouxZ(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[2] = *(const Real *)(value);
}

void TW_CALL getRestDarbouxX(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[0];
}

void TW_CALL getRestDarbouxY(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[1];
}

void TW_CALL getRestDarbouxZ(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[2];
}

void TW_CALL setBendingAndTwistingStiffnessX(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[0] = *(const Real *)(value);
}

void TW_CALL setBendingAndTwistingStiffnessY(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[1] = *(const Real *)(value);
}

void TW_CALL setBendingAndTwistingStiffnessZ(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[2] = *(const Real *)(value);
}

void TW_CALL getBendingAndTwistingStiffnessX(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[0];
}

void TW_CALL getBendingAndTwistingStiffnessY(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[1];
}

void TW_CALL getBendingAndTwistingStiffnessZ(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[2];
}

void TW_CALL setTimeStep(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	TimeManager::getCurrent()->setTimeStepSize(val);
}

void TW_CALL getTimeStep(void *value, void *clientData)
{
	*(Real *)(value) = TimeManager::getCurrent()->getTimeStepSize();
}

void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	((TimeStepController*)clientData)->setVelocityUpdateMethod((unsigned int)val);
}

void TW_CALL getVelocityUpdateMethod(void *value, void *clientData)
{
	*(short *)(value) = (short)((TimeStepController*)clientData)->getVelocityUpdateMethod();
}


#include <cradle/geometry/common.hpp>
#include <cradle/common.hpp>
#include <vector>
#include <boost/assign/std/vector.hpp>
#include <cradle/math/common.hpp>
#include <cradle/geometry/transformations.hpp>

#define BOOST_TEST_MODULE geometry
#include <cradle/test.hpp>

using namespace cradle;

BOOST_AUTO_TEST_CASE(sizeof_vector)
{
    // Check that vectors don't have any size overhead.
    BOOST_CHECK_EQUAL(sizeof(vector<3,int>), 3 * sizeof(int));
    BOOST_CHECK_EQUAL(sizeof(vector<1,float>), sizeof(float));
    BOOST_CHECK_EQUAL(sizeof(vector<2,double>), 2 * sizeof(double));
}

void f(vector3i const& v) {}

// confirm that the vector operators work as expected
BOOST_AUTO_TEST_CASE(vector_operators)
{
    vector<3,int> p = make_vector<int>(1, 1, 0),
        q = make_vector<int>(4, 2, 6);
    vector<3,int> v = make_vector<int>(3, 1, 2);

    BOOST_CHECK_EQUAL(p - q, make_vector<int>(-3, -1, -6));
    BOOST_CHECK_EQUAL(p + v, make_vector<int>(4, 2, 2));

    BOOST_CHECK_EQUAL(v * 3, make_vector<int>(9, 3, 6));
    BOOST_CHECK_EQUAL(v / 2, make_vector<int>(1, 0, 1));

    BOOST_CHECK(!(p == q));
    BOOST_CHECK(p != q);

    BOOST_CHECK(!(p - q == v));
    BOOST_CHECK(p - q != v);
}

BOOST_AUTO_TEST_CASE(vector_slice)
{
    {
    vector3i p = make_vector<int>(6, 7, 8);
    BOOST_CHECK_EQUAL(slice(p, 0), make_vector<int>(7, 8));
    BOOST_CHECK_EQUAL(slice(p, 1), make_vector<int>(6, 8));
    BOOST_CHECK_EQUAL(slice(p, 2), make_vector<int>(6, 7));
    }

    {
    vector2f p = make_vector<float>(9, 17);
    BOOST_CHECK_EQUAL(slice(p, 0), make_vector<float>(17));
    BOOST_CHECK_EQUAL(slice(p, 1), make_vector<float>(9));
    }

    {
    vector4i p = make_vector<int>(4, 3, 2, 1);
    BOOST_CHECK_EQUAL(slice(p, 0), make_vector<int>(3, 2, 1));
    BOOST_CHECK_EQUAL(slice(p, 1), make_vector<int>(4, 2, 1));
    BOOST_CHECK_EQUAL(slice(p, 2), make_vector<int>(4, 3, 1));
    BOOST_CHECK_EQUAL(slice(p, 3), make_vector<int>(4, 3, 2));
    }
}

BOOST_AUTO_TEST_CASE(test_unslice_vector)
{
    {
    vector3i p = make_vector<int>(6, 7, 8);
    BOOST_CHECK_EQUAL(unslice(p, 0, 0), make_vector<int>(0, 6, 7, 8));
    BOOST_CHECK_EQUAL(unslice(p, 1, 0), make_vector<int>(6, 0, 7, 8));
    BOOST_CHECK_EQUAL(unslice(p, 2, 0), make_vector<int>(6, 7, 0, 8));
    BOOST_CHECK_EQUAL(unslice(p, 3, 0), make_vector<int>(6, 7, 8, 0));
    }

    {
    vector2d p = make_vector<double>(9, 17);
    BOOST_CHECK_EQUAL(unslice(p, 0, 2.1), make_vector<double>(2.1, 9, 17));
    BOOST_CHECK_EQUAL(unslice(p, 1, 2.1), make_vector<double>(9, 2.1, 17));
    BOOST_CHECK_EQUAL(unslice(p, 2, 2.1), make_vector<double>(9, 17, 2.1));
    }
}

BOOST_AUTO_TEST_CASE(test_uniform_vector)
{
    BOOST_CHECK_EQUAL((uniform_vector<3,int>(0)), make_vector<int>(0, 0, 0));
    BOOST_CHECK_EQUAL((uniform_vector<4,unsigned>(1)),
        make_vector<unsigned>(1, 1, 1, 1));
    BOOST_CHECK_EQUAL((uniform_vector<2,float>(6)), make_vector<float>(6, 6));
}

BOOST_AUTO_TEST_CASE(vector_almost_equal)
{
    BOOST_CHECK(almost_equal(
        make_vector<double>(0, 0, 0), make_vector<double>(0, 0,
        default_equality_tolerance<double>() / 2)));
    BOOST_CHECK(!almost_equal(
        make_vector<float>(0, 0, 0), make_vector<float>(0, 0, 1)));
    BOOST_CHECK(almost_equal(
        make_vector<float>(0, 0, 0), make_vector<float>(0, 0, 1), 2));
}

BOOST_AUTO_TEST_CASE(vector_cross)
{
    // with objects
    BOOST_CHECK(
        almost_equal(
            cross(make_vector<double>(1, 0, 0), make_vector<double>(0, 1, 0)),
            make_vector<double>(0, 0, 1)));
    BOOST_CHECK(
        almost_equal(
            cross(make_vector<double>(0, 1, 0), make_vector<double>(1, 0, 0)),
            make_vector<double>(0, 0, -1)));

    // with expressions
    BOOST_CHECK(
        almost_equal(
            cross(
                make_vector<double>(1, 0, 0) - make_vector<double>(0, 0, 0),
                make_vector<double>(0, 1, 0) - make_vector<double>(0, 0, 0)),
            make_vector<double>(0, 0, 1)));

    // mixing expressions and objects
    BOOST_CHECK(
        almost_equal(
            cross(
                make_vector<double>(1, 0, 0) - make_vector<double>(0, 0, 0),
                make_vector<double>(0, 1, 0)),
            make_vector<double>(0, 0, 1)));
    BOOST_CHECK(
        almost_equal(
            cross(
                make_vector<double>(1, 0, 0),
                make_vector<double>(0, 1, 0) - make_vector<double>(0, 0, 0)),
            make_vector<double>(0, 0, 1)));
}

BOOST_AUTO_TEST_CASE(vector_dot)
{
    // with objects
    BOOST_CHECK(almost_equal(dot(make_vector<double>(1, 1),
        make_vector<double>(0.7, 0.3)), 1.));
    BOOST_CHECK(almost_equal(dot(make_vector<double>(1, 0, 0),
        make_vector<double>(0, 1, 0)), 0.));
    BOOST_CHECK(almost_equal(dot(make_vector<double>(1),
        make_vector<double>(0.6)), 0.6));
    BOOST_CHECK_EQUAL(dot(make_vector<int>(1, 2, 0),
        make_vector<int>(2, 3, 0)), 8);

    // with expressions
    BOOST_CHECK(almost_equal(dot(make_vector<double>(1, 0, 1) -
        make_vector<double>(0, 0, 0), make_vector<double>(0.7, 0, 0.3) -
        make_vector<double>(0, 0, 0)), 1.));

    // mixing expressions and objects
    BOOST_CHECK(almost_equal(dot(make_vector<float>(1, 1),
        make_vector<float>(0.7f,  0.3f) - make_vector<float>(0, 0)), 1.f));
    BOOST_CHECK(almost_equal(dot(make_vector<double>(1, 0, 1) -
        make_vector<double>(0, 0, 0), make_vector<double>(0.7, 0, 0.3)), 1.));
}

BOOST_AUTO_TEST_CASE(vector_length)
{
    // length2()
    BOOST_CHECK_EQUAL(length2(make_vector<int>(2, 0, 1)), 5);

    // length()
    BOOST_CHECK(almost_equal(
        length(make_vector<double>(2, 1)), sqrt(5.)));

    // length() with an expression
    BOOST_CHECK(almost_equal(
        length(make_vector<double>(2, 0, 1) - make_vector<double>(1, 0, 0)),
        sqrt(2.)));
}

BOOST_AUTO_TEST_CASE(unit_vector)
{
    BOOST_CHECK(almost_equal(
        unit(make_vector<double>(4, 0, 3)), make_vector<double>(0.8, 0, 0.6)));

    // with an expression
    BOOST_CHECK(almost_equal(
        unit(make_vector<double>(3, 0) - make_vector<double>(0, 4)),
        make_vector<double>(0.6, -0.8)));
}

BOOST_AUTO_TEST_CASE(perpendicular_vector)
{
    for (int x = -1; x < 2; ++x)
    {
        for (int y = -1; y < 2; ++y)
        {
            for (int z = -1; z < 2; ++z)
            {
                if (x != 0 || y != 0 || z != 0)
                {
                    vector<3,double> v = make_vector<double>(x, y, z);
                    CRADLE_CHECK_ALMOST_EQUAL(dot(v, get_perpendicular(v)),
                        0.);
                    CRADLE_CHECK_ALMOST_EQUAL(length(get_perpendicular(v)),
                        1.);
                }
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(product_test)
{
    BOOST_CHECK_EQUAL(product(make_vector<int>(2, 3, 1)), 6);
    BOOST_CHECK_EQUAL(product(make_vector<int>(2, -1, 3, 1)), -6);

    BOOST_CHECK(almost_equal(
        product(make_vector<float>(2.5, 4, 2)), 20.f));
    BOOST_CHECK(almost_equal(
        product(make_vector<double>(2.5, 4)), 10.));

    // with an expression
    BOOST_CHECK_EQUAL(
        product(make_vector<int>(2, -1, 3, 0) - make_vector<int>(6, 0, 0, 1)),
        -12);
}

BOOST_AUTO_TEST_CASE(vector_io)
{
    vector3i p = make_vector<int>(2, 0, 3);
    BOOST_CHECK_EQUAL(to_string(p), "(2, 0, 3)");
}

BOOST_AUTO_TEST_CASE(compute_mean_vector_test)
{
    using namespace boost::assign;

    std::vector<vector3d> vectors;
    vectors +=
        make_vector<double>(2, 0, 3),
        make_vector<double>(6, 1, 7),
        make_vector<double>(0, 0, 0),
        make_vector<double>(1, 2, 0),
        make_vector<double>(3, 2, 1),
        make_vector<double>(6, 4, 1);

    BOOST_CHECK(almost_equal(
        compute_mean(vectors, uniform_vector<3>(0.)),
        make_vector<double>(3, 1.5, 2)));
}

BOOST_AUTO_TEST_CASE(plane_test)
{
    plane<double> default_constructed;

    vector3d p = make_vector<double>(0, 0, 0);
    vector3d normal = make_vector<double>(1, 0, 0);

    plane<double> plane(p, normal);
    BOOST_CHECK_EQUAL(plane.get_point(), p);
    BOOST_CHECK_EQUAL(plane.get_normal(), normal);

    vector3d q = make_vector<double>(0, 0, 1);
    plane.set_point(q);
    BOOST_CHECK_EQUAL(plane.get_point(), q);
    BOOST_CHECK_EQUAL(plane.get_normal(), normal);

    normal = make_vector<double>(0, 1, 0);
    plane.set_normal(normal);
    BOOST_CHECK_EQUAL(plane.get_point(), q);
    BOOST_CHECK_EQUAL(plane.get_normal(), normal);
}

BOOST_AUTO_TEST_CASE(simple_box1i_test)
{
    box1i b(make_vector<int>(-1), make_vector<int>(4));

    BOOST_CHECK_EQUAL(get_center(b)[0], 1);
    BOOST_CHECK_EQUAL(b.corner[0], -1);
    BOOST_CHECK_EQUAL(b.size[0], 4);

    BOOST_CHECK(!is_inside(b, make_vector<int>(-2)));
    BOOST_CHECK(is_inside(b, make_vector<int>(-1)));
    BOOST_CHECK(is_inside(b, make_vector<int>(2)));
    BOOST_CHECK(!is_inside(b, make_vector<int>(3)));
    BOOST_CHECK(!is_inside(b, make_vector<int>(4)));
}

BOOST_AUTO_TEST_CASE(simple_box1d_test)
{
    box1d b(make_vector<double>(-1), make_vector<double>(3));

    CRADLE_CHECK_ALMOST_EQUAL(get_center(b), make_vector<double>(0.5));
    CRADLE_CHECK_ALMOST_EQUAL(b.corner, make_vector<double>(-1));
    CRADLE_CHECK_ALMOST_EQUAL(b.size, make_vector<double>(3));

    BOOST_CHECK(!is_inside(b, make_vector<double>(-2)));
    BOOST_CHECK(is_inside(b, make_vector<double>(-1)));
    BOOST_CHECK(is_inside(b, make_vector<double>(0)));
    BOOST_CHECK(is_inside(b, make_vector<double>(1)));
    BOOST_CHECK(is_inside(b, make_vector<double>(1.5)));
    BOOST_CHECK(is_inside(b, make_vector<double>(1.9)));
    BOOST_CHECK(!is_inside(b, make_vector<double>(2)));
    BOOST_CHECK(!is_inside(b, make_vector<double>(4)));
}

BOOST_AUTO_TEST_CASE(simple_box2d_test)
{
    box2d b(make_vector<double>(-1, -1), make_vector<double>(3, 3));

    CRADLE_CHECK_ALMOST_EQUAL(area(b), 9.);

    CRADLE_CHECK_ALMOST_EQUAL(get_center(b), make_vector<double>(0.5, 0.5));
    CRADLE_CHECK_ALMOST_EQUAL(b.corner, make_vector<double>(-1, -1));
    CRADLE_CHECK_ALMOST_EQUAL(b.size, make_vector<double>(3, 3));

    BOOST_CHECK(!is_inside(b, make_vector<double>(-2, -2)));
    BOOST_CHECK(!is_inside(b, make_vector<double>(-2, 0)));
    BOOST_CHECK(!is_inside(b, make_vector<double>(0, 4)));
    BOOST_CHECK(!is_inside(b, make_vector<double>(0, 2)));
    BOOST_CHECK(is_inside(b, make_vector<double>(-1, -1)));
    BOOST_CHECK(is_inside(b, make_vector<double>(0, 1.9)));
    BOOST_CHECK(is_inside(b, make_vector<double>(0, 0)));
    BOOST_CHECK(is_inside(b, make_vector<double>(1.5, 1.5)));
    BOOST_CHECK(is_inside(b, make_vector<double>(0, 1)));
}

BOOST_AUTO_TEST_CASE(box_slicing_test)
{
    BOOST_CHECK_EQUAL(
        slice(box3d(make_vector<double>(0, 2, 1),
            make_vector<double>(4, 3, 5)), 0),
        box2d(make_vector<double>(2, 1), make_vector<double>(3, 5)));
    BOOST_CHECK_EQUAL(
        slice(box3d(make_vector<double>(0, 2, 1),
            make_vector<double>(4, 3, 5)), 1),
        box2d(make_vector<double>(0, 1), make_vector<double>(4, 5)));
    BOOST_CHECK_EQUAL(
        slice(box3d(make_vector<double>(0, 2, 1),
            make_vector<double>(4, 3, 5)), 2),
        box2d(make_vector<double>(0, 2), make_vector<double>(4, 3)));

    BOOST_CHECK_EQUAL(
        slice(box2i(make_vector<int>(0, 2), make_vector<int>(4, 3)), 0),
            box1i(make_vector<int>(2), make_vector<int>(3)));
    BOOST_CHECK_EQUAL(
        slice(box2i(make_vector<int>(0, 2), make_vector<int>(4, 3)), 1),
            box1i(make_vector<int>(0), make_vector<int>(4)));
}

BOOST_AUTO_TEST_CASE(add_box_border_test)
{
    BOOST_CHECK_EQUAL(
        add_border(box3i(make_vector<int>(0, 2, 1),
            make_vector<int>(4, 3, 5)), 2),
        box3i(make_vector<int>(-2, 0, -1), make_vector<int>(8, 7, 9)));

    BOOST_CHECK_EQUAL(
        add_border(box3i(make_vector<int>(0, 2, 1), make_vector<int>(4, 3, 5)),
            make_vector<int>(2, 1, 0)),
        box3i(make_vector<int>(-2, 1, 1), make_vector<int>(8, 5, 5)));
}

BOOST_AUTO_TEST_CASE(simple_test)
{
    circle<double> c(make_vector<double>(0, 0), 1);
    CRADLE_CHECK_ALMOST_EQUAL(area(c), pi);
    BOOST_CHECK(!is_inside(c, make_vector<double>(0, 2)));
    BOOST_CHECK(!is_inside(c, make_vector<double>(2, 0)));
    BOOST_CHECK(!is_inside(c, make_vector<double>(1.1, 0)));
    BOOST_CHECK(!is_inside(c, make_vector<double>(0.9, 0.9)));
    BOOST_CHECK(!is_inside(c, make_vector<double>(0, -1.1)));
    BOOST_CHECK(is_inside(c, make_vector<double>(0.9, 0)));
    BOOST_CHECK(is_inside(c, make_vector<double>(0, 0)));
    BOOST_CHECK(is_inside(c, make_vector<double>(0, -0.9)));
    BOOST_CHECK(is_inside(c, make_vector<double>(-0.7, 0.7)));
    BOOST_CHECK(is_inside(c, make_vector<double>(0.3, 0.5)));
}

BOOST_AUTO_TEST_CASE(off_center_test)
{
    circle<double> c(make_vector<double>(4, 1), 2);
    CRADLE_CHECK_ALMOST_EQUAL(area(c), 4 * pi);
    BOOST_CHECK(!is_inside(c, make_vector<double>(0, 0)));
    BOOST_CHECK(!is_inside(c, make_vector<double>(1.9, 1)));
    BOOST_CHECK(!is_inside(c, make_vector<double>(6.1, 1)));
    BOOST_CHECK(!is_inside(c, make_vector<double>(4, 3.1)));
    BOOST_CHECK(!is_inside(c, make_vector<double>(4, -1.1)));
    BOOST_CHECK(is_inside(c, make_vector<double>(4, 1)));
    BOOST_CHECK(is_inside(c, make_vector<double>(2.6, 2.4)));
}

BOOST_AUTO_TEST_CASE(by_value_test)
{
    vector2d p0 = make_vector<double>(0, 1), p1 = make_vector<double>(4, 4);
    line_segment<2,double> segment(p0, p1);

    BOOST_CHECK_EQUAL(segment[0], p0);
    BOOST_CHECK_EQUAL(segment[1], p1);
    CRADLE_CHECK_ALMOST_EQUAL(length(segment), 5.);
}

BOOST_AUTO_TEST_CASE(identity_matrix_test)
{
    BOOST_CHECK_EQUAL(
        (identity_matrix<4,double>()),
        make_matrix<double>(
            1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1));

    BOOST_CHECK_EQUAL(
        (identity_matrix<3,double>()),
        make_matrix<double>(
            1, 0, 0,
            0, 1, 0,
            0, 0, 1));
}

BOOST_AUTO_TEST_CASE(matrix_operations_test)
{
    matrix<3,3,double> m, i = identity_matrix<3,double>();

    m = i - 2 * i;
    BOOST_CHECK_EQUAL(
        m,
        make_matrix<double>(
            -1, 0, 0,
            0, -1, 0,
            0, 0, -1));

    m *= 2;
    BOOST_CHECK_EQUAL(
        m,
        make_matrix<double>(
            -2, 0, 0,
            0, -2, 0,
            0, 0, -2));

    m = i;
    m /= 2;
    BOOST_CHECK_EQUAL(
        m,
        make_matrix<double>(
            0.5, 0, 0,
            0, 0.5, 0,
            0, 0, 0.5));

    m += i * 3;
    BOOST_CHECK_EQUAL(
        m,
        make_matrix<double>(
            3.5, 0, 0,
            0, 3.5, 0,
            0, 0, 3.5));

    m -= 2 * i;
    BOOST_CHECK_EQUAL(
        m,
        make_matrix<double>(
            1.5, 0, 0,
            0, 1.5, 0,
            0, 0, 1.5));

    BOOST_CHECK(m == m);
    BOOST_CHECK(m != i);
}

BOOST_AUTO_TEST_CASE(matrix_conversion_test)
{
    matrix<3,3,double> m(identity_matrix<3,float>());
}

BOOST_AUTO_TEST_CASE(matrix_inverse3_test)
{
    matrix<4,4,double> m =
        translation(make_vector<double>(4, 3, 7)) *
        scaling_transformation(make_vector<double>(.1, 2, 1.2)) *
        rotation_about_x(angle<double,degrees>(90));

    matrix<4,4,double> inv_m = inverse(m);

    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m,
            transform_point(m, make_vector<double>(0, 0, 0))),
        make_vector<double>(0, 0, 0));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m,
            transform_point(m, make_vector<double>(2, 1, 7))),
        make_vector<double>(2, 1, 7));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m,
            transform_point(m, make_vector<double>(1, 0, 17))),
        make_vector<double>(1, 0, 17));
}

BOOST_AUTO_TEST_CASE(matrix_inverse2_test)
{
    matrix<3,3,double> m =
        translation(make_vector<double>(3, 7)) *
        scaling_transformation(make_vector<double>(.1, 1.2)) *
        rotation(angle<double,degrees>(90));

    matrix<3,3,double> inv_m = inverse(m);

    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(0, 0))),
        make_vector<double>(0, 0));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(1, 7))),
        make_vector<double>(1, 7));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(0, 17))),
        make_vector<double>(0, 17));
}

BOOST_AUTO_TEST_CASE(matrix_inverse1_test)
{
    matrix<2,2,double> m =
        translation(make_vector<double>(1)) *
        scaling_transformation(make_vector<double>(.1));

    matrix<2,2,double> inv_m = inverse(m);

    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(0))),
        make_vector<double>(0));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(7))),
        make_vector<double>(7));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(17))),
        make_vector<double>(17));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(1))),
        make_vector<double>(1));
}


#include "gtest/gtest.h"
#include <Eigen/Core>
#include "Node.h"
#include "Validate.h"

namespace {

  class NodeTest : public ::testing::Test {
  protected:

    ::testing::AssertionResult containsVector(Eigen::MatrixXd m,
					      double x,
					      double y,
					      double z);
  };

  ::testing::AssertionResult NodeTest::containsVector(Eigen::MatrixXd m,
						      double x,
						      double y,
						      double z)
  {
    //bool contains = false;

    Eigen::MatrixXd v = Eigen::MatrixXd(1,3);
    v << x,y,z;

    for (int i=0; i<m.rows(); i++)
      {
	if (m.row(i) == v)
	  {
	    return ::testing::AssertionSuccess();
	  }
      }

    return ::testing::AssertionFailure() << "does not contain";
  }

  //testing::AssertionResult AssertionSuccess();
  //testing::AssertionResult AssertionFailure();

  // create an object
  TEST_F(NodeTest, Object) {
    Eigen::MatrixXd V(3,3);
    Eigen::MatrixXi F(1,3);

    V << 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5, 0.0;
    F << 0,1,2;

    Geometry *g = new Geometry(V,F);

    GeometryNode *n = new GeometryNode(*g);

    EXPECT_EQ(3, n->g.V.rows());
  }

  // union between separate 2d geometries
  TEST_F(NodeTest, DISABLED_Union) {
    Eigen::MatrixXd V1(3,3);
    Eigen::MatrixXi F1(1,3);

    V1 << 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5, 0.0;
    F1 << 0,1,2;

    Eigen::MatrixXd V2(3,3);
    Eigen::MatrixXi F2(1,3);

    V2 << -0.5, -0.5, -0.1, -0.5, 0.0, 0.0, -0.1, -0.5, 0.0;
    F2 << 2,1,0;

    Geometry *g1 = new Geometry(V1,F1);
    Geometry *g2 = new Geometry(V2,F2);

    GeometryNode *n1 = new GeometryNode(*g1);
    GeometryNode *n2 = new GeometryNode(*g2);
    UnionNode *n3 = new UnionNode();

    n3->add(n1);

    n3->add(n2);

    n3->build();

    EXPECT_EQ(6, n3->g.V.rows());
    EXPECT_EQ(2, n3->g.F.rows());

    Eigen::MatrixXd v(1,3);
    Eigen::MatrixXi f(1,3);

    v << 0.5, 0.0, 0.0;
    EXPECT_EQ(v, n3->g.V.row(1));

    v << -0.1, -0.5, 0.0;
    EXPECT_EQ(v, n3->g.V.row(5));

    f << 2,1,0;
    EXPECT_EQ(f, n3->g.F.row(1));
  }

  // tall union tree
  TEST_F(NodeTest, UnionTree) {
    Eigen::MatrixXd V(3,3);
    Eigen::MatrixXi F(1,3);

    V << 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5, 0.0;
    F << 0,1,2;

    Geometry *g = new Geometry(V, F);
    GeometryNode *n = new GeometryNode(*g);

    UnionNode *n1 = new UnionNode();
    n1->add(n);
    UnionNode *n2 = new UnionNode();
    n2->add(n1);
    UnionNode *n3 = new UnionNode();
    n3->add(n2);
    UnionNode *n4 = new UnionNode();
    n4->add(n3);
    UnionNode *n5 = new UnionNode();
    n5->add(n4);

    n5->build();

    EXPECT_EQ(3, n5->g.V.rows());
    EXPECT_EQ(1, n5->g.F.rows());

    Eigen::MatrixXd v(1,3);
    Eigen::MatrixXi f(1,3);

    v << 0.5, 0.0, 0.0;
    EXPECT_EQ(v, n5->g.V.row(1));

    f << 0,1,2;
    EXPECT_EQ(f, n5->g.F.row(0));
  }

  // cube node
  TEST_F(NodeTest, CubeNode) {

    CubeNode * n = new CubeNode(10,10,10);

    EXPECT_TRUE(Geotree::Validate::geometry(n->g));

    EXPECT_EQ(8, n->g.V.rows());
    EXPECT_EQ(12, n->g.F.rows());
  }

  // translate node
  TEST_F(NodeTest, TranslateNode) {

    CubeNode * c = new CubeNode(10,10,10);
    TranslateNode * t = new TranslateNode(10,2,-5);

    t->add(c);

    t->build();

    EXPECT_EQ(8, t->g.V.rows());
    EXPECT_EQ(12, t->g.F.rows());

    double xmin = 1000.0, xmax = 0.0;
    double ymin = 1000.0, ymax = 0.0;
    double zmin = 1000.0, zmax = 0.0;

    for (int i=0; i < t->g.V.rows(); i++)
      {
	Eigen::Vector3d v = t->g.V.row(i);
	xmin = std::min(xmin,v(0));
	xmax = std::max(xmax,v(0));
	ymin = std::min(ymin,v(1));
	ymax = std::max(ymax,v(1));
	zmin = std::min(zmin,v(2));
	zmax = std::max(zmax,v(2));
      }

    EXPECT_EQ(10.0, xmin);
    EXPECT_EQ(20.0, xmax);
    EXPECT_EQ(2.0, ymin);
    EXPECT_EQ(12.0, ymax);
    EXPECT_EQ(-5.0, zmin);
    EXPECT_EQ(5.0, zmax);
  }

  // union between intersecting 3d cubes
  TEST_F(NodeTest, DISABLED_Union3D) {

    CubeNode * c1 = new CubeNode(10,10,10);
    CubeNode * c2 = new CubeNode(10,10,10);
    TranslateNode * t = new TranslateNode(1,2,3);
    UnionNode * u = new UnionNode();

    t->add(c2);
    t->build();

    GeometryNode * g = new GeometryNode(t->g);

    u->add(c1);
    u->add(g);
    u->build();

    EXPECT_EQ(20, u->g.V.rows());
    EXPECT_EQ(33, u->g.F.rows());

    EXPECT_TRUE(containsVector(u->g.V, 0.0, 0.0, 0.0));
  }
}


#include <iostream>
#include <armadillo>

int main() {
    std::cout << "arma version: " << arma::arma_version::as_string() << std::endl;

    arma::mat A = arma::randu<arma::mat>(2,3);
    A.print("A=");

    arma::mat B;
    B << 0.1 << 0.2 << 0.3 << arma::endr
      << 0.4 << 0.5 << 0.6 << arma::endr;
    B.print("B=");

    arma::mat C = A + 3.0 * B;
    C.print("C=");

    C.save("c_mat.h5", arma::hdf5_binary);
    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// statistics::model::wrap::aggregate::prior_model.hpp                       //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_MODEL_WRAP_AGGREGATE_PRIOR_MODEL_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_WRAP_AGGREGATE_PRIOR_MODEL_HPP_ER_2009
#include <boost/statistics/model/wrap/unary/prior.hpp>
#include <boost/statistics/model/wrap/unary/model.hpp>

namespace boost{
namespace statistics{
namespace model{

    template<typename D,typename M>
    class prior_model_ :
        public model::prior_wrapper<D>,
        public model::model_wrapper<M>
    {
        typedef model::prior_wrapper<D> prior_;
        typedef model::model_wrapper<M> model_;

    public:

        typedef prior_model_<D,M> prior_model_wrapper_type;

        // Construction
        prior_model_();
        prior_model_(prior_ p, model_ m);
        prior_model_(const prior_model_&);

    };

    // Implementation //

    // Implementation //

    template<typename D,typename M>
    prior_model_<D,M>::prior_model_() : prior_(), model_(){}

    template<typename D,typename M>
    prior_model_<D,M>::prior_model_( prior_ p, model_ m )
     : prior_( p ),model_( m ){}

    template<typename D,typename M>
    prior_model_<D,M>::prior_model_(const prior_model_& that)
    :prior_( that ), model_( that ){}

    // Free functions

    template<typename D,typename M>
    prior_model_<D,M>
    make_prior_model(const D& p,const M& m){
        typedef prior_model_<D,M> result_type;
        return result_type(p,m);
    }

}// model
}// statistics
}// boost

#endif


#include <boost/python.hpp>

double sum_rain(boost::python::list rain, int len) {

  double sum = 0.;

  for (int i = 0; i < len; i++){
    int val = boost::python::extract<int>(rain[i]);
    if(val == -1) {
       sum += 0.025;
    } else {
      sum += 0.1 * val;
    }
  }

  return sum;
}

BOOST_PYTHON_MODULE(rain) {
    using namespace boost::python;

    def("sum_rain", sum_rain);
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2003, 2004 Ferdinando Ametrano
 Copyright (C) 2008, 2017 StatPro Italia srl
 Copyright (C) 2009 Master IMAFA - Polytech'Nice Sophia - Université de Nice Sophia Antipolis

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef quantlib_test_asian_options_hpp
#define quantlib_test_asian_options_hpp

#include <boost/test/unit_test.hpp>

/* remember to document new and/or updated tests in the Doxygen
   comment block of the corresponding class */

class AsianOptionTest {
  public:
    static void testAnalyticContinuousGeometricAveragePrice();
    static void testAnalyticContinuousGeometricAveragePriceGreeks();
    static void testAnalyticDiscreteGeometricAveragePrice();
    static void testAnalyticDiscreteGeometricAverageStrike();
    static void testMCDiscreteGeometricAveragePrice();
    static void testMCDiscreteArithmeticAveragePrice();
    static void testMCDiscreteArithmeticAveragePriceHeston();
    static void testMCDiscreteArithmeticAverageStrike();
    static void testAnalyticDiscreteGeometricAveragePriceGreeks();
    static void testPastFixings();
    static void testAllFixingsInThePast();
    static void testLevyEngine();
    static void testVecerEngine();
    static boost::unit_test_framework::test_suite* suite();
    static boost::unit_test_framework::test_suite* experimental();
};

#endif