#include <gtest/gtest.h>

#include "scheme/nest/NEST.hh"
#include "scheme/nest/NEST_concepts.hh"
#include "scheme/nest/NEST_test_util.hh"
#include "scheme/util/meta/util.hh"
#include <boost/assign/std/vector.hpp> // for 'operator+=()'
#include <random>
#include "scheme/nest/pmap/UnitMap.hh"
#include "scheme/nest/pmap/DiscreteChoiceMap.hh"
#include <boost/foreach.hpp>
#include <iterator>

// #include <Eigen/Core>

namespace scheme {
namespace nest {

using std::cout;
using std::endl;

using namespace pmap;

TEST(NEST,concpets){
	NEST< 1, concept::ValueArchitype, concept::ParamMapArchitype >().set_state(1,0);
	// NEST< 1, double, concept::ParamMapArchitype >().get_index(1.0,0); // this should not compile!
	NEST< 1, concept::ValueArchitype, concept::ParamMapInvertableArchitype >().set_state(1,0);
	NEST< 1, concept::ValueArchitype, concept::ParamMapInvertableArchitype >().get_index(concept::ValueArchitype(),0);
	// NEST< 1, concept::ValueArchitype, UnitMap >().set_state(1,0);
	NEST< 1, concept::ArrayValueArchitype<1>, UnitMap >().set_state(1,0);
	NEST< 1, concept::ArrayValueArchitype<1>, UnitMap >().get_index(concept::ArrayValueArchitype<1>(),0);
	SUCCEED();
}

TEST(NEST,particular_cases){
	NEST<1>	nest;
	ASSERT_EQ( nest.set_and_get(0,0)[0], 0.5 );
	ASSERT_EQ( nest.set_and_get(0,1)[0], 0.25 );
	ASSERT_EQ( nest.set_and_get(1,1)[0], 0.75 );
	ASSERT_EQ( nest.set_and_get(0,2)[0], 0.125 );
	ASSERT_EQ( nest.set_and_get(1,2)[0], 0.375 );
	ASSERT_EQ( nest.set_and_get(2,2)[0], 0.625 );
	ASSERT_EQ( nest.set_and_get(3,2)[0], 0.875 );
	NEST<2>	nest2;
	ASSERT_EQ( nest2.set_and_get(0,0), NEST<2>::ValueType(0.5,0.5)   );
	ASSERT_EQ( nest2.set_and_get(0,1), NEST<2>::ValueType(0.25,0.25)   );
	ASSERT_EQ( nest2.set_and_get(1,1), NEST<2>::ValueType(0.75,0.25)   );
	ASSERT_EQ( nest2.set_and_get(2,1), NEST<2>::ValueType(0.25,0.75)   );
	ASSERT_EQ( nest2.set_and_get(3,1), NEST<2>::ValueType(0.75,0.75)   );
}

// // keep this out because #include <Eigen/Core> takes almost a second to compile
// TEST(NEST,works_with_eigen){
// 	typedef Eigen::Matrix<float,2,1> MAT;
// 	typedef NEST<2,MAT> Nest;
// 	Nest nest;
// 	MAT m; m << 0.5,0.5;
// 	ASSERT_EQ( nest.set_and_get(0,0), m );
// }

TEST(NEST,map_discrete) {
	using std::cout;
	using std::endl;

	using namespace boost::assign;
	std::vector<double> choices;
	choices += 42.0,152.345,8049782.83402;
	NEST<0,double,pmap::DiscreteChoiceMap,StoreValue> nest0(choices);
	ASSERT_EQ(choices.size(),nest0.size());
	ASSERT_EQ(choices.size(),nest0.size(0));
	ASSERT_EQ(choices.size(),nest0.size(3));
	ASSERT_EQ(choices.size(),nest0.size(4));
	for(size_t i = 0; i < nest0.size(); ++i){
		ASSERT_TRUE( nest0.set_state(i) );
		ASSERT_EQ(choices[i],nest0.set_and_get(i));
		ASSERT_EQ(choices[i],nest0.set_and_get(i,0));
		ASSERT_EQ(choices[i],nest0.set_and_get(i,7));
		ASSERT_EQ(choices[i],nest0.set_and_get(i,3));
		ASSERT_EQ(choices[i],nest0.set_and_get(i,8));
	}
	ASSERT_FALSE( nest0.set_state(5) );
}

template<int DIM>
void test_coverage_unit1cell(){
	std::mt19937 rng((unsigned int)time(0));
	std::uniform_real_distribution<> uniform;
	NEST<DIM> nest;
	size_t rmax = 9/DIM+1;
	for(size_t r = 0; r <= rmax; ++r){
		double cellradius = sqrt(DIM) * 0.5 / (1<<r);
		for(size_t iter = 0; iter < 100000/DIM; ++iter){
			util::SimpleArray<DIM,double> tgt;
			for(size_t i = 0; i < DIM; ++i) tgt[i] = uniform(rng);
			size_t index = nest.get_index(tgt,r);
			ASSERT_LT( index , nest.size(r) );
			util::SimpleArray<DIM,double> val = nest.set_and_get(index,r);
			ASSERT_LE( (tgt-val).norm(), cellradius );
		}
		util::SimpleArray<DIM,double> zeros; zeros.fill(0);
		ASSERT_LE( (zeros-nest.set_and_get(nest.get_index(zeros,r),r)).norm(), cellradius );
		util::SimpleArray<DIM,double> ones; ones.fill(0.999999);
		ASSERT_LE( (ones-nest.set_and_get(nest.get_index(ones,r),r)).norm(), cellradius );
	}
}

TEST(NEST,coverage_cube){
	test_coverage_unit1cell<1>();
	test_coverage_unit1cell<2>();
	test_coverage_unit1cell<3>();
	test_coverage_unit1cell<4>();
	test_coverage_unit1cell<5>();
	test_coverage_unit1cell<6>();
}

TEST(NEST,index_nesting){
	generic_test_index_nesting_of_bincenters( NEST<1>() );
	generic_test_index_nesting_of_bincenters( NEST<2>() );
	generic_test_index_nesting_of_bincenters( NEST<3>() );
	generic_test_index_nesting_of_bincenters( NEST<4>() );
	generic_test_index_nesting_of_bincenters( NEST<5>() );
	generic_test_index_nesting_of_bincenters( NEST<6>() );
}

template<int DIM>
void test_store_pointer_virtual(){
	typedef util::SimpleArray<DIM,double> VAL;
	NEST<DIM,VAL,UnitMap,StoreValue  ,size_t,double,true> nest_val(2);
	NEST<DIM,VAL,UnitMap,StorePointer,size_t,double,true> nest_ptr(2);
	NestBase<size_t> * nest_val_virtual = &nest_val;
	NestBase<size_t> * nest_ptr_virtual = &nest_ptr;
	StorePointer<VAL> * ptr_store      = dynamic_cast< StorePointer<VAL>* >(nest_ptr_virtual);
	StorePointer<VAL> * ptr_store_fail = dynamic_cast< StorePointer<VAL>* >(nest_val_virtual);
	ASSERT_TRUE(ptr_store);
	ASSERT_FALSE(ptr_store_fail);
	VAL val_pointed_to;
	ptr_store->set_pointer(&val_pointed_to);
	size_t rmax = 9/DIM+1;
	for(size_t r = 0; r <= rmax; ++r){
		for(size_t i = 0; i < nest_val.size(r); ++i){
			VAL tmp;
			boost::any a = &tmp;
			ASSERT_TRUE( nest_val_virtual->virtual_get_state(i,r,a) );
			ASSERT_TRUE( nest_ptr_virtual->virtual_get_state(i,r,a) );
			ASSERT_EQ( nest_val.value(), val_pointed_to );
			ASSERT_EQ( nest_val.value(), tmp );
			ASSERT_EQ( tmp, val_pointed_to );
		}
	}
}
TEST(NEST,store_pointer_virtual){
	test_store_pointer_virtual<1>();
	test_store_pointer_virtual<2>();
	test_store_pointer_virtual<3>();
	test_store_pointer_virtual<4>();
	test_store_pointer_virtual<5>();
	test_store_pointer_virtual<6>();
}

TEST(NEST,store_nothing){
	typedef util::SimpleArray<2,double> VAL;
	NEST<2,VAL,UnitMap,util::StoreNothing,size_t,double,true> nest;
	VAL tmp(-10,-10);
	boost::any a = &tmp;
	ASSERT_TRUE( nest.virtual_get_state(0,0,a) );
	ASSERT_EQ( tmp, VAL(0.5,0.5) );

	std::vector<size_t> vi(2);
	vi[0] = 1;
	vi[1] = 0;
	size_t ii=0;
	ASSERT_TRUE( nest.virtual_get_state(vi,0,ii,1,a) );
	ASSERT_EQ( tmp, VAL(0.75,0.25) );
	ASSERT_EQ( ii , 2 );

}

template<int DIM>
void test_uniformity(){
	typedef util::SimpleArray<DIM,double> VAL;
	NEST<DIM,VAL,UnitMap,StoreValue  > nest(1);
	size_t rmax = 9/DIM+1;
	for(size_t r = 0; r <= rmax; ++r){
		double scale = 1<<r;
		std::vector<int> counts(1<<r,0);
		for(size_t i = 0; i < nest.size(r); ++i){
			ASSERT_TRUE( nest.set_state(i,r) );
			for(size_t j = 0; j < DIM; ++j){
				counts[nest.value()[j]*scale]++;
			}
		}
		ASSERT_EQ( (1<<((DIM-1)*r))*DIM, *std::min_element(counts.begin(),counts.end()) );
		ASSERT_EQ( (1<<((DIM-1)*r))*DIM, *std::max_element(counts.begin(),counts.end()) );
	}
}

TEST(NEST,uniformity){
	test_uniformity<1>();
	test_uniformity<2>();
	test_uniformity<3>();
	test_uniformity<4>();
	test_uniformity<5>();
	test_uniformity<6>();
}

TEST(NEST,bounds){
	NEST<1> nest;
	ASSERT_TRUE (nest.set_state(0,0));
	ASSERT_FALSE(nest.set_state(1,0));
	#ifndef NDEBUG
	#ifndef CXX14
	ASSERT_DEATH( nest.set_and_get(1,0), "Assertion.*failed" );
	#endif
	#endif
}

TEST(NEST,virtual_get_index){
	std::mt19937 rng((unsigned int)time(0));
	std::uniform_real_distribution<> uniform;

	typedef util::SimpleArray<2,double> VAL;
	NEST<2,VAL,UnitMap,util::StoreNothing,uint64_t,double,true> nest;
	NestBase<uint64_t> *nestp = &nest;

	boost::any a;
	for(int resl = 0; resl < 6; ++resl){
		for(int iter = 0; iter < 10000; ++iter){
			VAL v( uniform(rng), uniform(rng) );
			a = &v;
			ASSERT_EQ( nest.get_index( v, resl ) , nestp->virtual_get_index( a, resl ) );
		}
	}

	NEST<2,VAL,nest::concept::ParamMapArchitype> nest2;
	#ifndef CXX14
	ASSERT_DEATH( nest2.virtual_get_index( a, 0 ), ".*" );
	#endif

}

}
}


