// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <string>
#include <vector>
#include <boost/timer.hpp>

#ifdef MTL_HAS_PAPI
#  include <papi.h>
#endif

#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/operation/print_matrix.hpp>
#include <boost/numeric/mtl/operation/matrix_mult.hpp>
#include <boost/numeric/mtl/matrix/hessian_setup.hpp>
#include <boost/numeric/mtl/operation/assign_mode.hpp>
#include <boost/numeric/mtl/utility/papi.hpp>

// /san/intel/cce/9.0/bin/icc -g no_inline_papi_timing.cpp -o no_inline_papi_timing -I${MTL_BOOST_ROOT} -I${BOOST_ROOT} -I/usr/local/include -L/usr/local/lib -lpapi -DMTL_HAVE_PAPI -DMTL_HAS_BLAS -lblas -DMTL_HAS_LAPACK -llapack

// or
// g++4 -DNDEBUG -O3 -ffast-math no_inline_papi_timing.cpp -o no_inline_papi_timing -I${MTL_BOOST_ROOT} -I${BOOST_ROOT} -I/usr/local/include -L/usr/local/lib -lpapi -DMTL_HAS_PAPI -DMTL_HAS_BLAS -lblas -DMTL_HAS_LAPACK -llapack

/* **** on odin ****
setenv LD_LIBRARY_PATH /usr/local/lib64
-I/usr/local/include -L/usr/local/lib -lpapi
*  ****         ****/

extern "C" {
void dgemm_(const char* transa, const char* transb,
	    const int* m, const int* n, const int* k,
	    const double* alpha,  const double *da,  const int* lda,
	    const double *db, const int* ldb, const double* dbeta,
	    double *dc, const int* ldc);

// Cholesky factorization
void dpotrf_(const char* uplo, const int* n, double *a, const int* ld, int* info);
} // extern "C"

using namespace mtl;
using namespace mtl::recursion;
using namespace std;

// Maximum time for a single measurement
// is 20 min
const double max_time= 900;

    // Bitmasks:
    const unsigned long morton_mask= generate_mask<true, 0, row_major, 0>::value,
	morton_z_mask= generate_mask<false, 0, row_major, 0>::value,
	doppled_16_row_mask= generate_mask<true, 4, row_major, 0>::value,
	doppled_16_col_mask= generate_mask<true, 4, col_major, 0>::value,
	doppled_32_row_mask= generate_mask<true, 5, row_major, 0>::value,
	doppled_32_col_mask= generate_mask<true, 5, col_major, 0>::value,
	doppled_z_32_row_mask= generate_mask<false, 5, row_major, 0>::value,
	doppled_z_32_col_mask= generate_mask<false, 5, col_major, 0>::value,
	doppled_64_row_mask= generate_mask<true, 6, row_major, 0>::value,
	doppled_64_col_mask= generate_mask<true, 6, col_major, 0>::value,
	doppled_z_64_row_mask= generate_mask<false, 6, row_major, 0>::value,
	doppled_z_64_col_mask= generate_mask<false, 6, col_major, 0>::value,
	doppled_128_row_mask= generate_mask<true, 7, row_major, 0>::value,
	doppled_128_col_mask= generate_mask<true, 7, col_major, 0>::value,
	shark_32_row_mask= generate_mask<true, 5, row_major, 1>::value,
	shark_32_col_mask= generate_mask<true, 5, col_major, 1>::value,
	shark_z_32_row_mask= generate_mask<false, 5, row_major, 1>::value,
	shark_z_32_col_mask= generate_mask<false, 5, col_major, 1>::value,
	shark_64_row_mask= generate_mask<true, 6, row_major, 1>::value,
	shark_64_col_mask= generate_mask<true, 6, col_major, 1>::value,
	shark_z_64_row_mask= generate_mask<false, 6, row_major, 1>::value,
	shark_z_64_col_mask= generate_mask<false, 6, col_major, 1>::value;

typedef assign::plus_sum                            ama_t;

typedef recursion::bound_test_static<32>                    test32_t;
typedef recursion::bound_test_static<64>                    test64_t;

typedef gen_dense_mat_mat_mult_t<assign::plus_sum>  base_mult_t;
typedef gen_recursive_dense_mat_mat_mult_t<base_mult_t>     rec_mult_t;

typedef gen_tiling_22_dense_mat_mat_mult_t<assign::plus_sum>  tiling_22_base_mult_t;
typedef gen_tiling_44_dense_mat_mat_mult_t<assign::plus_sum>  tiling_44_base_mult_t;
typedef gen_tiling_dense_mat_mat_mult_t<2, 4, assign::plus_sum>  tiling_24_base_mult_t;

#ifdef MTL_HAS_PAPI
int retval, Counters, EventSet=PAPI_NULL, aktivEvents=0, EventCode, l2_code, tlb_code, l2_index, tlb_index;
long_long *values;
#endif

utility::papi_t papi;
int l1i= papi.add_event("PAPI_L1_DCM");
int l2i= papi.add_event("PAPI_L2_DCM");
int tlbi= papi.add_event("PAPI_TLB_DM");

void print_time_and_mflops(double time, double size)
{
    // time and MFlops of single measure
    std::cout << time << ", " << 2.0 * size * size * size / time / 1e6f << ", ";
}

// Matrices are only placeholder to provide the type
template <typename MatrixA, typename MatrixB, typename MatrixC, typename Mult>
void single_measure(MatrixA&, MatrixB&, MatrixC&, Mult mult, unsigned size, std::vector<int>& enabled, int i)
{

    MatrixA a(size, size);
    MatrixB b(size, size);
    MatrixC c(size, size);
    hessian_setup(a, 1.0);
    hessian_setup(b, 1.0);

    if (enabled[i])
    {

	int reps= 0;
	boost::timer start;
	papi.reset();

	for (; start.elapsed() < 1; reps++)
	    mult(a, b, c);
	papi.read();
	double time= start.elapsed() / double(reps);

	print_time_and_mflops(time, a.num_rows());
	std::cout << papi[l1i]/reps << ", " << papi[l2i]/reps << ", " << papi[tlbi]/reps << ", ";

	if (time > max_time)
	    enabled[i]= 0;
    } else
	std::cout << ", , , , , ";
}

template <typename Functor, typename Result, typename Arg1, typename Arg2, typename Arg3>
struct no_inline3
{
    Result operator()(Arg1& arg1, Arg2& arg2, Arg3& arg3)
    {
	static Functor* f= new(Functor);
	return apply(f, arg1, arg2, arg3);
    }

    Result apply(Functor* f, Arg1& arg1, Arg2& arg2, Arg3& arg3)
    {
	return (*f)(arg1, arg2, arg3);
    }
};

#if 0
// Specialization not needed, at least not with g++
template <typename Functor, typename Arg1, typename Arg2, typename Arg3>
struct no_inline3<Functor, void, Arg1, Arg2, Arg3>
{

    void operator()(Arg1& arg1, Arg2& arg2, Arg3& arg3)
    {
	Functor* f= new(Functor);
	apply(f, arg1, arg2, arg3);
	free(f);
    }

    void apply(Functor* f, Arg1& arg1, Arg2& arg2, Arg3& arg3)
    {
	(*f)(arg1, arg2, arg3);
    }
};
#endif

typedef dense2D<double> rmt;
typedef dense2D<double, mat::parameters<col_major> > cmt;

// C must have even dimensions
template <typename MatrixA, typename MatrixB, typename MatrixC>
void mult_simple_ptu22t(const MatrixA& a, const MatrixB& b, MatrixC& c)
{
    typedef typename MatrixC::value_type  value_type;
    const value_type z= math::zero(c[0][0]);    // if this are matrices we need their size

    set_to_zero(c);

    // Temporary solution; dense matrices need to return const referencens
    MatrixA& aref= const_cast<MatrixA&>(a);
    MatrixB& bref= const_cast<MatrixB&>(b);

    size_t ari= &aref(1, 0) - &aref(0, 0), // how much is the offset of A's entry increased by incrementing row
	aci= &aref(0, 1) - &aref(0, 0), bri= &bref(1, 0) - &bref(0, 0), bci= &bref(0, 1) - &bref(0, 0);

    for (unsigned i= 0; i < c.num_rows(); i+=2)
	for (unsigned k= 0; k < c.num_cols(); k+=2) {
	    int ld= b.num_rows();
	    value_type tmp00= z, tmp01= z, tmp10= z, tmp11= z;

	    const value_type *begin_a= &aref[i][0], *end_a= &aref[i][a.num_cols()];
	    const value_type *begin_b= &bref[0][k];
	    for (; begin_a != end_a; begin_a+= aci, begin_b+= bri) {
		tmp00+= *begin_a * *begin_b;
		tmp01+= *begin_a * *(begin_b+bci);
		tmp10+= *(begin_a+ari) * *begin_b;
		tmp11+= *(begin_a+ari) * *(begin_b+bci);
	    }
	    assign::assign_sum::update(c[i][k], tmp00);
	    assign::assign_sum::update(c[i][k+1], tmp01);
	    assign::assign_sum::update(c[i+1][k], tmp10);
	    assign::assign_sum::update(c[i+1][k+1], tmp11);

#if 0
	    c[i][k]= tmp00; c[i][k+1]= tmp01;
	    c[i+1][k]= tmp10; c[i+1][k+1]= tmp11;
#endif
	}
}

template <typename Matrix, typename MatrixB>
void measure_unrolling(unsigned size, std::vector<int>& enabled, Matrix& matrix, MatrixB& matrixb)
{
    std::cout << size << ", ";

    dense2D<double> dense(4, 4);
    dense2D<double, mat::parameters<col_major> >    denseb(4, 4);

    gen_recursive_dense_mat_mat_mult_t<base_mult_t>           mult;
    gen_recursive_dense_mat_mat_mult_t<tiling_22_base_mult_t> mult_22;
    gen_recursive_dense_mat_mat_mult_t<tiling_44_base_mult_t> mult_44;
    gen_recursive_dense_mat_mat_mult_t<tiling_24_base_mult_t> mult_24;

    typedef gen_tiling_22_dense_mat_mat_mult_ft<Matrix, MatrixB, Matrix, assign::plus_sum>
      tiling_22_t;
    typedef no_inline3<tiling_22_t, void, const Matrix, const MatrixB, Matrix> tiling_22_no_inline_t;

    typedef gen_tiling_44_dense_mat_mat_mult_ft<Matrix, MatrixB, Matrix, assign::plus_sum>
      tiling_44_t;
    typedef no_inline3<tiling_44_t, void, const Matrix, const MatrixB, Matrix> tiling_44_no_inline_t;

    // gen_recursive_dense_mat_mat_mult_t<ext_mult_44>           rec_ext_mult_44;

    typedef typename base_case_matrix<Matrix, test64_t>::type    BaseMatrix;
    typedef typename base_case_matrix<MatrixB, test64_t>::type   BaseMatrixB;
    typedef no_inline3<tiling_44_t, void, const BaseMatrix, const BaseMatrixB, BaseMatrix> tiling_44_base_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_44_base_mult_t>    rec_no_inline_mult_44;

    single_measure(matrix, matrixb, matrix, mult, size, enabled, 0);
    single_measure(matrix, matrixb, matrix, mult_22, size, enabled, 1);
    single_measure(matrix, matrixb, matrix, mult_44, size, enabled, 2);
    single_measure(dense, denseb, dense, mult_simple_ptu22t<rmt, cmt, rmt>, size, enabled, 9);

    std::cout << "0\n";  std::cout.flush();
}

