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

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>
#include <boost/math/special_functions/legendre.hpp>
#include <boost/math/special_functions/legendre_stieltjes.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

using boost::math::legendre_stieltjes;
using boost::math::legendre_p;
using boost::multiprecision::cpp_bin_float_quad;

template<class Real>
void test_legendre_stieltjes()
{
    std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
    using std::sqrt;
    using std::abs;
    using boost::math::constants::third;
    using boost::math::constants::half;

    Real tol = std::numeric_limits<Real>::epsilon();
    legendre_stieltjes<Real> ls1(1);
    legendre_stieltjes<Real> ls2(2);
    legendre_stieltjes<Real> ls3(3);
    legendre_stieltjes<Real> ls4(4);
    legendre_stieltjes<Real> ls5(5);
    legendre_stieltjes<Real> ls8(8);
    Real x = -1;
    while(x <= 1)
    {
        BOOST_CHECK_CLOSE_FRACTION(ls1(x), x, tol);
        BOOST_CHECK_CLOSE_FRACTION(ls1.prime(x), 1, tol);

        Real p2 = legendre_p(2, x);
        BOOST_CHECK_CLOSE_FRACTION(ls2(x), p2 - 2/static_cast<Real>(5), tol);
        BOOST_CHECK_CLOSE_FRACTION(ls2.prime(x), 3*x, tol);

        Real p3 = legendre_p(3, x);
        BOOST_CHECK_CLOSE_FRACTION(ls3(x), p3 - 9*x/static_cast<Real>(14), 100*tol);
        BOOST_CHECK_CLOSE_FRACTION(ls3.prime(x), 15*x*x*half<Real>() -3*half<Real>()-9/static_cast<Real>(14), 100*tol);

        Real p4 = legendre_p(4, x);
        //-20P_2(x)/27 + 14P_0(x)/891
        Real E4 = p4 - 20*p2/static_cast<Real>(27) + 14/static_cast<Real>(891);
        BOOST_CHECK_CLOSE_FRACTION(ls4(x), E4, 250*tol);
        BOOST_CHECK_CLOSE_FRACTION(ls4.prime(x), 35*x*(9*x*x -5)/static_cast<Real>(18), 250*tol);

        Real p5 = legendre_p(5, x);
        Real E5 = p5 - 35*p3/static_cast<Real>(44) + 135*x/static_cast<Real>(12584);
        BOOST_CHECK_CLOSE_FRACTION(ls5(x), E5, 29000*tol);
        Real E5prime = (315*(123 + 143*x*x*(11*x*x-9)))/static_cast<Real>(12584);
        BOOST_CHECK_CLOSE_FRACTION(ls5.prime(x), E5prime, 29000*tol);
        x += 1/static_cast<Real>(1 << 9);
    }

    // Test norm:
    // E_1 = x
    Real expected_norm_sq = 2*third<Real>();
    BOOST_CHECK_CLOSE_FRACTION(expected_norm_sq, ls1.norm_sq(), tol);

    // E_2 = P[sub 2](x) - 2P[sup 0](x)/5
    expected_norm_sq = 2/static_cast<Real>(5) + 8/static_cast<Real>(25);
    BOOST_CHECK_CLOSE_FRACTION(expected_norm_sq, ls2.norm_sq(), tol);

    // E_3 = P[sub 3](x) - 9P[sub 1]/14
    expected_norm_sq = 2/static_cast<Real>(7) + 9*9*2*third<Real>()/static_cast<Real>(14*14);
    BOOST_CHECK_CLOSE_FRACTION(expected_norm_sq, ls3.norm_sq(), tol);

    // E_4 = P[sub 4](x) -20P[sub 2](x)/27 + 14P[sub 0](x)/891
    expected_norm_sq = static_cast<Real>(2)/static_cast<Real>(9) + static_cast<Real>(20*20*2)/static_cast<Real>(27*27*5) + 14*14*2/static_cast<Real>(891*891);
    BOOST_CHECK_CLOSE_FRACTION(expected_norm_sq, ls4.norm_sq(), tol);

    // E_5 = P[sub 5](x) - 35P[sub 3](x)/44 + 135P[sub 1](x)/12584
    expected_norm_sq = 2/static_cast<Real>(11) + (35*35/static_cast<Real>(44*44))*(2/static_cast<Real>(7)) + (135*135/static_cast<Real>(12584*12584))*2*third<Real>();
    BOOST_CHECK_CLOSE_FRACTION(expected_norm_sq, ls5.norm_sq(), tol);

    // Only zero of E1 is 0:
    std::vector<Real> zeros = ls1.zeros();
    BOOST_CHECK(zeros.size() == 1);
    BOOST_CHECK_SMALL(zeros[0], tol);
    BOOST_CHECK_SMALL(ls1(zeros[0]), tol);

    zeros = ls2.zeros();
    BOOST_CHECK(zeros.size() == 1);
    BOOST_CHECK_CLOSE_FRACTION(zeros[0], sqrt(3/static_cast<Real>(5)), tol);
    BOOST_CHECK_SMALL(ls2(zeros[0]), tol);

    zeros = ls3.zeros();
    BOOST_CHECK(zeros.size() == 2);
    BOOST_CHECK_SMALL(zeros[0], tol);
    BOOST_CHECK_CLOSE_FRACTION(zeros[1], sqrt(6/static_cast<Real>(7)), tol);

    zeros = ls4.zeros();
    BOOST_CHECK(zeros.size() == 2);
    Real expected = sqrt( (55 - 2*sqrt(static_cast<Real>(330)))/static_cast<Real>(11) )/static_cast<Real>(3);
    BOOST_CHECK_CLOSE_FRACTION(zeros[0], expected, tol);

    expected = sqrt( (55 + 2*sqrt(static_cast<Real>(330)))/static_cast<Real>(11) )/static_cast<Real>(3);
    BOOST_CHECK_CLOSE_FRACTION(zeros[1], expected, 10*tol);

    zeros = ls5.zeros();
    BOOST_CHECK(zeros.size() == 3);
    BOOST_CHECK_SMALL(zeros[0], tol);

    expected = sqrt( ( 195 - sqrt(static_cast<Real>(6045)) )/static_cast<Real>(286));
    BOOST_CHECK_CLOSE_FRACTION(zeros[1], expected, tol);

    expected = sqrt( ( 195 + sqrt(static_cast<Real>(6045)) )/static_cast<Real>(286));
    BOOST_CHECK_CLOSE_FRACTION(zeros[2], expected, tol);

    for (size_t i = 6; i < 50; ++i)
    {
        legendre_stieltjes<Real> En(i);
        zeros = En.zeros();
        for(auto const & zero : zeros)
        {
            BOOST_CHECK_SMALL(En(zero), 50*tol);
        }
    }
}

BOOST_AUTO_TEST_CASE(LegendreStieltjesZeros)
{
    test_legendre_stieltjes<double>();
    test_legendre_stieltjes<long double>();
    test_legendre_stieltjes<cpp_bin_float_quad>();
    //test_legendre_stieltjes<boost::multiprecision::cpp_bin_float_100>();
}


#include <emp-tool-tg/emp-tool/emp-tool.h>
#include <boost/program_options.hpp>
#include <boost/format.hpp>
#include "tinygarble/program_interface.h"
#include "tinygarble/program_interface_sh.h"
#include "tinygarble/TinyGarble_config.h"

using namespace std;
namespace po = boost::program_options;

void millionaire(auto TGPI){

	uint64_t bit_width = 64;
    int64_t a = 0, b = 0;

	cout << "Input wealth: ";
    if (TGPI->party == ALICE){
        cin >> a;
        cout << "ALICE's wealth: $" << a << endl;
    }
    else{
        cin >> b;
        cout << "BOB's wealth: $" << b << endl;
    }

    auto a_x = TGPI->TG_int_init(ALICE, bit_width, a);
    auto b_x = TGPI->TG_int_init(BOB, bit_width, b);

    TGPI->gen_input_labels();

    TGPI->retrieve_input_labels(a_x, ALICE, bit_width);
    TGPI->retrieve_input_labels(b_x, BOB, bit_width);

    auto res_x = TGPI->TG_int(1);
    TGPI->lt(res_x, a_x, b_x, bit_width);
    int64_t res = TGPI->reveal(res_x, 1, false);

#if !SEC_SH
    if (TGPI->party == BOB) //authenticated garbling allows only evaluator to compute the result
#endif
        if (res == 1) cout << "BOB is richer" << endl;
        else cout << "ALICE is richer" << endl;

	TGPI->clear_TG_int(a_x);
	TGPI->clear_TG_int(b_x);
	TGPI->clear_TG_int(res_x);

	delete TGPI;
}

int main(int argc, char** argv) {
	int party = 1, port = 1234;
	string netlist_address;
	string server_ip;

	po::options_description desc{"Yao's Millionair's Problem \nAllowed options"};
	desc.add_options()  //
	("help,h", "produce help message")  //
	("party,k", po::value<int>(&party)->default_value(1), "party id: 1 for garbler, 2 for evaluator")  //
	("port,p", po::value<int>(&port)->default_value(1234), "socket port")  //
	("server_ip,s", po::value<string>(&server_ip)->default_value("127.0.0.1"), "server's IP.")
	("sh", "semi-honest setting (default is malicious)");

	po::variables_map vm;
	try {
		po::parsed_options parsed = po::command_line_parser(argc, argv).options(desc).allow_unregistered().run();
		po::store(parsed, vm);
		if (vm.count("help")) {
			cout << desc << endl;
			return 0;
		}
		po::notify(vm);
	}catch (po::error& e) {
		cout << "ERROR: " << e.what() << endl << endl;
		cout << desc << endl;
		return -1;
	}

	NetIO* io = new NetIO(party==ALICE ? nullptr:server_ip.c_str(), port, true);
	io->set_nodelay();

	TinyGarblePI_SH* TGPI_SH;
	TinyGarblePI* TGPI;

	if (vm.count("sh")){
		cout << "testing program interface in semi-honest setting" << endl;
		TGPI_SH = new TinyGarblePI_SH(io, party);
		io->flush();
		millionaire(TGPI_SH);
	}
	else {
		cout << "Millionair's Problem in malicious setting" << endl;
		TGPI = new TinyGarblePI(io, party, 192, 64);
		io->flush();
		millionaire(TGPI);
	}

	delete io;

	return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

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

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/iota.hpp>
#include <boost/compute/algorithm/max_element.hpp>
#include <boost/compute/algorithm/min_element.hpp>
#include <boost/compute/algorithm/minmax_element.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/transform_iterator.hpp>

#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(int_min_max)
{
    int data[] = { 9, 15, 1, 4 };
    boost::compute::vector<int> vector(data, data + 4);

    boost::compute::vector<int>::iterator min_iter =
        boost::compute::min_element(vector.begin(), vector.end());
    BOOST_CHECK(min_iter == vector.begin() + 2);
    BOOST_CHECK_EQUAL(*min_iter, 1);

    boost::compute::vector<int>::iterator max_iter =
        boost::compute::max_element(vector.begin(), vector.end());
    BOOST_CHECK(max_iter == vector.begin() + 1);
    BOOST_CHECK_EQUAL(*max_iter, 15);
}

BOOST_AUTO_TEST_CASE(iota_min_max)
{
    boost::compute::vector<int> vector(5000);

    // fill with 0 -> 4999
    boost::compute::iota(vector.begin(), vector.end(), 0);

    boost::compute::vector<int>::iterator min_iter =
        boost::compute::min_element(vector.begin(), vector.end());
    BOOST_CHECK(min_iter == vector.begin());
    BOOST_CHECK_EQUAL(*min_iter, 0);

    boost::compute::vector<int>::iterator max_iter =
        boost::compute::max_element(vector.begin(), vector.end());
    BOOST_CHECK(max_iter == vector.end() - 1);
    BOOST_CHECK_EQUAL(*max_iter, 4999);

    min_iter =
        boost::compute::min_element(
            vector.begin() + 1000,
            vector.end() - 1000
        );
    BOOST_CHECK(min_iter == vector.begin() + 1000);
    BOOST_CHECK_EQUAL(*min_iter, 1000);

    max_iter =
        boost::compute::max_element(
            vector.begin() + 1000,
            vector.end() - 1000
        );
    BOOST_CHECK(max_iter == vector.begin() + 3999);
    BOOST_CHECK_EQUAL(*max_iter, 3999);

    // fill with -2500 -> 2499
    boost::compute::iota(vector.begin(), vector.end(), -2500);
    min_iter =
        boost::compute::min_element(vector.begin(), vector.end());
    BOOST_CHECK(min_iter == vector.begin());
    BOOST_CHECK_EQUAL(*min_iter, -2500);

    max_iter =
        boost::compute::max_element(vector.begin(), vector.end());
    BOOST_CHECK(max_iter == vector.end() - 1);
    BOOST_CHECK_EQUAL(*max_iter, 2499);
}

// uses max_element() and length() to find the longest 2d vector
BOOST_AUTO_TEST_CASE(max_vector_length)
{
    float data[] = { -1.5f, 3.2f,
                     10.0f, 0.0f,
                     -4.2f, 2.0f,
                     0.0f, 0.5f,
                     1.9f, 1.9f };
    boost::compute::vector<boost::compute::float2_> vector(
        reinterpret_cast<boost::compute::float2_ *>(data),
        reinterpret_cast<boost::compute::float2_ *>(data) + 5
    );

    // find length of the longest vector
    typedef boost::compute::transform_iterator<
                boost::compute::vector<boost::compute::float2_>::iterator,
                boost::compute::length<boost::compute::float2_>
            > length_transform_iter;

    length_transform_iter max_iter =
        boost::compute::max_element(
            boost::compute::make_transform_iterator(
                vector.begin(),
                boost::compute::length<boost::compute::float2_>()
            ),
            boost::compute::make_transform_iterator(
                vector.end(),
                boost::compute::length<boost::compute::float2_>()
            )
        );
    BOOST_CHECK(
        max_iter == boost::compute::make_transform_iterator(
                        vector.begin() + 1,
                        boost::compute::length<boost::compute::float2_>()
                    )
    );
    BOOST_CHECK(max_iter.base() == vector.begin() + 1);
    BOOST_CHECK_EQUAL(*max_iter, float(10.0));

    // find length of the shortest vector
    length_transform_iter min_iter =
        boost::compute::min_element(
            boost::compute::make_transform_iterator(
                vector.begin(),
                boost::compute::length<boost::compute::float2_>()
            ),
            boost::compute::make_transform_iterator(
                vector.end(),
                boost::compute::length<boost::compute::float2_>()
            )
        );
    BOOST_CHECK(
        min_iter == boost::compute::make_transform_iterator(
                        vector.begin() + 3,
                        boost::compute::length<boost::compute::float2_>()
                    )
    );
    BOOST_CHECK(min_iter.base() == vector.begin() + 3);
    BOOST_CHECK_EQUAL(*min_iter, float(0.5));
}

// uses max_element() and popcount() to find the value with the most 1 bits
BOOST_AUTO_TEST_CASE(max_bits_set)
{
    using boost::compute::uint_;

    uint_ data[] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9 };
    boost::compute::vector<uint_> vector(data, data + 10);

    boost::compute::vector<uint_>::iterator iter =
        boost::compute::max_element(
            boost::compute::make_transform_iterator(
                vector.begin(),
                boost::compute::popcount<uint_>()
            ),
            boost::compute::make_transform_iterator(
                vector.end(),
                boost::compute::popcount<uint_>()
            )
        ).base();

    BOOST_CHECK(iter == vector.begin() + 7);
    BOOST_CHECK_EQUAL(uint_(*iter), uint_(7));
}