/** @file InterpolationTest.cpp
 *
 * Copyright (c) 2020 IACE
 */

#define BOOST_TEST_MODULE InterpolationTest

#include "Interpolation.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE( LinearInterpolatorBoundaryTest ) {

    double dx[2] = {1, 2};
    double dy[2] = {2, 4};
    LinearInterpolator li(dx, dy, 2);

    BOOST_CHECK_EQUAL(li(10), 4);
    BOOST_CHECK_EQUAL(li(0), 2);
}

BOOST_AUTO_TEST_CASE( LinearInterpolatorInterpolateTest ) {

    double dx[2] = {1, 2};
    double dy[2] = {2, 4};
    LinearInterpolator li(dx, dy, 2);

    BOOST_CHECK_EQUAL(li(1.5), 3);
}

BOOST_AUTO_TEST_CASE( LinearInterpolatorUpdateDataTest ) {

    double dx[2] = {1, 2};
    double dy[2] = {2, 4};
    LinearInterpolator li(dx, dy, 2);

    BOOST_CHECK_EQUAL(li(1.5), 3);

    double dxN[2] = {2, 3};
    double dyN[2] = {4, 6};

    li.updateData(dxN, dyN);
    BOOST_CHECK_EQUAL(li(2.5), 5);
}

BOOST_AUTO_TEST_CASE( LinearInterpolatorInitUpdateDataTest ) {

    double dx[0] = {};
    double dy[0] = {};
    LinearInterpolator li(dx, dy, 2);

    double dxN[2] = {2, 3};
    double dyN[2] = {4, 6};

    li.updateData(dxN, dyN);
    BOOST_CHECK_EQUAL(li(2.5), 5);
}

BOOST_AUTO_TEST_CASE( LinearInterpolatorSetDataTest ) {

    double dx[2] = {1, 2};
    double dy[2] = {2, 4};
    LinearInterpolator li(dx, dy, 2);

    BOOST_CHECK_EQUAL(li(1.5), 3);

    double dxN[4] = {1, 2, 3, 4};
    double dyN[4] = {2, 4, 2, 4};
    li.setData(dxN, dyN, 4);
    BOOST_CHECK_EQUAL(li(0.5), 2);
}