void measure_unrolling_hybrid(unsigned size, std::vector<int>& enabled)
{
    morton_dense<double,  doppled_64_row_mask>     d64r(4, 4);
    morton_dense<double,  doppled_64_col_mask>     d64c(4, 4);
    measure_unrolling(size, enabled, d64r, d64c);
}

void measure_unrolling_dense(unsigned size, std::vector<int>& enabled)
{
    dense2D<double> dense(4, 4);
    dense2D<double, mat::parameters<col_major> >    b(4, 4);
    measure_unrolling(size, enabled, dense, b);
}

template <typename Measure>
void series(unsigned steps, unsigned max_size, Measure measure, const string& comment)
{
    std::cout << "# " << comment << '\n';
    std::cout << "# Gnu-Format size, (time, MFlops, L1 misses, L2 misses, TLB misses, ), ...\n"; std::cout.flush();

    std::vector<int> enabled(16, 1);
    for (unsigned i= steps; i <= max_size; i+= steps)
	measure(i, enabled);
}

void init_papi()
{
#ifdef MTL_HAS_PAPI

  retval = PAPI_library_init(PAPI_VER_CURRENT);
  if (retval != PAPI_VER_CURRENT && retval > 0) {
    printf("PAPI Library version mismatch! Library not initialized\n");
    exit(1);
  }

  if ((retval = PAPI_get_opt(PAPI_MAX_HWCTRS, NULL)) <= PAPI_OK) { printf("papi error\n");  exit(1);}
  else {
    Counters =retval;
    printf("%i Counters found = OK\n",Counters);
  }
  if ((values = (long_long*)calloc((size_t)Counters,sizeof(long_long))) == NULL ) {
    fprintf(stderr,"Allocation memory for countervalues failed");  exit(1);}

  retval = PAPI_create_eventset(&EventSet);
  if (retval != PAPI_OK) { printf("papi error (create_eventset)\n"); exit(1);}
  else printf("Create EventSet = OK (%i)\n", EventSet);

  if ((retval = PAPI_event_name_to_code("PAPI_TLB_TL",&tlb_code)) != PAPI_OK )
    {printf("event_anme_to_code papi error\n"); exit(1);}
  else if ((retval = PAPI_query_event(tlb_code)) != PAPI_OK) { printf("Can't add event : ->"); }
  else if ((retval = PAPI_add_event(EventSet,tlb_code)) != PAPI_OK) {printf("papi add_event error\n"); exit(1);}
  else {printf("added to Eventset = OK\n"); aktivEvents++;}
  tlb_index= 0;

  if ((retval = PAPI_event_name_to_code("PAPI_L2_TCM",&l2_code)) != PAPI_OK )
    {printf("event_anme_to_code papi error\n"); exit(1);}
  else if ((retval = PAPI_query_event(l2_code)) != PAPI_OK) { printf("Can't add event : ->"); }
  else if ((retval = PAPI_add_event(EventSet, l2_code)) != PAPI_OK) {printf("papi add_event error\n"); exit(1);}
  else {printf("added to Eventset = OK\n"); l2_index= aktivEvents++;}

  if ((retval = PAPI_start(EventSet)) != PAPI_OK) { printf("papi start error\n");exit(1);}

  if ((retval = PAPI_reset(EventSet)) != PAPI_OK) { printf("papi reset error\n");exit(1);}

#endif
}

void test_blas()
{
    dense2D<double> a(4, 4), c(4, 4);
    dense2D<double, mat::parameters<col_major> >    b(4, 4);
    int size= 4; double one= 1.0;

#ifdef MTL_HAS_BLAS
    // cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 4, 4, 4, 1.0, &a[0][0], 4, &b[0][0], 4, 1.0, &c[0][0], 4);
    dgemm_("N", "T", &size, &size, &size, &one, &a[0][0], &size, &b[0][0], &size, &one, &c[0][0], &size);

#endif

#ifdef MTL_HAS_LAPACK
    int info;
    dpotrf_("U", &size, &a[0][0], &size, &info);
#endif
}

int main(int argc, char* argv[])
{
    //init_papi();
    papi.start();

    test_blas();

    std::vector<std::string> scenarii;
    scenarii.push_back(string("Comparing different unrolling for hybrid row-major matrices"));

    using std::cout;
    if (argc < 3) {
	cerr << "usage: recursive_mult_timing  <steps> <max_size>\n";
	exit(1);
    }
    unsigned int steps= atoi(argv[1]), max_size= atoi(argv[2]), size= 32;
    series(steps, max_size, measure_unrolling_hybrid, scenarii[0]);

    return 0;

}


// Boost.Geometry Index
// Additional tests

// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.

// 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 <iostream>
#include <fstream>

#define BOOST_GEOMETRY_INDEX_DETAIL_EXPERIMENTAL

#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/geometry/index/detail/rtree/utilities/statistics.hpp>

#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/serialization/vector.hpp>

#include <boost/foreach.hpp>
#include <boost/timer.hpp>

template <typename T, size_t I = 0, size_t S = boost::tuples::length<T>::value>
struct print_tuple
{
    template <typename Os>
    static inline Os & apply(Os & os, T const& t)
    {
        os << boost::get<I>(t) << ", ";
        return print_tuple<T, I+1>::apply(os, t);
    }
};

template <typename T, size_t S>
struct print_tuple<T, S, S>
{
    template <typename Os>
    static inline Os & apply(Os & os, T const&)
    {
        return os;
    }
};

int main()
{
    namespace bg = boost::geometry;
    namespace bgi = bg::index;

    typedef boost::tuple<std::size_t, std::size_t, std::size_t, std::size_t, std::size_t, std::size_t> S;

    typedef bg::model::point<double, 2, bg::cs::cartesian> P;
    typedef bg::model::box<P> B;
    typedef B V;
    //typedef bgi::rtree<V, bgi::linear<16> > RT;
    //typedef bgi::rtree<V, bgi::quadratic<8, 3> > RT;
    //typedef bgi::rtree<V, bgi::rstar<8, 3> > RT;
    typedef bgi::rtree<V, bgi::dynamic_linear > RT;

    //RT tree;
    RT tree(bgi::dynamic_linear(16));
    std::vector<V> vect;

    boost::timer t;

    //insert values
    {
        for ( double x = 0 ; x < 1000 ; x += 1 )
            for ( double y = 0 ; y < 1000 ; y += 1 )
                vect.push_back(B(P(x, y), P(x+0.5, y+0.5)));
        RT tmp(vect, tree.parameters());
        tree = boost::move(tmp);
    }
    B q(P(5, 5), P(6, 6));
    S s;

    std::cout << "vector and tree created in: " << t.elapsed() << std::endl;

    print_tuple<S>::apply(std::cout, bgi::detail::rtree::utilities::statistics(tree)) << std::endl;
    std::cout << boost::get<0>(s) << std::endl;
    BOOST_FOREACH(V const& v, tree | bgi::adaptors::queried(bgi::intersects(q)))
        std::cout << bg::wkt<V>(v) << std::endl;

    // save
    {
        std::ofstream ofs("serialized_vector.bin", std::ios::binary | std::ios::trunc);
        boost::archive::binary_oarchive oa(ofs);
        t.restart();
        oa << vect;
        std::cout << "vector saved to bin in: " << t.elapsed() << std::endl;
    }
    {
        std::ofstream ofs("serialized_tree.bin", std::ios::binary | std::ios::trunc);
        boost::archive::binary_oarchive oa(ofs);
        t.restart();
        oa << tree;
        std::cout << "tree saved to bin in: " << t.elapsed() << std::endl;
    }
    {
        std::ofstream ofs("serialized_tree.xml", std::ios::trunc);
        boost::archive::xml_oarchive oa(ofs);
        t.restart();
        oa << boost::serialization::make_nvp("rtree", tree);
        std::cout << "tree saved to xml in: " << t.elapsed() << std::endl;
    }

    t.restart();
    vect.clear();
    std::cout << "vector cleared in: " << t.elapsed() << std::endl;

    t.restart();
    tree.clear();
    std::cout << "tree cleared in: " << t.elapsed() << std::endl;

    // load

    {
        std::ifstream ifs("serialized_vector.bin", std::ios::binary);
        boost::archive::binary_iarchive ia(ifs);
        t.restart();
        ia >> vect;
        std::cout << "vector loaded from bin in: " << t.elapsed() << std::endl;
        t.restart();
        RT tmp(vect, tree.parameters());
        tree = boost::move(tmp);
        std::cout << "tree rebuilt from vector in: " << t.elapsed() << std::endl;
    }

    t.restart();
    tree.clear();
    std::cout << "tree cleared in: " << t.elapsed() << std::endl;

    {
        std::ifstream ifs("serialized_tree.bin", std::ios::binary);
        boost::archive::binary_iarchive ia(ifs);
        t.restart();
        ia >> tree;
        std::cout << "tree loaded from bin in: " << t.elapsed() << std::endl;
    }

    std::cout << "loaded from bin" << std::endl;
    print_tuple<S>::apply(std::cout, bgi::detail::rtree::utilities::statistics(tree)) << std::endl;
    BOOST_FOREACH(V const& v, tree | bgi::adaptors::queried(bgi::intersects(q)))
        std::cout << bg::wkt<V>(v) << std::endl;

    t.restart();
    tree.clear();
    std::cout << "tree cleared in: " << t.elapsed() << std::endl;

    {
        std::ifstream ifs("serialized_tree.xml");
        boost::archive::xml_iarchive ia(ifs);
        t.restart();
        ia >> boost::serialization::make_nvp("rtree", tree);
        std::cout << "tree loaded from xml in: " << t.elapsed() << std::endl;
    }

    std::cout << "loaded from xml" << std::endl;
    print_tuple<S>::apply(std::cout, bgi::detail::rtree::utilities::statistics(tree)) << std::endl;
    BOOST_FOREACH(V const& v, tree | bgi::adaptors::queried(bgi::intersects(q)))
        std::cout << bg::wkt<V>(v) << std::endl;

    return 0;
}


/*
 * Borrows from old color_detector_avc and from image_transform.
 * N.B. pass in a binary image in 1-channel "mono8" format.
 * Any non-zero element in the input image becomes a point in a
 * cloud.
 */

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <pcl/point_cloud.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <boost/algorithm/string.hpp>

using namespace std;

using uchar = unsigned char;

map<string, ros::Publisher> cloud_pubs;
double pxPerMeter;

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;