BOOST_AUTO_TEST_SUITE_END()


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <boost/compute/core.hpp>
#include <boost/compute/closure.hpp>
#include <boost/compute/algorithm/copy_if.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/random/default_random_engine.hpp>
#include <boost/compute/random/uniform_int_distribution.hpp>
#include <boost/compute/random/uniform_real_distribution.hpp>

#include "perf.hpp"

namespace compute = boost::compute;

void test_copy_if_odd(compute::command_queue &queue)
{
    // create input and output vectors on the device
    const compute::context &context = queue.get_context();
    compute::vector<int> input(PERF_N, context);
    compute::vector<int> output(PERF_N, context);

    // generate random numbers between 1 and 10
    compute::default_random_engine rng(queue);
    compute::uniform_int_distribution<int> d(1, 10);
    d.generate(input.begin(), input.end(), rng, queue);

    BOOST_COMPUTE_FUNCTION(bool, is_odd, (int x),
    {
        return x & 1;
    });

    perf_timer t;
    for(size_t trial = 0; trial < PERF_TRIALS; trial++){
        t.start();
        compute::vector<int>::iterator i = compute::copy_if(
            input.begin(), input.end(), output.begin(), is_odd, queue
        );
        queue.finish();
        t.stop();

        float ratio = float(std::distance(output.begin(), i)) / PERF_N;
        if(PERF_N > 1000 && (ratio < 0.45f || ratio > 0.55f)){
            std::cerr << "error: ratio is " << ratio << std::endl;
            std::cerr << "error: ratio should be around 45-55%" << std::endl;
        }
    }
    std::cout << "time: " << t.min_time() / 1e6 << " ms" << std::endl;
}

void test_copy_if_in_sphere(compute::command_queue &queue)
{
    using boost::compute::float4_;

    // create input and output vectors on the device
    const compute::context &context = queue.get_context();
    compute::vector<float4_> input_points(PERF_N, context);
    compute::vector<float4_> output_points(PERF_N, context);

    // generate random numbers in a cube
    float radius = 5.0f;
    compute::default_random_engine rng(queue);
    compute::uniform_real_distribution<float> d(-radius, +radius);
    d.generate(
        compute::make_buffer_iterator<float>(input_points.get_buffer(), 0),
        compute::make_buffer_iterator<float>(input_points.get_buffer(), PERF_N * 4),
        rng,
        queue
    );

    // predicate which returns true if the point lies within the sphere
    BOOST_COMPUTE_CLOSURE(bool, is_in_sphere, (float4_ point), (radius),
    {
        // ignore fourth component
        point.w = 0;

        return length(point) < radius;
    });

    perf_timer t;
    for(size_t trial = 0; trial < PERF_TRIALS; trial++){
        t.start();
        compute::vector<float4_>::iterator i = compute::copy_if(
            input_points.begin(),
            input_points.end(),
            output_points.begin(),
            is_in_sphere,
            queue
        );
        queue.finish();
        t.stop();

        float ratio = float(std::distance(output_points.begin(), i)) / PERF_N;
        if(PERF_N > 1000 && (ratio < 0.5f || ratio > 0.6f)){
            std::cerr << "error: ratio is " << ratio << std::endl;
            std::cerr << "error: ratio should be around 50-60%" << std::endl;
        }
    }
    std::cout << "time: " << t.min_time() / 1e6 << " ms" << std::endl;
}

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

    // setup context and queue for the default device
    boost::compute::device device = boost::compute::system::default_device();
    boost::compute::context context(device);
    boost::compute::command_queue queue(context, device);
    std::cout << "device: " << device.name() << std::endl;

    test_copy_if_odd(queue);

    return 0;
}