BOOST_AUTO_TEST_CASE( LinearInterpolatorChangeDataTest ) {

    LinearInterpolator li;
    BOOST_CHECK_EQUAL(li(1), 0);

    double dxN[4] = {1, 2, 3, 4};
    double dyN[4] = {2, 4, 2, 4};
    li.setData(dxN, dyN, 4);
    BOOST_CHECK_EQUAL(li(0.5), 2);
}

BOOST_AUTO_TEST_CASE( LinearInterpolatorInterpolateMultipleTest ) {

    double dx[4] = {1, 2, 3, 4};
    double dy[4] = {2, 4, 2, 4};
    LinearInterpolator li(dx, dy, 4);

    BOOST_CHECK_EQUAL(li(0.5), 2);
    BOOST_CHECK_EQUAL(li(1),2);
    BOOST_CHECK_EQUAL(li(1.5), 3);
    BOOST_CHECK_EQUAL(li(2),4);
    BOOST_CHECK_EQUAL(li(2.5), 3);
    BOOST_CHECK_EQUAL(li(3),2);
    BOOST_CHECK_EQUAL(li(3.5), 3);
    BOOST_CHECK_EQUAL(li(4),4);
    BOOST_CHECK_EQUAL(li(4.5), 4);
}


/*
   Copyright (c) Marshall Clow 2017.

   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)

    For more information, see http://www.boost.org
*/

#include <vector>
#include <functional>
#include <numeric>

#include <boost/config.hpp>
#include <boost/algorithm/cxx11/iota.hpp>
#include <boost/algorithm/cxx17/transform_exclusive_scan.hpp>

#include "iterator_test.hpp"

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

namespace ba = boost::algorithm;

int triangle(int n) { return n*(n+1)/2; }

template <class _Tp>
struct identity
{
    const _Tp& operator()(const _Tp& __x) const { return __x;}
};

template <class Iter1, class BOp, class UOp, class T, class Iter2>
void
test(Iter1 first, Iter1 last, BOp bop, UOp uop, T init, Iter2 rFirst, Iter2 rLast)
{
    std::vector<typename std::iterator_traits<Iter1>::value_type> v;
//  Test not in-place
    ba::transform_exclusive_scan(first, last, std::back_inserter(v), init, bop, uop);
    BOOST_CHECK(std::distance(rFirst, rLast) == v.size());
    BOOST_CHECK(std::equal(v.begin(), v.end(), rFirst));

//  Test in-place
    v.clear();
    v.assign(first, last);
    ba::transform_exclusive_scan(v.begin(), v.end(), v.begin(), init, bop, uop);
    BOOST_CHECK(std::distance(rFirst, rLast) == v.size());
    BOOST_CHECK(std::equal(v.begin(), v.end(), rFirst));
}