void transformedImageCB(const sensor_msgs::ImageConstPtr& msg, const string& topic) {
    if(cloud_pubs[topic].getNumSubscribers() == 0) {
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptr;
    try {
        cv_ptr = cv_bridge::toCvShare(msg);
    } catch(cv_bridge::Exception& e) {
        ROS_ERROR("CV_Bridge error: %s", e.what());
        return;
    }

    const cv::Mat& in_image = cv_ptr->image;

    // get outline
    cv::Mat transformed;
    cv::Laplacian(in_image, transformed, CV_16SC1);

    cloud->clear();
    for(int r = 0; r < transformed.rows; r++) {
        auto* row = transformed.ptr<int16_t>(r);
        for(int c = 0; c < transformed.cols; c++) {
            if(row[c] != 0) {
                pcl::PointXYZ point;
                point.y = static_cast<float>(((transformed.cols / 2.0f) - c) / pxPerMeter);
                point.x = static_cast<float>((transformed.rows - r) / pxPerMeter);
                point.z = 0.0;

                cloud->push_back(point);
            }
        }
    }

    pcl::PCLPointCloud2 cloud_pc2;
    pcl::toPCLPointCloud2(*cloud, cloud_pc2);
    sensor_msgs::PointCloud2 cloud_msg;
    pcl_conversions::fromPCL(cloud_pc2, cloud_msg);
    cloud_msg.header.frame_id = "base_footprint";
    cloud_msg.header.stamp = ros::Time::now();
    cloud_pubs[topic].publish(cloud_msg);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "image_pcl_converter");
    ros::NodeHandle nh;
    ros::NodeHandle nhp("~");

    string topicsConcat;
    nhp.getParam("image_topics", topicsConcat);
    nhp.getParam("px_per_meter", pxPerMeter);

    cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);

    vector<string> topics;
    boost::split(topics, topicsConcat, boost::is_any_of(" ,"));
    vector<ros::Subscriber> image_subs;
    for(const string &topic : topics) {
        if(topic.size() == 0) continue;

        auto bound_callback = boost::bind(transformedImageCB, _1, topic);
        auto sub = nh.subscribe<sensor_msgs::Image>(topic, 1, bound_callback);
        image_subs.push_back(sub);

        ROS_INFO_STREAM("image_pcl_converter subscribed to " << topic);
        string newTopic = topic + "_cloud";
        ROS_INFO_STREAM("Creating new topic " << newTopic);
        cloud_pubs[topic] = nh.advertise<sensor_msgs::PointCloud2>(newTopic, 1);
    }

    ros::spin();
    return 0;
}


// TODO: create info to this file

#pragma once

#include <memory>
#include <utility>

#include <Eigen/StdVector>
#include <Eigen/StdDeque>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
//#include <Eigen/>

namespace cfe {

// Typedefs for Eigen types
typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >
  PointVector;
typedef std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond> >
  QuaternionVector;
typedef Eigen::Quaterniond Quaternion;
typedef Eigen::Transform<double,3,Eigen::Affine> PoseTransform;
typedef Eigen::Translation<double,3> PoseTranslation;
typedef Eigen::Matrix<double, 28, 1> StateQuat5;
//typedef std::vector<Eigen::Transform, Eigen::aligned_allocator<Eigen::Transform> > PoseTransformVector;

// Typedefs

typedef struct Point{
    double x,y,z;
} point3d;
//ADD Transformation Matrices
//ADD States
//ADD ....
//  typedef std::vector<Eigen::Affine3f, Eigen::aligned_allocator<Eigen::Affine3f>> VectorAffine3f;

} // namespace cfe


/*
 *  Copyright 2013 Néstor Morales Hernández <nestor@isaatc.ull.es>
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

#include "coherentpointdrift.h"
#include "decomposelu.h"
#include <pcl/common/io.h>

#include <stdio.h>
#include <boost/graph/graph_concepts.hpp>

#include <omp.h>

#include <opencv2/opencv.hpp>

#include <cuda_runtime.h>

#define IDX2C(i,j,ld) (((j)*(ld))+(i))

using namespace std;

CoherentPointDrift::CoherentPointDrift()
{
    // Default parameters
    m_tolerance = 1e-5f;
    m_beta = 2.0f;
    m_maxIt = 150;
    m_omega = 0.1f;
    m_lambda = 3.0f;

    checkCublasReturn(cublasCreate_v2(&m_cublasHandle), __LINE__);
    checkCudaError(cudaGetDeviceProperties(&m_deviceProp, 0), __LINE__);

    cout << m_deviceProp.name << endl;
}

void CoherentPointDrift::allocateMatrices()
{
    // TODO: Improve the efficiency doing an allocation that takes into account the size of the block
    cudaError_t error;

    // Sizes of matrices
    uint32_t size_X = m_N * m_D;
    uint32_t size_Y = m_M * m_D;
    uint32_t size_W = m_M * m_D;
    uint32_t size_G = m_M * m_M;
    uint32_t size_P = m_M * m_N;
    uint32_t size_P1 = m_M;
    uint32_t size_Pt1 = m_N;
    uint32_t size_PX = m_M * m_D;
    uint32_t size_Pivot = m_M;
    uint32_t size_GW = m_M * m_D;
    uint32_t size_DxD = m_D * m_D;
    uint32_t size_MxD = m_M * m_D;
    uint32_t size_MxN = m_M * m_N;
    uint32_t size_MxM = m_M * m_M;
    uint32_t size_Dx1 = m_D;
    uint32_t size_1xD = m_D;
    uint32_t size_1xM = m_M;
    uint32_t size_1xN = m_N;

    // Mem sizes
    uint32_t mem_size_X = sizeof(float) * size_X;
    uint32_t mem_size_Y = sizeof(float) * size_Y;
    uint32_t mem_size_W = sizeof(float) * size_W;
    uint32_t mem_size_G = sizeof(float) * size_G;
    uint32_t mem_size_P = sizeof(float) * size_P;
    uint32_t mem_size_P1 = sizeof(float) * size_P1;
    uint32_t mem_size_Pt1 = sizeof(float) * size_Pt1;
    uint32_t mem_size_PX = sizeof(float) * size_PX;
    uint32_t mem_size_Pivot = sizeof(int) * size_Pivot;
    uint32_t mem_size_GW = sizeof(float) * size_GW;
    uint32_t mem_size_DxD = sizeof(float) * size_DxD;
    uint32_t mem_size_MxD = sizeof(float) * size_MxD;
    uint32_t mem_size_MxN = sizeof(float) * size_MxN;
    uint32_t mem_size_MxM = sizeof(float) * size_MxM;
    uint32_t mem_size_Dx1 = sizeof(float) * size_Dx1;
    uint32_t mem_size_1xD = sizeof(float) * size_1xD;
    uint32_t mem_size_1xM = sizeof(float) * size_1xM;
    uint32_t mem_size_1xN = sizeof(float) * size_1xN;

    // Host allocation
    h_X = (float *)malloc(mem_size_X);
    h_Y = (float *)malloc(mem_size_Y);
    float * h_W = (float *)malloc(mem_size_W);
    int * h_pivotArray = (int *)malloc(mem_size_Pivot);
    float * h_ones1xM = (float *)malloc(mem_size_1xM);
    float * h_ones1xN = (float *)malloc(mem_size_1xN);

    // Initialization
    for (uint32_t i = 0; i < m_N; i++) {
        h_X[IDX2C(i, 0, m_N)] = m_cloudX->at(i).x;
        h_X[IDX2C(i, 1, m_N)] = m_cloudX->at(i).y;
        h_X[IDX2C(i, 2, m_N)] = m_cloudX->at(i).z;
        h_ones1xN[i] = 1.0;
    }

    for (uint32_t i = 0; i < m_M; i++) {
        h_Y[IDX2C(i, 0, m_M)] = m_cloudY->at(i).x;
        h_Y[IDX2C(i, 1, m_M)] = m_cloudY->at(i).y;
        h_Y[IDX2C(i, 2, m_M)] = m_cloudY->at(i).z;
        h_ones1xM[i] = 1.0;
        h_pivotArray[i] = 1;
    }

    for (uint32_t i = 0; i < size_Y; i++) {
        h_W[i] = 0.0f;
    }

    // allocate device memory
    checkCudaError(cudaMalloc((void **) &d_X, mem_size_X), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_Y, mem_size_Y), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_W, mem_size_W), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_G, mem_size_G), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_P, mem_size_P), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_P1, mem_size_P1), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_Pt1, mem_size_Pt1), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_PX, mem_size_PX), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_pivotArray, mem_size_Pivot), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_GW, mem_size_GW), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_tmpDxD, mem_size_DxD), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_tmpMxD, mem_size_MxD), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_tmpMxDb, mem_size_MxD), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_tmpMxN, mem_size_MxN), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_tmpMxM, mem_size_MxM), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_tmpMxMb, mem_size_MxM), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_tmpDx1, mem_size_Dx1), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_tmp1xD, mem_size_1xD), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_tmp1xN, mem_size_1xN), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_ones1xM, mem_size_1xM), __LINE__);
    checkCudaError(cudaMalloc((void **) &d_ones1xN, mem_size_1xN), __LINE__);

    // copy host memory to device
    checkCudaError(cudaMemcpy(d_X, h_X, mem_size_X, cudaMemcpyHostToDevice), __LINE__);
    checkCudaError(cudaMemcpy(d_Y, h_Y, mem_size_Y, cudaMemcpyHostToDevice), __LINE__);
    checkCudaError(cudaMemcpy(d_W, h_W, mem_size_W, cudaMemcpyHostToDevice), __LINE__);
    checkCudaError(cudaMemcpy(d_pivotArray, h_pivotArray, mem_size_Pivot, cudaMemcpyHostToDevice), __LINE__);
    checkCudaError(cudaMemcpy(d_ones1xM, h_ones1xM, mem_size_1xM, cudaMemcpyHostToDevice), __LINE__);
    checkCudaError(cudaMemcpy(d_ones1xN, h_ones1xN, mem_size_1xN, cudaMemcpyHostToDevice), __LINE__);
}

bool CoherentPointDrift::compute(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloudX, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloudY)
{
    // FIXME: At this point, the number of dimensions is limited to 3
    m_D = 3;
    m_N = /*5; //*/cloudX->size();
    m_M = /*6; //*/cloudY->size();

    m_cloudX = cloudX;
    m_cloudY = cloudY;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudT(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*cloudY, *cloudT);

    allocateMatrices();

    m_sigma2 = getSigma2();
    float sigma2Init = m_sigma2;
    cout << "sigma2 " << m_sigma2 << endl;

    getG();

    uint32_t iter = 0;
    float nTolerance = m_tolerance + 10.0f;
    float L = 1.0;
    float Lold;

    while ((iter < m_maxIt) && (nTolerance > m_tolerance) && (m_sigma2 > 1e-8)) {
        Lold = L;

        getP();

        getW();

        // TODO: By the moment, we do not use FGT
        exit(0);
    }

}