/*
 * fpu.cpp
 *
 * This example demonstrates how one can use odeint to solve the Fermi-Pasta-Ulam system.

 *  Created on: July 13, 2011
 *
 * Copyright 2011-2012 Karsten Ahnert
 * Copyright 2011 Mario Mulansky
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

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

#include <boost/numeric/odeint.hpp>

#ifndef M_PI //not there on windows
#define M_PI 3.1415927 //...
#endif

using namespace std;
using namespace boost::numeric::odeint;

//[ fpu_system_function
typedef vector< double > container_type;

struct fpu
{
    const double m_beta;

    fpu( const double beta = 1.0 ) : m_beta( beta ) { }

    // system function defining the ODE
    void operator()( const container_type &q , container_type &dpdt ) const
    {
        size_t n = q.size();
        double tmp = q[0] - 0.0;
        double tmp2 = tmp + m_beta * tmp * tmp * tmp;
        dpdt[0] = -tmp2;
        for( size_t i=0 ; i<n-1 ; ++i )
        {
            tmp = q[i+1] - q[i];
            tmp2 = tmp + m_beta * tmp * tmp * tmp;
            dpdt[i] += tmp2;
            dpdt[i+1] = -tmp2;
        }
        tmp = - q[n-1];
        tmp2 = tmp + m_beta * tmp * tmp * tmp;
        dpdt[n-1] += tmp2;
    }

    // calculates the energy of the system
    double energy( const container_type &q , const container_type &p ) const
    {
        // ...
        //<-
        double energy = 0.0;
        size_t n = q.size();

        double tmp = q[0];
        energy += 0.5 * tmp * tmp + 0.25 * m_beta * tmp * tmp * tmp * tmp;
        for( size_t i=0 ; i<n-1 ; ++i )
        {
            tmp = q[i+1] - q[i];
            energy += 0.5 * ( p[i] * p[i] + tmp * tmp ) + 0.25 * m_beta * tmp * tmp * tmp * tmp;
        }
        energy += 0.5 * p[n-1] * p[n-1];
        tmp = q[n-1];
        energy += 0.5 * tmp * tmp + 0.25 * m_beta * tmp * tmp * tmp * tmp;

        return energy;
        //->
    }

    // calculates the local energy of the system
    void local_energy( const container_type &q , const container_type &p , container_type &e ) const
    {
        // ...
        //<-
        size_t n = q.size();
        double tmp = q[0];
        double tmp2 = 0.5 * tmp * tmp + 0.25 * m_beta * tmp * tmp * tmp * tmp;
        e[0] = tmp2;
        for( size_t i=0 ; i<n-1 ; ++i )
        {
            tmp = q[i+1] - q[i];
            tmp2 = 0.25 * tmp * tmp + 0.125 * m_beta * tmp * tmp * tmp * tmp;
            e[i] += 0.5 * p[i] * p[i] + tmp2 ;
            e[i+1] = tmp2;
        }
        tmp = q[n-1];
        tmp2 = 0.5 * tmp * tmp + 0.25 * m_beta * tmp * tmp * tmp * tmp;
        e[n-1] += 0.5 * p[n-1] * p[n-1] + tmp2;
        //->
    }
};
//]

//[ fpu_observer
struct streaming_observer
{
    std::ostream& m_out;
    const fpu &m_fpu;
    size_t m_write_every;
    size_t m_count;

    streaming_observer( std::ostream &out , const fpu &f , size_t write_every = 100 )
    : m_out( out ) , m_fpu( f ) , m_write_every( write_every ) , m_count( 0 ) { }

    template< class State >
    void operator()( const State &x , double t )
    {
        if( ( m_count % m_write_every ) == 0 )
        {
            container_type &q = x.first;
            container_type &p = x.second;
            container_type energy( q.size() );
            m_fpu.local_energy( q , p , energy );
            for( size_t i=0 ; i<q.size() ; ++i )
            {
                m_out << t << "\t" << i << "\t" << q[i] << "\t" << p[i] << "\t" << energy[i] << "\n";
            }
            m_out << "\n";
            clog << t << "\t" << accumulate( energy.begin() , energy.end() , 0.0 ) << "\n";
        }
        ++m_count;
    }
};
//]

int main( int argc , char **argv )
{
    //[ fpu_integration
    const size_t n = 64;
    container_type q( n , 0.0 ) , p( n , 0.0 );

    for( size_t i=0 ; i<n ; ++i )
    {
        p[i] = 0.0;
        q[i] = 32.0 * sin( double( i + 1 ) / double( n + 1 ) * M_PI );
    }

    const double dt = 0.1;

    typedef symplectic_rkn_sb3a_mclachlan< container_type > stepper_type;
    fpu fpu_instance( 8.0 );

    integrate_const( stepper_type() , fpu_instance ,
            make_pair( boost::ref( q ) , boost::ref( p ) ) ,
            0.0 , 1000.0 , dt , streaming_observer( cout , fpu_instance , 10 ) );
    //]

    return 0;
}


#include "mtf/SSM/Affine.h"
#include "mtf/SSM/AffineEstimator.h"
#include "mtf/Utilities/warpUtils.h"
#include "mtf/Utilities/miscUtils.h"

#include <Eigen/SVD>

#define VALIDATE_AFFINE_WARP(warp)\
	assert(warp(2, 0) == 0.0 && warp(2, 1) == 0.0);\
	assert(warp(2, 2) == 1.0)

#define AFF_NORMALIZED_INIT 0
#define AFF_PT_BASED_SAMPLING 0
#define AFF_DEBUG_MODE 0

_MTF_BEGIN_NAMESPACE

AffineParams::AffineParams(const SSMParams *ssm_params,
bool _normalized_init, int _pt_based_sampling,
bool _debug_mode) :
SSMParams(ssm_params),
normalized_init(_normalized_init),
pt_based_sampling(_pt_based_sampling),
debug_mode(_debug_mode){}

AffineParams::AffineParams(const AffineParams *params) :
SSMParams(params),
normalized_init(AFF_NORMALIZED_INIT),
pt_based_sampling(AFF_PT_BASED_SAMPLING),
debug_mode(AFF_DEBUG_MODE){
	if(params){
		normalized_init = params->normalized_init;
		pt_based_sampling = params->pt_based_sampling;
		debug_mode = params->debug_mode;
	}
}
Affine::Affine(
	const ParamType *_params) : ProjectiveBase(_params),
	params(_params){

	printf("\n");
	printf("Using Affine SSM with:\n");
	printf("resx: %d\n", resx);
	printf("resy: %d\n", resy);
	printf("normalized_init: %d\n", params.normalized_init);
	printf("pt_based_sampling: %d\n", params.pt_based_sampling);
	printf("debug_mode: %d\n", params.debug_mode);

	name = "affine";
	state_size = 6;
	curr_state.resize(state_size);

	utils::getNormUnitSquarePts(norm_pts, norm_corners, resx, resy,
		1 - resx / 2.0, 1 - resy / 2.0, resx / 2.0, resy / 2.0);
	utils::homogenize(norm_pts, norm_pts_hm);
	utils::homogenize(norm_corners, norm_corners_hm);

	init_corners = getNormCorners();
	init_corners_hm = getHomNormCorners();
	init_pts = getNormPts();
	init_pts_hm = getHomNormPts();
}

void Affine::setCorners(const CornersT& corners){
	if(params.normalized_init){
		curr_warp = utils::computeAffineNDLT(init_corners, corners);
		getStateFromWarp(curr_state, curr_warp);

		curr_pts.noalias() = curr_warp.topRows<2>() * init_pts_hm;
		curr_corners.noalias() = curr_warp.topRows<2>() * init_corners_hm;

		utils::homogenize(curr_pts, curr_pts_hm);
		utils::homogenize(curr_corners, curr_corners_hm);
	} else {
		curr_corners = corners;
		utils::homogenize(curr_corners, curr_corners_hm);

		getPtsFromCorners(curr_warp, curr_pts, curr_pts_hm, curr_corners);

		init_corners = curr_corners;
		init_pts = curr_pts;
		utils::homogenize(init_corners, init_corners_hm);
		utils::homogenize(init_pts, init_pts_hm);

		curr_warp = Matrix3d::Identity();
		curr_state.fill(0);
	}
}

void Affine::compositionalUpdate(const VectorXd& state_update){
	validate_ssm_state(state_update);

	getWarpFromState(warp_update_mat, state_update);
	curr_warp = curr_warp * warp_update_mat;
	getStateFromWarp(curr_state, curr_warp);

	//curr_pts_hm.noalias() = curr_warp * init_pts_hm;
	//curr_corners_hm.noalias() = curr_warp * init_corners_hm;
	//utils::dehomogenize(curr_pts_hm, curr_pts);
	//utils::dehomogenize(curr_corners_hm, curr_corners);

	curr_pts.noalias() = curr_warp.topRows<2>() * init_pts_hm;
	curr_corners.noalias() = curr_warp.topRows<2>() * init_corners_hm;

	//utils::printMatrix(curr_warp, "curr_warp", "%15.9f");
	//utils::printMatrix(affine_warp_mat, "affine_warp_mat", "%15.9f");
}

void Affine::setState(const VectorXd &ssm_state){
	validate_ssm_state(ssm_state);
	curr_state = ssm_state;
	getWarpFromState(curr_warp, curr_state);
	curr_pts.noalias() = curr_warp.topRows<2>() * init_pts_hm;
	curr_corners.noalias() = curr_warp.topRows<2>() * init_corners_hm;
}

void Affine::getWarpFromState(Matrix3d &warp_mat,
	const VectorXd& ssm_state){
	validate_ssm_state(ssm_state);

	warp_mat(0, 0) = 1 + ssm_state(2);
	warp_mat(0, 1) = ssm_state(3);
	warp_mat(0, 2) = ssm_state(0);
	warp_mat(1, 0) = ssm_state(4);
	warp_mat(1, 1) = 1 + ssm_state(5);
	warp_mat(1, 2) = ssm_state(1);
	warp_mat(2, 0) = 0;
	warp_mat(2, 1) = 0;
	warp_mat(2, 2) = 1;
}

void Affine::getStateFromWarp(VectorXd &state_vec,
	const Matrix3d& warp_mat){
	validate_ssm_state(state_vec);
	VALIDATE_AFFINE_WARP(warp_mat);

	state_vec(0) = warp_mat(0, 2);
	state_vec(1) = warp_mat(1, 2);
	state_vec(2) = warp_mat(0, 0) - 1;
	state_vec(3) = warp_mat(0, 1);
	state_vec(4) = warp_mat(1, 0);
	state_vec(5) = warp_mat(1, 1) - 1;
}

void Affine::invertState(VectorXd& inv_state, const VectorXd& state){
	getWarpFromState(warp_mat, state);
	inv_warp_mat = warp_mat.inverse();
	inv_warp_mat /= inv_warp_mat(2, 2);
	getStateFromWarp(inv_state, inv_warp_mat);
}

void Affine::getInitPixGrad(Matrix2Xd &ssm_grad, int pix_id) {
	double x = init_pts(0, pix_id);
	double y = init_pts(1, pix_id);
	ssm_grad <<
		1, 0, x, y, 0, 0,
		0, 1, 0, 0, x, y;
}

void Affine::cmptInitPixJacobian(MatrixXd &dI_dp,
	const PixGradT &dI_dx){
	validate_ssm_jacobian(dI_dp, dI_dx);
	unsigned int ch_pt_id = 0;
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		spi_pt_check_mc(spi_mask, pt_id, ch_pt_id);

		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);
		for(unsigned int ch_id = 0; ch_id < n_channels; ++ch_id){
			double Ix = dI_dx(ch_pt_id, 0);
			double Iy = dI_dx(ch_pt_id, 1);
			dI_dp(ch_pt_id, 0) = Ix;
			dI_dp(ch_pt_id, 1) = Iy;
			dI_dp(ch_pt_id, 2) = Ix * x;
			dI_dp(ch_pt_id, 3) = Ix * y;
			dI_dp(ch_pt_id, 4) = Iy * x;
			dI_dp(ch_pt_id, 5) = Iy * y;
			++ch_pt_id;
		}
	}
}

void Affine::cmptApproxPixJacobian(MatrixXd &dI_dp,
	const PixGradT &dI_dx) {
	validate_ssm_jacobian(dI_dp, dI_dx);
	double a = curr_state(2) + 1, b = curr_state(3);
	double c = curr_state(4), d = curr_state(5) + 1;
	double inv_det = 1.0 / (a*d - b*c);
	unsigned int ch_pt_id = 0;
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		spi_pt_check_mc(spi_mask, pt_id, ch_pt_id);

		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);
		for(unsigned int ch_id = 0; ch_id < n_channels; ++ch_id){
			double Ix = dI_dx(ch_pt_id, 0);
			double Iy = dI_dx(ch_pt_id, 1);
			double Ixx = Ix * x;
			double Ixy = Ix * y;
			double Iyy = Iy * y;
			double Iyx = Iy * x;
			dI_dp(ch_pt_id, 0) = (Ix*d - Iy*c) * inv_det;
			dI_dp(ch_pt_id, 1) = (Iy*a - Ix*b) * inv_det;
			dI_dp(ch_pt_id, 2) = (Ixx*d - Iyx*c) * inv_det;
			dI_dp(ch_pt_id, 3) = (Ixy*d - Iyy*c) * inv_det;
			dI_dp(ch_pt_id, 4) = (Iyx*a - Ixx*b) * inv_det;
			dI_dp(ch_pt_id, 5) = (Iyy*a - Ixy*b) * inv_det;
			++ch_pt_id;
		}
	}
}

void Affine::cmptWarpedPixJacobian(MatrixXd &dI_dp,
	const PixGradT &dI_dx) {
	validate_ssm_jacobian(dI_dp, dI_dx);
	double a = curr_state(2) + 1, b = curr_state(3);
	double c = curr_state(4), d = curr_state(5) + 1;

	unsigned int ch_pt_id = 0;
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		spi_pt_check_mc(spi_mask, pt_id, ch_pt_id);

		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);
		for(unsigned int ch_id = 0; ch_id < n_channels; ++ch_id){
			double Ix = dI_dx(ch_pt_id, 0);
			double Iy = dI_dx(ch_pt_id, 1);
			double Ixx = Ix * x;
			double Ixy = Ix * y;
			double Iyy = Iy * y;
			double Iyx = Iy * x;

			dI_dp(ch_pt_id, 0) = Ix*a + Iy*c;
			dI_dp(ch_pt_id, 1) = Ix*b + Iy*d;
			dI_dp(ch_pt_id, 2) = Ixx*a + Iyx*c;
			dI_dp(ch_pt_id, 3) = Ixy*a + Iyy*c;
			dI_dp(ch_pt_id, 4) = Ixx*b + Iyx*d;
			dI_dp(ch_pt_id, 5) = Ixy*b + Iyy*d;
			++ch_pt_id;
		}
	}
}
void Affine::cmptInitPixHessian(MatrixXd &d2I_dp2, const PixHessT &d2I_dw2,
	const PixGradT &dI_dw){
	validate_ssm_hessian(d2I_dp2, d2I_dw2, dI_dw);

	unsigned int ch_pt_id = 0;
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		spi_pt_check_mc(spi_mask, pt_id, ch_pt_id);

		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);
		Matrix26d dw_dp;
		dw_dp <<
			1, 0, x, y, 0, 0,
			0, 1, 0, 0, x, y;
		for(unsigned int ch_id = 0; ch_id < n_channels; ++ch_id){
			Map<Matrix6d>(d2I_dp2.col(ch_pt_id).data()) = dw_dp.transpose()*
				Map<const Matrix2d>(d2I_dw2.col(ch_pt_id).data())*dw_dp;
			++ch_pt_id;
		}
	}
}
void Affine::cmptWarpedPixHessian(MatrixXd &d2I_dp2, const PixHessT &d2I_dw2,
	const PixGradT &dI_dw) {
	validate_ssm_hessian(d2I_dp2, d2I_dw2, dI_dw);
	double a2 = curr_state(2) + 1, a3 = curr_state(3);
	double a4 = curr_state(4), a5 = curr_state(5) + 1;
	Matrix2d dw_dx;
	dw_dx <<
		a2, a3,
		a4, a5;

	unsigned int ch_pt_id = 0;
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id) {
		spi_pt_check_mc(spi_mask, pt_id, ch_pt_id);

		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);

		Matrix26d dw_dp;
		dw_dp <<
			1, 0, x, y, 0, 0,
			0, 1, 0, 0, x, y;

		for(unsigned int ch_id = 0; ch_id < n_channels; ++ch_id){
			Map<Matrix6d>(d2I_dp2.col(ch_pt_id).data()) = dw_dp.transpose()*
				dw_dx.transpose()*Map<const Matrix2d>(d2I_dw2.col(ch_pt_id).data())*dw_dx*dw_dp;
			++ch_pt_id;
		}
	}
}
void Affine::updateGradPts(double grad_eps){
	Vector2d diff_vec_x_warped = curr_warp.topRows<2>().col(0) * grad_eps;
	Vector2d diff_vec_y_warped = curr_warp.topRows<2>().col(1) * grad_eps;

	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		spi_pt_check(spi_mask, pt_id);

		grad_pts(0, pt_id) = curr_pts(0, pt_id) + diff_vec_x_warped(0);
		grad_pts(1, pt_id) = curr_pts(1, pt_id) + diff_vec_x_warped(1);

		grad_pts(2, pt_id) = curr_pts(0, pt_id) - diff_vec_x_warped(0);
		grad_pts(3, pt_id) = curr_pts(1, pt_id) - diff_vec_x_warped(1);

		grad_pts(4, pt_id) = curr_pts(0, pt_id) + diff_vec_y_warped(0);
		grad_pts(5, pt_id) = curr_pts(1, pt_id) + diff_vec_y_warped(1);

		grad_pts(6, pt_id) = curr_pts(0, pt_id) - diff_vec_y_warped(0);
		grad_pts(7, pt_id) = curr_pts(1, pt_id) - diff_vec_y_warped(1);
	}
}

void Affine::updateHessPts(double hess_eps){
	double hess_eps2 = 2 * hess_eps;

	Vector2d diff_vec_xx_warped = curr_warp.topRows<2>().col(0) * hess_eps2;
	Vector2d diff_vec_yy_warped = curr_warp.topRows<2>().col(1) * hess_eps2;
	Vector2d diff_vec_xy_warped = (curr_warp.topRows<2>().col(0) + curr_warp.topRows<2>().col(1)) * hess_eps;
	Vector2d diff_vec_yx_warped = (curr_warp.topRows<2>().col(0) - curr_warp.topRows<2>().col(1)) * hess_eps;

	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		spi_pt_check(spi_mask, pt_id);

		hess_pts(0, pt_id) = curr_pts(0, pt_id) + diff_vec_xx_warped(0);
		hess_pts(1, pt_id) = curr_pts(1, pt_id) + diff_vec_xx_warped(1);

		hess_pts(2, pt_id) = curr_pts(0, pt_id) - diff_vec_xx_warped(0);
		hess_pts(3, pt_id) = curr_pts(1, pt_id) - diff_vec_xx_warped(1);

		hess_pts(4, pt_id) = curr_pts(0, pt_id) + diff_vec_yy_warped(0);
		hess_pts(5, pt_id) = curr_pts(1, pt_id) + diff_vec_yy_warped(1);

		hess_pts(6, pt_id) = curr_pts(0, pt_id) - diff_vec_yy_warped(0);
		hess_pts(7, pt_id) = curr_pts(1, pt_id) - diff_vec_yy_warped(1);

		hess_pts(8, pt_id) = curr_pts(0, pt_id) + diff_vec_xy_warped(0);
		hess_pts(9, pt_id) = curr_pts(1, pt_id) + diff_vec_xy_warped(1);

		hess_pts(10, pt_id) = curr_pts(0, pt_id) - diff_vec_xy_warped(0);
		hess_pts(11, pt_id) = curr_pts(1, pt_id) - diff_vec_xy_warped(1);

		hess_pts(12, pt_id) = curr_pts(0, pt_id) + diff_vec_yx_warped(0);
		hess_pts(13, pt_id) = curr_pts(1, pt_id) + diff_vec_yx_warped(1);

		hess_pts(14, pt_id) = curr_pts(0, pt_id) - diff_vec_yx_warped(0);
		hess_pts(15, pt_id) = curr_pts(1, pt_id) - diff_vec_yx_warped(1);
	}
}

void Affine::estimateWarpFromCorners(VectorXd &state_update, const Matrix24d &in_corners,
	const Matrix24d &out_corners){
	validate_ssm_state(state_update);
	Matrix3d warp_update_mat = utils::computeAffineDLT(in_corners, out_corners);
	getStateFromWarp(state_update, warp_update_mat);
}

void Affine::estimateWarpFromPts(VectorXd &state_update, vector<uchar> &mask,
	const vector<cv::Point2f> &in_pts, const vector<cv::Point2f> &out_pts,
	const EstimatorParams &est_params){
	cv::Mat warp_mat_cv = estimateAffine(in_pts, out_pts, mask, est_params);
	state_update(0) = warp_mat_cv.at<double>(0, 2);
	state_update(1) = warp_mat_cv.at<double>(1, 2);
	state_update(2) = warp_mat_cv.at<double>(0, 0) - 1;
	state_update(3) = warp_mat_cv.at<double>(0, 1);
	state_update(4) = warp_mat_cv.at<double>(1, 0);
	state_update(5) = warp_mat_cv.at<double>(1, 1) - 1;
}

void Affine::applyWarpToCorners(Matrix24d &warped_corners, const Matrix24d &orig_corners,
	const VectorXd &ssm_state){
	getWarpFromState(warp_mat, ssm_state);
	for(unsigned int corner_id = 0; corner_id < 4; corner_id++){
		warped_corners(0, corner_id) = warp_mat(0, 0)*orig_corners(0, corner_id) + warp_mat(0, 1)*orig_corners(1, corner_id) +
			warp_mat(0, 2);
		warped_corners(1, corner_id) = warp_mat(1, 0)*orig_corners(0, corner_id) + warp_mat(1, 1)*orig_corners(1, corner_id) +
			warp_mat(1, 2);
	}
}

void Affine::applyWarpToPts(Matrix2Xd &warped_pts, const Matrix2Xd &orig_pts,
	const VectorXd &ssm_state){
	getWarpFromState(warp_mat, ssm_state);
	unsigned int n_pts = orig_pts.cols();
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		warped_pts(0, pt_id) = warp_mat(0, 0)*orig_pts(0, pt_id) + warp_mat(0, 1)*orig_pts(1, pt_id) +
			warp_mat(0, 2);
		warped_pts(1, pt_id) = warp_mat(1, 0)*orig_pts(0, pt_id) + warp_mat(1, 1)*orig_pts(1, pt_id) +
			warp_mat(1, 2);
	}
}
Vector6d Affine::geomToState(const Vector6d &geom){
	double s = geom[2], r = geom[4];
	double theta = geom[3], phi = geom[5];
	double cos_theta = cos(theta), sin_theta = sin(theta);
	double cos_phi = cos(phi), sin_phi = sin(phi);
	double ccc = cos_theta*cos_phi*cos_phi;
	double ccs = cos_theta*cos_phi*sin_phi;
	double css = cos_theta*sin_phi*sin_phi;
	double scc = sin_theta*cos_phi*cos_phi;
	double scs = sin_theta*cos_phi*sin_phi;
	double sss = sin_theta*sin_phi*sin_phi;
	Vector6d state;
	state[0] = geom[0];
	state[1] = geom[1];
	state[2] = s*(ccc + scs + r*(css - scs)) - 1;
	state[3] = s*(r*(ccs - scc) - ccs - sss);
	state[4] = s*(scc - ccs + r*(ccs + sss));
	state[5] = s*(r*(ccc + scs) - scs + css) - 1;
	return state;
}
Vector6d Affine::stateToGeom(const Vector6d &est){
	Matrix2d A;
	A << est[2] + 1, est[3], est[4], est[5] + 1;
	JacobiSVD<Matrix2d> svd(A, ComputeFullU | ComputeFullV);
	Vector2d singular_vals = svd.singularValues();
	Matrix2d S = singular_vals.asDiagonal();
	Matrix2d V = svd.matrixV().transpose();
	Matrix2d U = svd.matrixU();
	if(U.determinant() < 0){
		Matrix2d U_temp;
		U_temp << U(0, 1), U(0, 0), U(1, 1), U(1, 0);
		U = U_temp;
		Matrix2d V_temp;
		V_temp << V(0, 1), V(0, 0), V(1, 1), V(1, 0);
		V = V_temp;
		Matrix2d S_temp;
		S_temp << S(1, 1), S(1, 0), S(0, 1), S(0, 0);
		S = S_temp;
	}
	Vector6d q;
	q[0] = est[0];
	q[1] = est[1];
	q[3] = atan2(U(1, 0) * V(0, 0) + U(1, 1) * V(0, 1),
		U(0, 0) * V(0, 0) + U(0, 1) * V(0, 1));

	double phi = atan2(V(0, 1), V(0, 0));
	const double pi = 3.14159265358979323846;
	if(phi <= -pi / 2){
		double cos_phi = cos(-pi / 2);
		double sin_phi = sin(-pi / 2);
		Matrix2d R;
		R << cos_phi, -sin_phi, sin_phi, cos_phi;
		V = V * R;
		S = R.transpose()*S*R;
	}

	if(phi >= pi / 2){
		double cos_phi = cos(pi / 2);
		double sin_phi = sin(pi / 2);
		Matrix2d R;
		R << cos_phi, -sin_phi, sin_phi, cos_phi;
		V = V * R;
		S = R.transpose()*S*R;
	}
	q[2] = S(0, 0);
	q[4] = S(1, 1) / S(0, 0);
	q[5] = atan2(V(0, 1), V(0, 0));
	return q;
}

void Affine::generatePerturbation(VectorXd &perturbation){
	assert(perturbation.size() == state_size);
	if(params.pt_based_sampling){
		//! perturb three canonical points and estimate affine transformation using DLT
		Matrix23d orig_pts, perturbed_pts;
		//! use the bottom left, bottom right and top center points
		//! as canaonical points to add the random perturbations to;
		orig_pts.col(0) = init_corners.col(2);
		orig_pts.col(1) = init_corners.col(3);
		orig_pts.col(2) = (init_corners.col(0) + init_corners.col(1)) / 2.0;

		if(params.pt_based_sampling == 1){
			perturbed_pts = orig_pts;
			perturbed_pts(0, 0) += rand_dist[0](rand_gen[0]);
			perturbed_pts(1, 0) += rand_dist[1](rand_gen[1]);
			perturbed_pts(0, 1) += rand_dist[2](rand_gen[2]);
			perturbed_pts(1, 1) += rand_dist[3](rand_gen[3]);
			perturbed_pts(0, 2) += rand_dist[4](rand_gen[4]);
			perturbed_pts(1, 2) += rand_dist[5](rand_gen[5]);
		} else {
			//! different perturbation for x,y coordinates of each point
			//! followed by consistent translational perturbation to all corners
			Matrix23d rand_d;
			for(unsigned int pt_id = 0; pt_id < 3; ++pt_id){
				rand_d(0, pt_id) = rand_dist[1](rand_gen[1]);
				rand_d(1, pt_id) = rand_dist[1](rand_gen[1]);
			}
			perturbed_pts = (orig_pts + rand_d).colwise() + Vector2d(rand_dist[0](rand_gen[0]), rand_dist[0](rand_gen[0]));
		}
		Matrix3d aff_warp = utils::computeAffineDLT(orig_pts, perturbed_pts);
		getStateFromWarp(perturbation, aff_warp);
	} else{
		//! perform geometric perturbation
		Vector6d geom_perturbation;
		for(unsigned int state_id = 0; state_id < 6; state_id++){
			geom_perturbation(state_id) = rand_dist[state_id](rand_gen[state_id]);
		}
		perturbation = geomToState(geom_perturbation);
	}

}

// use Random Walk model to generate perturbed sample
void Affine::additiveRandomWalk(VectorXd &perturbed_state,
	const VectorXd &base_state){
	if(params.pt_based_sampling){
		throw mtf::utils::FunctonNotImplemented("Affine::additiveRandomWalk :: point based sampling is not implemented yet");
	} else{
		Vector6d geom_perturbation;
		for(unsigned int state_id = 0; state_id < 6; ++state_id){
			geom_perturbation(state_id) = rand_dist[state_id](rand_gen[state_id]);
		}
		Vector6d base_geom = stateToGeom(base_state);
		Vector6d perturbed_geom = base_geom + geom_perturbation;
		perturbed_state = geomToState(perturbed_geom);
	}
}

// use first order Auto Regressive model to generate perturbed sample
void Affine::additiveAutoRegression1(VectorXd &perturbed_state, VectorXd &perturbed_ar,
	const VectorXd &base_state, const VectorXd &base_ar, double a){
	if(params.pt_based_sampling){
		throw mtf::utils::FunctonNotImplemented("Affine::additiveAutoRegression1 :: point based sampling is not implemented yet");
	} else{
		Vector6d geom_perturbation;
		for(unsigned int state_id = 0; state_id < 6; ++state_id){
			geom_perturbation(state_id) = rand_dist[state_id](rand_gen[state_id]);
		}
		Vector6d base_geom = stateToGeom(base_state);
		Vector6d base_ar_geom = stateToGeom(base_ar);
		Vector6d perturbed_geom = base_geom + base_ar_geom + geom_perturbation;
		Vector6d perturbed_ar_geom = a*(perturbed_geom - base_geom);
		perturbed_state = geomToState(perturbed_geom);
		perturbed_ar = geomToState(perturbed_ar_geom);
	}
}
void Affine::compositionalRandomWalk(VectorXd &perturbed_state,
	const VectorXd &base_state){
	if(params.pt_based_sampling){
		generatePerturbation(state_perturbation);
		ProjWarpT base_warp, warp_perturbation;
		getWarpFromState(base_warp, base_state);
		getWarpFromState(warp_perturbation, state_perturbation);
		ProjWarpT perturbed_warp = base_warp * warp_perturbation;
		getStateFromWarp(perturbed_state, perturbed_warp);
	} else{
		throw mtf::utils::FunctonNotImplemented("Affine::compositionalRandomWalk :: geometric sampling is not implemented yet");

	}
}
_MTF_END_NAMESPACE


#include "exception.hh"
#include "network.hh"
#include "timer.hh"

#include <Eigen/Dense>
#include <iostream>
#include <utility>

#include <sys/resource.h>
#include <sys/time.h>

using namespace std;
using namespace Eigen;

constexpr size_t batch_size = 1;
constexpr size_t input_size = 4;

void program_body( const unsigned int num_iterations, const float epsilon )
{
  /* remove limit on stack size */
  const rlimit limits { RLIM_INFINITY, RLIM_INFINITY };
  CheckSystemCall( "setrlimit", setrlimit( RLIMIT_STACK, &limits ) );

  /* seed C RNG for Eigen random weight initialization */
  // srand( Timer::timestamp_ns() );
  srand( 0 );

  /* construct neural network on heap */
  auto nn = make_unique<Network<float, batch_size, input_size, 4, 64, 1>>();
  nn->initializeWeightsRandomly();

  srand( 10 );
  /* initialize inputs */
  Matrix<float, batch_size, input_size> input = Matrix<float, batch_size, input_size>::Random();

  /* forward prop */
  nn->apply( input );

  /* print */
  const IOFormat CleanFmt( 4, 0, ", ", "\n", "[", "]" );
  cout << "input:" << endl << input.format( CleanFmt ) << endl << endl;
  nn->print();

  vector<unsigned int> gradientLayerNums;
  vector<unsigned int> gradientWeightNums;

  unsigned int numLayers = nn->getNumLayers();

  for ( unsigned int layerNum = 0; layerNum < numLayers; layerNum++ ) {
    cout << "Layer " << layerNum << "\n";
    const unsigned int input_size_ = nn->getLayerInputSize( layerNum );
    const unsigned int output_size_ = nn->getLayerOutputSize( layerNum );
    cout << " input size: " << input_size_ << " -> "
         << "output_size: " << output_size_ << endl
         << endl;

    unsigned int numParams = nn->getNumParams( layerNum );
    vector<float> gradients( numParams, 0 );

    /* numerical gradient computation */
    for ( unsigned int paramNum = 0; paramNum < numParams; paramNum++ ) {
      float gradient = nn->calculateNumericalGradient( input, layerNum, paramNum, epsilon );
      gradients[paramNum] = gradient;
    }

    cout << "  weightGradients ";
    for ( unsigned int paramNum = 0; paramNum < numParams; paramNum++ ) {
      if ( paramNum % output_size_ == 0 )
        cout << endl << "   ";
      if ( paramNum == input_size_ * output_size_ ) {
        cout << endl;
        cout << "  biasGradients" << endl << "   ";
      }
      cout << gradients[paramNum] << " ";
    }
    cout << endl << endl << endl;
  }

  // Code to generate random no. of such gradients
  (void)num_iterations;
  // for ( unsigned int i = 0; i < num_iterations; i++ ) {
  //   unsigned int layerNum = rand() % numLayers;
  //   gradientLayerNums.emplace_back( layerNum );
  //   unsigned int numParams = nn->getNumParams( layerNum );
  //   gradientWeightNums.emplace_back( rand() % numParams );
  // }

  // for ( unsigned int i = 0; i < num_iterations; i++ ) {
  //   float gradient = nn->calculateNumericalGradient( input, gradientLayerNums[i], gradientWeightNums[i], epsilon
  //   ); cout << "Iteration " << i << ":\n"
  //        << "  Layer " << gradientLayerNums[i] << "\n"
  //        << "  Weight " << gradientWeightNums[i] << "\n"
  //        << "  Gradient " << gradient << "\n\n\n\n";
  // }
}