template <class Iter>
void
test()
{
          int ia[]     = { 1,  3,  5,   7,   9};
    const int pResI0[] = { 0,  1,  4,   9,  16};        // with identity
    const int mResI0[] = { 0,  0,  0,   0,   0};
    const int pResN0[] = { 0, -1, -4,  -9, -16};        // with negate
    const int mResN0[] = { 0,  0,  0,   0,   0};
    const int pResI2[] = { 2,  3,  6,  11,  18};        // with identity
    const int mResI2[] = { 2,  2,  6,  30, 210};
    const int pResN2[] = { 2,  1, -2,  -7, -14};        // with negate
    const int mResN2[] = { 2, -2,  6, -30, 210};
    const unsigned sa = sizeof(ia) / sizeof(ia[0]);
    BOOST_CHECK(sa == sizeof(pResI0) / sizeof(pResI0[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(mResI0) / sizeof(mResI0[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(pResN0) / sizeof(pResN0[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(mResN0) / sizeof(mResN0[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(pResI2) / sizeof(pResI2[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(mResI2) / sizeof(mResI2[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(pResN2) / sizeof(pResN2[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(mResN2) / sizeof(mResN2[0]));       // just to be sure

    for (unsigned int i = 0; i < sa; ++i ) {
        test(Iter(ia), Iter(ia + i), std::plus<int>(),       identity<int>(),    0, pResI0, pResI0 + i);
        test(Iter(ia), Iter(ia + i), std::multiplies<int>(), identity<int>(),    0, mResI0, mResI0 + i);
        test(Iter(ia), Iter(ia + i), std::plus<int>(),       std::negate<int>(), 0, pResN0, pResN0 + i);
        test(Iter(ia), Iter(ia + i), std::multiplies<int>(), std::negate<int>(), 0, mResN0, mResN0 + i);
        test(Iter(ia), Iter(ia + i), std::plus<int>(),       identity<int>(),    2, pResI2, pResI2 + i);
        test(Iter(ia), Iter(ia + i), std::multiplies<int>(), identity<int>(),    2, mResI2, mResI2 + i);
        test(Iter(ia), Iter(ia + i), std::plus<int>(),       std::negate<int>(), 2, pResN2, pResN2 + i);
        test(Iter(ia), Iter(ia + i), std::multiplies<int>(), std::negate<int>(), 2, mResN2, mResN2 + i);
        }
}

void basic_tests()
{
    {
    std::vector<int> v(10);
    std::fill(v.begin(), v.end(), 3);
    ba::transform_exclusive_scan(v.begin(), v.end(), v.begin(), 50, std::plus<int>(), identity<int>());
    for (size_t i = 0; i < v.size(); ++i)
        BOOST_CHECK(v[i] == 50 + (int) i * 3);
    }

    {
    std::vector<int> v(10);
    ba::iota(v.begin(), v.end(), 0);
    ba::transform_exclusive_scan(v.begin(), v.end(), v.begin(), 30, std::plus<int>(), identity<int>());
    for (size_t i = 0; i < v.size(); ++i)
        BOOST_CHECK(v[i] == 30 + triangle(i-1));
    }

    {
    std::vector<int> v(10);
    ba::iota(v.begin(), v.end(), 1);
    ba::transform_exclusive_scan(v.begin(), v.end(), v.begin(), 40, std::plus<int>(), identity<int>());
    for (size_t i = 0; i < v.size(); ++i)
        BOOST_CHECK(v[i] == 40 + triangle(i));
    }

    {
    std::vector<int> v, res;
    ba::transform_exclusive_scan(v.begin(), v.end(), std::back_inserter(res), 40, std::plus<int>(), identity<int>());
    BOOST_CHECK(res.empty());
    }
}

void test_transform_exclusive_scan_init()
{
	basic_tests();
//  All the iterator categories
    test<input_iterator        <const int*> >();
    test<forward_iterator      <const int*> >();
    test<bidirectional_iterator<const int*> >();
    test<random_access_iterator<const int*> >();
    test<const int*>();
    test<      int*>();
}

BOOST_AUTO_TEST_CASE( test_main )
{
  test_transform_exclusive_scan_init();
}


#pragma once

#include <Eigen/Core>

namespace msccd {
namespace root_finder {

    bool vertexFaceMSCCD(
        const Eigen::Vector3d& vertex_start,
        const Eigen::Vector3d& face_vertex0_start,
        const Eigen::Vector3d& face_vertex1_start,
        const Eigen::Vector3d& face_vertex2_start,
        const Eigen::Vector3d& vertex_end,
        const Eigen::Vector3d& face_vertex0_end,
        const Eigen::Vector3d& face_vertex1_end,
        const Eigen::Vector3d& face_vertex2_end,
        double min_distance,
        double& toi);

    bool edgeEdgeMSCCD(
        const Eigen::Vector3d& edge0_vertex0_start,
        const Eigen::Vector3d& edge0_vertex1_start,
        const Eigen::Vector3d& edge1_vertex0_start,
        const Eigen::Vector3d& edge1_vertex1_start,
        const Eigen::Vector3d& edge0_vertex0_end,
        const Eigen::Vector3d& edge0_vertex1_end,
        const Eigen::Vector3d& edge1_vertex0_end,
        const Eigen::Vector3d& edge1_vertex1_end,
        double min_distance,
        double& toi);

    ///////////////////////////////////////////////////////////////////////////
    // Helper functions

    int areProximate(
        const Eigen::Vector3d& x0,
        const Eigen::Vector3d& x1,
        const Eigen::Vector3d& x2,
        const Eigen::Vector3d& x3,
        const Eigen::Vector3d& v0,
        const Eigen::Vector3d& v1,
        const Eigen::Vector3d& v2,
        const Eigen::Vector3d& v3,
        double h,
        double* dt);

    int arePointPointIntersecting(
        const Eigen::Vector3d& x0,
        const Eigen::Vector3d& x1,
        const Eigen::Vector3d& v0,
        const Eigen::Vector3d& v1,
        double h,
        double* dt);

    int arePointLineIntersecting(
        const Eigen::Vector3d& x0,
        const Eigen::Vector3d& x1,
        const Eigen::Vector3d& x2,
        const Eigen::Vector3d& v0,
        const Eigen::Vector3d& v1,
        const Eigen::Vector3d& v2,
        double h,
        double* dt);

    double vertexFaceDist(
        const Eigen::Vector3d& p,
        const Eigen::Vector3d& a,
        const Eigen::Vector3d& b,
        const Eigen::Vector3d& c,
        double& t1,
        double& t2,
        double& t3);

    int areEEProximate(
        const Eigen::Vector3d& x0,
        const Eigen::Vector3d& x1,
        const Eigen::Vector3d& x2,
        const Eigen::Vector3d& x3,
        const Eigen::Vector3d& v0,
        const Eigen::Vector3d& v1,
        const Eigen::Vector3d& v2,
        const Eigen::Vector3d& v3,
        double h,
        double* dt);

    double edgeEdgeDist(
        const Eigen::Vector3d& p1,
        const Eigen::Vector3d& q1,
        const Eigen::Vector3d& p2,
        const Eigen::Vector3d& q2,
        double& s,
        double& t);

    int areEdgeEdgeIntersecting(
        const Eigen::Vector3d& x0,
        const Eigen::Vector3d& x1,
        const Eigen::Vector3d& x2,
        const Eigen::Vector3d& x3,
        const Eigen::Vector3d& v0,
        const Eigen::Vector3d& v1,
        const Eigen::Vector3d& v2,
        const Eigen::Vector3d& v3,
        double h,
        double* dt);

    int areLineLineIntersecting(
        const Eigen::Vector3d& x0,
        const Eigen::Vector3d& x1,
        const Eigen::Vector3d& x2,
        const Eigen::Vector3d& x3,
        const Eigen::Vector3d& v0,
        const Eigen::Vector3d& v1,
        const Eigen::Vector3d& v2,
        const Eigen::Vector3d& v3,
        double h,
        double* dt);

} // namespace root_finder
} // namespace msccd


#include <stan/math/prim/arr.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <vector>

TEST(MathFunctions, dot_self) {
  std::vector<double> x(3), y(3);
  x[0] = 2.33;
  x[1] = 8.88;
  x[2] = 9.81;
  y[0] = 2.46;
  y[1] = 4.45;
  y[2] = 1.03;

  EXPECT_FLOAT_EQ(180.5194, stan::math::dot_self(x));
  EXPECT_FLOAT_EQ(26.915, stan::math::dot_self(y));
}

TEST(MathFunctions, dot_self_nan) {
  std::vector<double> x(3);
  x[0] = 2.33;
  x[1] = 8.88;
  x[2] = 9.81;

  double nan = std::numeric_limits<double>::quiet_NaN();
  x[2] = nan;

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::dot_self(x));

  x[0] = nan;
  x[1] = nan;
  x[2] = nan;
  EXPECT_PRED1(boost::math::isnan<double>, stan::math::dot_self(x));
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#ifndef BOOST_GRAPH_RELAX_HPP
#define BOOST_GRAPH_RELAX_HPP

#include <functional>
#include <boost/limits.hpp> // for numeric limits
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

namespace boost
{

// The following version of the plus functor prevents
// problems due to overflow at positive infinity.

template < class T > struct closed_plus
{
    const T inf;

    closed_plus() : inf((std::numeric_limits< T >::max)()) {}
    closed_plus(T inf) : inf(inf) {}

    T operator()(const T& a, const T& b) const
    {
        using namespace std;
        if (a == inf)
            return inf;
        if (b == inf)
            return inf;
        return a + b;
    }
};

template < class Graph, class WeightMap, class PredecessorMap,
    class DistanceMap, class BinaryFunction, class BinaryPredicate >
bool relax(typename graph_traits< Graph >::edge_descriptor e, const Graph& g,
    const WeightMap& w, PredecessorMap& p, DistanceMap& d,
    const BinaryFunction& combine, const BinaryPredicate& compare)
{
    typedef typename graph_traits< Graph >::directed_category DirCat;
    bool is_undirected = is_same< DirCat, undirected_tag >::value;
    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
    Vertex u = source(e, g), v = target(e, g);
    typedef typename property_traits< DistanceMap >::value_type D;
    typedef typename property_traits< WeightMap >::value_type W;
    const D d_u = get(d, u);
    const D d_v = get(d, v);
    const W& w_e = get(w, e);

    // The seemingly redundant comparisons after the distance puts are to
    // ensure that extra floating-point precision in x87 registers does not
    // lead to relax() returning true when the distance did not actually
    // change.
    if (compare(combine(d_u, w_e), d_v))
    {
        put(d, v, combine(d_u, w_e));
        if (compare(get(d, v), d_v))
        {
            put(p, v, u);
            return true;
        }
        else
        {
            return false;
        }
    }
    else if (is_undirected && compare(combine(d_v, w_e), d_u))
    {
        put(d, u, combine(d_v, w_e));
        if (compare(get(d, u), d_u))
        {
            put(p, u, v);
            return true;
        }
        else
        {
            return false;
        }
    }
    else
        return false;
}

template < class Graph, class WeightMap, class PredecessorMap,
    class DistanceMap, class BinaryFunction, class BinaryPredicate >
bool relax_target(typename graph_traits< Graph >::edge_descriptor e,
    const Graph& g, const WeightMap& w, PredecessorMap& p, DistanceMap& d,
    const BinaryFunction& combine, const BinaryPredicate& compare)
{
    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
    typedef typename property_traits< DistanceMap >::value_type D;
    typedef typename property_traits< WeightMap >::value_type W;
    const Vertex u = source(e, g);
    const Vertex v = target(e, g);
    const D d_u = get(d, u);
    const D d_v = get(d, v);
    const W& w_e = get(w, e);

    // The seemingly redundant comparisons after the distance puts are to
    // ensure that extra floating-point precision in x87 registers does not
    // lead to relax() returning true when the distance did not actually
    // change.
    if (compare(combine(d_u, w_e), d_v))
    {
        put(d, v, combine(d_u, w_e));
        if (compare(get(d, v), d_v))
        {
            put(p, v, u);
            return true;
        }
    }
    return false;
}

template < class Graph, class WeightMap, class PredecessorMap,
    class DistanceMap >
bool relax(typename graph_traits< Graph >::edge_descriptor e, const Graph& g,
    WeightMap w, PredecessorMap p, DistanceMap d)
{
    typedef typename property_traits< DistanceMap >::value_type D;
    typedef closed_plus< D > Combine;
    typedef std::less< D > Compare;
    return relax(e, g, w, p, d, Combine(), Compare());
}

} // namespace boost

#endif /* BOOST_GRAPH_RELAX_HPP */


#include "fieldtrack/FixedFrameTracker.h"

#include "argus_utils/utils/MatrixUtils.h"
#include "argus_utils/utils/ParamUtils.h"
#include "argus_utils/utils/YamlUtils.h"

#include "argus_msgs/CompactOdometryArray.h"

#include <boost/foreach.hpp>

using namespace argus_msgs;
using namespace geometry_msgs;

namespace argus
{

typedef ConstantVelocityFilterSE3 CVF3;

FixedFrameTracker::FixedFrameTracker( ros::NodeHandle& nh, ros::NodeHandle& ph )
: nodeHandle( nh ), privHandle( ph ), targetManager( lookupInterface )
{

	if( !privHandle.getParam( "reference_frame", referenceFrame ) )
	{
		ROS_ERROR_STREAM( "Must specify reference frame." );
		exit( -1 );
	}

	std::string lookupNamespace;
	privHandle.param<std::string>( "lookup_namespace", lookupNamespace, "/lookup" );
	lookupInterface.SetLookupNamespace( lookupNamespace );

	double pubFreq;
	ph.param<double>( "update_rate", pubFreq, 10.0 );
	timer = std::make_shared<ros::Timer>
		( nodeHandle.createTimer( ros::Duration( 1.0/pubFreq ),
		                          &FixedFrameTracker::TimerCallback,
		                          this ) );
	ROS_INFO_STREAM( "Updating and publishing at rate " << pubFreq );

	targetPub = nodeHandle.advertise<CompactOdometryArray>( "target_state", 10 );
	targetPoseSub = nodeHandle.subscribe( "target_pose",
	                                      10,
	                                      &FixedFrameTracker::TargetPoseCallback,
	                                      this );
	targetVelSub = nodeHandle.subscribe( "target_velocity",
	                                     10,
	                                     &FixedFrameTracker::TargetVelocityCallback,
	                                     this );
}

void FixedFrameTracker::TimerCallback( const ros::TimerEvent& event )
{
	CompactOdometryArray::Ptr msg = boost::make_shared<CompactOdometryArray>();

	BOOST_FOREACH( TargetRegistry::value_type& item, targetRegistry )
	{
		CompactOdometry odom;
		odom.header.frame_id = referenceFrame;
		odom.header.stamp = event.current_real;
		odom.child_frame_id = item.first;

		TargetRegistration& registration = item.second;

		if( event.current_real > registration.filterTime )
		{
			double dt = (event.current_real - registration.filterTime).toSec();
			registration.filter.Predict( registration.Qrate * dt, dt );
			registration.filterTime = event.current_real;
		}

		odom.pose.pose = PoseToMsg( registration.filter.Pose() );
		SerializeSymmetricMatrix( registration.filter.PoseCov(), odom.pose.covariance );

		PoseSE3::TangentVector tangents = registration.filter.Derivs().head<6>();
		odom.twist.twist = TangentToMsg( tangents );
		SerializeSymmetricMatrix( registration.filter.DerivsCov().topLeftCorner<6,6>(), odom.twist.covariance );

		msg->odometry.push_back( odom );
	}

	msg->header.frame_id = referenceFrame;
	msg->header.stamp = event.current_real;
	targetPub.publish( msg );
}

void FixedFrameTracker::TargetPoseCallback( const RelativePoseWithCovariance::ConstPtr& msg )
{

	if( msg->relative_pose.observer_name != referenceFrame )
	{
		ROS_WARN_STREAM( "Received relative pose message with observer " << msg->relative_pose.observer_name
			<< " unrelated to reference " << referenceFrame );
		return;
	}

	std::string targetName = msg->relative_pose.target_name;
	if( targetRegistry.count( targetName ) == 0 ) { RegisterTarget( targetName ); }

	TargetRegistration& registration = targetRegistry[ targetName ];
	PoseSE3 pose = MsgToPose( msg->relative_pose.relative_pose ); // Target pose relative to observer
	PoseSE3::CovarianceMatrix relCov;
	ParseSymmetricMatrix( msg->covariance, relCov );

	if( registration.poseInitialized )
	{
		if( msg->header.stamp > registration.filterTime )
		{
			double dt = (msg->header.stamp - registration.filterTime).toSec();
			registration.filter.Predict( registration.Qrate * dt, dt );
			registration.filterTime = msg->header.stamp;
		}
		registration.filter.UpdatePose( pose, relCov );
	}
	else
	{
		registration.filter.Pose() = pose;
		registration.filter.PoseCov() = relCov;
		registration.poseInitialized = true;
		registration.filterTime = msg->header.stamp;
	}

}

void FixedFrameTracker::TargetVelocityCallback( const TwistWithCovarianceStamped::ConstPtr& msg )
{
	const std::string& targetName = msg->header.frame_id;

	if( targetRegistry.count( targetName ) == 0 ) { RegisterTarget( targetName ); }
	TargetRegistration& registration = targetRegistry[ targetName ];

	if( msg->header.stamp > registration.filterTime )
	{
		double dt = (msg->header.stamp - registration.filterTime).toSec();
		registration.filter.Predict( registration.Qrate * dt, dt );
		registration.filterTime = msg->header.stamp;
	}

	PoseSE3::TangentVector velocity = MsgToTangent( msg->twist.twist );
	PoseSE3::CovarianceMatrix velocityCov;
	ParseMatrix( msg->twist.covariance, velocityCov );
	registration.filter.UpdateDerivs( velocity, CVF3::DerivObsMatrix::Identity(6,6),
	                                  velocityCov );

}

void FixedFrameTracker::RegisterTarget( const std::string& name )
{
	if( targetRegistry.count( name ) > 0 ) { return; }

	ROS_INFO_STREAM( "Registering target " << name << " for tracking." );

	targetRegistry[ name ];

}

FixedFrameTracker::TargetRegistration::TargetRegistration()
: poseInitialized( false ), filterTime( ros::Time::now() ),
filter( PoseSE3(), CVF3::DerivsType::Zero(), 10*CVF3::FullCovType::Identity() ),
Qrate( CVF3::FullCovType::Identity() )
{}

} // end namespace fieldtrack


#include <iostream>
#include <vector>
#include <fstream>
#include <Eigen\Dense>
#include <Image.h>

double mult(const Image &img, const Hasher &hasher)
{
	size_t size = std::min(img.v.size(), hasher.v.size());
	double result = hasher.bias;
	for (int i = 0; i < size; ++i)
		result += img.v[i] / 255.0 * hasher.v[i];

	return result;
}

std::string hash(const Image &img, const std::vector<Hasher> &hashers)
{
	std::string h;

	for (int i = 0; i < hashers.size(); ++i)
	{
		double p = mult(img, hashers[i]);

		if (p > 0)
			h.push_back('1');
		else
			h.push_back('0');
	}

	return h;
}

std::string hashTab(const Image &img, const std::vector<Hasher> &hashers)
{
	std::string h;

	for (int i = 0; i < hashers.size(); ++i)
	{
		double p = mult(img, hashers[i]);

		if (p > 0)
			h.push_back('1');
		else
			h.push_back('0');
		h.push_back('\t');
	}

	return h;
}

int main(int argc, char **argv)
{
	Eigen::setNbThreads(4);
	std::clog << "Eigen::nbThreads() = " << Eigen::nbThreads() << std::endl;

	std::clog << "Eigen::nbThreads() = " << Eigen::nbThreads() << std::endl;

	std::vector<Image> images;

	std::clog << "Loading data... " << std::endl;

	for (int i = 2; i < argc; ++i)
	{
		std::clog << "\t" << argv[i] << std::endl;
		std::ifstream input(argv[i], std::ios::binary);
		auto imgs = LoadBatch(input);
		images.insert(images.end(), imgs.begin(), imgs.end());
	}

	//while (images.size() > 2)
	//	images.pop_back();

	std::clog << "DONE" << std::endl;

	int N = (int)images.size();
	int M = 32 * 32 * 3;
	//int M = 1024;

	std::clog << "Loading hashers... " << std::endl;

	std::vector<Hasher> hashers;
	{
		std::clog << "\t" << argv[1] << std::endl;
		std::ifstream input(argv[1]);
		hashers = LoadHashers(input, 64);
	}

	std::clog << "DONE" << std::endl;
	/*
	for (const auto &hash : hashers)
	{
		double bias = 0;
		for (const auto& img : images)
		{
			for (int i = 0; i < img.v.size(); ++i)
				bias += img.v[i] / 255.0 * hash.v[i];
		}
		bias /= images.size();
		bias = -bias;
		std::clog << hash.bias << " _ " << bias << "(" << (abs(bias - hash.bias) < 0.001) << ")" << std::endl;
	}
	*/
	for (const auto& img : images)
	{
		//if (img.label == 255)
		//	continue;
		std::string h = hash(img, hashers);
		std::cout << img.label << "\t" << h << std::endl;
	}

	return 0;
}


/**
 * @file to_string_test.cpp
 * @author Ryan Birmingham
 *
 * Test of the toString functionality.
 **/

#include <mlpack/core.hpp>
#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

#include <mlpack/core/metrics/ip_metric.hpp>
#include <mlpack/core/metrics/lmetric.hpp>
#include <mlpack/core/metrics/mahalanobis_distance.hpp>

#include <mlpack/core/kernels/pspectrum_string_kernel.hpp>
#include <mlpack/core/kernels/example_kernel.hpp>

#include <mlpack/core/optimizers/aug_lagrangian/aug_lagrangian.hpp>
#include <mlpack/core/optimizers/lbfgs/lbfgs.hpp>
#include <mlpack/core/optimizers/sdp/lrsdp.hpp>
#include <mlpack/core/optimizers/sgd/sgd.hpp>
#include <mlpack/methods/nca/nca_softmax_error_function.hpp>
#include <mlpack/core/optimizers/aug_lagrangian/aug_lagrangian_test_functions.hpp>

#include <mlpack/core/tree/ballbound.hpp>
#include <mlpack/core/tree/binary_space_tree.hpp>
#include <mlpack/core/tree/bounds.hpp>
#include <mlpack/core/tree/hrectbound.hpp>
#include <mlpack/core/tree/statistic.hpp>

#include <mlpack/methods/cf/cf.hpp>
#include <mlpack/methods/det/dtree.hpp>
#include <mlpack/methods/emst/dtb.hpp>
#include <mlpack/methods/fastmks/fastmks.hpp>
#include <mlpack/methods/gmm/gmm.hpp>
#include <mlpack/methods/hmm/hmm.hpp>
#include <mlpack/methods/kernel_pca/kernel_pca.hpp>
#include <mlpack/methods/kmeans/kmeans.hpp>
#include <mlpack/methods/lars/lars.hpp>
#include <mlpack/methods/linear_regression/linear_regression.hpp>
#include <mlpack/methods/local_coordinate_coding/lcc.hpp>
#include <mlpack/methods/logistic_regression/logistic_regression.hpp>
#include <mlpack/methods/lsh/lsh_search.hpp>
#include <mlpack/methods/neighbor_search/neighbor_search.hpp>
#include <mlpack/methods/amf/amf.hpp>
#include <mlpack/methods/nca/nca.hpp>
#include <mlpack/methods/pca/pca.hpp>
#include <mlpack/methods/radical/radical.hpp>
#include <mlpack/methods/range_search/range_search.hpp>
#include <mlpack/methods/rann/ra_search.hpp>
#include <mlpack/methods/sparse_coding/sparse_coding.hpp>

using namespace mlpack;
using namespace mlpack::kernel;
using namespace mlpack::distribution;
using namespace mlpack::metric;
using namespace mlpack::nca;
using namespace mlpack::bound;
using namespace mlpack::tree;
using namespace mlpack::neighbor;

//using namespace mlpack::optimization;

BOOST_AUTO_TEST_SUITE(ToStringTest);

std::ostringstream testOstream;

BOOST_AUTO_TEST_CASE(DiscreteDistributionString)
{
  DiscreteDistribution d("0.4 0.5 0.1");
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(GaussianDistributionString)
{
  GaussianDistribution d("0.1 0.3", "1.0 0.1; 0.1 1.0");
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(CosineDistanceString)
{
  CosineDistance d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(EpanechnikovKernelString)
{
  EpanechnikovKernel d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(ExampleKernelString)
{
  ExampleKernel d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(GaussianKernelString)
{
  GaussianKernel d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(HyperbolicTangentKernelString)
{
  HyperbolicTangentKernel d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(LaplacianKernelString)
{
  LaplacianKernel d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(LinearKernelString)
{
  LinearKernel d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(PolynomialKernelString)
{
  PolynomialKernel d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(PSpectrumStringKernelString)
{
  const std::vector<std::vector<std::string> > s;
  const size_t t = 1;
  PSpectrumStringKernel d(s, t);
  Log::Debug << d;
  testOstream << d;
  std::string sttm = d.ToString();
  BOOST_REQUIRE_NE(sttm, "");
}

BOOST_AUTO_TEST_CASE(SphericalKernelString)
{
  SphericalKernel d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(TriangularKernelString)
{
  TriangularKernel d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(IPMetricString)
{
  IPMetric<TriangularKernel> d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(LMetricString)
{
  LMetric<1> d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(MahalanobisDistanceString)
{
  MahalanobisDistance<> d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(SGDString)
{
  const arma::mat g(2, 2);
  const arma::Col<size_t> v(2);
  SoftmaxErrorFunction<> a(g, v);
  mlpack::optimization::SGD<SoftmaxErrorFunction<> > d(a);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(L_BFGSString)
{
  const arma::mat g(2, 2);
  const arma::Col<size_t> v(2);
  SoftmaxErrorFunction<> a(g, v);
  mlpack::optimization::L_BFGS<SoftmaxErrorFunction<> > d(a);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(AugLagString)
{
  mlpack::optimization::AugLagrangianTestFunction a;
  mlpack::optimization::AugLagrangian<
      mlpack::optimization::AugLagrangianTestFunction> d(a);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(LRSDPString)
{
  arma::mat c(40, 40);
  c.randn();
  const size_t b=3;
  mlpack::optimization::LRSDP<mlpack::optimization::SDP<arma::sp_mat>> d(b,b,c);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(BallBoundString)
{
  BallBound<> d(3.5, "1.0 2.0");
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(BinSpaceString)
{
  arma::mat q(2, 50);
  q.randu();
  KDTree<ManhattanDistance, EmptyStatistic, arma::mat> d(q);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(CoverTreeString)
{
  arma::mat q(2, 50);
  q.randu();
  StandardCoverTree<EuclideanDistance, EmptyStatistic, arma::mat> d(q);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(CFString)
{
  arma::mat c(3, 3);
  c(0, 0) = 1;
  c(1, 0) = 2;
  c(2, 0) = 1.5;
  c(0, 1) = 2;
  c(1, 1) = 3;
  c(2, 1) = 2.0;
  c(0, 2) = 1;
  c(1, 2) = 3;
  c(2, 2) = 0.7;
  mlpack::cf::CF<> d(c);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(DetString)
{
  arma::mat c(4, 4);
  c.randn();
  mlpack::det::DTree d(c);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(EmstString)
{
  arma::mat c(4, 4);
  c.randu();
  mlpack::emst::DualTreeBoruvka<> d(c);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(FastMKSString)
{
  arma::mat c(4, 4);
  c.randn();
  mlpack::fastmks::FastMKS<LinearKernel> d(c);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(GMMString)
{
  arma::mat c(400, 40);
  c.randn();
  mlpack::gmm::GMM<> d(5, 4);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(HMMString)
{
  mlpack::hmm::HMM<> d(5, 4);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(KPCAString)
{
  LinearKernel k;
  mlpack::kpca::KernelPCA<LinearKernel> d(k, false);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(KMeansString)
{
  mlpack::kmeans::KMeans<metric::ManhattanDistance> d(100);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(LarsString)
{
  mlpack::regression::LARS d(false);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(LinRegString)
{
  arma::mat c(40, 40);
  arma::mat b(40, 1);
  c.randn();
  b.randn();
  mlpack::regression::LinearRegression d(c, b);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(LCCString)
{
  arma::mat c(40,40);
  const size_t b=3;
  const double a=1;
  c.randn();
  mlpack::lcc::LocalCoordinateCoding<> d(c, b, a);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(LogRegString)
{
  arma::mat c(40, 40);
  arma::Row<size_t> b(40);
  c.randn();
  b.randu();
  mlpack::regression::LogisticRegression<> d(c, b);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(LSHString)
{
  arma::mat c(40, 40);
  const size_t b=3;
  c.randn();
  mlpack::neighbor::LSHSearch<NearestNeighborSort> d(c, b, b);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(NeighborString)
{
  arma::mat c(40, 40);
  c.randn();
  mlpack::neighbor::NeighborSearch<> d(c);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

/*
BOOST_AUTO_TEST_CASE(NMFString)
{
  arma::mat c(40, 40);
  c.randn();
  mlpack::amf::AMF<> d;
  Log::Debug << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}
*/

BOOST_AUTO_TEST_CASE(NCAString)
{
  arma::mat c(40, 40);
  arma::Col<size_t> b(3);
  c.randn();
  mlpack::nca::NCA<> d(c, b);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(PCAString)
{
  mlpack::pca::PCA d(true);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(RadicalString)
{
  mlpack::radical::Radical d;
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(RangeSearchString)
{
  arma::mat c(40, 40);
  c.randn();
  mlpack::range::RangeSearch<> d(c);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(RannString)
{
  arma::mat c(40, 40);
  c.randn();
  mlpack::neighbor::RASearch<> d(c);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_CASE(SparseCodingString)
{
  arma::mat c(40, 40);
  c.randn();
  const size_t b=3;
  double a=0.1;
  mlpack::sparse_coding::SparseCoding<> d(c,b,a);
  Log::Debug << d;
  testOstream << d;
  std::string s = d.ToString();
  BOOST_REQUIRE_NE(s, "");
}

BOOST_AUTO_TEST_SUITE_END();


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_CUMPROD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CUMPROD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-reduction
    Function object implementing cumprod capabilities

    Computes the cumulated product of the vector elements

    Take care that overflow is very easy to get here especially for integral types.
    cimprod of char and unsigned char almost always overflow in SIMD !

    @par semantic:
    For any given vector @c x of type @c T:

    @code
    T r = cumprod(x);
    @endcode

    The function coincide with plus for floating point parameters.
    For integers is similar to:

    @code
    T r =x;
    for(int i=0;i < T::static_size; ++i)
      r[i]*= r[i-1];
    @endcode

  **/
  const boost::dispatch::functor<tag::cumprod_> cumprod = {};
} }
#endif

#include <boost/simd/function/scalar/cumprod.hpp>
#include <boost/simd/function/simd/cumprod.hpp>

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2020 Oded Stein <oded.stein@columbia.edu>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "edge_vectors.h"

#include <Eigen/Geometry>
#include "per_face_normals.h"

#include "PI.h"

template<typename DerivedV,typename DerivedF,typename DerivedE,
typename DerivedoE, typename Derivedvec>
IGL_INLINE void
igl::edge_vectors(
                  const Eigen::MatrixBase<DerivedV> &V,
                  const Eigen::MatrixBase<DerivedF> &F,
                  const Eigen::MatrixBase<DerivedE> &E,
                  const Eigen::MatrixBase<DerivedoE> &oE,
                  Eigen::PlainObjectBase<Derivedvec> &vec)
{
  Eigen::Matrix<typename Derivedvec::Scalar, Eigen::Dynamic, Eigen::Dynamic>
  dummy;
  edge_vectors<false>(V, F, E, oE, vec, dummy);
}

template<bool computePerpendicular,
typename DerivedV,typename DerivedF,typename DerivedE,
typename DerivedoE, typename DerivedvecParallel,
typename DerivedvecPerpendicular>
IGL_INLINE void
igl::edge_vectors(
                  const Eigen::MatrixBase<DerivedV> &V,
                  const Eigen::MatrixBase<DerivedF> &F,
                  const Eigen::MatrixBase<DerivedE> &E,
                  const Eigen::MatrixBase<DerivedoE> &oE,
                  Eigen::PlainObjectBase<DerivedvecParallel> &vecParallel,
                  Eigen::PlainObjectBase<DerivedvecPerpendicular> &vecPerpendicular)
{
  using Scalar = typename DerivedvecParallel::Scalar;
  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;

  assert(E.rows()==F.rows() && "E does not match dimensions of F.");
  assert(oE.rows()==F.rows() && "oE does not match dimensions of F.");
  assert(E.cols()==3 && F.cols()==3 && oE.cols()==3 &&
         "This method is for triangle meshes.");
  assert(F.maxCoeff()<V.rows() && "V does not seem to belong to F.");

  const typename DerivedE::Scalar m = E.maxCoeff()+1;

  //Compute edge-based normal
  MatX N, edgeN(m, 3);
  edgeN.setZero();
  per_face_normals(V, F, N);
  for(Eigen::Index i=0; i<E.rows(); ++i) {
    for(int j=0; j<3; ++j) {
      edgeN.row(E(i,j)) += N.row(i);
    }
  }
  edgeN.rowwise().normalize();

  //Compute edge vectors
  vecParallel.resize(m, 3);
  if(computePerpendicular) { //This should ideally be an if constexpr
    vecPerpendicular.resize(m, 3);
  }
  for(Eigen::Index i=0; i<E.rows(); ++i) {
    for(int j=0; j<3; ++j) {
      if(oE(i,j)<0) {
        continue;
      }
      const typename DerivedE::Scalar e=E(i,j);
      const typename DerivedF::Scalar vi=F(i,(j+1)%3), vj=F(i,(j+2)%3);
      vecParallel.row(e) = (V.row(vj)-V.row(vi)).normalized();
      if(computePerpendicular) { //This should ideally be an if constexpr
        vecPerpendicular.row(e) =
        Eigen::AngleAxis<Scalar>(0.5*PI, edgeN.row(e)) *
        vecParallel.row(e).transpose();
      }
    }
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::edge_vectors<true, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


#include "mymath.h"

#include <Eigen/Dense>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include <string>

// Create the Python module
PYBIND11_MODULE(bindings, module)
{
    namespace py = ::pybind11;
    module.doc() = "mymath bindings";

    // These methods accept numpy array but return float / list
    module.def("dot", &mymath::dot, py::arg("vector1"), py::arg("vector2"));
    module.def("normalize", &mymath::normalize, py::arg("data"));

    // In order to return directly a numpy object, we can pass through Eigen
    module.def(
        "normalize_numpy",
        [](const std::vector<double>& data) -> Eigen::VectorXd {
            const auto output = mymath::normalize(data);
            return Eigen::Map<Eigen::VectorXd>(
                const_cast<double*>(output.data()), output.size());
        },
        py::arg("data"));
}


/*
	Copyright (C) 2003-2014 by David White <davewx7@gmail.com>

	This software is provided 'as-is', without any express or implied
	warranty. In no event will the authors be held liable for any damages
	arising from the use of this software.

	Permission is granted to anyone to use this software for any purpose,
	including commercial applications, and to alter it and redistribute it
	freely, subject to the following restrictions:

	   1. The origin of this software must not be misrepresented; you must not
	   claim that you wrote the original software. If you use this software
	   in a product, an acknowledgement in the product documentation would be
	   appreciated but is not required.

	   2. Altered source versions must be plainly marked as such, and must not be
	   misrepresented as being the original software.

	   3. This notice may not be removed or altered from any source
	   distribution.
*/

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <ctime>

#include "random.hpp"

namespace rng
{
	namespace
	{
		boost::random::mt19937 state;
		boost::random::uniform_int_distribution<> generator(0,0xFFFFFF);
		bool rng_init = false;
	}

	int generate()
	{
		if(!rng_init) {
			// using std::time to initialise a mersienne twister is a really pitiful and inadequate idea.
			seed_from_int(static_cast<unsigned int>(std::time(NULL)));
		}
		return generator(state);
	}

	void seed_from_int(unsigned int seed)
	{
		rng_init = true;
		state = boost::random::mt19937(seed);
	}

	void set_seed(const Seed& seed)
	{
		state = seed;
	}

	Seed get_seed()
	{
		return state;
	}
}