float CoherentPointDrift::getSigma2()
{
    cublasStatus_t retCublas;

    float sigma2 = 0.0;
    const float alpha = 1.0f;
    const float beta  = 0.0f;

    // Let's obtain M * trace(X' * X)
    {
        checkCublasReturn(cublasSgemm_v2(m_cublasHandle, CUBLAS_OP_T, CUBLAS_OP_N,
                                        m_D, m_D, m_N, &alpha, d_X, m_N, d_X, m_N, &beta, d_tmpDxD, m_D), __LINE__);
        sigma2 = m_M * trace(d_tmpDxD, m_D, m_D);
    }

    // Let's obtain N * trace(Y' * Y)
    {
        checkCublasReturn(cublasSgemm_v2(m_cublasHandle, CUBLAS_OP_T, CUBLAS_OP_N,
                                         m_D, m_D, m_M, &alpha, d_Y, m_M, d_Y, m_M, &beta, d_tmpDxD, m_D), __LINE__);
        sigma2 += m_N * trace(d_tmpDxD, m_D, m_D);
    }

    // Let's obtain 2 * sum(X) * sum(Y)'
    {
        // Let's obtain sum(X)'
        float * d_sumXt = d_tmp1xD;
        checkCublasReturn(cublasSgemv_v2(m_cublasHandle, CUBLAS_OP_T,
                                         m_N, m_D, &alpha, d_X, m_N, d_ones1xN, 1, &beta, d_sumXt, 1), __LINE__);

        // Let's obtain sum(Y)'
        float * d_sumYt = d_tmpDx1;
        checkCublasReturn(cublasSgemv_v2(m_cublasHandle, CUBLAS_OP_T,
                                         m_M, m_D, &alpha, d_Y, m_M, d_ones1xM, 1, &beta, d_sumYt, 1), __LINE__);

        // Let's obtain 2 * sum(X) * sum(Y)'
        float * h_sumX, * h_sumY;
        getHostMatrix(d_sumXt, h_sumX, 1, m_D);
        getHostMatrix(d_sumYt, h_sumY, m_D, 1);

        float result = 0.0f;
        for (uint32_t i = 0; i < 3; i++) {
            result += h_sumX[i] * h_sumY[i];
        }

        sigma2 -= 2 * result;
    }
    sigma2 /= m_M * m_N * m_D;

    return sigma2;
}

void CoherentPointDrift::getG()
{
    launchGetG(d_G, d_Y, m_M, m_D, m_beta, m_deviceProp);
}

void CoherentPointDrift::getP()
{
    // Let's obtain G * W
    {
        const float alpha = 1.0f;
        const float beta  = 0.0f;
        checkCublasReturn(cublasSgemm_v2(m_cublasHandle, CUBLAS_OP_N, CUBLAS_OP_N,
                                        m_M, m_D, m_M, &alpha, d_G, m_M, d_W, m_M, &beta, d_GW, m_M), __LINE__);
    }
    // Let's obtain Y + G * W
    {
        const float alpha = 1.0f;
        const float beta  = 1.0f;
        checkCublasReturn(cublasSgeam(m_cublasHandle, CUBLAS_OP_N, CUBLAS_OP_N,
                                         m_M, m_D, &alpha, d_Y, m_M, &beta, d_GW, m_M, d_tmpMxD, m_M), __LINE__);
    }

    launchGetP(d_X, d_tmpMxD, d_ones1xM, d_ones1xN, d_tmpMxN, d_tmp1xN, d_P,
                m_M, m_N, m_D, m_omega, m_sigma2, m_deviceProp, m_cublasHandle);

    // Let's obtain P1, Pt1
    {
        const float alpha = 1.0f;
        const float beta  = 0.0f;
        checkCublasReturn(cublasSgemv_v2(m_cublasHandle, CUBLAS_OP_N,
                                         m_M, m_N, &alpha, d_P, m_M, d_ones1xN, 1, &beta, d_P1, 1), __LINE__);
        checkCublasReturn(cublasSgemv_v2(m_cublasHandle, CUBLAS_OP_T,
                                         m_M, m_N, &alpha, d_P, m_M, d_ones1xM, 1, &beta, d_Pt1, 1), __LINE__);
        checkCublasReturn(cublasSgemm_v2(m_cublasHandle, CUBLAS_OP_N, CUBLAS_OP_N,
                                         m_M, m_D, m_N, &alpha, d_P, m_M, d_X, m_N, &beta, d_PX, m_M), __LINE__);
    }
}

void CoherentPointDrift::getW()
{
    // (dP1 * G + lambda * sigma2 * I(MxM)) * W = (PX - dP1 * Y)

    // Let's obtain (dP1 * G)'
    float * d_dP1Gt = d_tmpMxM;
    checkCublasReturn(cublasSdgmm(m_cublasHandle, CUBLAS_SIDE_LEFT,
                                  m_M, m_M, d_G, m_M, d_P1, 1, d_dP1Gt, m_M), __LINE__);

    // Let's obtain dP1 * G + lambda * sigma2 * I(MxM)
    float * d_term1 = d_dP1Gt;
    launchAddScalarDiagonal(d_term1, m_lambda * m_sigma2, m_M, m_deviceProp);

    // Let's obtain dP * Y
    float * d_dPY = d_tmpMxD;
    checkCublasReturn(cublasSdgmm(m_cublasHandle, CUBLAS_SIDE_LEFT,
                                  m_M, m_D, d_Y, m_M, d_P1, 1, d_dPY, m_M), __LINE__);

    // Let's obtain PX - dP * Y
    float * d_term2 = d_tmpMxDb;
    {
        const float alpha = 1.0f;
        const float beta  = -1.0f;
        checkCublasReturn(cublasSgeam(m_cublasHandle, CUBLAS_OP_N, CUBLAS_OP_N,
                                      m_M, m_D, &alpha, d_PX, m_M, &beta, d_dPY, m_M, d_term2, m_M), __LINE__);
    }

    cout << "term1 = " << endl;
    showDeviceMatrix(d_term1, m_M, m_M);
    cout << "term2 = " << endl;
    showDeviceMatrix(d_term2, m_M, m_D);
    cout << "pivotArray = " << endl;
    showDeviceMatrix(d_pivotArray, m_M, 1);

    // Now: the tricky part!!! W = term1 \ term2
//     solveWinCPU(d_term1, d_term2);
//     launchQRDecomposition(d_term1, m_M, m_M, m_deviceProp);
//     cout << "LU = " << endl;
//     showDeviceMatrix(d_term1, m_M, m_M);
//     cudaInvertMatrix(m_M, d_term1);
//
//     float * h_term1;
//     getHostMatrix(d_term1, h_term1, m_M, m_M);
//
//     for (uint32_t i = 0; i < min(10, (int)m_M); i++) {
//         for (uint32_t j = 0; j < min(10, (int)m_M); j++) {
// //             cout << h_term1[i * m_M + j] << " ";
// //             printf("%3.4f\t", min(fabs(h_term1[j * m_M + i]), 100.0f));
// //             printf("%3.4f\t", h_term1[j * m_M + i]);
//             if (h_term1[j * m_M + i] != 0.0f)
//                 cout << "X ";
//             else cout << "0 ";
//         }
//         cout << endl;
//     }

    //TODO: Try with the code at
    // https://devtalk.nvidia.com/default/topic/405197/cuda-programming-and-performance/lu-qr-and-cholesky-factorizations-using-gpu/

//     float * h_term2 = (float*)malloc(m_M * m_M * sizeof(float));
//     for (uint32_t i = 0; i < m_M; i++) {
//         for (uint32_t j = 0; j < m_M; j++) {
//             h_term2[i * m_M + j] = h_term1[j * m_M + i];
//         }
//     }
//     checkCudaError(cudaMemcpy(d_term1, h_term2, m_M * m_M * sizeof(float), cudaMemcpyHostToDevice), __LINE__);

//     showDeviceMatrix(d_term1, m_M, m_M);
//     int info;
//     cout << info << endl;
//     checkCublasReturn(cublasSgetrfBatched(m_cublasHandle, m_M, &d_term1, m_M, d_pivotArray, &info, 0), __LINE__);
//     cout << info << endl;
//     cout << "LU = " << endl;
//     showDeviceMatrix(d_pivotArray, m_M, 1);

}

template <typename T>
void CoherentPointDrift::getHostMatrix(const T* d_matrix, T * &h_matrix, const uint32_t& rows, const uint32_t& cols)
{
    cudaError_t error;
    h_matrix = new T[rows * cols];
    /*checkCudaError(*/cudaMemcpy(h_matrix, d_matrix, rows * cols * sizeof(T), cudaMemcpyDeviceToHost)/*, __LINE__)*/;
}

float CoherentPointDrift::trace(const float* d_matrix, const uint32_t& rows, const uint32_t& cols)
{
    assert(rows == cols);

    float * h_matrix;
    getHostMatrix(d_matrix, h_matrix, rows, cols);

    float trace = 0.0;
    for (uint32_t i = 0; i < rows; i++) {
        trace += h_matrix[IDX2C(i, i, rows)];
    }

    return trace;
}

template <typename T>
void CoherentPointDrift::showDeviceMatrix(const T * d_matrix, const uint32_t & rows, const uint32_t & cols)
{
    T * h_matrix;
    getHostMatrix(d_matrix, h_matrix, rows, cols);

    for (uint32_t i = 0; i < min(10, (int)rows); i++) {
        for (uint32_t j = 0; j < min(10, (int)cols); j++) {
            cout << h_matrix[IDX2C(i, j, rows)] << " ";
        }
        cout << endl;
    }
}

void CoherentPointDrift::solveWinCPU(float* & d_term1, float* & d_term2)
{
    double startWallTime = omp_get_wtime();

    cout << __LINE__ << endl;

    cv::Mat term1(m_M, m_M, CV_32FC1);
    cv::Mat term2(m_M, m_D, CV_32FC1);

    cout << __LINE__ << endl;

    float * h_term1 = new float[m_M * m_M];
    float * h_term2 = new float[m_M * m_D];

    cout << __LINE__ << endl;

    checkCudaError(cudaMemcpy(h_term1, d_term1, m_M * m_M * sizeof(float), cudaMemcpyDeviceToHost), __LINE__);
    checkCudaError(cudaMemcpy(h_term2, d_term2, m_M * m_D * sizeof(float), cudaMemcpyDeviceToHost), __LINE__);

    cout << __LINE__ << endl;

    for (uint32_t i = 0; i < m_M; i++) {
        for (uint32_t j = 0; j < m_M; j++) {
            term1.at<float>(i, j) = h_term1[j * m_M + i];
            if (j < m_D)
                term2.at<float>(i, j) = h_term2[j * m_M + i];
        }
    }

    cout << __LINE__ << endl;

    cv::Mat W;
    cv::solve(term1, term2, W, cv::DECOMP_SVD);

    cout << __LINE__ << endl;

    cout << "Time for " << __FUNCTION__ << ": " << omp_get_wtime() - startWallTime << endl;
    startWallTime = omp_get_wtime();
    cout << "********************************" << endl;

    cout << "W" << endl;
    cout << W << endl;
//     cout << "term1" << endl;
//     cout << term1 << endl;
//     cout << "term2" << endl;
//     cout << term2 << endl;
}


//=======================================================================
// Copyright 2013 University of Warsaw.
// Authors: Piotr Wygocki
//
// 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 SAMPLE_GRAPH_UNDIRECTED_HPP
#define SAMPLE_GRAPH_UNDIRECTED_HPP

#include <iostream>
#include <cstdlib>
#include <boost/graph/adjacency_list.hpp>

namespace boost
{
struct SampleGraph
{
    typedef adjacency_list_traits< vecS, vecS, directedS > Traits;

    typedef adjacency_list< vecS, vecS, directedS, no_property,
        property< edge_capacity_t, long,
            property< edge_residual_capacity_t, long,
                property< edge_reverse_t, Traits::edge_descriptor,
                    property< edge_weight_t, long > > > > >
        Graph;
    typedef property_map< Graph, edge_capacity_t >::type Capacity;
    typedef property_map< Graph, edge_residual_capacity_t >::type
        ResidualCapacity;
    typedef property_map< Graph, edge_weight_t >::type Weight;
    typedef property_map< Graph, edge_reverse_t >::type Reversed;
    typedef boost::graph_traits< Graph >::vertices_size_type size_type;
    typedef Traits::vertex_descriptor vertex_descriptor;