int main( int argc, char* argv[] )
{
  try {
    if ( argc <= 0 ) {
      abort();
    }

    if ( argc != 3 ) {
      cerr << "Usage: " << argv[0] << " NUM_ITERATIONS EPSILON\n";
      return EXIT_FAILURE;
    }

    program_body( stoi( argv[1] ), stof( argv[2] ) );

    return EXIT_SUCCESS;
  } catch ( const exception& e ) {
    cerr << e.what() << "\n";
    return EXIT_FAILURE;
  }
}


#include "eigen-dense.h"
#include <Eigen/Core>
#include <Eigen/LU>

using namespace Eigen;

template <class T>
Map< Matrix<T,Dynamic,Dynamic> > matrix(void* p, int r, int c) {
    return Map< Matrix<T,Dynamic,Dynamic> >((T*)p, r, c);
}

template <class T>
Map< Matrix<T,Dynamic,Dynamic> > matrix(const void* p, int r, int c) {
    return Map< Matrix<T,Dynamic,Dynamic> >((T*)p, r, c);
}

#define RET const char*

#define API(name,args,call) \
extern "C" RET eigen_##name args {\
    GUARD_START\
    switch (code) {\
        case 0: return name<T0>call;\
        case 1: return name<T1>call;\
        case 2: return name<T2>call;\
        case 3: return name<T3>call;\
    }\
    GUARD_END\
}

#define BINOP(name,op) \
template <class T>\
RET name(void* p, int r, int c,\
    const void* p1, int r1, int c1,\
    const void* p2, int r2, int c2)\
{\
    matrix<T>(p,r,c) = matrix<T>(p1,r1,c1) op matrix<T>(p2,r2,c2);\
    return 0;\
}\
API(name, (int code,\
    void* p, int r, int c,\
    const void* p1, int r1, int c1,\
    const void* p2, int r2, int c2), (p,r,c,p1,r1,c1,p2,r2,c2));

BINOP(add,+);
BINOP(sub,-);
BINOP(mul,*);

#define PROP(name) \
extern "C" RET __attribute__ ((noinline)) eigen_##name(int code, void* q, const void* p, int r, int c) {\
        GUARD_START\
        switch (code) {\
            case 0: *(T0*)q = matrix<T0>(p,r,c).name(); break;\
            case 1: *(T1*)q = matrix<T1>(p,r,c).name(); break;\
            case 2: *(T2*)q = matrix<T2>(p,r,c).name(); break;\
            case 3: *(T3*)q = matrix<T3>(p,r,c).name(); break;\
        }\
        GUARD_END\
    }

PROP(norm);
PROP(squaredNorm);
PROP(blueNorm);
PROP(hypotNorm);
PROP(sum);
PROP(prod);
PROP(mean);
PROP(trace);
PROP(determinant);

#define UNOP(name) \
extern "C" RET __attribute__((noinline)) eigen_##name(int code, void* p, int r, int c, const void* p1, int r1, int c1) {\
        GUARD_START\
        switch (code) {\
            case 0: matrix<T0>(p,r,c) = matrix<T0>(p1,r1,c1).name(); break;\
            case 1: matrix<T1>(p,r,c) = matrix<T1>(p1,r1,c1).name(); break;\
            case 2: matrix<T2>(p,r,c) = matrix<T2>(p1,r1,c1).name(); break;\
            case 3: matrix<T3>(p,r,c) = matrix<T3>(p1,r1,c1).name(); break;\
        }\
        GUARD_END\
    }

UNOP(inverse);
UNOP(adjoint);
UNOP(conjugate);
UNOP(diagonal);
UNOP(transpose);

extern "C" RET eigen_normalize(int code, void* p, int r, int c)
{
    GUARD_START
    switch (code) {
        case 0: matrix<T0>(p,r,c).normalize(); break;
        case 1: matrix<T1>(p,r,c).normalize(); break;
        case 2: matrix<T2>(p,r,c).normalize(); break;
        case 3: matrix<T3>(p,r,c).normalize(); break;
    }
    GUARD_END
}

extern "C" RET eigen_random(int code, void* p, int r, int c)
{
    GUARD_START
    switch (code) {
        case 0: matrix<T0>(p,r,c) = MatrixXf::Random(r,c); break;
        case 1: matrix<T1>(p,r,c) = MatrixXd::Random(r,c); break;
        case 2: matrix<T2>(p,r,c) = MatrixXcf::Random(r,c); break;
        case 3: matrix<T3>(p,r,c) = MatrixXcd::Random(r,c); break;
    }
    GUARD_END
}

extern "C" RET eigen_identity(int code, void* p, int r, int c)
{
    GUARD_START
    switch (code) {
        case 0: matrix<T0>(p,r,c) = MatrixXf::Identity(r,c); break;
        case 1: matrix<T1>(p,r,c) = MatrixXd::Identity(r,c); break;
        case 2: matrix<T2>(p,r,c) = MatrixXcf::Identity(r,c); break;
        case 3: matrix<T3>(p,r,c) = MatrixXcd::Identity(r,c); break;
    }
    GUARD_END
}