    template < class Graph, class Weight, class Capacity, class Reversed,
        class ResidualCapacity >
    class EdgeAdder
    {
    public:
        EdgeAdder(
            Graph& g, Weight& w, Capacity& c, Reversed& rev, ResidualCapacity&)
        : m_g(g), m_w(w), m_cap(c), m_rev(rev)
        {
        }
        void addEdge(vertex_descriptor v, vertex_descriptor w, long weight,
            long capacity)
        {
            Traits::edge_descriptor e, f;
            e = add(v, w, weight, capacity);
            f = add(w, v, -weight, 0);
            m_rev[e] = f;
            m_rev[f] = e;
        }

    private:
        Traits::edge_descriptor add(vertex_descriptor v, vertex_descriptor w,
            long weight, long capacity)
        {
            bool b;
            Traits::edge_descriptor e;
            boost::tie(e, b) = add_edge(vertex(v, m_g), vertex(w, m_g), m_g);
            if (!b)
            {
                std::cerr << "Edge between " << v << " and " << w
                          << " already exists." << std::endl;
                std::abort();
            }
            m_cap[e] = capacity;
            m_w[e] = weight;
            return e;
        }
        Graph& m_g;
        Weight& m_w;
        Capacity& m_cap;
        Reversed& m_rev;
    };

    static void getSampleGraph(
        Graph& g, vertex_descriptor& s, vertex_descriptor& t)
    {
        Capacity capacity = get(edge_capacity, g);
        Reversed rev = get(edge_reverse, g);
        ResidualCapacity residual_capacity = get(edge_residual_capacity, g);
        Weight weight = get(edge_weight, g);
        getSampleGraph(g, s, t, capacity, residual_capacity, weight, rev);
    }

    template < class Graph, class Weight, class Capacity, class Reversed,
        class ResidualCapacity >
    static void getSampleGraph(Graph& g, vertex_descriptor& s,
        vertex_descriptor& t, Capacity capacity,
        ResidualCapacity residual_capacity, Weight weight, Reversed rev)
    {
        size_type N(6);

        for (size_type i = 0; i < N; ++i)
        {
            add_vertex(g);
        }

        s = 0;
        t = 5;

        EdgeAdder< Graph, Weight, Capacity, Reversed, ResidualCapacity > ea(
            g, weight, capacity, rev, residual_capacity);

        ea.addEdge(0, 1, 4, 2);
        ea.addEdge(0, 2, 2, 2);

        ea.addEdge(1, 3, 2, 2);
        ea.addEdge(1, 4, 1, 1);
        ea.addEdge(2, 3, 1, 1);
        ea.addEdge(2, 4, 1, 1);

        ea.addEdge(3, 5, 4, 20);
        ea.addEdge(4, 5, 2, 20);
    }

    static void getSampleGraph2(
        Graph& g, vertex_descriptor& s, vertex_descriptor& t)
    {

        const boost::graph_traits< Graph >::vertices_size_type N(5);
        typedef property_map< Graph, edge_reverse_t >::type Reversed;

        for (size_type i = 0; i < N; ++i)
        {
            add_vertex(g);
        }

        Capacity capacity = get(edge_capacity, g);
        Reversed rev = get(edge_reverse, g);
        ResidualCapacity residual_capacity = get(edge_residual_capacity, g);
        Weight weight = get(edge_weight, g);

        s = 0;
        t = 4;

        EdgeAdder< Graph, Weight, Capacity, Reversed, ResidualCapacity > ea(
            g, weight, capacity, rev, residual_capacity);

        ea.addEdge(0, 1, 4, 2);
        ea.addEdge(0, 2, 2, 2);
        ea.addEdge(1, 2, 2, 2);
        ea.addEdge(2, 3, 1, 1);
        ea.addEdge(2, 4, 1, 1);
        ea.addEdge(3, 4, 1, 1);

        ea.addEdge(1, 0, 2, 2);
        ea.addEdge(2, 0, 1, 1);
        ea.addEdge(2, 1, 5, 2);
        ea.addEdge(3, 2, 1, 1);
        ea.addEdge(4, 2, 2, 2);
        ea.addEdge(4, 3, 1, 3);
    }
};
} // boost

#endif /* SAMPLE_GRAPH_UNDIRECTED_HPP */


/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Mar 11, 2015
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file operators.hpp
 * @brief File with some helper functions related to matrix/vector operations.
 * @author Stefan Leutenegger
 */

#ifndef INCLUDE_OKVIS_KINEMATICS_OPERATORS_HPP_
#define INCLUDE_OKVIS_KINEMATICS_OPERATORS_HPP_

#include <stdint.h>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>

/// \brief okvis Main namespace of this package.
namespace okvis {

/// \brief kinematics Namespace for kinematics functionality, i.e. transformations and stuff.
namespace kinematics {

// some free helper functions

/// \brief Cross matrix of a vector [x,y,z].
///        Adopted from Schweizer-Messer by Paul Furgale.
/// \tparam Scalar_T The scalar type, auto-deducible (typically double).
/// @param[in] x First vector element.
/// @param[in] y Second vector element.
/// @param[in] z Third vector element.
template<typename Scalar_T>
inline Eigen::Matrix<Scalar_T, 3, 3> crossMx(Scalar_T x, Scalar_T y, Scalar_T z)
{
  Eigen::Matrix<Scalar_T, 3, 3> C;
  C(0, 0) = 0.0;
  C(0, 1) = -z;
  C(0, 2) = y;
  C(1, 0) = z;
  C(1, 1) = 0.0;
  C(1, 2) = -x;
  C(2, 0) = -y;
  C(2, 1) = x;
  C(2, 2) = 0.0;
  return C;
}

/// \brief Cross matrix of a vector v.
///        Adopted from Schweizer-Messer by Paul Furgale.
/// \tparam Derived_T The vector type, auto-deducible.
/// @param[in] v The vector.
template<typename Derived_T>
inline Eigen::Matrix<typename Eigen::internal::traits<Derived_T>::Scalar, 3, 3> crossMx(
    Eigen::MatrixBase<Derived_T> const & v)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived_T>, 3);
  assert((v.cols()==3 && v.rows()==1)||(v.rows()==3 && v.cols()==1));
  return crossMx(v(0, 0), v(1, 0), v(2, 0));
}

/// \brief Plus matrix of a quaternion, i.e. q_AB*q_BC = plus(q_AB)*q_BC.coeffs().
/// @param[in] q_AB A Quaternion.
inline Eigen::Matrix4d plus(const Eigen::Quaterniond & q_AB) {
  Eigen::Vector4d q = q_AB.coeffs();
  Eigen::Matrix4d Q;
  Q(0,0) =  q[3]; Q(0,1) = -q[2]; Q(0,2) =  q[1]; Q(0,3) =  q[0];
  Q(1,0) =  q[2]; Q(1,1) =  q[3]; Q(1,2) = -q[0]; Q(1,3) =  q[1];
  Q(2,0) = -q[1]; Q(2,1) =  q[0]; Q(2,2) =  q[3]; Q(2,3) =  q[2];
  Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) =  q[3];
  return Q;
}

/// \brief Oplus matrix of a quaternion, i.e. q_AB*q_BC = oplus(q_BC)*q_AB.coeffs().
/// @param[in] q_BC A Quaternion.
inline Eigen::Matrix4d oplus(const Eigen::Quaterniond & q_BC) {
  Eigen::Vector4d q = q_BC.coeffs();
  Eigen::Matrix4d Q;
  Q(0,0) =  q[3]; Q(0,1) =  q[2]; Q(0,2) = -q[1]; Q(0,3) =  q[0];
  Q(1,0) = -q[2]; Q(1,1) =  q[3]; Q(1,2) =  q[0]; Q(1,3) =  q[1];
  Q(2,0) =  q[1]; Q(2,1) = -q[0]; Q(2,2) =  q[3]; Q(2,3) =  q[2];
  Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) =  q[3];
  return Q;
}

template<class Derived>
Eigen::Quaternion<typename Derived::Scalar> rvec2quat(const Eigen::MatrixBase<Derived> & rvec)
{
    typedef typename Derived::Scalar Scalar;
    EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
    //normalize
    Scalar rot_ang=rvec.norm();    //assume always positive
    if (rot_ang<Scalar(1e-18))
        return Eigen::Quaternion<Scalar>(Scalar(1), Scalar(0),Scalar(0),Scalar(0));
    else{
        Scalar f= sin(rot_ang*Scalar(0.5))/rot_ang;
        return Eigen::Quaternion<Scalar>(cos(rot_ang*Scalar(0.5)), rvec[0] * f, rvec[1] * f, rvec[2] * f);
    }
}
} // namespace kinematics
} // namespace okvis

#endif /* INCLUDE_OKVIS_KINEMATICS_OPERATORS_HPP_ */


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::model::functional::include.hpp                      //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_MODEL_FUNCTIONAL_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_MODEL_FUNCTIONAL_INCLUDE_HPP_ER_2009

#include <boost/statistics/survival/model/functional/experiment_simulator.hpp>
#include <boost/statistics/survival/model/functional/prepare_weights.hpp>

#endif


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE test_nlopt

#include <boost/test/unit_test.hpp>

#include <limbo/limbo.hpp>
#include <limbo/opt/nlopt_grad.hpp>
#include <limbo/opt/nlopt_no_grad.hpp>

using namespace limbo;

struct Params {
    struct opt_nloptgrad : public defaults::opt_nloptgrad {
        BO_PARAM(int, iterations, 200);
    };

    struct opt_nloptnograd : public defaults::opt_nloptnograd {
        BO_PARAM(int, iterations, 200);
    };
};

opt::eval_t my_function(const Eigen::VectorXd& params, bool eval_grad)
{
    double v = -params(0) * params(0) - params(1) * params(1);
    if (!eval_grad)
        return opt::no_grad(v);
    Eigen::VectorXd grad(2);
    grad(0) = -2 * params(0);
    grad(1) = -2 * params(1);
    return {v, grad};
}

opt::eval_t my_constraint(const Eigen::VectorXd& params, bool eval_grad)
{
    double v = params(0) + 3. * params(1) - 10.;
    if (!eval_grad)
        return opt::no_grad(v);
    Eigen::VectorXd grad(2);
    grad(0) = 1.;
    grad(1) = 3.;
    return {v, grad};
}

opt::eval_t my_inequality_constraint(const Eigen::VectorXd& params, bool eval_grad)
{
    double v = -params(0) - 3. * params(1) + 10.;
    if (!eval_grad)
        return opt::no_grad(v);
    Eigen::VectorXd grad(2);
    grad(0) = -1.;
    grad(1) = -3.;
    return {v, grad};
}

BOOST_AUTO_TEST_CASE(test_nlopt_grad_simple)
{
    opt::NLOptGrad<Params, nlopt::LD_MMA> optimizer;
    Eigen::VectorXd g = optimizer(my_function, tools::random_vector(2), false);

    BOOST_CHECK_SMALL(g(0), 0.00000001);
    BOOST_CHECK_SMALL(g(1), 0.00000001);
}