/* Boost libs/numeric/odeint/examples/openmp/lorenz_ensemble_simple.cpp

 Copyright 2013 Karsten Ahnert
 Copyright 2013 Mario Mulansky
 Copyright 2013 Pascal Germroth

 Parallelized Lorenz ensembles

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

#include <omp.h>
#include <vector>
#include <iostream>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/openmp/openmp.hpp>
#include "point_type.hpp"

using namespace std;

typedef vector<double> vector_type;
typedef point<double, 3> point_type;
typedef vector<point_type> state_type;

const double sigma = 10.0;
const double b = 8.0 / 3.0;

struct sys_func {
    const vector_type &R;
    sys_func( const vector_type &_R ) : R( _R ) { }

    void operator()( const state_type &x , state_type &dxdt , double t ) const {
        const size_t n = x.size();
#       pragma omp parallel for
        for(size_t i = 0 ; i < n ; i++) {
            const point_type &xi = x[i];
            point_type &dxdti = dxdt[i];
            dxdti[0] = -sigma * (xi[0] - xi[1]);
            dxdti[1] = R[i] * xi[0] - xi[1] - xi[0] * xi[2];
            dxdti[2] = -b * xi[2] + xi[0] * xi[1];
        }
    }
};

int main() {
    using namespace boost::numeric::odeint;

    const size_t n = 1024;
    vector_type R(n);
    const double Rmin = 0.1, Rmax = 50.0;
#   pragma omp parallel for
    for(size_t i = 0 ; i < n ; i++)
        R[i] = Rmin + (Rmax - Rmin) / (n - 1) * i;

    state_type X(n, point_type(10, 10, 10));

    typedef runge_kutta4<
        state_type, double,
        state_type, double,
        openmp_range_algebra
    > stepper;

    const double t_max = 10.0, dt = 0.01;

    integrate_const(
        stepper(),
        sys_func(R), X,
        0.0, t_max, dt
    );

    copy( X.begin(), X.end(), ostream_iterator<point_type>(cout, "\n") );

    return 0;
}


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

#include <algorithm>
#include <cmath>
#include <iostream>

#include <Eigen/Dense>
using namespace Eigen;

#include "functions.hpp"
#include "layers.hpp"
#include "lstm/network.hpp"

namespace nn {

struct LSTMState {
    // hidden state
    MatrixXf h;
    // cell state
    MatrixXf c;
};

class LSTMCell {
  private:
    int batch_size;
    int hidden_size;
    int embedding_dim;

    Dense i2h;
    Dense h2h;

  protected:
    LSTMState state;

    friend class LSTMNetwork;

  public:
    explicit LSTMCell(const int &, const int &, const int &);

    MatrixXf &operator()(const MatrixXf &);

    MatrixXf &forward(const MatrixXf &);

    // MatrixXf backward(const MatrixXf &, const MatrixXf &);
};

/**
 * LSTMCell Constructor.
 *
 * @param hidden_size the size of the hidden state and cell state.
 * @param batch_size the size of batch used during training (for vectorization purposes).
 */
LSTMCell::LSTMCell(const int &hidden_size, const int &embedding_dim, const int &batch_size)
    : i2h(Dense(embedding_dim, 4 * hidden_size)), h2h(Dense(hidden_size, 4 * hidden_size)) {

    this->hidden_size = hidden_size;
    this->embedding_dim = embedding_dim;
    this->batch_size = batch_size;

    this->state = LSTMState{MatrixXf(batch_size, hidden_size).setRandom() * F::glorot_uniform(batch_size, hidden_size),
                            MatrixXf(batch_size, hidden_size).setRandom() * F::glorot_uniform(batch_size, hidden_size)};
}

MatrixXf &LSTMCell::operator()(const MatrixXf &xt) {
    return forward(xt);
}

/**
 * LSTMCell Forward Pass.
 *
 * @param xt the input vector at time-step t.
 * @returns the next hidden state for input into next lstm layer.
 */
MatrixXf &LSTMCell::forward(const MatrixXf &xt) {
    // i2h + h2h = [it_pre, ft_pre, ot_pre, x_pre]
    MatrixXf preactivations = i2h(xt) + h2h(state.h);
    // all pre sigmoid gates chunk
    MatrixXf pre_sigmoid_chunk = preactivations.block(0, 0, batch_size, 3 * hidden_size);
    // compute sigmoid on gates chunk
    MatrixXf all_gates = F::sigmoid(pre_sigmoid_chunk);
    // compute c_in (x_transform) i.e. information vector
    MatrixXf x_pre = preactivations.block(0, 3 * hidden_size, batch_size, hidden_size);
    MatrixXf x_transform = F::tanh(x_pre);
    // single out all the gates
    MatrixXf it = all_gates.block(0, 0, batch_size, hidden_size);
    MatrixXf ft = all_gates.block(0, hidden_size, batch_size, hidden_size);
    MatrixXf ot = all_gates.block(0, 2 * hidden_size, batch_size, hidden_size);
    // update cell state
    MatrixXf c_forget = ft.cwiseProduct(state.c);
    MatrixXf c_input = it.cwiseProduct(x_transform);
    state.c = c_forget + c_input;
    // compute next hidden state
    MatrixXf c_transform = F::tanh(state.c);
    state.h = ot.cwiseProduct(c_transform);
    return state.h;
}

/**
 * LSTMCell Backward Pass.
 *
 * @param inputs the input vector given at time-step t.
 * @param gradients the gradients from upper layers computed using chain rule.
 * @returns the output i.e. the hidden state at time-step t.
 */
// MatrixXf LSTMCell::backward(const MatrixXf &inputs, const MatrixXf &gradients) {
// }
} // namespace nn


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"
#include <boost/test/unit_test.hpp>
#include <ql/quotes/simplequote.hpp>
#include <qle/quotes/logquote.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;
using QuantExt::LogQuote;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(LogQuoteTest)

BOOST_AUTO_TEST_CASE(testLogQuote) {

    BOOST_TEST_MESSAGE("Testing QuantExt::LogQuote...");
    boost::shared_ptr<SimpleQuote> quote(new QuantLib::SimpleQuote(1.0));
    Handle<Quote> qh(quote);
    Handle<Quote> logQuote(boost::shared_ptr<Quote>(new LogQuote(qh)));

    BOOST_CHECK_EQUAL(logQuote->value(), std::log(quote->value()));

    quote->setValue(2.0);
    BOOST_CHECK_EQUAL(logQuote->value(), std::log(quote->value()));

    quote->setValue(3.0);
    BOOST_CHECK_EQUAL(logQuote->value(), std::log(quote->value()));

    quote->setValue(123.0);
    BOOST_CHECK_EQUAL(logQuote->value(), std::log(quote->value()));

    // LogQuote should throw when a negative value is set
    BOOST_CHECK_THROW(quote->setValue(-1.0), std::exception);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <ros/ros.h>
#include <string>
#include <Eigen/Core>
#include "inekf_msgs/KinematicsArray.h"
#include "sensor_msgs/JointState.h"

#include <iostream>
#include <fstream>

// forward kinematics from FROST
#include "kin/H_Body_to_FrontLeftFoot.h"
#include "kin/H_Body_to_FrontRightFoot.h"
#include "kin/H_Body_to_HindLeftFoot.h"
#include "kin/H_Body_to_HindRightFoot.h"
#include "kin/Jb_Body_to_FrontLeftFoot.h"
#include "kin/Jb_Body_to_FrontRightFoot.h"
#include "kin/Jb_Body_to_HindLeftFoot.h"
#include "kin/Jb_Body_to_HindRightFoot.h"

#include <lcm/lcm-cpp.hpp>

namespace cheetah_inekf_ros {
  template <unsigned int ENCODER_DIM>
  class KinematicsPublisher {
  public:

    KinematicsPublisher(ros::NodeHandle n) : n_(n) {
      // Create private node handle
      ros::NodeHandle nh("~");
      std::string joint_state_topic, kinematics_topic;
      nh.param<std::string>("joint_state_topic", joint_state_topic, "joint_states");
      nh.param<std::string>("kinematics_topic", kinematics_topic, "kinematics");
      nh.param<std::string>("imu_frame_id", imu_frame_id_, "imu_frame_id");
      joint_state_sub_ = nh.subscribe(joint_state_topic, 1000, &KinematicsPublisher::joint_state_ros_callback, this);
      kinematic_pub_ = n_.advertise<inekf_msgs::KinematicsArray>(kinematics_topic, 10);
      double encoder_std, kinematic_prior_orientation_std, kinematic_prior_position_std;
      nh.param<double>("encoder_std", encoder_std, 0.0174533); // 1 deg std
      nh.param<double>("kinematic_prior_orientation_std", kinematic_prior_orientation_std, 0.174533); // 10 deg std
      nh.param<double>("kinematic_prior_position_std", kinematic_prior_position_std, 0.05); // 5cm std

      cov_encoders_ = encoder_std*encoder_std*Eigen::Matrix<double,ENCODER_DIM,ENCODER_DIM>::Identity();
      cov_prior_ = Eigen::Matrix<double,6,6>::Identity();
      cov_prior_.block<3,3>(0,0) = kinematic_prior_orientation_std*kinematic_prior_orientation_std*Eigen::Matrix<double,3,3>::Identity();
      cov_prior_.block<3,3>(3,3) = kinematic_prior_position_std*kinematic_prior_position_std*Eigen::Matrix<double,3,3>::Identity();

    }

    ~KinematicsPublisher(){

    }

    static inekf_msgs::KinematicsArray callback_handler(std_msgs::Header & header,  const Eigen::Matrix<double, ENCODER_DIM, 1> & encoders,
                                                        const Eigen::Matrix<double, ENCODER_DIM, ENCODER_DIM> & cov_encoder,
                                                        const Eigen::Matrix<double, 6,6> & cov_prior) {
      inekf_msgs::Kinematics front_left_foot, front_right_foot, hind_left_foot, hind_right_foot;

      Eigen::Matrix<double,3,1> p_offset; p_offset << 0, -0.02, 0; // TODO: tune
      Eigen::Matrix<double,4,4> H_offset = Eigen::Matrix<double,4,4>::Identity();
      H_offset.block<3,1>(0,3) = p_offset;

      Eigen::Matrix<double,4,4> H_FL = H_Body_to_FrontLeftFoot(encoders)*H_offset;
      Eigen::Matrix<double,4,4> H_FR = H_Body_to_FrontRightFoot(encoders)*H_offset;
      Eigen::Matrix<double,4,4> H_HL = H_Body_to_HindLeftFoot(encoders)*H_offset;
      Eigen::Matrix<double,4,4> H_HR = H_Body_to_HindRightFoot(encoders)*H_offset;

      Eigen::Quaternion<double> q_FL(H_FL.block<3,3>(0,0)); q_FL.normalize();
      Eigen::Quaternion<double> q_FR(H_FR.block<3,3>(0,0)); q_FR.normalize();
      Eigen::Quaternion<double> q_HL(H_HL.block<3,3>(0,0)); q_HL.normalize();
      Eigen::Quaternion<double> q_HR(H_HR.block<3,3>(0,0)); q_HR.normalize();

      Eigen::Matrix<double,6,ENCODER_DIM> J_FL = Jb_Body_to_FrontLeftFoot(encoders); // body manipulator Jacobian
      Eigen::Matrix<double,6,ENCODER_DIM> J_FR = Jb_Body_to_FrontRightFoot(encoders); // body manipulator Jacobian
      Eigen::Matrix<double,6,ENCODER_DIM> J_HL = Jb_Body_to_HindLeftFoot(encoders); // body manipulator Jacobian
      Eigen::Matrix<double,6,ENCODER_DIM> J_HR = Jb_Body_to_HindRightFoot(encoders); // body manipulator Jacobian

      Eigen::Matrix<double,6,6> cov_FL = J_FL * cov_encoder * J_FL.transpose() + cov_prior;
      Eigen::Matrix<double,6,6> cov_FR = J_FR * cov_encoder * J_FR.transpose() + cov_prior;
      Eigen::Matrix<double,6,6> cov_HL = J_HL * cov_encoder * J_HL.transpose() + cov_prior;
      Eigen::Matrix<double,6,6> cov_HR = J_HR * cov_encoder * J_HR.transpose() + cov_prior;

      front_left_foot.id = 1;
      front_left_foot.pose.pose.orientation.w = q_FL.w();
      front_left_foot.pose.pose.orientation.x = q_FL.x();
      front_left_foot.pose.pose.orientation.y = q_FL.y();
      front_left_foot.pose.pose.orientation.z = q_FL.z();
      front_left_foot.pose.pose.position.x = H_FL(0,3);
      front_left_foot.pose.pose.position.y = H_FL(1,3);
      front_left_foot.pose.pose.position.z = H_FL(2,3);
      for (int i=0; i<6; ++i) {
        for (int j=0; j<6; ++j) {
          front_left_foot.pose.covariance[i*6+j] = cov_FL(i,j);
        }
      }
      front_right_foot.id = 0;
      front_right_foot.pose.pose.orientation.w = q_FR.w();
      front_right_foot.pose.pose.orientation.x = q_FR.x();
      front_right_foot.pose.pose.orientation.y = q_FR.y();
      front_right_foot.pose.pose.orientation.z = q_FR.z();
      front_right_foot.pose.pose.position.x = H_FR(0,3);
      front_right_foot.pose.pose.position.y = H_FR(1,3);
      front_right_foot.pose.pose.position.z = H_FR(2,3);
      for (int i=0; i<6; ++i) {
        for (int j=0; j<6; ++j) {
          front_right_foot.pose.covariance[i*6+j] = cov_FR(i,j);
        }
      }

      hind_left_foot.id = 3;
      hind_left_foot.pose.pose.orientation.w = q_HL.w();
      hind_left_foot.pose.pose.orientation.x = q_HL.x();
      hind_left_foot.pose.pose.orientation.y = q_HL.y();
      hind_left_foot.pose.pose.orientation.z = q_HL.z();
      hind_left_foot.pose.pose.position.x = H_HL(0,3);
      hind_left_foot.pose.pose.position.y = H_HL(1,3);
      hind_left_foot.pose.pose.position.z = H_HL(2,3);
      for (int i=0; i<6; ++i) {
        for (int j=0; j<6; ++j) {
          hind_left_foot.pose.covariance[i*6+j] = cov_HL(i,j);
        }
      }

      hind_right_foot.id = 2;
      hind_right_foot.pose.pose.orientation.w = q_HR.w();
      hind_right_foot.pose.pose.orientation.x = q_HR.x();
      hind_right_foot.pose.pose.orientation.y = q_HR.y();
      hind_right_foot.pose.pose.orientation.z = q_HR.z();
      hind_right_foot.pose.pose.position.x = H_HR(0,3);
      hind_right_foot.pose.pose.position.y = H_HR(1,3);
      hind_right_foot.pose.pose.position.z = H_HR(2,3);
      for (int i=0; i<6; ++i) {
        for (int j=0; j<6; ++j) {
          hind_right_foot.pose.covariance[i*6+j] = cov_HR(i,j);
        }
      }

      inekf_msgs::KinematicsArray kinematics_msg;
      kinematics_msg.frames.push_back(front_right_foot);
      kinematics_msg.frames.push_back(front_left_foot);
      kinematics_msg.frames.push_back(hind_right_foot);
      kinematics_msg.frames.push_back(hind_left_foot);
      // kinematic_pub_.publish(kinematics_msg);

      // std::ofstream fLogCSV;
      // fLogCSV.open("/media/curly_ssd_justin/code/minicheetah-perception/fk_log.csv", std::ios::app);
      // fLogCSV<<front_right_foot.pose.pose.position.x<<","<<front_right_foot.pose.pose.position.y<<","<<front_right_foot.pose.pose.position.z<<","\
      // <<front_left_foot.pose.pose.position.x<<","<<front_left_foot.pose.pose.position.y<<","<<front_left_foot.pose.pose.position.z<<","\
      // <<hind_right_foot.pose.pose.position.x<<","<<hind_right_foot.pose.pose.position.y<<","<<hind_right_foot.pose.pose.position.z<<","\
      // <<hind_left_foot.pose.pose.position.x<<","<<hind_left_foot.pose.pose.position.y<<","<<hind_left_foot.pose.pose.position.z<<"\n";
      // fLogCSV.flush();
      // fLogCSV.close();

      return kinematics_msg;

    }

  private:
    ros::NodeHandle n_;
    ros::Subscriber joint_state_sub_;
    ros::Publisher kinematic_pub_;

    std::string imu_frame_id_;
    //const unsigned int encoder_dim_;
    Eigen::Matrix<double,ENCODER_DIM,ENCODER_DIM> cov_encoders_;
    Eigen::Matrix<double,6,6> cov_prior_;

    void joint_state_ros_callback(const sensor_msgs::JointState::ConstPtr msg) {
      std_msgs::Header header = msg->header;
      header.frame_id = imu_frame_id_;
      Eigen::Matrix<double,ENCODER_DIM,1> encoders;
      for (int i=0; i<ENCODER_DIM; ++i) {
        encoders(i) = msg->position[i];
      }
      auto kinematics_msg =  callback_handler(header, encoders, cov_encoders_, cov_prior_);
      kinematic_pub_.publish(kinematics_msg);

    }

  };

}


#include <cmath>
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <algorithm>
#include <boost/range/algorithm.hpp>
#include <boost/integer/common_factor_rt.hpp>
#define MAX 100000000

using namespace std;

int A, B;

int main()
{
    while (cin >> A >> B)
    {
        if (abs(A - B) % 2 == 0)
        {
            cout << abs(A - B) / 2 + min(A, B) << endl;
        }
        else
        {
            cout << "IMPOSSIBLE" << endl;
        }
    }

    return 0;
}


#include "PhysicsTools/Utilities/interface/Likelihood.h"
#include "PhysicsTools/Utilities/interface/BreitWigner.h"
#include "PhysicsTools/Utilities/interface/RootMinuitCommands.h"
#include "PhysicsTools/Utilities/interface/RootMinuit.h"
#include "PhysicsTools/Utilities/interface/Parameter.h"
#include "PhysicsTools/Utilities/interface/rootTf1.h"
#include "PhysicsTools/Utilities/interface/rootPlot.h"
#include "TFile.h"
#include "TH1.h"
#include "TF1.h"
#include "TCanvas.h"
#include "TROOT.h"
#include <boost/shared_ptr.hpp>
#include <iostream>
#include "PhysicsTools/Utilities/interface/Operations.h"
//using namespace std;
//using namespace boost;

int main() {
  gROOT->SetStyle("Plain");
  typedef funct::BreitWigner PDF;
  typedef std::vector<double> Sample;
  typedef funct::Product<funct::Parameter, PDF>::type FitFunction;
  typedef fit::Likelihood<Sample, FitFunction, funct::Parameter> Likelihood;
  try {
    fit::RootMinuitCommands<Likelihood> commands("PhysicsTools/Utilities/test/testZMassFitLikelihood.txt");

    const char* kYield = "Yield";
    const char* kMass = "Mass";
    const char* kGamma = "Gamma";

    funct::Parameter yield(kYield, commands.par(kYield));
    funct::Parameter mass(kMass, commands.par(kMass));
    funct::Parameter gamma(kGamma, commands.par(kGamma));
    funct::BreitWigner bw(mass, gamma);

    PDF pdf = bw;
    FitFunction f = yield * pdf;
    TF1 startFun = root::tf1("startFun", f, 0, 200, yield, mass, gamma);
    TH1D histo("histo", "Z mass (GeV/c)", 200, 0, 200);
    Sample sample;
    sample.reserve(yield);
    for (unsigned int i = 0; i < yield; ++i) {
      double m = startFun.GetRandom();
      histo.Fill(m);
      sample.push_back(m);
    }
    TCanvas canvas;
    startFun.Draw();
    canvas.SaveAs("breitWigner.eps");
    histo.Draw();
    canvas.SaveAs("breitWignerHisto.eps");
    startFun.Draw("same");
    canvas.SaveAs("breitWignerHistoFun.eps");
    histo.Draw("e");
    startFun.Draw("same");

    Likelihood like(sample, f, yield);
    fit::RootMinuit<Likelihood> minuit(like, true);
    commands.add(minuit, yield);
    commands.add(minuit, mass);
    commands.add(minuit, gamma);
    commands.run(minuit);
    ROOT::Math::SMatrix<double, 3, 3, ROOT::Math::MatRepSym<double, 3> > err;
    minuit.getErrorMatrix(err);
    std::cout << "error matrix:" << std::endl;
    for (size_t i = 0; i < 3; ++i) {
      for (size_t j = 0; j < 3; ++j) {
        std::cout << err(i, j) << "\t";
      }
      std::cout << std::endl;
    }
    root::plot<FitFunction>("breitWignerHistoFunFit.eps", histo, f, 80, 120, yield, mass, gamma);
  } catch (std::exception& err) {
    std::cerr << "Exception caught:\n" << err.what() << std::endl;
    return 1;
  }

  return 0;
}


/*
	Satellite Attitude Dynamics Stepper

	@author		:	siddharth deore
	@licence	:	MIT
*/

#include "Satellite.h"
#include <iostream>
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include "ode_wrapper.h"

#include <fstream>
#include <windows.h>
HANDLE hOut= GetStdHandle(STD_OUTPUT_HANDLE);;
void clearScreen()
{
	COORD Position;

	Position.X = 0;
	Position.Y = 0;
	SetConsoleCursorPosition(hOut, Position);
}

// set bool visible = 0 - invisible, bool visible = 1 - visible
void setcursor(bool visible, DWORD size)
{
	if (size == 0)
	{
		// default cursor size Changing to numbers from 1 to 20, decreases cursor width
		size = 20;
	}
	CONSOLE_CURSOR_INFO lpCursor;
	lpCursor.bVisible = visible;
	lpCursor.dwSize = size;
	SetConsoleCursorInfo(hOut, &lpCursor);
}
namespace odeint = boost::numeric::odeint;

// Static variables common to all instance
double Satellite::Ixx = 1.0;
double Satellite::Iyy = 1.0;
double Satellite::Izz = 1.0;

double Satellite::Kp = 0.0;
double Satellite::Kd = 1.0;

double Satellite::qd[4] = { 1.0,0.0,0.0,0.0 };

void Satellite::Satelliite() {
	state_type X = { { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  } }; // initial conditions
}

int Satellite::setQuaternion(double q0, double q1, double q2, double q3) {
	this->X[0] = q0;
	this->X[1] = q1;
	this->X[2] = q2;
	this->X[3] = q3;
	return 0;
}

int Satellite::setAngulerVeolcities(double wx, double wy, double wz) {
	this->X[4] = wx;
	this->X[5] = wy;
	this->X[6] = wz;
	return 0;
}

int Satellite::setState(double q0, double q1, double q2, double q3, double wx, double wy, double wz) {
	setQuaternion(q0, q1, q2, q3);
	setAngulerVeolcities(wx, wy, wz);
	return 0;
}

int Satellite::getState(double& q0, double& q1, double& q2, double& q3, double& wx, double& wy, double& wz) {
	q0 = this->X[0];
	q1 = this->X[1];
	q2 = this->X[2];
	q3 = this->X[3];
	wx = this->X[5];
	wy = this->X[6];
	wz = this->X[7];
	return 0;
}

void Satellite::dynamics(const state_type& x, state_type& dxdt, const double t) {
	double q0 = x[0];
	double q1 = x[1];
	double q2 = x[2];
	double q3 = x[3];
	double norm = normalizeQuaternions(q0, q1, q2, q3);
	dxdt[0] = 0.5 * (q1 * x[6] - q2 * x[5] + q3 * x[4]);
	dxdt[1] = 0.5 * (q2 * x[4] - q0 * x[6] + q3 * x[5]);
	dxdt[2] = 0.5 * (q0 * x[5] - q1 * x[4] + q3 * x[6]);
	dxdt[3] = 0.5 * (-q0 * x[4] - q1 * x[5] - q2 * x[6]);

	qe[0] = q0 * this->qd[3] + q1 * this->qd[2] - q2 * this->qd[1] - q3 * this->qd[0];
	qe[1] = q2 * this->qd[0] - q0 * this->qd[2] + q1 * this->qd[3] - q3 * this->qd[1];
	qe[2] = q0 * this->qd[1] - q1 * this->qd[0] + q2 * this->qd[3] - q3 * this->qd[2];
	qe[3] = q0 * this->qd[0] + q1 * this->qd[1] + q2 * this->qd[2] + q3 * this->qd[3];
	double PD[3] = {
					this.Kp * qe[0] * qe[3] + this.Kd * x[4],
					this.Kp * qe[1] * qe[3] + this.Kd * x[5],
					this.Kp * qe[2] * qe[3] + this.Kd * x[6]
				};

	dxdt[4] = ((this->Iyy - this->Izz) * x[5] * x[6] - PD[0]) / this->Ixx;
	dxdt[5] = ((this->Izz - this->Ixx) * x[6] * x[4] - PD[1]) / this->Iyy;
	dxdt[6] = ((this->Ixx - this->Iyy) * x[4] * x[5] - PD[2]) / this->Izz;
}

double Satellite::normalizeQuaternions(double& _q0, double& _q1, double& _q2, double& _q3)
{
	double norm = std::sqrt(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3);
	_q0 /= norm;
	_q1 /= norm;
	_q2 /= norm;
	_q3 /= norm;
	return norm;
}

int Satellite::step(double final_time, double dt, state_type& new_state)
{
	odeint::integrate(
		make_ode_wrapper(Satellite(), &Satellite::dynamics), // ODE funtion
		X,				// Initial state
		0.0,			// initial time
		final_time,		// final time
		dt,				// timestep
		make_observer_wrapper(Satellite(), &Satellite::write_state)// observer function
	);

	// return value to caller
	new_state = X;
	return 0;
}

void Satellite::setTargetQuaternion(double q0, double q1, double q2, double q3)
{
	this->qd[0] = q0;
	this->qd[1] = q1;
	this->qd[2] = q2;
	this->qd[3] = q3;
}

void Satellite::write_state(const state_type& state, const double t) {

	clearScreen(); // clear screen
	setcursor(0, 1); // Hide cursor

	std::cout << char(218) << std::string(83, char(196))<< char(191) << std::endl;
	std::cout<<"|     time    |    q0   |    q1   |    q2   |    q3   |    wx   |    wy   |    wz   |" << std::endl;
	std::cout << char(195) << std::string(83, char(196)) << char(180) << std::endl;
	std::cout << "|" << std::setw(12) << std::fixed << std::setprecision(4) << t << " |";
	std::cout << std::setw(8) << state[0] << " |";
	std::cout << std::setw(8) << state[1] << " |";
	std::cout << std::setw(8) << state[2] << " |";
	std::cout << std::setw(8) << state[3] << " |";
	std::cout << std::setw(8) << state[4] << " |";
	std::cout << std::setw(8) << state[5] << " |";
	std::cout << std::setw(8) << state[6] << " |" << std::endl;
	std::cout << char(192) << std::string(83, char(196)) << char(217) << std::endl;

	//Progress bar
	std::cout << std::string(int(t / 10000 * 85), char(178)) << std::string(int(85 - t / 10000 * 85), char(176)) << std::endl;

	// write to file
	std::string s =
		std::to_string(t) +		   ", " +
		std::to_string(state[0]) + ", " +
		std::to_string(state[1]) + ", " +
		std::to_string(state[2]) + ", " +
		std::to_string(state[3]) + ", " +
		std::to_string(state[4]) + ", " +
		std::to_string(state[5]) + ", " +
		std::to_string(state[6]) + "\n";
	std::ofstream outfile;

	outfile.open("results.csv", std::ios_base::app); // append instead of overwrite
	outfile << s;
	outfile.close();
	//

}

void Satellite::setInnertia(double Ix, double Iy, double Iz) {
	this->Ixx = Ix;
	this->Iyy = Iy;
	this->Izz = Iz;
}

void Satellite::setControllerGains(double Kp, double Kd)
{
	this->Kp = Kp;
	this->Kd = Kd;
}


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/lazy.hpp>
#include <mpllibs/metamonad/metafunction.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/eval_if.hpp>

#include <iostream>

using boost::mpl::plus;
using boost::mpl::minus;
using boost::mpl::less;
using boost::mpl::eval_if;
using boost::mpl::int_;

using mpllibs::metamonad::lazy;

// Without lazy
///////////////

template <class N>
struct fib_without_lazy;

MPLLIBS_METAFUNCTION(fib_without_lazy_helper, (N))
((
  plus<
    typename fib_without_lazy<typename minus<N, int_<1> >::type>::type,
    typename fib_without_lazy<typename minus<N, int_<2> >::type>::type
  >
));

MPLLIBS_METAFUNCTION(fib_without_lazy, (N))
((
  eval_if<
    typename less<N, int_<2> >::type,
    int_<1>,
    fib_without_lazy_helper<N>
  >
));

// With lazy
////////////

MPLLIBS_METAFUNCTION(fib, (N))
((
  eval_if<
    typename less<N, int_<2> >::type,
    int_<1>,
    lazy<plus<fib<minus<N, int_<1> > >, fib<minus<N, int_<2> > > > >
  >
));

///////////

int main()
{
  using std::endl;

  std::cout
    << "With lazy: " << endl
    << "fib(0) == " << fib_without_lazy<int_<0> >::type::value << endl
    << "fib(1) == " << fib_without_lazy<int_<1> >::type::value << endl
    << "fib(2) == " << fib_without_lazy<int_<2> >::type::value << endl
    << "fib(3) == " << fib_without_lazy<int_<3> >::type::value << endl
    << "fib(4) == " << fib_without_lazy<int_<4> >::type::value << endl
    << "fib(5) == " << fib_without_lazy<int_<5> >::type::value << endl
    << endl
    << "With lazy: " << endl
    << "fib(0) == " << fib<int_<0> >::type::value << endl
    << "fib(1) == " << fib<int_<1> >::type::value << endl
    << "fib(2) == " << fib<int_<2> >::type::value << endl
    << "fib(3) == " << fib<int_<3> >::type::value << endl
    << "fib(4) == " << fib<int_<4> >::type::value << endl
    << "fib(5) == " << fib<int_<5> >::type::value << endl;
}


#ifndef NETWORK
#define NETWORK

#include <vector>
#include <iostream>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/function.hpp>

using namespace boost::numeric::ublas;

typedef boost::function<vector<double>* (const vector<double>&)> Function;

class Network
{
	double												learningRate;
	int 													nbLayers;
	std::vector<vector<double>*>	errors;
	std::vector<vector<double>*>	inputs;
	std::vector<vector<double>*>	outputs;
	std::vector<matrix<double>*>	weights;
	std::vector<vector<double>*>	biases;

	std::vector<Function*>				functions;
	std::vector<Function*>				derivates;

public:
	Network(const std::vector<int>&, double=0.01);
	virtual ~Network();

	void						fit(matrix<double>&, matrix<double>&, const int=100);
	vector<double> 	*predict(const vector<double>&);

	friend std::ostream& operator<<(std::ostream&, const Network&);

private:

	void 						initializeNetwork(const std::vector<int>&);
	void 						updateWeights(vector<double>*, vector<double>*);
	vector<double> 	*feedForward(const vector<double>&);
	vector<double> 	*row2vec(const matrix_row<matrix<double> >&) const;

	static vector<double>	*sigmoid(const vector<double>&);
	static vector<double> *sigmoidPrime(const vector<double>&);
	static vector<double>	*identity(const vector<double>&);
	static vector<double>	*identityPrime(const vector<double>&);
};

#endif


#include <ayla/geometry/intersection.hpp>
#include <ayla/geometry/vector.hpp>

#include <glm/gtx/rotate_vector.hpp>
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE( ayla )
BOOST_AUTO_TEST_SUITE( intersection )

BOOST_AUTO_TEST_CASE( ray_aab_intersection ) {
	AxisAlignedBox aab( glm::vec3( 0.0, 0.0, 0.0 ), 5.0, 5.0, 5.0 );
	Float d;

	for (int i = -5; i < 5; i++) {
		Ray ray( glm::vec3( 0.0, 0.0, 0.0 ), glm::normalize(glm::vec3( i, 1.0, 0.0 )) );
		BOOST_CHECK( ayla::intersection::rayAab( ray, aab, d ) );
	}

	for (int i = -5; i < 5; i++) {
		Ray ray(glm::vec3(0.0, 5.1, 0.0), glm::normalize(glm::vec3(i, 1.0, 0.0)));
		BOOST_CHECK( ! ayla::intersection::rayAab( ray, aab, d ) );
	}

	Ray rayA( glm::vec3( 0.0, 6.0, 0.0 ), glm::vec3( 1.0, 0.0, 0.0)  );
	BOOST_CHECK( ! ayla::intersection::rayAab( rayA, aab, d ) );

	Ray rayB(glm::vec3(0.0, 6.0, 0.0), glm::normalize(glm::vec3(1.0, -1.0, 0.0)));
	BOOST_CHECK( ayla::intersection::rayAab( rayB, aab, d ) );

	Ray rayC(glm::vec3(10.0, 10.0, 10.0), glm::normalize(glm::vec3(-1.0, -1.0, -1.0)));
	BOOST_CHECK(  ayla::intersection::rayAab( rayC, aab, d ) );
}

BOOST_AUTO_TEST_CASE( ray_box_intersection ) {
	Box box(glm::vec3(0.0, 0.0, 0.0), 5.0, 5.0, 5.0);
	Float d;

	RigidTransform transform = RigidTransform::getIdentity();

	// Testing first when the Box is aligned. Then rotating 90 degrees around the Y axis.
	for (int k = 0; k < 3; k++) {
		for (int i = -5; i < 5; i++) {
			Ray ray(glm::vec3(0.0, 0.0, 0.0), glm::normalize(glm::vec3(i, 1.0, 0.0)));
			BOOST_CHECK( ayla::intersection::rayBox( ray, box, d ) );
		}

		for (int i = -5; i < 5; i++) {
			Ray ray(glm::vec3(0.0, 5.1, 0.0), glm::normalize(glm::vec3(i, 1.0, 0.0)));
			BOOST_CHECK( ! ayla::intersection::rayBox( ray, box, d ) );
		}

		Ray rayA( glm::vec3( 0.0, 6.0, 0.0 ), glm::vec3( 1.0, 0.0, 0.0)  );
		BOOST_CHECK( ! ayla::intersection::rayBox( rayA, box, d ) );

		Ray rayB(glm::vec3(0.0, 6.0, 0.0), glm::normalize(glm::vec3(1.0, -1.0, 0.0)));
		BOOST_CHECK( ayla::intersection::rayBox( rayB, box, d ) );

		Ray rayC(glm::vec3(10.0, 10.0, 10.0), glm::normalize(glm::vec3(-1.0, -1.0, -1.0)));
		BOOST_CHECK(  ayla::intersection::rayBox( rayC, box, d ) );

		transform.rotate( glm::pi<Float>() / 2.0f, glm::vec3( 0.0, 1.0, 0.0 ) );
		box.rotateBase( transform.getRotationMatrix() );
	}

	transform.rotate(glm::pi<Float>() / 4.0f, glm::vec3(0.0, 1.0, 0.0));
	box.rotateBase( transform.getRotationMatrix() );

	Ray rayA( glm::vec3( 5.0, 5.0, 5.0 ), glm::vec3( 1.0, 0.0, 0.0 ) );
	BOOST_CHECK( ! ayla::intersection::rayBox( rayA, box, d ) );
}

BOOST_AUTO_TEST_CASE( ray_sphere_intersection ) {
	Sphere sphere( glm::vec3( 0.0, 0.0, 0.0 ), 10.0 );
	Float d;

	for (int i = 0; i < 10; i++) {
		Ray ray(glm::vec3(20.0, i, 0.0), glm::vec3(-1.0, 0.0, 0.0));
		BOOST_CHECK(ayla::intersection::raySphere( ray, sphere, d));
	}

	for (int i = -10; i < 10; i++) {
		for (int j = -10; j < 10; j++) {
			//inside out
			Ray rayA(glm::vec3(0.0, 0.0, 0.0), glm::normalize(glm::vec3(i, j, 10.0)));
			Ray rayB(glm::vec3(0.0, 0.0, 0.0), glm::normalize(glm::vec3(i, 10.0, j)));
			Ray rayC(glm::vec3(0.0, 0.0, 0.0), glm::normalize(glm::vec3(10.0, i, j)));
			BOOST_CHECK( ayla::intersection::raySphere(rayA, sphere, d ) );
			BOOST_CHECK( ayla::intersection::raySphere(rayB, sphere, d ) );
			BOOST_CHECK( ayla::intersection::raySphere(rayC, sphere, d ) );

			//outside in
			Ray rayD(glm::vec3(i, j, 10.0), glm::normalize(-glm::vec3(i, j, 10.0)));
			Ray rayE(glm::vec3(i, 10.0, j), glm::normalize(-glm::vec3(i, 10.0, j)));
			Ray rayF(glm::vec3(10.0, i, j), glm::normalize(-glm::vec3(10.0, i, j)));
			BOOST_CHECK( ayla::intersection::raySphere(rayD, sphere, d ) );
			BOOST_CHECK( ayla::intersection::raySphere(rayE, sphere, d ) );
			BOOST_CHECK( ayla::intersection::raySphere(rayF, sphere, d ) );

			//so if we change the ray direction it shouldn't intersect
			Ray rayG(glm::vec3(i, j, 11.0), glm::normalize(glm::vec3(i, j, 10.0)));
			Ray rayH(glm::vec3(i, 11.0, j), glm::normalize(glm::vec3(i, 10.0, j)));
			Ray rayI(glm::vec3(11.0, i, j), glm::normalize(glm::vec3(10.0, i, j)));
			BOOST_CHECK( ! ayla::intersection::raySphere(rayG, sphere, d ) );
			BOOST_CHECK( ! ayla::intersection::raySphere(rayH, sphere, d ) );
			BOOST_CHECK( ! ayla::intersection::raySphere(rayI, sphere, d ) );
		}
	}
}

BOOST_AUTO_TEST_CASE( ray_plane_intersection ) {
	// XZ plane
	Plane plane( glm::vec3( 0.0, 1.0, 0.0 ), 0.0 );
	Float d;

	for (int i = 0; i < 10; i++) {
		for (int j = 0; j < 10; j++) {
			Ray ray(glm::vec3(i, 10.0, j), glm::normalize(-glm::vec3(i, 10.0, j)));
			BOOST_CHECK(ayla::intersection::rayPlane(ray, plane, d));

			Ray another(glm::vec3(i, 10.0, j), glm::normalize(glm::vec3(i, 10.0, j)));
			BOOST_CHECK(!ayla::intersection::rayPlane(another, plane, d));
		}
	}

	//parallel to the plane
	for (int i = 1; i < 10; i++) {
		for (int j = 1; j < 10; j++) {
			Ray parallel(glm::vec3(0.0, 1.0, 0.0), glm::normalize(glm::vec3(i, 0.0, j)));
			BOOST_CHECK(!ayla::intersection::rayPlane(parallel, plane, d));
		}
	}

	//inside the plane
	for (int i = 1; i < 10; i++) {
		for (int j = 1; j < 10; j++) {
			Ray inside(glm::vec3(0.0, 0.0, 0.0), glm::normalize(glm::vec3(i, 0.0, j)));
			BOOST_CHECK(ayla::intersection::rayPlane(inside, plane, d));
		}
	}

	Plane rotatedPlane(glm::normalize(glm::vec3( 1.0, 1.0, 0.0 )), 0.0);

	glm::vec3 offset(1.0, 0.0, 1.0);
	for (int j = 0; j < 10; j++ ) {
		glm::vec3 position = glm::vec3( 10.0, 10.0, 0.0 ) + offset;
		Ray ray(  position, glm::normalize(-position) );
		BOOST_CHECK( ayla::intersection::rayPlane( ray, rotatedPlane, d ) );

		Ray another( position, glm::normalize(position) );
		BOOST_CHECK( ! ayla::intersection::rayPlane( another, rotatedPlane, d ) );

		offset = glm::rotate<Float>(offset, glm::pi<Float>() / 8.0f, glm::vec3(1.0, 1.0, 0.0));
	}

	//parallel to the plane
	Ray parallel( glm::vec3( 1.0, 1.0, 0.0 ), glm::normalize(glm::vec3( 1.0, 0.0, 1.0 )) );
	for (int i = 1; i < 8; i++) {
		BOOST_CHECK( ! ayla::intersection::rayPlane( parallel, rotatedPlane, d ) );
		glm::vec3 rotatedDirection = glm::rotate<Float>(parallel.getDirection(), glm::pi<Float>() / 4.0f, glm::vec3(1.0, 1.0, 0.0));
		parallel.setDirection( rotatedDirection );
	}

	//inside the plane
	Ray inside( glm::vec3( 0.0, 0.0, 0.0 ), glm::normalize(glm::vec3( 1.0, 0.0, 1.0 )) );
	for (int i = 1; i < 8; i++) {
		BOOST_CHECK( ayla::intersection::rayPlane( inside, rotatedPlane, d ) );
		glm::vec3 rotatedDirection = glm::rotate<Float>(inside.getDirection(), glm::pi<Float>() / 4.0f, glm::vec3(1.0, 1.0, 0.0));
		inside.setDirection( rotatedDirection );
	}

	//Not in the origin
}

BOOST_AUTO_TEST_CASE( sphere_aab_intersection ) {
	Sphere sphere( glm::vec3( 0.0, 0.0, 0.0 ), 10.0 );

	for (int i = -11; i <= 11; i++) {
		AxisAlignedBox aab( glm::vec3( i, 0.0, 0.0 ), 1.0, 1.0, 1.0 );
		BOOST_CHECK( ayla::intersection::sphereAab( sphere, aab ) );
	}

	{
		AxisAlignedBox aab( glm::vec3( 0.0, 0.0, 0.0 ), 1.0, 1.0, 1.0 );
		for (int i = 0; i < 10; i++) {
			Sphere sphere( glm::vec3( 1.0 + cos( glm::pi<Float>()/4.0f), (0.1 * i) + sin( glm::pi<Float>()/4.0f), 0.0 ), 1.0  );
			BOOST_CHECK( ayla::intersection::sphereAab( sphere, aab ) );
		}
	}

	{
		AxisAlignedBox aab( glm::vec3( 0.0, 0.0, 0.0 ), 1.0, 1.0, 1.0 );
		for (int i = -10; i < 10; i++) {
			Sphere sphereA( glm::vec3( i, 2.0, 0.0 ), 0.5 );
			Sphere sphereB( glm::vec3( 2.0, i, 0.0 ), 0.5 );
			Sphere sphereC( glm::vec3( 0.0, 2.0, i ), 0.5 );

			BOOST_CHECK( ! ayla::intersection::sphereAab( sphereA, aab ) );
			BOOST_CHECK( ! ayla::intersection::sphereAab( sphereB, aab ) );
			BOOST_CHECK( ! ayla::intersection::sphereAab( sphereC, aab ) );
		}
	}

	{
		AxisAlignedBox aab( glm::vec3( 0.0, 0.0, 0.0 ), 10.0, 10.0, 10.0 );
		Sphere contained( glm::vec3( 1.0, 0.0, 1.0 ), 1.0 );
		Sphere containing( glm::vec3( 0.0, 1.0, 2.0 ), 100.0 );

		BOOST_CHECK( ayla::intersection::sphereAab( contained, aab) );
		BOOST_CHECK( ayla::intersection::sphereAab( containing, aab ) );
	}
}

BOOST_AUTO_TEST_CASE( triangle_line_segment_intersection ) {
	Triangle triangle(glm::vec3( 3.0, 0.0, 0.0 ), glm::vec3( 0.0, 4.0, 0.0 ), glm::vec3( 0.0, 0.0, 0.0 ));

	{
		LineSegment axisX( glm::vec3( -5.0, 4.0, 0.0 ), glm::vec3( 5.0, 4.0, 0.0 ) );
		LineSegment axisY( glm::vec3( 3.0, -5.0, 0.0 ), glm::vec3( 3.0, 5.0, 0.0 ) );
		LineSegment axisZ( glm::vec3( 0.0, 0.0, -5.0 ), glm::vec3( 0.0, 0.0, 5.0 ) );
		LineSegment segment( glm::vec3( 3.0, 4.0, 0.0 ), glm::vec3( 0.0, 0.0, 0.0 ) );
		LineSegment contained( glm::vec3( 0.1, 0.1, 0.0 ), glm::vec3( 0.2, 0.2, 0.0 ) );
		LineSegment inEdge( glm::vec3( 0.0, 0.0, 0.0 ), glm::vec3( 4.0, 0.0, 0.0 ) );
		glm::vec3 intersectionPoint;

		BOOST_CHECK( ayla::intersection::triangleLineSegment(triangle, segment) );
		BOOST_CHECK( ayla::intersection::triangleLineSegment(triangle, contained) );
		BOOST_CHECK( ayla::intersection::triangleLineSegment(triangle, inEdge ) );

		BOOST_CHECK( ayla::intersection::triangleLineSegment( triangle, axisX, intersectionPoint ) );
		BOOST_CHECK( ayla::epsilonEqual( intersectionPoint, glm::vec3( 0.0, 4.0, 0.0 ) ) );

		BOOST_CHECK( ayla::intersection::triangleLineSegment( triangle, axisY, intersectionPoint ) );
		BOOST_CHECK( ayla::epsilonEqual( intersectionPoint, glm::vec3( 3.0, 0.0, 0.0 ) ) );

		BOOST_CHECK( ayla::intersection::triangleLineSegment( triangle, axisZ, intersectionPoint ) );
		BOOST_CHECK( ayla::epsilonEqual( intersectionPoint, glm::vec3( 0.0, 0.0, 0.0 ) ) );
	}
}

BOOST_AUTO_TEST_CASE( plane_line_segment_intersection ) {
	Plane planeXZ( glm::vec3( 0.0, 1.0, 0.0 ), 0.0 );

	LineSegment contained( glm::vec3( 1.0, 0.0, 1.0 ) , glm::vec3( -1.0, 0.0, -2.0 ) );
	LineSegment parallel( glm::vec3( 1.0, 1.0, 1.0 ), glm::vec3( -1.0, 1.0, -2.0 ) );
	glm::vec3 intersectionPoint;

	BOOST_CHECK( ayla::intersection::planeLineSegment( planeXZ, contained, intersectionPoint ) );
	BOOST_CHECK( ayla::epsilonEqual( intersectionPoint, glm::vec3( 1.0, 0.0, 1.0 ) ) );

	BOOST_CHECK( ! ayla::intersection::planeLineSegment( planeXZ, parallel, intersectionPoint ) );

	for (int i = -10; i < 10; i++) {
		for (int j = -10; j < 10; j++) {
			LineSegment segment( glm::vec3( i, 10.0, j ), glm::vec3( 0.0, -1.0, 0.0 ) );
			BOOST_CHECK( ayla::intersection::planeLineSegment( planeXZ, segment, intersectionPoint ) );

			LineSegment pColliding( glm::vec3( 0.0, 0.0, 0.0 ) , glm::vec3( i, -1.0, j ) );
			BOOST_CHECK( ayla::intersection::planeLineSegment( planeXZ, pColliding, intersectionPoint ) );
			BOOST_CHECK( ayla::epsilonEqual( intersectionPoint, pColliding.getP() ) );

			LineSegment qColliding( glm::vec3( i, 1.0, j ), glm::vec3( 0.0, 0.0, 0.0 ) );
			BOOST_CHECK( ayla::intersection::planeLineSegment( planeXZ, qColliding, intersectionPoint ) );
			BOOST_CHECK( ayla::epsilonEqual( intersectionPoint, qColliding.getQ() ) );
		}
	}
}

BOOST_AUTO_TEST_CASE( aab_line_segment_intersection ) {
	AxisAlignedBox aab( glm::vec3( 0.0, 0.0, 0.0 ), 10.0, 10.0, 10.0 );

	for (int i = -10; i < 10; i++) {
		for (int j = -10; j < 10; j++) {
			LineSegment inwards( glm::vec3( i, 20.0, j ), glm::vec3( 0.0, 0.0, 0.0 ) );
			BOOST_CHECK( ayla::intersection::aabLineSegment( aab, inwards ) );

			LineSegment outwards( glm::vec3( i, 11.0, j ), glm::vec3( 2*i, 22.0, 2*j ) );
			BOOST_CHECK( ! ayla::intersection::aabLineSegment( aab, outwards ) );
		}
	}

	LineSegment parallel( glm::vec3( 20.0, 0.0, 0.0 ), glm::vec3( 20.0, 10.0, 0.0 ) );
	LineSegment contained( glm::vec3( -3.0, 0.0, 0.0 ), glm::vec3( 3.0, 0.0, 0.0 ) );
	LineSegment collidesVertex( glm::vec3( 20.0, 0.0, 10.0 ), glm::vec3( 0.0, 20.0, 10.0 ) );
	LineSegment collidesEdgeA( glm::vec3( 20.0, 0.0, 0.0 ), glm::vec3( 0.0, 20.0, 0.0 ) );
	LineSegment collidesEdgeB( glm::vec3( 20.0, 0.0, 0.0 ), glm::vec3( 0.0, 0.0, 20.0 ) );
	LineSegment collidesEdgeC( glm::vec3( 0.0, 20.0, 0.0 ), glm::vec3( 0.0, 0.0, 20.0 ) );

	BOOST_CHECK( ! ayla::intersection::aabLineSegment( aab, parallel ) );
	BOOST_CHECK( ayla::intersection::aabLineSegment( aab, contained ) );
	BOOST_CHECK( ayla::intersection::aabLineSegment( aab, collidesVertex ) );
	BOOST_CHECK( ayla::intersection::aabLineSegment( aab, collidesEdgeA ) );
	BOOST_CHECK( ayla::intersection::aabLineSegment( aab, collidesEdgeB ) );
	BOOST_CHECK( ayla::intersection::aabLineSegment( aab, collidesEdgeC ) );
}

BOOST_AUTO_TEST_CASE( aab_plane_intersection ) {
	AxisAlignedBox aab( glm::vec3( 0.0, 0.0, 0.0 ), 10.0, 10.0, 10.0 );

	glm::vec3 normal( 0.0, 1.0, 0.0 );
	for (int i = 0; i < 10; i++) {
		Plane plane( normal, 0.0 );
		BOOST_CHECK( ayla::intersection::aabPlane( aab, plane ) );

		normal = glm::rotate<Float>( normal, glm::pi<Float>()/6.0f, glm::vec3( 1.0f, 0.0f, 0.0 ) );
	}

	Plane parallelToFace( glm::vec3( 0.0, 1.0, 0.0 ), 11.0 );
	Plane containsFace( glm::vec3( 0.0, 1.0, 0.0 ), 10.0 );
	Plane containsEdge( glm::normalize(glm::vec3( 1.0f, 1.0f, 0.0f )), sqrt(2.0f) * 10.0f );
	Plane farAway( glm::normalize(glm::vec3( 2.0, 3.3, -5.0 )), 100.0 );

	BOOST_CHECK( ! ayla::intersection::aabPlane( aab, parallelToFace ) );
	BOOST_CHECK( ayla::intersection::aabPlane( aab, containsFace ) );
	BOOST_CHECK( ayla::intersection::aabPlane( aab, containsEdge ) );
	BOOST_CHECK( ! ayla::intersection::aabPlane( aab, farAway ) );
}

BOOST_AUTO_TEST_CASE( aab_triangle_intersection ) {
	AxisAlignedBox aab( glm::vec3( 0.0f, 0.0f, 0.0f ), 10.0f, 10.0f, 10.0f );

	glm::vec3 direction( 1.0f, 0.0f, 0.0f );

	for (int i = 0; i < 10; i++) {
		glm::vec3 origin( 0.0f, 11.0f, 0.0f );
		Triangle tri( origin, origin + direction * 10.0f, glm::vec3( 0.0f, 0.0f, 0.0f ) );
		BOOST_CHECK( ayla::intersection::aabTriangle( aab, tri ) );
		direction = glm::rotate<Float>( direction, glm::pi<Float>() / 6.0f, glm::vec3( 0.0f, 1.0f, 0.0f ) );
	}

	Triangle contained( glm::vec3( -3.0f, 0.0f, 0.0f ), glm::vec3( 2.0f, 5.0f, -3.0f ), glm::vec3( 3.0f, 2.0f, 3.0f ) );
	Triangle perpendicularToFaceOut( glm::vec3( 12.0f, 0.0f, 0.0f ), glm::vec3( 20.0f, 0.0f, 0.0f ), glm::vec3( 13.0f, 5.0f, 0.0f ) );
	Triangle perpendicularToFaceIn( glm::vec3( 7.0f, 0.0f, 0.0f ), glm::vec3( 15.0f, 0.0f, 0.0f ), glm::vec3( 9.0f, 5.0f, 0.0f ) );
	Triangle parallelToFace( glm::vec3( 12.0f, 0.0f, 10.0f ), glm::vec3( 12.0f, 0.0f, -10.0f ), glm::vec3( 12.0f, 5.0f, 0.0f ) );
	Triangle containedFace( glm::vec3( 10.0f, 0.0f, -5.0f ), glm::vec3( 10.0f, 0.0f, 5.0f ), glm::vec3( 10.0f, 5.0f, 0.0f ) );
	Triangle containsFace( glm::vec3( 10.0f, -10.0f, -50.0f ), glm::vec3( 10.0f, -10.0f, 50.0f ), glm::vec3( 10.0f, 50.0f, 0.0f ) );
	Triangle collidingVertex( glm::vec3( 10.0f, 10.0f, 0.0f ), glm::vec3( 20.0f, 10.0f, 0.0f ), glm::vec3( 20.0f, 20.0f, 0.0f ) );
	Triangle collidingEdge( glm::vec3( 10.0f, -10.0f, -50.0f ), glm::vec3( 10.0f, -10.0f, 50.0f ), glm::vec3( 0.0f, -50.0f, 0.0f ) );

	BOOST_CHECK( ayla::intersection::aabTriangle( aab, contained ) );
	BOOST_CHECK( ! ayla::intersection::aabTriangle( aab, perpendicularToFaceOut ) );
	BOOST_CHECK( ayla::intersection::aabTriangle( aab, perpendicularToFaceIn ) );
	BOOST_CHECK( ! ayla::intersection::aabTriangle( aab, parallelToFace ) );
	BOOST_CHECK( ayla::intersection::aabTriangle( aab, containedFace ) );
	BOOST_CHECK( ayla::intersection::aabTriangle( aab, containsFace ) );
	BOOST_CHECK( ayla::intersection::aabTriangle( aab, collidingVertex ) );
	BOOST_CHECK( ayla::intersection::aabTriangle( aab, collidingEdge ) );

}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()


/* test_lognormal_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/lognormal_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::lognormal_distribution<>
#define BOOST_RANDOM_ARG1 m
#define BOOST_RANDOM_ARG2 s
#define BOOST_RANDOM_ARG1_DEFAULT 0.0
#define BOOST_RANDOM_ARG2_DEFAULT 1.0
#define BOOST_RANDOM_ARG1_VALUE 7.5
#define BOOST_RANDOM_ARG2_VALUE 0.25

#define BOOST_RANDOM_DIST0_MIN 0.0
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST1_MIN 0.0
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST2_MIN 0.0
#define BOOST_RANDOM_DIST2_MAX (std::numeric_limits<double>::infinity)()

#define BOOST_RANDOM_TEST1_PARAMS (-100.0)
#define BOOST_RANDOM_TEST1_MAX 1

#define BOOST_RANDOM_TEST2_PARAMS (100.0)
#define BOOST_RANDOM_TEST2_MIN 1

#include "test_distribution.ipp"


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

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

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

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

#include <boost/geometry/algorithms/assign.hpp>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/strategies/geographic/distance_andoyer.hpp>
#include <boost/geometry/strategies/geographic/distance_vincenty.hpp>
#include <boost/geometry/extensions/gis/latlong/latlong.hpp>

#include <test_common/test_point.hpp>

int test_main(int, char* [])
{
    using namespace bg::strategy::distance;

    bg::model::ll::point<bg::degree> paris;
    paris.lat(bg::dms<bg::north>(48, 52, 0));
    paris.lon(bg::dms<bg::east>(2, 19, 59));

    bg::model::ll::point<bg::degree> amsterdam;
    amsterdam.lat(bg::dms<bg::north>(52, 22, 23));
    amsterdam.lon(bg::dms<bg::east>(4, 53, 32));

    bg::model::ll::point<bg::radian> paris_rad, amsterdam_rad;
    transform(amsterdam, amsterdam_rad);
    transform(paris, paris_rad);

    // Distance paris-amsterdam is about 430 km
    double expected = 429.984 * 1000.0;
    double tolerance = 0.001;

    // Combinations deg-deg, rad-rad, deg-rad, rad-de
    BOOST_CHECK_CLOSE(distance(paris, amsterdam), expected, tolerance);
    BOOST_CHECK_CLOSE(distance(paris_rad, amsterdam_rad), expected, tolerance);
    BOOST_CHECK_CLOSE(distance(paris, amsterdam_rad), expected, tolerance);
    BOOST_CHECK_CLOSE(distance(paris_rad, amsterdam), expected, tolerance);

    // With specified strategy
    vincenty<double> the_strategy;
    BOOST_CHECK_CLOSE(distance(paris, amsterdam, the_strategy), expected, tolerance);
    BOOST_CHECK_CLOSE(distance(paris_rad, amsterdam_rad, the_strategy), expected, tolerance);
    BOOST_CHECK_CLOSE(distance(paris, amsterdam_rad, the_strategy), expected, tolerance);
    BOOST_CHECK_CLOSE(bg::distance(paris_rad, amsterdam, the_strategy), expected, tolerance);

    // Distance point-linestring, linestring-point...
    bg::model::ll::point<bg::degree> barcelona(
        bg::latitude<>(bg::dms<bg::north>(41, 23)),
        bg::longitude<>(bg::dms<bg::east>(2, 11))
        );

    bg::model::linestring<bg::model::ll::point<bg::degree> > ab;
    ab.push_back(amsterdam);
    ab.push_back(barcelona);

    // Distance paris to line amsteram-barcelona is about 113 km
    expected = 113.168 * 1000.0;

    BOOST_CHECK_CLOSE(distance(ab, paris), expected, tolerance);
    BOOST_CHECK_CLOSE(distance(paris, ab), expected, tolerance);
    BOOST_CHECK_CLOSE(distance(paris, ab, the_strategy), expected, tolerance);
    BOOST_CHECK_CLOSE(distance(ab, paris, the_strategy), expected, tolerance);

    // line-type in degrees, point-type in radians (supported since new distance-strategy approach)
    BOOST_CHECK_CLOSE(distance(ab, paris_rad, the_strategy), expected, tolerance);

    return 0;
}