BOOST_AUTO_TEST_CASE(test_nlopt_no_grad_simple)
{
    opt::NLOptNoGrad<Params, nlopt::LN_COBYLA> optimizer;
    Eigen::VectorXd best(2);
    best << 1, 1;
    size_t N = 10;
    for (size_t i = 0; i < N; i++) {
        Eigen::VectorXd g = optimizer(my_function, tools::random_vector(2), false);
        if (g.norm() < best.norm()) {
            best = g;
        }
    }

    BOOST_CHECK_SMALL(best(0), 0.00000001);
    BOOST_CHECK_SMALL(best(1), 0.00000001);
}

BOOST_AUTO_TEST_CASE(test_nlopt_no_grad_constraint)
{
    opt::NLOptNoGrad<Params, nlopt::LN_COBYLA> optimizer;
    optimizer.initialize(2);
    optimizer.add_equality_constraint(my_constraint);

    Eigen::VectorXd best = tools::random_vector(2).array() * 50.; // some random big value
    Eigen::VectorXd target(2);
    target << 1., 3.;
    size_t N = 10;
    for (size_t i = 0; i < N; i++) {
        Eigen::VectorXd g = optimizer(my_function, tools::random_vector(2), false);
        if ((g - target).norm() < (best - target).norm()) {
            best = g;
        }
    }

    BOOST_CHECK_SMALL(std::abs(1. - best(0)), 0.000001);
    BOOST_CHECK_SMALL(std::abs(3. - best(1)), 0.000001);
}

BOOST_AUTO_TEST_CASE(test_nlopt_grad_constraint)
{
    opt::NLOptGrad<Params, nlopt::LD_AUGLAG_EQ> optimizer;
    optimizer.initialize(2);
    optimizer.add_inequality_constraint(my_inequality_constraint);

    Eigen::VectorXd best = tools::random_vector(2).array() * 50.; // some random big value
    Eigen::VectorXd target(2);
    target << 1., 3.;
    size_t N = 10;
    for (size_t i = 0; i < N; i++) {
        Eigen::VectorXd g = optimizer(my_function, tools::random_vector(2), false);
        if ((g - target).norm() < (best - target).norm()) {
            best = g;
        }
    }

    BOOST_CHECK_SMALL(std::abs(1. - best(0)), 0.0001);
    BOOST_CHECK_SMALL(std::abs(3. - best(1)), 0.0001);
}


/* boost random/uniform_real.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: uniform_real.hpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 * Revision history
 *  2001-04-08  added min<max assertion (N. Becker)
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_UNIFORM_REAL_HPP
#define BOOST_RANDOM_UNIFORM_REAL_HPP

#include <boost/assert.hpp>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/random/uniform_real_distribution.hpp>

namespace boost {

/**
 * The distribution function uniform_real models a random distribution.
 * On each invocation, it returns a random floating-point value uniformly
 * distributed in the range [min..max).
 *
 * This class is deprecated.  Please use @c uniform_real_distribution in
 * new code.
 */
template<class RealType = double>
class uniform_real : public random::uniform_real_distribution<RealType>
{
    typedef random::uniform_real_distribution<RealType> base_type;
public:

    class param_type : public base_type::param_type
    {
    public:
        typedef uniform_real distribution_type;
        /**
         * Constructs the parameters of a uniform_real distribution.
         *
         * Requires: min <= max
         */
        explicit param_type(RealType min_arg = RealType(0.0),
                            RealType max_arg = RealType(1.0))
          : base_type::param_type(min_arg, max_arg)
        {}
    };

    /**
     * Constructs a uniform_real object. @c min and @c max are the
     * parameters of the distribution.
     *
     * Requires: min <= max
     */
    explicit uniform_real(RealType min_arg = RealType(0.0),
                          RealType max_arg = RealType(1.0))
      : base_type(min_arg, max_arg)
    {
        BOOST_ASSERT(min_arg <= max_arg);
    }

    /** Constructs a uniform_real distribution from its parameters. */
    explicit uniform_real(const param_type& parm)
      : base_type(parm)
    {}

    /** Returns the parameters of the distribution */
    param_type param() const { return param_type(this->a(), this->b()); }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm) { this->base_type::param(parm); }
};

} // namespace boost

#endif // BOOST_RANDOM_UNIFORM_REAL_HPP


/*
 * PolynomialSplineContainer.cpp
 *
 *  Created on: Dec 8, 2014
 *      Author: C. Dario Bellicoso, Peter Fankhauser
 */

#include "curves/PolynomialSplineContainer.hpp"

// std
#include <iostream>

// boost
#include <boost/math/special_functions/pow.hpp>

namespace curves {

constexpr double PolynomialSplineContainer::undefinedValue;

// Import spline types from class.
using SplineType = PolynomialSplineContainer::SplineType;
using SplineList = PolynomialSplineContainer::SplineList;

PolynomialSplineContainer::PolynomialSplineContainer():
    timeOffset_(0.0),
    containerTime_(0.0),
    containerDuration_(0.0),
    activeSplineIdx_(0)
{
  // Make sure that the container is correctly emptied
  reset();
}

PolynomialSplineContainer::~PolynomialSplineContainer()
{
  // TODO Auto-generated destructor stub
}

bool PolynomialSplineContainer::advance(double dt)
{
  // Check container size
//  if (containerSize == 0 || containerSize == activeSplineIdx_) {
////    throw std::runtime_error("splinecontainer::advance");
//    return false;
//  }

  if (splines_.empty() || containerTime_ >= containerDuration_ || activeSplineIdx_ == splines_.size()) {
    return false;
  }

//  // Advance time from 0 to tf
//  if (time_ < splines_[activeSplineIdx_].getSplineDuration()) {
//    time_ += dt;
//  } else {
//    // Reset time
//    time_ = 0.0;
//    timeOffset_ += splines_[activeSplineIdx_].getSplineDuration();
//    activeSplineIdx_++;
//  }

  containerTime_ += dt;

  if ((containerTime_ - timeOffset_ >= splines_[activeSplineIdx_].getSplineDuration())) {
    if (activeSplineIdx_ < (splines_.size() - 1)) {
      timeOffset_ += splines_[activeSplineIdx_].getSplineDuration();
    }
    activeSplineIdx_++;
  }

  return true;
}

void PolynomialSplineContainer::setContainerTime(double t)
{
  containerTime_ = t;
  double timeOffset;
  activeSplineIdx_ = getActiveSplineIndexAtTime(t, timeOffset);
}

/*
 * aijh:
 *  i --> spline id (1,...,n)
 *  j --> spline coefficient aj (a5,...,a1,a0)
 *  h --> dimX, dimY
 *
 * Coefficient vector is:
 *    q = [a15x a14x ... a10x a15y ... a10y a25x ... a20y ... an5x ... an0y]
 */
int PolynomialSplineContainer::getCoeffIndex(int splineIdx, int aIdx) const {
  return splineIdx*6 + aIdx;
}

int PolynomialSplineContainer::getSplineColumnIndex(int splineIdx) const
{
  return getCoeffIndex(splineIdx, 0);
}

void PolynomialSplineContainer::setData(const std::vector<double>& knotPositions,
                                        const std::vector<double>& knotValues,
                                        double initialVelocity, double initialAcceleration,
                                        double finalVelocity, double finalAcceleration) {
  reset();

  const unsigned int num_splines = knotPositions.size()-1;
  constexpr auto num_coeffs_spline = SplineType::coefficientCount;
  const unsigned int num_coeffs = num_splines*num_coeffs_spline;
  const unsigned int num_knots = knotPositions.size();

  const unsigned int num_initial_constraints = 3;
  const unsigned int num_final_constraints = 3;

  const unsigned int num_constraints = (num_splines-1)*4 + num_initial_constraints + num_final_constraints;

  std::vector<double> tfs;// (num_splines);
  for (unsigned int i=0; i<num_splines; i++) {
    tfs.push_back(knotPositions[i+1]-knotPositions[i]);
  }

  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num_constraints, num_coeffs);
  Eigen::VectorXd coeffs = Eigen::VectorXd::Zero(num_coeffs);
  Eigen::VectorXd b = Eigen::VectorXd::Zero(num_constraints);

  // time containers
  SplineType::EigenTimeVectorType timeVec, dTimeVec, ddTimeVec;
  SplineType::EigenTimeVectorType timeVecTf, dTimeVecTf, ddTimeVecTf;

  SplineType::getTimeVector(timeVec, 0.0);
  SplineType::getdTimeVector(dTimeVec, 0.0);
  SplineType::getddTimeVector(ddTimeVec, 0.0);

  // define convenience quantities
//  int dimX = 0;
//  int a5 = 0;
//  int physPos = 0;
//  int physVel = 1;
//  int physAcc = 2;

  int constraintIdx = 0;
//  A.block(0, getCoeffIndex(1, a5),1,6) = timeVec;    // a_10_x
  A.block(constraintIdx, getSplineColumnIndex(0), 1, num_coeffs_spline) = timeVec;
  b(constraintIdx) = knotValues[0];
  constraintIdx++;

  A.block(constraintIdx, getSplineColumnIndex(0), 1, num_coeffs_spline) = dTimeVec;   // a_11_x
  b(constraintIdx) = initialVelocity; // initial velocity
  constraintIdx++;

  A.block(constraintIdx, getSplineColumnIndex(0), 1, num_coeffs_spline) = ddTimeVec;  // a_12_x
  b(constraintIdx) = initialAcceleration; // initial acceleration
  constraintIdx++;

  // Final conditions
//  int lastSplineId = num_splines; // doesnt have to be -1
  const double tf = tfs.back();
//  int rows = A.rows();

  SplineType::getTimeVector(timeVec, tf);
  SplineType::getdTimeVector(dTimeVec, tf);
  SplineType::getddTimeVector(ddTimeVec, tf);

  A.block(constraintIdx, getSplineColumnIndex(num_splines-1), 1, num_coeffs_spline) = timeVec;
  b(constraintIdx) = knotValues.back();
  constraintIdx++;

  A.block(constraintIdx, getSplineColumnIndex(num_splines-1), 1, num_coeffs_spline) = dTimeVec;
  b(constraintIdx) = finalVelocity;
  constraintIdx++;

  A.block(constraintIdx, getSplineColumnIndex(num_splines-1), 1, num_coeffs_spline) = ddTimeVec;
  b(constraintIdx) = finalAcceleration;
  constraintIdx++;
  /***************************/

  /**********************************
   * Set spline junction conditions *
   **********************************/
  for (size_t k=0; k<num_splines-1; k++) {

    const int prevSplineId = k;
    const int nextSplineId = k+1;

    const double tf = tfs[k];

    SplineType::getTimeVector(timeVec, 0.0);
    SplineType::getdTimeVector(dTimeVec, 0.0);
    SplineType::getddTimeVector(ddTimeVec, 0.0);

    SplineType::getTimeVector(timeVecTf, tf);
    SplineType::getdTimeVector(dTimeVecTf, tf);
    SplineType::getddTimeVector(ddTimeVecTf, tf);

    A.block(constraintIdx, getSplineColumnIndex(prevSplineId), 1, num_coeffs_spline) = timeVecTf;
    b(constraintIdx) = knotValues[k+1];
    constraintIdx++;

    A.block(constraintIdx, getSplineColumnIndex(nextSplineId), 1, num_coeffs_spline) = timeVec;
    b(constraintIdx) = knotValues[k+1];
    constraintIdx++;

    A.block(constraintIdx, getSplineColumnIndex(prevSplineId), 1, num_coeffs_spline) = dTimeVecTf;
    A.block(constraintIdx, getSplineColumnIndex(nextSplineId), 1, num_coeffs_spline) = -dTimeVec;
    b(constraintIdx) = 0.0;
    constraintIdx++;

    A.block(constraintIdx, getSplineColumnIndex(prevSplineId), 1, num_coeffs_spline) = ddTimeVecTf;
    A.block(constraintIdx, getSplineColumnIndex(nextSplineId), 1, num_coeffs_spline) = -ddTimeVec;
    b(constraintIdx) = 0.0;
    constraintIdx++;
  }
  /**********************************/

  coeffs = A.colPivHouseholderQr().solve(b);

  SplineType spline;
  SplineType::SplineCoefficients coefficients;

  for (unsigned int i = 0; i <num_splines; i++) {
    Eigen::Map<Eigen::VectorXd>(coefficients.data(), num_coeffs_spline, 1) = coeffs.segment<num_coeffs_spline>(getSplineColumnIndex(i));
    spline.setCoefficientsAndDuration(coefficients, tfs[i]);
    this->addSpline(spline);
  }

}

int PolynomialSplineContainer::getActiveSplineIndex() const
{
  return activeSplineIdx_;
}

bool PolynomialSplineContainer::addSpline(const SplineType& spline)
{
  splines_.push_back(spline);
  containerDuration_ += spline.getSplineDuration();
  return true;
}

bool PolynomialSplineContainer::addSpline(SplineType&& spline) {
  containerDuration_ += spline.getSplineDuration();
  splines_.emplace_back(spline);
  return true;
}

bool PolynomialSplineContainer::reset()
{
  splines_.clear();
  activeSplineIdx_ = 0;
  containerDuration_ = 0.0;
  resetTime();
  return true;
}

bool PolynomialSplineContainer::resetTime()
{
  timeOffset_ = 0.0;
  containerTime_ = 0.0;
  activeSplineIdx_ = 0;
  return true;
}

double PolynomialSplineContainer::getContainerDuration() const
{
  return containerDuration_;
}

SplineType* PolynomialSplineContainer::getSpline(int splineIndex)
{
  return &splines_.at(splineIndex);
}

double PolynomialSplineContainer::getContainerTime() const
{
  return containerTime_;
}

bool PolynomialSplineContainer::isEmpty() const
{
  return splines_.empty();
}

double PolynomialSplineContainer::getPosition() const
{
  if (splines_.empty()) return 0.0;
  if (activeSplineIdx_ == splines_.size())
    return splines_.at(activeSplineIdx_ - 1).getPositionAtTime(containerTime_ - timeOffset_);
  return splines_.at(activeSplineIdx_).getPositionAtTime(containerTime_ - timeOffset_);
}

double PolynomialSplineContainer::getVelocity() const {
  if (splines_.empty()) return 0.0;
  if (activeSplineIdx_ == splines_.size())
    return splines_.at(activeSplineIdx_ - 1).getVelocityAtTime(containerTime_ - timeOffset_);
  return splines_.at(activeSplineIdx_).getVelocityAtTime(containerTime_ - timeOffset_);
}

double PolynomialSplineContainer::getAcceleration() const
{
  if (splines_.empty()) return 0.0;
  if (activeSplineIdx_ == splines_.size())
    return splines_.at(activeSplineIdx_ - 1).getAccelerationAtTime(containerTime_ - timeOffset_);
  return splines_.at(activeSplineIdx_).getAccelerationAtTime(containerTime_ - timeOffset_);
}

double PolynomialSplineContainer::getPositionAtTime(double t) const
{
  double timeOffset = 0.0;
  int activeSplineIdx = getActiveSplineIndexAtTime(t, timeOffset);

  if (activeSplineIdx < 0) {
    return splines_.at(0).getPositionAtTime(0.0);
  }
  if (activeSplineIdx == splines_.size()) {
    return splines_.at(activeSplineIdx - 1).getPositionAtTime(
        splines_.at(activeSplineIdx - 1).getSplineDuration());
  }
  return splines_.at(activeSplineIdx).getPositionAtTime(t - timeOffset);
}

int PolynomialSplineContainer::getActiveSplineIndexAtTime(double t, double& timeOffset) const {
  if (splines_.empty()) return -1;
  timeOffset = 0.0;

  for (size_t i = 0; i < splines_.size(); i++) {
    if ((t - timeOffset < splines_[i].getSplineDuration())) {
      return i;
    }
    if (i < (splines_.size() - 1)) {
      timeOffset += splines_[i].getSplineDuration();
    }
  }

  return (splines_.size() - 1);
}

double PolynomialSplineContainer::getVelocityAtTime(double t) const
{
  double timeOffset = 0.0;
  int activeSplineIdx = getActiveSplineIndexAtTime(t, timeOffset);
  if (activeSplineIdx < 0) {
    return splines_.at(0).getVelocityAtTime(0.0);
  }
  if (activeSplineIdx == splines_.size()) {
    return splines_.at(activeSplineIdx - 1).getVelocityAtTime(
        splines_.at(activeSplineIdx - 1).getSplineDuration());
  }
  return splines_.at(activeSplineIdx).getVelocityAtTime(t - timeOffset);
}

double PolynomialSplineContainer::getAccelerationAtTime(double t) const
{
  double timeOffset = 0.0;
  int activeSplineIdx = getActiveSplineIndexAtTime(t, timeOffset);
  if (activeSplineIdx < 0) {
    return splines_.at(0).getAccelerationAtTime(0.0);
  }
  if (activeSplineIdx == splines_.size()) {
    return splines_.at(activeSplineIdx - 1).getAccelerationAtTime(
        splines_.at(activeSplineIdx - 1).getSplineDuration());
  }
  return splines_.at(activeSplineIdx).getAccelerationAtTime(t - timeOffset);
}

double PolynomialSplineContainer::getEndPosition() const
{
  double lastSplineDuration = splines_.at(splines_.size() - 1).getSplineDuration();
  return splines_.at(splines_.size() - 1).getPositionAtTime(lastSplineDuration);
}

double PolynomialSplineContainer::getEndVelocity() const
{
  double lastSplineDuration = splines_.at(splines_.size() - 1).getSplineDuration();
  return splines_.at(splines_.size() - 1).getVelocityAtTime(lastSplineDuration);
}

double PolynomialSplineContainer::getEndAcceleration() const
{
  double lastSplineDuration = splines_.at(splines_.size() - 1).getSplineDuration();
  return splines_.at(splines_.size() - 1).getAccelerationAtTime(lastSplineDuration);
}

const SplineList& PolynomialSplineContainer::getSplines() const {
  return splines_;
}

} /* namespace */


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE LibmeshEntityTester
#include <boost/test/included/unit_test.hpp>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <vector>

#include <LibmeshAdjacencies.hpp>
#include <LibmeshEntity.hpp>
#include <LibmeshEntityExtraData.hpp>

#include <Teuchos_Array.hpp>
#include <Teuchos_DefaultComm.hpp>
#include <Teuchos_DefaultMpiComm.hpp>
#include <Teuchos_FancyOStream.hpp>
#include <Teuchos_OpaqueWrapper.hpp>
#include <Teuchos_RCP.hpp>
#include <Teuchos_Tuple.hpp>
#include <Teuchos_VerboseObject.hpp>

#include <libmesh/cell_hex8.h>
#include <libmesh/libmesh.h>
#include <libmesh/mesh.h>
#include <libmesh/node.h>
#include <libmesh/parallel.h>
#include <libmesh/point.h>

//---------------------------------------------------------------------------//
// MPI Setup
//---------------------------------------------------------------------------//

template <class Ordinal>
Teuchos::RCP<const Teuchos::Comm<Ordinal>> getDefaultComm()
{
#ifdef HAVE_MPI
    return Teuchos::DefaultComm<Ordinal>::getComm();
#else
    return Teuchos::rcp( new Teuchos::SerialComm<Ordinal>() );
#endif
}

//---------------------------------------------------------------------------//
// Hex-8 test.
BOOST_AUTO_TEST_CASE( checkLibmeshEntity )
{
	const std::string argv_string = "unit_test --keep-cout";
	const char *argv_char = argv_string.c_str();
	Teuchos::GlobalMPISession mpiSession(
			&boost::unit_test::framework::master_test_suite().argc,
			&boost::unit_test::framework::master_test_suite().argv);
	auto comm = Teuchos::DefaultComm<int>::getComm();
	auto mpi_comm = Teuchos::rcp_dynamic_cast<const Teuchos::MpiComm<int>>(
			comm);
	auto opaque_comm = mpi_comm->getRawMpiComm();
	auto raw_comm = (*opaque_comm)();

    // Create the mesh.
    int space_dim = 3;
    libMesh::LibMeshInit libmesh_init( 1, &argv_char, raw_comm );
    BOOST_VERIFY( libMesh::initialized() );
    BOOST_VERIFY( (int)libmesh_init.comm().rank() == comm->getRank() );
    Teuchos::RCP<libMesh::Mesh> mesh =
        Teuchos::rcp( new libMesh::Mesh( libmesh_init.comm(), space_dim ) );

    // Create the nodes.
    int rank = comm->getRank();
    Teuchos::Array<libMesh::Node *> nodes( 8 );
    double node_coords[3];
    node_coords[0] = 0.0;
    node_coords[1] = 0.0;
    node_coords[2] = 0.0;
    nodes[0] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 0,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 0.0;
    node_coords[2] = 0.0;
    nodes[1] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 1,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 1.0;
    node_coords[2] = 0.0;
    nodes[2] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 2,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 1.0;
    node_coords[2] = 0.0;
    nodes[3] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 3,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 0.0;
    node_coords[2] = 1.0;
    nodes[4] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 4,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 0.0;
    node_coords[2] = 1.0;
    nodes[5] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 5,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 1.0;
    node_coords[2] = 1.0;
    nodes[6] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 6,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 1.0;
    node_coords[2] = 1.0;
    nodes[7] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 7,
        rank );

    // Make a hex-8.
    libMesh::Elem *hex_elem = mesh->add_elem( new libMesh::Hex8 );
    hex_elem->processor_id() = rank;
    hex_elem->set_id() = 2 * rank;
    for ( int i = 0; i < 8; ++i )
        hex_elem->set_node( i ) = nodes[i];

    // Make 2 subdomains and put the hex-8 in the first subdomain.
    int subdomain_1_id = 1;
    int subdomain_2_id = 2;
    std::set<libMesh::subdomain_id_type> subdomain_ids;
    subdomain_ids.insert( subdomain_1_id );
    subdomain_ids.insert( subdomain_2_id );
    hex_elem->subdomain_id() = subdomain_1_id;

    // Make 2 boundaries and add the first elem side to one and first node to
    // the second.
    int boundary_1_id = 1;
    int boundary_2_id = 2;
    mesh->get_boundary_info().add_side( hex_elem, 0, boundary_1_id );
    mesh->get_boundary_info().add_node( nodes[0], boundary_2_id );

    // Check libmesh validity.
    mesh->libmesh_assert_valid_parallel_ids();

    // Make an adjacency data structure.
    LibmeshAdapter::LibmeshAdjacencies adjacencies( mesh );

    // Create a  entity for the hex.
   auto dtk_entity =
        LibmeshAdapter::LibmeshEntity<libMesh::Elem>(
            Teuchos::ptr( hex_elem ), mesh.ptr(),
            Teuchos::ptrFromRef( adjacencies ) );

    // Print out the entity.
    Teuchos::RCP<Teuchos::FancyOStream> fancy_out =
        Teuchos::VerboseObjectBase::getDefaultOStream();
//    dtk_entity.describe( *fancy_out );

    // Test the entity.
    BOOST_VERIFY( hex_elem->id() == dtk_entity.id() );
    BOOST_VERIFY( rank == dtk_entity.ownerRank() );
    BOOST_VERIFY( space_dim == dtk_entity.topologicalDimension() );
    BOOST_VERIFY( space_dim == dtk_entity.physicalDimension() );

    BOOST_VERIFY( dtk_entity.inBlock( subdomain_1_id ) );
    BOOST_VERIFY( !dtk_entity.inBlock( subdomain_2_id ) );

    BOOST_VERIFY( dtk_entity.onBoundary( boundary_1_id ) );
    BOOST_VERIFY( !dtk_entity.onBoundary( boundary_2_id ) );

   auto elem_extra_data =
        dtk_entity.extraData();
   BOOST_VERIFY( hex_elem ==
                   Teuchos::rcp_dynamic_cast<
                       LibmeshAdapter::LibmeshEntityExtraData<libMesh::Elem>>(
                       elem_extra_data )
                       ->d_libmesh_geom.getRawPtr() );

    Teuchos::Tuple<double, 6> hex_bounds;
    dtk_entity.boundingBox( hex_bounds );
    BOOST_VERIFY( 0.0 == hex_bounds[0] );
    BOOST_VERIFY( 0.0 == hex_bounds[1] );
    BOOST_VERIFY( 0.0 == hex_bounds[2] );
    BOOST_VERIFY( 1.0 == hex_bounds[3] );
    BOOST_VERIFY( 1.0 == hex_bounds[4] );
    BOOST_VERIFY( 1.0 == hex_bounds[5] );

    // Test a node.
  auto dtk_node =
        LibmeshAdapter::LibmeshEntity<libMesh::Node>(
            Teuchos::ptr( nodes[0] ), mesh.ptr(),
            Teuchos::ptrFromRef( adjacencies ) );
  BOOST_VERIFY( nodes[0]->id() == dtk_node.id() );
  BOOST_VERIFY( rank == dtk_node.ownerRank() );
  BOOST_VERIFY( 0 == dtk_node.topologicalDimension() );
  BOOST_VERIFY( space_dim == dtk_node.physicalDimension() );

  BOOST_VERIFY( dtk_node.inBlock( subdomain_1_id ) );
  BOOST_VERIFY( !dtk_node.inBlock( subdomain_2_id ) );

  BOOST_VERIFY( !dtk_node.onBoundary( boundary_1_id ) );
  BOOST_VERIFY( dtk_node.onBoundary( boundary_2_id ) );

    auto node_extra_data =
        dtk_node.extraData();
    BOOST_VERIFY( nodes[0] ==
                   Teuchos::rcp_dynamic_cast<
				   LibmeshAdapter::LibmeshEntityExtraData<libMesh::Node>>(
                       node_extra_data )
                       ->d_libmesh_geom.getRawPtr() );

    Teuchos::Tuple<double, 6> node_bounds;
    dtk_node.boundingBox( node_bounds );
    BOOST_VERIFY( 0.0 == node_bounds[0] );
    BOOST_VERIFY( 0.0 == node_bounds[1] );
    BOOST_VERIFY( 0.0 == node_bounds[2] );
    BOOST_VERIFY( 0.0 == node_bounds[3] );
    BOOST_VERIFY( 0.0 == node_bounds[4] );
    BOOST_VERIFY( 0.0 == node_bounds[5] );

    // Print out the node.
//    dtk_node.describe( *fancy_out );
}

//---------------------------------------------------------------------------//
// end tstLibmeshEntity.cpp
//---------------------------------------------------------------------------//


#if 0
#######################################################################################
# The MIT License

# Copyright (c) 2014       Hannes Schulz, University of Bonn  <schulz@ais.uni-bonn.de>
# Copyright (c) 2013       Benedikt Waldvogel, University of Bonn <mail@bwaldvogel.de>
# Copyright (c) 2008-2009  Sebastian Nowozin                       <nowozin@gmail.com>

# 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.
#######################################################################################
#endif
#define BOOST_TEST_MODULE example

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/test/included/unit_test.hpp>
#include <cmath>

#include "random_tree.h"
#include "test_common.h"

using namespace curfil;
using namespace cuv;

BOOST_AUTO_TEST_SUITE(RandomTreeTest)

BOOST_AUTO_TEST_CASE(testNormalizeHistogramEqualPriorDistribution) {

    const LabelType NUM_LABELS = 3;

    cuv::ndarray<WeightType, cuv::host_memory_space> histogram(NUM_LABELS);
    histogram[0] = 850;
    histogram[1] = 50;
    histogram[2] = 100;

    cuv::ndarray<WeightType, cuv::host_memory_space> priorDistribution(NUM_LABELS);
    for (LabelType label = 0; label < NUM_LABELS; label++) {
        priorDistribution[label] = 100;
    }

    ndarray<double, host_memory_space> normalizedHistogram = curfil::detail::normalizeHistogram(histogram,
            priorDistribution, 0.0, true);

    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[0]), 0.850 / 3.0, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[1]), 0.050 / 3.0, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[2]), 0.100 / 3.0, 1e-15);
}

BOOST_AUTO_TEST_CASE(testNormalizeHistogramEqualHistogramUnequalPriorDistribution) {

    const LabelType NUM_LABELS = 3;

    cuv::ndarray<WeightType, cuv::host_memory_space> histogram(NUM_LABELS);
    histogram[0] = 50;
    histogram[1] = 50;
    histogram[2] = 50;

    cuv::ndarray<WeightType, cuv::host_memory_space> priorDistribution(NUM_LABELS);
    priorDistribution[0] = 80;
    priorDistribution[1] = 10;
    priorDistribution[2] = 10;

    ndarray<double, host_memory_space> normalizedHistogram = curfil::detail::normalizeHistogram(histogram,
            priorDistribution, 0.0, true);

    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[0]), (1 / 3.0) * 0.8, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[1]), (1 / 3.0) * 0.1, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[2]), (1 / 3.0) * 0.1, 1e-15);
}

BOOST_AUTO_TEST_CASE(testNormalizeHistogramHighBias) {

    const LabelType NUM_LABELS = 3;

    cuv::ndarray<WeightType, cuv::host_memory_space> histogram(NUM_LABELS);
    histogram[0] = 50;
    histogram[1] = 50;
    histogram[2] = 50;

    cuv::ndarray<WeightType, cuv::host_memory_space> priorDistribution(NUM_LABELS);
    priorDistribution[0] = 80;
    priorDistribution[1] = 10;
    priorDistribution[2] = 10;

    ndarray<double, host_memory_space> normalizedHistogram = curfil::detail::normalizeHistogram(histogram,
            priorDistribution, 0.5, true);

    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[0]), 0.0, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[1]), 0.0, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[2]), 0.0, 1e-15);
}

BOOST_AUTO_TEST_CASE(testNormalizeHistogramMediumBias) {

    const LabelType NUM_LABELS = 3;

    cuv::ndarray<WeightType, cuv::host_memory_space> histogram(NUM_LABELS);
    histogram[0] = 60;
    histogram[1] = 20;
    histogram[2] = 20;

    cuv::ndarray<WeightType, cuv::host_memory_space> priorDistribution(NUM_LABELS);
    priorDistribution[0] = 50;
    priorDistribution[1] = 25;
    priorDistribution[2] = 25;

    ndarray<double, host_memory_space> normalizedHistogram = curfil::detail::normalizeHistogram(histogram,
            priorDistribution, 0.5, true);

    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[0]), 0.5, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[1]), 0.0, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[2]), 0.0, 1e-15);
}

BOOST_AUTO_TEST_CASE(testNormalizeHistogramLowBias) {

    const LabelType NUM_LABELS = 3;

    cuv::ndarray<WeightType, cuv::host_memory_space> histogram(NUM_LABELS);
    histogram[0] = 20;
    histogram[1] = 40;
    histogram[2] = 40;

    cuv::ndarray<WeightType, cuv::host_memory_space> priorDistribution(NUM_LABELS);
    priorDistribution[0] = 20;
    priorDistribution[1] = 10;
    priorDistribution[2] = 10;

    ndarray<double, host_memory_space> normalizedHistogram = curfil::detail::normalizeHistogram(histogram,
            priorDistribution, 0.2, true);

    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[0]), 0.0, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[1]), 0.5 * 0.25, 1e-15);
    BOOST_CHECK_CLOSE(static_cast<double>(normalizedHistogram[2]), 0.5 * 0.25, 1e-15);
}

BOOST_AUTO_TEST_CASE(testReservoirSampler) {

    size_t sampleSize = 1000;
    const int MAX = 100000;

    RandomSource randomSource(4711);
    Sampler sampler = randomSource.uniformSampler(0, 10 * MAX);

    ReservoirSampler<int> reservoirSampler(sampleSize);
    for (int i = 0; i < MAX; i++) {
        reservoirSampler.sample(sampler, i);
    }

    const auto& reservoir = reservoirSampler.getReservoir();
    BOOST_REQUIRE_EQUAL(reservoir.size(), sampleSize);

    boost::accumulators::accumulator_set<int,
            boost::accumulators::features<
                    boost::accumulators::tag::min,
                    boost::accumulators::tag::max,
                    boost::accumulators::tag::mean,
                    boost::accumulators::tag::variance> > acc;

    acc = std::for_each(reservoir.begin(), reservoir.end(), acc);

    double min = boost::accumulators::min(acc);
    double max = boost::accumulators::max(acc);
    double mean = boost::accumulators::mean(acc);
    double stddev = std::sqrt(static_cast<double>(boost::accumulators::variance(acc)));

    /*
     * values are empirically determined with python and numpy
     *
     * >>> import random, numpy as np
     * >>> a = np.arange(100000)
     * >>> random.shuffle(a)
     * >>> b = a[:10000]
     * >>> b.mean()
     * 50271.870999999999
     *
     * >>> np.sqrt(b.var())
     * 28804.962206705768
     *
     * >>> b.min()
     * 12.0
     *
     * >>> b.max()
     * 99996.0
     */
    BOOST_REQUIRE_LT(min, 0.01 * MAX);
    BOOST_REQUIRE_GT(max, 0.99 * MAX);
    BOOST_REQUIRE_CLOSE(mean, 50000.0, 0.05);
    BOOST_REQUIRE_CLOSE(stddev, 28000.0, 2.0);
}
BOOST_AUTO_TEST_SUITE_END()