//============================================================================
// Name        : benchmark_main.cc
// Author      : Giovanni Azua (bravegag@hotmail.com)
// Since       : 25.07.2013
// Description : Main application for benchmarking Eigen with MAGMA and MKL
//============================================================================

#include <assert.h>
#include <iostream>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <vector>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>

#include <boost/program_options/cmdline.hpp>
#include <boost/program_options/config.hpp>
#include <boost/program_options/environment_iterator.hpp>
#include <boost/program_options/eof_iterator.hpp>
#include <boost/program_options/errors.hpp>
#include <boost/program_options/option.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/positional_options.hpp>
#include <boost/program_options/value_semantic.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/program_options/version.hpp>

#include <boost/chrono.hpp>

#include <boost/tokenizer.hpp>

#include <Eigen/Dense>

/**
 * Define reusable Eigen vector and matrix types.
 */
template<typename T>
struct BenchType {
	typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> MatrixX;
	typedef Eigen::Matrix<T, Eigen::Dynamic, 1, Eigen::ColMajor> VectorX;
};

typedef BenchType<double>::MatrixX MatrixXd;
typedef BenchType<double>::VectorX VectorXd;

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

typedef double (*workload_type)(long);

typedef accumulator_set<double, stats<tag::mean, tag::variance> > bench_accumulator;
static bench_accumulator real_time_acc, gflops_acc;

// reusable vector and matrices. Note we need to use pointers to make sure
// that the memory is deallocated *before* MAGMA/CUDA shutdown takes place.
MatrixXd *pA = new MatrixXd(), *pB = new MatrixXd(), *pC = new MatrixXd(), *pL = new MatrixXd();
VectorXd *pa = new VectorXd(), *pb = new VectorXd(), *pc = new VectorXd();
Eigen::ColPivHouseholderQR<MatrixXd> *pAqr = new Eigen::ColPivHouseholderQR<MatrixXd>();

// continue using them as before
MatrixXd &A = *pA, &B = *pB, &C = *pC, &L = *pL;
VectorXd &a = *pa, &b = *pb, &c = *pc;
Eigen::ColPivHouseholderQR<MatrixXd>& Aqr = *pAqr;

/// Generic benchmarking
/**
 * Generic benchmarking, implement the different workload types according to the expected signature.
 */
static void run_benchmark(long N, int warm_ups, int num_runs, workload_type workload) {
	// warm up runs
	for (int i = 0; i < warm_ups; ++i) {
		// invoke workload
		(*workload)(N);

		fprintf(stderr, ".");
		fflush (stderr);
	}

	// actual measurements
	for (int i = 0; i < num_runs; ++i) {
		double real_time, cpu_time, gflops;
		double flop_count;
		system_clock::time_point start;

		// benchmark using high-resolution timer
		start = system_clock::now();

		// invoke workload
		flop_count = (*workload)(N);

		// use high-resolution timer
		duration<double> sec = system_clock::now() - start;
		real_time = sec.count();
		gflops = flop_count / (1e9 * real_time);

		// feed the accumulators
		real_time_acc(real_time);
		gflops_acc(gflops);

		fprintf(stderr, ".");
		fflush (stderr);
	}
}

EIGEN_DONT_INLINE
static double dgemm(long N) {
	C = A * B;
	// flops see http://www.netlib.org/lapack/lawnspdf/lawn41.pdf page 120
	return 2 * N * N * N;
}

EIGEN_DONT_INLINE
static double dgeqp3(long N) {
	Eigen::ColPivHouseholderQR<MatrixXd> qr = A.colPivHouseholderQr();
	// flops see http://www.netlib.org/lapack/lawnspdf/lawn41.pdf page 121
	return N * N * N - (2 / 3) * N * N * N + N * N + N * N + (14 / 3) * N;
}

EIGEN_DONT_INLINE
static double dgeqrf(long N) {
	Eigen::HouseholderQR<MatrixXd> qr = A.householderQr();
	// flops see http://www.netlib.org/lapack/lawnspdf/lawn41.pdf page 121
	return 2 * N * N * N - (2 / 3) * N * N * N + 3 * N * N - N * N + (14 / 3) * N;
}

EIGEN_DONT_INLINE
static double dgemv(long N) {
	C = A * b;
	return 2 * N * N - N;
}

EIGEN_DONT_INLINE
static double dtrsm(long N) {
	C = Aqr.solve(B);
	return N * N * N;
}

EIGEN_DONT_INLINE
static double dpotrf(long N) {
	Eigen::LLT<MatrixXd> lltOfA(A);
	L = lltOfA.matrixL();
	return N * N * N / 3.0 + N * N / 2.0 + N / 6.0;
}

EIGEN_DONT_INLINE
static double dgesvd(long N) {
	Eigen::JacobiSVD<MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
	return 22 * N * N * N;
}

int main(int argc, char** argv) {
#if !defined(NDEBUG) || defined(DEBUG)
	cerr << "Warning: you are running in debug mode - assertions are enabled." << endl;
#endif

	try {
		// program arguments
		string function;
		string range;
		int warm_ups, num_runs, device_id = 0;

		po::options_description desc("Benchmark main options");
		desc.add_options()("help", "produce help message")
			("warm-up-runs", po::value<int>(&warm_ups)->default_value(1), "warm up runs e.g. 1")
			("num-runs", po::value<int>(&num_runs)->default_value(10), "number of runs e.g. 10")
			("function", po::value < string > (&function)->default_value("dgemm"), "Function to test e.g. dgemm, dgeqp3")
			("range", po::value < string > (&range)->default_value("1024:10240:1024"), "N range i.e. start:stop:step")
			("device-id", po::value<int>(&device_id)->default_value(0), "device id e.g. 0");

		po::variables_map vm;
		po::store(po::parse_command_line(argc, argv, desc), vm);
		po::notify(vm);

#if defined(EIGEN_USE_MAGMA_ALL)
		cudaSetDevice(device_id);
		magma_init();
#endif
		if (vm.count("help")) {
			cout << desc << "\n";
			return EXIT_FAILURE;

		} else {
			string temp = range;
			boost::tokenizer<> tok(temp);
			vector<long> range_values;
			for (boost::tokenizer<>::iterator current = tok.begin(); current != tok.end(); ++current) {
				range_values.push_back(boost::lexical_cast<long>(*current));
			}

			if (range_values.size() != 3) {
				throw std::runtime_error("Illegal range input: '" + range + "'");
			}

			workload_type workload;
			if (function == "dgemm") {
				workload = dgemm;
			} else if (function == "dgeqp3") {
				workload = dgeqp3;
			} else if (function == "dgeqrf") {
				workload = dgeqrf;
			} else if (function == "dgemv") {
				workload = dgemv;
			} else if (function == "dtrsm") {
				workload = dtrsm;
			} else if (function == "dpotrf") {
				workload = dpotrf;
			} else if (function == "dgesvd") {
				workload = dgesvd;
			} else {
				throw std::runtime_error("Sorry, the function '" + function + "' is not yet implemented.");
			}

			for (long N = range_values[0]; N <= range_values[1]; N += range_values[2]) {
				// prepare the input data
				A = MatrixXd::Random(N, N);
				B = MatrixXd::Random(N, N);
				b = VectorXd::Random(N);

				// function-specific input data
				if (function == "dtrsm") {
					Aqr = A.colPivHouseholderQr();
				} else if (function == "dpotrf") {
					// make sure A is SPD
					A = A.adjoint() * A;
				}

				real_time_acc = bench_accumulator();
				gflops_acc = bench_accumulator();

				// run the benchmark
				run_benchmark(N, warm_ups, num_runs, workload);

				fprintf(stdout, "%d\t%e\t%e\n", N, mean(real_time_acc), mean(gflops_acc));
				fflush (stdout);
				fprintf(stderr, "%d,%e,%e\n", N, mean(real_time_acc), mean(gflops_acc));
				fflush (stderr);
			}
		}

		// now is safe to clean up
		delete pa;
		delete pb;
		delete pc;

		delete pA;
		delete pB;
		delete pC;
		delete pL;

		delete pAqr;

#if defined(EIGEN_USE_MAGMA_ALL)
		magma_finalize();
#endif
	}
	catch (std::exception& e) {
		cerr << "Exception: " << e.what() << "\n";
		return EXIT_FAILURE;
	}
	catch (...) {
		cerr << "Exception of unknown type!\n";
		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}


//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef _BOOST_UBLAS_OPERATION_
#define _BOOST_UBLAS_OPERATION_

#include <boost/numeric/ublas/matrix_proxy.hpp>

/** \file operation.hpp
 *  \brief This file contains some specialized products.
 */

// axpy-based products
// Alexei Novakov had a lot of ideas to improve these. Thanks.
// Hendrik Kueck proposed some new kernel. Thanks again.

namespace boost { namespace numeric { namespace ublas {

    template<class V, class T1, class L1, class IA1, class TA1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const compressed_matrix<T1, L1, 0, IA1, TA1> &e1,
               const vector_expression<E2> &e2,
               V &v, row_major_tag) {
        typedef typename V::size_type size_type;
        typedef typename V::value_type value_type;

        for (size_type i = 0; i < e1.filled1 () -1; ++ i) {
            size_type begin = e1.index1_data () [i];
            size_type end = e1.index1_data () [i + 1];
            value_type t (v (i));
            for (size_type j = begin; j < end; ++ j)
                t += e1.value_data () [j] * e2 () (e1.index2_data () [j]);
            v (i) = t;
        }
        return v;
    }

    template<class V, class T1, class L1, class IA1, class TA1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const compressed_matrix<T1, L1, 0, IA1, TA1> &e1,
               const vector_expression<E2> &e2,
               V &v, column_major_tag) {
        typedef typename V::size_type size_type;

        for (size_type j = 0; j < e1.filled1 () -1; ++ j) {
            size_type begin = e1.index1_data () [j];
            size_type end = e1.index1_data () [j + 1];
            for (size_type i = begin; i < end; ++ i)
                v (e1.index2_data () [i]) += e1.value_data () [i] * e2 () (j);
        }
        return v;
    }

    // Dispatcher
    template<class V, class T1, class L1, class IA1, class TA1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const compressed_matrix<T1, L1, 0, IA1, TA1> &e1,
               const vector_expression<E2> &e2,
               V &v, bool init = true) {
        typedef typename V::value_type value_type;
        typedef typename L1::orientation_category orientation_category;

        if (init)
            v.assign (zero_vector<value_type> (e1.size1 ()));
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type verrorbound (norm_1 (v) + norm_1 (e1) * norm_1 (e2));
        indexing_vector_assign<scalar_plus_assign> (cv, prod (e1, e2));
#endif
        axpy_prod (e1, e2, v, orientation_category ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (v - cv) <= 2 * std::numeric_limits<real_type>::epsilon () * verrorbound, internal_logic ());
#endif
        return v;
    }
    template<class V, class T1, class L1, class IA1, class TA1, class E2>
    BOOST_UBLAS_INLINE
    V
    axpy_prod (const compressed_matrix<T1, L1, 0, IA1, TA1> &e1,
               const vector_expression<E2> &e2) {
        typedef V vector_type;

        vector_type v (e1.size1 ());
        return axpy_prod (e1, e2, v, true);
    }

    template<class V, class T1, class L1, class IA1, class TA1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const coordinate_matrix<T1, L1, 0, IA1, TA1> &e1,
               const vector_expression<E2> &e2,
               V &v, bool init = true) {
        typedef typename V::size_type size_type;
        typedef typename V::value_type value_type;
        typedef L1 layout_type;

        size_type size1 = e1.size1();
        size_type size2 = e1.size2();

        if (init) {
            noalias(v) = zero_vector<value_type>(size1);
        }

        for (size_type i = 0; i < e1.nnz(); ++i) {
            size_type row_index = layout_type::index_M( e1.index1_data () [i], e1.index2_data () [i] );
            size_type col_index = layout_type::index_m( e1.index1_data () [i], e1.index2_data () [i] );
            v( row_index ) += e1.value_data () [i] * e2 () (col_index);
        }
        return v;
    }

    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const matrix_expression<E1> &e1,
               const vector_expression<E2> &e2,
               V &v, packed_random_access_iterator_tag, row_major_tag) {
        typedef const E1 expression1_type;
        typedef typename V::size_type size_type;

        typename expression1_type::const_iterator1 it1 (e1 ().begin1 ());
        typename expression1_type::const_iterator1 it1_end (e1 ().end1 ());
        while (it1 != it1_end) {
            size_type index1 (it1.index1 ());
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression1_type::const_iterator2 it2 (it1.begin ());
            typename expression1_type::const_iterator2 it2_end (it1.end ());
#else
            typename expression1_type::const_iterator2 it2 (boost::numeric::ublas::begin (it1, iterator1_tag ()));
            typename expression1_type::const_iterator2 it2_end (boost::numeric::ublas::end (it1, iterator1_tag ()));
#endif
            while (it2 != it2_end) {
                v (index1) += *it2 * e2 () (it2.index2 ());
                ++ it2;
            }
            ++ it1;
        }
        return v;
    }

    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const matrix_expression<E1> &e1,
               const vector_expression<E2> &e2,
               V &v, packed_random_access_iterator_tag, column_major_tag) {
        typedef const E1 expression1_type;
        typedef typename V::size_type size_type;

        typename expression1_type::const_iterator2 it2 (e1 ().begin2 ());
        typename expression1_type::const_iterator2 it2_end (e1 ().end2 ());
        while (it2 != it2_end) {
            size_type index2 (it2.index2 ());
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression1_type::const_iterator1 it1 (it2.begin ());
            typename expression1_type::const_iterator1 it1_end (it2.end ());
#else
            typename expression1_type::const_iterator1 it1 (boost::numeric::ublas::begin (it2, iterator2_tag ()));
            typename expression1_type::const_iterator1 it1_end (boost::numeric::ublas::end (it2, iterator2_tag ()));
#endif
            while (it1 != it1_end) {
                v (it1.index1 ()) += *it1 * e2 () (index2);
                ++ it1;
            }
            ++ it2;
        }
        return v;
    }

    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const matrix_expression<E1> &e1,
               const vector_expression<E2> &e2,
               V &v, sparse_bidirectional_iterator_tag) {
        typedef const E2 expression2_type;

        typename expression2_type::const_iterator it (e2 ().begin ());
        typename expression2_type::const_iterator it_end (e2 ().end ());
        while (it != it_end) {
            v.plus_assign (column (e1 (), it.index ()) * *it);
            ++ it;
        }
        return v;
    }

    // Dispatcher
    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const matrix_expression<E1> &e1,
               const vector_expression<E2> &e2,
               V &v, packed_random_access_iterator_tag) {
        typedef typename E1::orientation_category orientation_category;
        return axpy_prod (e1, e2, v, packed_random_access_iterator_tag (), orientation_category ());
    }

  /** \brief computes <tt>v += A x</tt> or <tt>v = A x</tt> in an
          optimized fashion.

          \param e1 the matrix expression \c A
          \param e2 the vector expression \c x
          \param v  the result vector \c v
          \param init a boolean parameter

          <tt>axpy_prod(A, x, v, init)</tt> implements the well known
          axpy-product.  Setting \a init to \c true is equivalent to call
          <tt>v.clear()</tt> before <tt>axpy_prod</tt>. Currently \a init
          defaults to \c true, but this may change in the future.

          Up to now there are some specialisation for compressed
          matrices that give a large speed up compared to prod.

          \ingroup blas2

          \internal

          template parameters:
          \param V type of the result vector \c v
          \param E1 type of a matrix expression \c A
          \param E2 type of a vector expression \c x
  */
    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const matrix_expression<E1> &e1,
               const vector_expression<E2> &e2,
               V &v, bool init = true) {
        typedef typename V::value_type value_type;
        typedef typename E2::const_iterator::iterator_category iterator_category;

        if (init)
            v.assign (zero_vector<value_type> (e1 ().size1 ()));
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type verrorbound (norm_1 (v) + norm_1 (e1) * norm_1 (e2));
        indexing_vector_assign<scalar_plus_assign> (cv, prod (e1, e2));
#endif
        axpy_prod (e1, e2, v, iterator_category ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (v - cv) <= 2 * std::numeric_limits<real_type>::epsilon () * verrorbound, internal_logic ());
#endif
        return v;
    }
    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V
    axpy_prod (const matrix_expression<E1> &e1,
               const vector_expression<E2> &e2) {
        typedef V vector_type;

        vector_type v (e1 ().size1 ());
        return axpy_prod (e1, e2, v, true);
    }

    template<class V, class E1, class T2, class IA2, class TA2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const compressed_matrix<T2, column_major, 0, IA2, TA2> &e2,
               V &v, column_major_tag) {
        typedef typename V::size_type size_type;
        typedef typename V::value_type value_type;

        for (size_type j = 0; j < e2.filled1 () -1; ++ j) {
            size_type begin = e2.index1_data () [j];
            size_type end = e2.index1_data () [j + 1];
            value_type t (v (j));
            for (size_type i = begin; i < end; ++ i)
                t += e2.value_data () [i] * e1 () (e2.index2_data () [i]);
            v (j) = t;
        }
        return v;
    }

    template<class V, class E1, class T2, class IA2, class TA2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const compressed_matrix<T2, row_major, 0, IA2, TA2> &e2,
               V &v, row_major_tag) {
        typedef typename V::size_type size_type;

        for (size_type i = 0; i < e2.filled1 () -1; ++ i) {
            size_type begin = e2.index1_data () [i];
            size_type end = e2.index1_data () [i + 1];
            for (size_type j = begin; j < end; ++ j)
                v (e2.index2_data () [j]) += e2.value_data () [j] * e1 () (i);
        }
        return v;
    }

    // Dispatcher
    template<class V, class E1, class T2, class L2, class IA2, class TA2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const compressed_matrix<T2, L2, 0, IA2, TA2> &e2,
               V &v, bool init = true) {
        typedef typename V::value_type value_type;
        typedef typename L2::orientation_category orientation_category;

        if (init)
            v.assign (zero_vector<value_type> (e2.size2 ()));
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type verrorbound (norm_1 (v) + norm_1 (e1) * norm_1 (e2));
        indexing_vector_assign<scalar_plus_assign> (cv, prod (e1, e2));
#endif
        axpy_prod (e1, e2, v, orientation_category ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (v - cv) <= 2 * std::numeric_limits<real_type>::epsilon () * verrorbound, internal_logic ());
#endif
        return v;
    }
    template<class V, class E1, class T2, class L2, class IA2, class TA2>
    BOOST_UBLAS_INLINE
    V
    axpy_prod (const vector_expression<E1> &e1,
               const compressed_matrix<T2, L2, 0, IA2, TA2> &e2) {
        typedef V vector_type;

        vector_type v (e2.size2 ());
        return axpy_prod (e1, e2, v, true);
    }

    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               V &v, packed_random_access_iterator_tag, column_major_tag) {
        typedef const E2 expression2_type;
        typedef typename V::size_type size_type;

        typename expression2_type::const_iterator2 it2 (e2 ().begin2 ());
        typename expression2_type::const_iterator2 it2_end (e2 ().end2 ());
        while (it2 != it2_end) {
            size_type index2 (it2.index2 ());
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression2_type::const_iterator1 it1 (it2.begin ());
            typename expression2_type::const_iterator1 it1_end (it2.end ());
#else
            typename expression2_type::const_iterator1 it1 (boost::numeric::ublas::begin (it2, iterator2_tag ()));
            typename expression2_type::const_iterator1 it1_end (boost::numeric::ublas::end (it2, iterator2_tag ()));
#endif
            while (it1 != it1_end) {
                v (index2) += *it1 * e1 () (it1.index1 ());
                ++ it1;
            }
            ++ it2;
        }
        return v;
    }

    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               V &v, packed_random_access_iterator_tag, row_major_tag) {
        typedef const E2 expression2_type;
        typedef typename V::size_type size_type;

        typename expression2_type::const_iterator1 it1 (e2 ().begin1 ());
        typename expression2_type::const_iterator1 it1_end (e2 ().end1 ());
        while (it1 != it1_end) {
            size_type index1 (it1.index1 ());
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression2_type::const_iterator2 it2 (it1.begin ());
            typename expression2_type::const_iterator2 it2_end (it1.end ());
#else
            typename expression2_type::const_iterator2 it2 (boost::numeric::ublas::begin (it1, iterator1_tag ()));
            typename expression2_type::const_iterator2 it2_end (boost::numeric::ublas::end (it1, iterator1_tag ()));
#endif
            while (it2 != it2_end) {
                v (it2.index2 ()) += *it2 * e1 () (index1);
                ++ it2;
            }
            ++ it1;
        }
        return v;
    }

    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               V &v, sparse_bidirectional_iterator_tag) {
        typedef const E1 expression1_type;

        typename expression1_type::const_iterator it (e1 ().begin ());
        typename expression1_type::const_iterator it_end (e1 ().end ());
        while (it != it_end) {
            v.plus_assign (*it * row (e2 (), it.index ()));
            ++ it;
        }
        return v;
    }

    // Dispatcher
    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               V &v, packed_random_access_iterator_tag) {
        typedef typename E2::orientation_category orientation_category;
        return axpy_prod (e1, e2, v, packed_random_access_iterator_tag (), orientation_category ());
    }

  /** \brief computes <tt>v += AT x</tt> or <tt>v = AT x</tt> in an
          optimized fashion.

          \param e1 the vector expression \c x
          \param e2 the matrix expression \c A
          \param v  the result vector \c v
          \param init a boolean parameter

          <tt>axpy_prod(x, A, v, init)</tt> implements the well known
          axpy-product.  Setting \a init to \c true is equivalent to call
          <tt>v.clear()</tt> before <tt>axpy_prod</tt>. Currently \a init
          defaults to \c true, but this may change in the future.

          Up to now there are some specialisation for compressed
          matrices that give a large speed up compared to prod.

          \ingroup blas2

          \internal

          template parameters:
          \param V type of the result vector \c v
          \param E1 type of a vector expression \c x
          \param E2 type of a matrix expression \c A
  */
    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               V &v, bool init = true) {
        typedef typename V::value_type value_type;
        typedef typename E1::const_iterator::iterator_category iterator_category;

        if (init)
            v.assign (zero_vector<value_type> (e2 ().size2 ()));
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type verrorbound (norm_1 (v) + norm_1 (e1) * norm_1 (e2));
        indexing_vector_assign<scalar_plus_assign> (cv, prod (e1, e2));
#endif
        axpy_prod (e1, e2, v, iterator_category ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (v - cv) <= 2 * std::numeric_limits<real_type>::epsilon () * verrorbound, internal_logic ());
#endif
        return v;
    }
    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V
    axpy_prod (const vector_expression<E1> &e1,
               const matrix_expression<E2> &e2) {
        typedef V vector_type;

        vector_type v (e2 ().size2 ());
        return axpy_prod (e1, e2, v, true);
    }

    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               M &m, TRI,
               dense_proxy_tag, row_major_tag) {

        typedef typename M::size_type size_type;

#if BOOST_UBLAS_TYPE_CHECK
        typedef typename M::value_type value_type;
        matrix<value_type, row_major> cm (m);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_plus_assign> (cm, prod (e1, e2), row_major_tag ());
#endif
        size_type size1 (e1 ().size1 ());
        size_type size2 (e1 ().size2 ());
        for (size_type i = 0; i < size1; ++ i)
            for (size_type j = 0; j < size2; ++ j)
                row (m, i).plus_assign (e1 () (i, j) * row (e2 (), j));
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               M &m, TRI,
               sparse_proxy_tag, row_major_tag) {

        typedef TRI triangular_restriction;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;

#if BOOST_UBLAS_TYPE_CHECK
        typedef typename M::value_type value_type;
        matrix<value_type, row_major> cm (m);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_plus_assign> (cm, prod (e1, e2), row_major_tag ());
#endif
        typename expression1_type::const_iterator1 it1 (e1 ().begin1 ());
        typename expression1_type::const_iterator1 it1_end (e1 ().end1 ());
        while (it1 != it1_end) {
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression1_type::const_iterator2 it2 (it1.begin ());
            typename expression1_type::const_iterator2 it2_end (it1.end ());
#else
            typename expression1_type::const_iterator2 it2 (boost::numeric::ublas::begin (it1, iterator1_tag ()));
            typename expression1_type::const_iterator2 it2_end (boost::numeric::ublas::end (it1, iterator1_tag ()));
#endif
            while (it2 != it2_end) {
                // row (m, it1.index1 ()).plus_assign (*it2 * row (e2 (), it2.index2 ()));
                matrix_row<expression2_type> mr (e2 (), it2.index2 ());
                typename matrix_row<expression2_type>::const_iterator itr (mr.begin ());
                typename matrix_row<expression2_type>::const_iterator itr_end (mr.end ());
                while (itr != itr_end) {
                    if (triangular_restriction::other (it1.index1 (), itr.index ()))
                        m (it1.index1 (), itr.index ()) += *it2 * *itr;
                    ++ itr;
                }
                ++ it2;
            }
            ++ it1;
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               M &m, TRI,
               dense_proxy_tag, column_major_tag) {
        typedef typename M::size_type size_type;

#if BOOST_UBLAS_TYPE_CHECK
        typedef typename M::value_type value_type;
        matrix<value_type, column_major> cm (m);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_plus_assign> (cm, prod (e1, e2), column_major_tag ());
#endif
        size_type size1 (e2 ().size1 ());
        size_type size2 (e2 ().size2 ());
        for (size_type j = 0; j < size2; ++ j)
            for (size_type i = 0; i < size1; ++ i)
                column (m, j).plus_assign (e2 () (i, j) * column (e1 (), i));
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               M &m, TRI,
               sparse_proxy_tag, column_major_tag) {
        typedef TRI triangular_restriction;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;

#if BOOST_UBLAS_TYPE_CHECK
        typedef typename M::value_type value_type;
        matrix<value_type, column_major> cm (m);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_plus_assign> (cm, prod (e1, e2), column_major_tag ());
#endif
        typename expression2_type::const_iterator2 it2 (e2 ().begin2 ());
        typename expression2_type::const_iterator2 it2_end (e2 ().end2 ());
        while (it2 != it2_end) {
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression2_type::const_iterator1 it1 (it2.begin ());
            typename expression2_type::const_iterator1 it1_end (it2.end ());
#else
            typename expression2_type::const_iterator1 it1 (boost::numeric::ublas::begin (it2, iterator2_tag ()));
            typename expression2_type::const_iterator1 it1_end (boost::numeric::ublas::end (it2, iterator2_tag ()));
#endif
            while (it1 != it1_end) {
                // column (m, it2.index2 ()).plus_assign (*it1 * column (e1 (), it1.index1 ()));
                matrix_column<expression1_type> mc (e1 (), it1.index1 ());
                typename matrix_column<expression1_type>::const_iterator itc (mc.begin ());
                typename matrix_column<expression1_type>::const_iterator itc_end (mc.end ());
                while (itc != itc_end) {
                    if(triangular_restriction::other (itc.index (), it2.index2 ()))
                       m (itc.index (), it2.index2 ()) += *it1 * *itc;
                    ++ itc;
                }
                ++ it1;
            }
            ++ it2;
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    // Dispatcher
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               M &m, TRI, bool init = true) {
        typedef typename M::value_type value_type;
        typedef typename M::storage_category storage_category;
        typedef typename M::orientation_category orientation_category;
        typedef TRI triangular_restriction;

        if (init)
            m.assign (zero_matrix<value_type> (e1 ().size1 (), e2 ().size2 ()));
        return axpy_prod (e1, e2, m, triangular_restriction (), storage_category (), orientation_category ());
    }
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               TRI) {
        typedef M matrix_type;
        typedef TRI triangular_restriction;

        matrix_type m (e1 ().size1 (), e2 ().size2 ());
        return axpy_prod (e1, e2, m, triangular_restriction (), true);
    }

  /** \brief computes <tt>M += A X</tt> or <tt>M = A X</tt> in an
          optimized fashion.

          \param e1 the matrix expression \c A
          \param e2 the matrix expression \c X
          \param m  the result matrix \c M
          \param init a boolean parameter

          <tt>axpy_prod(A, X, M, init)</tt> implements the well known
          axpy-product.  Setting \a init to \c true is equivalent to call
          <tt>M.clear()</tt> before <tt>axpy_prod</tt>. Currently \a init
          defaults to \c true, but this may change in the future.

          Up to now there are no specialisations.

          \ingroup blas3

          \internal

          template parameters:
          \param M type of the result matrix \c M
          \param E1 type of a matrix expression \c A
          \param E2 type of a matrix expression \c X
  */
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M &
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               M &m, bool init = true) {
        typedef typename M::value_type value_type;
        typedef typename M::storage_category storage_category;
        typedef typename M::orientation_category orientation_category;

        if (init)
            m.assign (zero_matrix<value_type> (e1 ().size1 (), e2 ().size2 ()));
        return axpy_prod (e1, e2, m, full (), storage_category (), orientation_category ());
    }
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2) {
        typedef M matrix_type;

        matrix_type m (e1 ().size1 (), e2 ().size2 ());
        return axpy_prod (e1, e2, m, full (), true);
    }

    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M &
    opb_prod (const matrix_expression<E1> &e1,
              const matrix_expression<E2> &e2,
              M &m,
              dense_proxy_tag, row_major_tag) {
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;

#if BOOST_UBLAS_TYPE_CHECK
        matrix<value_type, row_major> cm (m);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_plus_assign> (cm, prod (e1, e2), row_major_tag ());
#endif
        size_type size (BOOST_UBLAS_SAME (e1 ().size2 (), e2 ().size1 ()));
        for (size_type k = 0; k < size; ++ k) {
            vector<value_type> ce1 (column (e1 (), k));
            vector<value_type> re2 (row (e2 (), k));
            m.plus_assign (outer_prod (ce1, re2));
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M &
    opb_prod (const matrix_expression<E1> &e1,
              const matrix_expression<E2> &e2,
              M &m,
              dense_proxy_tag, column_major_tag) {
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;

#if BOOST_UBLAS_TYPE_CHECK
        matrix<value_type, column_major> cm (m);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_plus_assign> (cm, prod (e1, e2), column_major_tag ());
#endif
        size_type size (BOOST_UBLAS_SAME (e1 ().size2 (), e2 ().size1 ()));
        for (size_type k = 0; k < size; ++ k) {
            vector<value_type> ce1 (column (e1 (), k));
            vector<value_type> re2 (row (e2 (), k));
            m.plus_assign (outer_prod (ce1, re2));
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    // Dispatcher

  /** \brief computes <tt>M += A X</tt> or <tt>M = A X</tt> in an
          optimized fashion.

          \param e1 the matrix expression \c A
          \param e2 the matrix expression \c X
          \param m  the result matrix \c M
          \param init a boolean parameter

          <tt>opb_prod(A, X, M, init)</tt> implements the well known
          axpy-product. Setting \a init to \c true is equivalent to call
          <tt>M.clear()</tt> before <tt>opb_prod</tt>. Currently \a init
          defaults to \c true, but this may change in the future.

          This function may give a speedup if \c A has less columns than
          rows, because the product is computed as a sum of outer
          products.

          \ingroup blas3

          \internal

          template parameters:
          \param M type of the result matrix \c M
          \param E1 type of a matrix expression \c A
          \param E2 type of a matrix expression \c X
  */
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M &
    opb_prod (const matrix_expression<E1> &e1,
              const matrix_expression<E2> &e2,
              M &m, bool init = true) {
        typedef typename M::value_type value_type;
        typedef typename M::storage_category storage_category;
        typedef typename M::orientation_category orientation_category;

        if (init)
            m.assign (zero_matrix<value_type> (e1 ().size1 (), e2 ().size2 ()));
        return opb_prod (e1, e2, m, storage_category (), orientation_category ());
    }
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M
    opb_prod (const matrix_expression<E1> &e1,
              const matrix_expression<E2> &e2) {
        typedef M matrix_type;

        matrix_type m (e1 ().size1 (), e2 ().size2 ());
        return opb_prod (e1, e2, m, true);
    }

}}}

#endif


/* boost random/mersenne_twister.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: mersenne_twister.hpp,v 1.20 2005/07/21 22:04:31 jmaurer Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_MERSENNE_TWISTER_HPP
#define BOOST_RANDOM_MERSENNE_TWISTER_HPP

#include <iostream>
#include <algorithm>     // std::copy
#include <stdexcept>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/integer_traits.hpp>
#include <boost/cstdint.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/detail/workaround.hpp>
#include <boost/random/detail/ptr_helper.hpp>

namespace boost {
namespace random {

// http://www.math.keio.ac.jp/matumoto/emt.html
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
class mersenne_twister
{
public:
  typedef UIntType result_type;
  BOOST_STATIC_CONSTANT(int, word_size = w);
  BOOST_STATIC_CONSTANT(int, state_size = n);
  BOOST_STATIC_CONSTANT(int, shift_size = m);
  BOOST_STATIC_CONSTANT(int, mask_bits = r);
  BOOST_STATIC_CONSTANT(UIntType, parameter_a = a);
  BOOST_STATIC_CONSTANT(int, output_u = u);
  BOOST_STATIC_CONSTANT(int, output_s = s);
  BOOST_STATIC_CONSTANT(UIntType, output_b = b);
  BOOST_STATIC_CONSTANT(int, output_t = t);
  BOOST_STATIC_CONSTANT(UIntType, output_c = c);
  BOOST_STATIC_CONSTANT(int, output_l = l);

  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);

  mersenne_twister() { seed(); }

#if defined(__SUNPRO_CC) && (__SUNPRO_CC <= 0x520)
  // Work around overload resolution problem (Gennadiy E. Rozental)
  explicit mersenne_twister(const UIntType& value)
#else
  explicit mersenne_twister(UIntType value)
#endif
  { seed(value); }
  template<class It> mersenne_twister(It& first, It last) { seed(first,last); }

  template<class Generator>
  explicit mersenne_twister(Generator & gen) { seed(gen); }

  // compiler-generated copy ctor and assignment operator are fine

  void seed() { seed(UIntType(5489)); }

#if defined(__SUNPRO_CC) && (__SUNPRO_CC <= 0x520)
  // Work around overload resolution problem (Gennadiy E. Rozental)
  void seed(const UIntType& value)
#else
  void seed(UIntType value)
#endif
  {
    // New seeding algorithm from
    // http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/MT2002/emt19937ar.html
    // In the previous versions, MSBs of the seed affected only MSBs of the
    // state x[].
    const UIntType mask = ~0u;
    x[0] = value & mask;
    for (i = 1; i < n; i++) {
      // See Knuth "The Art of Computer Programming" Vol. 2, 3rd ed., page 106
      x[i] = (1812433253UL * (x[i-1] ^ (x[i-1] >> (w-2))) + i) & mask;
    }
  }

  // For GCC, moving this function out-of-line prevents inlining, which may
  // reduce overall object code size.  However, MSVC does not grok
  // out-of-line definitions of member function templates.
  template<class Generator>
  void seed(Generator & gen)
  {
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    BOOST_STATIC_ASSERT(!std::numeric_limits<result_type>::is_signed);
#endif
    // I could have used std::generate_n, but it takes "gen" by value
    for(int j = 0; j < n; j++)
      x[j] = gen();
    i = n;
  }

  template<class It>
  void seed(It& first, It last)
  {
    int j;
    for(j = 0; j < n && first != last; ++j, ++first)
      x[j] = *first;
    i = n;
    if(first == last && j < n)
      throw std::invalid_argument("mersenne_twister::seed");
  }

  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return 0; }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const
  {
    // avoid "left shift count >= with of type" warning
    result_type res = 0;
    for(int i = 0; i < w; ++i)
      res |= (1u << i);
    return res;
  }

  result_type operator()();
  static bool validation(result_type v) { return val == v; }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const mersenne_twister& mt)
  {
    for(int j = 0; j < mt.state_size; ++j)
      os << mt.compute(j) << " ";
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, mersenne_twister& mt)
  {
    for(int j = 0; j < mt.state_size; ++j)
      is >> mt.x[j] >> std::ws;
    // MSVC (up to 7.1) and Borland (up to 5.64) don't handle the template
    // value parameter "n" available from the class template scope, so use
    // the static constant with the same value
    mt.i = mt.state_size;
    return is;
  }
#endif

  friend bool operator==(const mersenne_twister& x, const mersenne_twister& y)
  {
    for(int j = 0; j < state_size; ++j)
      if(x.compute(j) != y.compute(j))
        return false;
    return true;
  }

  friend bool operator!=(const mersenne_twister& x, const mersenne_twister& y)
  { return !(x == y); }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(const mersenne_twister& rhs) const
  {
    for(int j = 0; j < state_size; ++j)
      if(compute(j) != rhs.compute(j))
        return false;
    return true;
  }

  bool operator!=(const mersenne_twister& rhs) const
  { return !(*this == rhs); }
#endif

private:
  // returns x(i-n+index), where index is in 0..n-1
  UIntType compute(unsigned int index) const
  {
    // equivalent to (i-n+index) % 2n, but doesn't produce negative numbers
    return x[ (i + n + index) % (2*n) ];
  }
  void twist(int block);

  // state representation: next output is o(x(i))
  //   x[0]  ... x[k] x[k+1] ... x[n-1]     x[n]     ... x[2*n-1]   represents
  //  x(i-k) ... x(i) x(i+1) ... x(i-k+n-1) x(i-k-n) ... x[i(i-k-1)]
  // The goal is to always have x(i-n) ... x(i-1) available for
  // operator== and save/restore.

  UIntType x[2*n];
  int i;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const bool mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::has_fixed_range;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::state_size;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::shift_size;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::mask_bits;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const UIntType mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::parameter_a;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::output_u;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::output_s;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const UIntType mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::output_b;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::output_t;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const UIntType mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::output_c;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::output_l;
#endif

template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
void mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::twist(int block)
{
  const UIntType upper_mask = (~0u) << r;
  const UIntType lower_mask = ~upper_mask;

  if(block == 0) {
    for(int j = n; j < 2*n; j++) {
      UIntType y = (x[j-n] & upper_mask) | (x[j-(n-1)] & lower_mask);
      x[j] = x[j-(n-m)] ^ (y >> 1) ^ (y&1 ? a : 0);
    }
  } else if (block == 1) {
    // split loop to avoid costly modulo operations
    {  // extra scope for MSVC brokenness w.r.t. for scope
      for(int j = 0; j < n-m; j++) {
        UIntType y = (x[j+n] & upper_mask) | (x[j+n+1] & lower_mask);
        x[j] = x[j+n+m] ^ (y >> 1) ^ (y&1 ? a : 0);
      }
    }

    for(int j = n-m; j < n-1; j++) {
      UIntType y = (x[j+n] & upper_mask) | (x[j+n+1] & lower_mask);
      x[j] = x[j-(n-m)] ^ (y >> 1) ^ (y&1 ? a : 0);
    }
    // last iteration
    UIntType y = (x[2*n-1] & upper_mask) | (x[0] & lower_mask);
    x[n-1] = x[m-1] ^ (y >> 1) ^ (y&1 ? a : 0);
    i = 0;
  }
}

template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
inline typename mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::result_type
mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::operator()()
{
  if(i == n)
    twist(0);
  else if(i >= 2*n)
    twist(1);
  // Step 4
  UIntType z = x[i];
  ++i;
  z ^= (z >> u);
  z ^= ((z << s) & b);
  z ^= ((z << t) & c);
  z ^= (z >> l);
  return z;
}

} // namespace random

typedef random::mersenne_twister<uint32_t,32,351,175,19,0xccab8ee7,11,
  7,0x31b6ab00,15,0xffe50000,17, 0xa37d3c92> mt11213b;

// validation by experiment from mt19937.c
typedef random::mersenne_twister<uint32_t,32,624,397,31,0x9908b0df,11,
  7,0x9d2c5680,15,0xefc60000,18, 3346425566U> mt19937;

} // namespace boost

BOOST_RANDOM_PTR_HELPER_SPEC(boost::mt19937)

#endif // BOOST_RANDOM_MERSENNE_TWISTER_HPP


#define BOOST_NO_RTTI
#include <boost/math/tools/roots.hpp>

#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <sstream>

constexpr long double Q_E = 1.6021766208e-19;

struct PotLoopBase {
    Float
        phi_t, g_A, psi_a, psi_b, n_i,
        N_A, eps_si, phi_ms, Q_0, g_t, cox;
    std::vector<Float> psi_t{}, N_t{};

    unsigned tol_bits{31}; //result will be exact to 2^(1-tol_bits)
    unsigned long max_iter{2000};

    inline Float exp_phi_t(Float x) const {
        return std::exp(x / phi_t);
    }

    inline Float fs_ea(Float psi_s, Float v_ch) const {
        return 1./(1. + g_A * exp_phi_t(psi_a - psi_s + v_ch));
    }

    inline Float fb_ea() const {
        return 1./(1. + g_A * exp_phi_t(psi_a - psi_b));
    }

    inline Float fs_Et(Float psi_t, Float psi_s, Float v_ch) const {
        return 1. / (1. + g_t * exp_phi_t(+psi_t - psi_s + v_ch));
    }

    Float Q_it(Float psi_s, Float v_ch) const {
        Float ret = 0;

        for(size_t i=0; i < psi_t.size(); ++i) {
            if(g_t == 0)
                ret += (-Q_E) * N_t[i];
            else
                ret += (-Q_E) * N_t[i] * fs_Et(psi_t[i], psi_s, v_ch);
        }
        return ret;
    }

    Float v_fb(Float psi_s, Float v_ch) const {
        return phi_ms + (Q_0 - Q_it(psi_s, v_ch)) / cox;
    }
};

struct PotLoop : public PotLoopBase {
    inline Float Es_x(Float psi_s, Float v_ch, Float fb_ea_) const {
        auto fs_ea_ = fs_ea(psi_s, v_ch);

        auto fac1 = 2. * Q_E / eps_si;
        auto fac2 = exp_phi_t(psi_s - v_ch) + exp_phi_t(-psi_s) - exp_phi_t(psi_b - v_ch) - exp_phi_t(-psi_b);
        auto fac3 = psi_s - psi_b - phi_t * std::log(fs_ea_ / fb_ea_);
        auto es2 = n_i * phi_t * fac1 * fac2 + fac1 * N_A * fac3;
        if(psi_s >= psi_b)
            return std::sqrt(es2);
        else
            return -std::sqrt(es2);
    }

    Float Es(Float psi_s, Float v_ch) const {
        auto fb_ea_ = fb_ea();
        return Es_x(psi_s, v_ch, fb_ea_);
    }

    std::pair<Float, Float> psi_s_x(Float v_ch, Float v_gb, boost::uintmax_t& iter, Float start=1.0) const {
        using boost::math::tools::bracket_and_solve_root;
        using boost::math::tools::eps_tolerance;

        const Float fb_ea_ = fb_ea();  //precalculate

        auto rootfun = [=](Float psi_s) {
            psi_s -= 1.;
            return v_fb(psi_s, v_ch) + eps_si * Es_x(psi_s, v_ch, fb_ea_) / cox + psi_s - psi_b - v_gb;
        };

        //TOMS748 instead?
        return bracket_and_solve_root(rootfun, start, 1.2, true, eps_tolerance<Float>(tol_bits), iter);
    }
};

// fermi-dirac solution
struct PotLoopFD : public PotLoopBase {
    // TODO: are these really necessary?
    Float E_i, E_v, E_c, N_c, N_v;

    inline Float fdint(Float e) const {
        // return fdk(k=0.5, phi=E / (k * T))
        static fdint_method<> fd1h("fd1h");
        return fd1h(e / (Q_E * phi_t));
    }

    inline Float Es(Float psi_s, Float v_ch) const {
        using boost::math::quadrature::gauss_kronrod;
        gauss_kronrod<double, 15> integrator;

        auto fac = 2. * Q_E / eps_si;

        const auto S2PI = 2. * boost::math::constants::one_div_root_pi<Float>();

        auto int_fun = [&](Float psi) {
            auto n_fd = N_c * S2PI * fdint(Q_E * (psi - v_ch) + E_i - E_c);
            auto p_fd = N_v * S2PI * fdint(E_v - Q_E * psi - E_i);
            auto na_min = N_A / (1. + g_A * exp_phi_t(psi_a - psi + v_ch));
            return n_fd - p_fd + na_min;
        };

        Float error, ret;
        try {
            if(psi_s >= psi_b)
                ret = sqrt(fac * integrator.integrate(int_fun, psi_b, psi_s, 5, 1e-18, &error));
            else
                ret = -sqrt(-fac * integrator.integrate(int_fun, psi_s, psi_b, 5, 1e-18, &error));
        } catch(std::domain_error const& ex) {
            std::stringstream msg;
            msg << ex.what() << " psi_s = " << psi_s;
            throw std::domain_error(msg.str());
        }

        return ret;
    }

    std::pair<Float, Float> psi_s_x(Float v_ch, Float v_gb, boost::uintmax_t& iter, Float start=1.0) const {
        using boost::math::tools::bracket_and_solve_root;
        using boost::math::tools::eps_tolerance;

        auto rootfun = [=](Float psi_s) {
            psi_s -= 1.;
            return v_fb(psi_s, v_ch) + eps_si * Es(psi_s, v_ch) / cox + psi_s - psi_b - v_gb;
        };

        //TOMS748 instead?
        return bracket_and_solve_root(rootfun, start, 1.2, true, eps_tolerance<Float>(tol_bits), iter);
    }
};

struct PotLoopGildenblat : public PotLoopBase {
    Float lam_bulk, bulk_n, bulk_p;

    inline Float Es(Float psi_s, Float v_ch) const {
        auto k_0 = std::exp(-v_ch / phi_t);
        auto phi_s = psi_s - psi_b;
        auto u = phi_s / phi_t;
        auto g_fun = 1. / lam_bulk * std::log(1. + lam_bulk * (std::exp(u) - 1));
        auto h2 = std::exp(-u) - 1 + g_fun + bulk_n / bulk_p * k_0 * (std::exp(u) - 1. - g_fun);

        auto es2 = 2 * Q_E * bulk_p * phi_t / eps_si * h2;

        if(psi_s >= psi_b)
            return std::sqrt(es2);
        else
            return -std::sqrt(es2);
    }

    std::pair<Float, Float> psi_s_x(Float v_ch, Float v_gb, boost::uintmax_t& iter, Float start=1.0) const {
        using boost::math::tools::bracket_and_solve_root;
        using boost::math::tools::eps_tolerance;

        const Float fb_ea_ = fb_ea();  //precalculate

        auto rootfun = [=](Float psi_s) {
            psi_s -= 1.;
            return v_fb(psi_s, v_ch) + eps_si * Es(psi_s, v_ch) / cox + psi_s - psi_b - v_gb;
        };

        //TOMS748 instead?
        return bracket_and_solve_root(rootfun, start, 1.2, true, eps_tolerance<Float>(tol_bits), iter);
    }
};

template<class L>
Float psi_s(L& l, Float v_ch, Float v_gb) {
    static Float last_root = 1.0;

    boost::uintmax_t iter = l.max_iter;
    auto root = l.psi_s_x(v_ch, v_gb, iter, last_root);

    if(iter == l.max_iter)
        throw std::runtime_error("no solution found in max_iter iterations");

    if(root.first + root.second < 1e-6)
        throw std::runtime_error("ran into psi_s < -1.0");

    if(!isfinite(root.first) || !isfinite(root.second))
        throw std::runtime_error("no solution found (nan or inf)");

    last_root = (root.first + root.second) / 2;
    return last_root - 1.;
}

template<class L>
py::tuple psi_s2(const L& l, Float v_ch, Float v_gb) {
    boost::uintmax_t iter = l.max_iter;
    auto root = l.psi_s_x(v_ch, v_gb, iter);
    return py::make_tuple(root.first, root.second, iter);
}


// standard includes and external libraries
#include <iostream>
#include <random>
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <nlohmann/json.hpp>
#include <ctime>
// project includes
#include "abstractclasses.h"
#include "bandit.h"
#include "oful.h"
#include "leader.h"
#include "regbalancing.h"
#include "adversarial_master.h"
#include "finitelinrep.h"
#include "utils.h"
#include "gzip.h"

using json = nlohmann::json;
using namespace std;
using namespace Eigen;

size_t PREC = 4;   // for saving numbers are rounded to PREC decimals
size_t EVERY = 1;  // save EVERY round

int main()
{
    std::time_t t = std::time(nullptr);
    char MY_TIME[100];
    std::strftime(MY_TIME, sizeof(MY_TIME), "%Y%m%d%H%M%S", std::localtime(&t));
    std::cout << MY_TIME << '\n';

    typedef std::vector<std::vector<double>> vec2double;

    int seed = time(NULL);
    // seed= 1611835201;
    seed=1611836268;
    srand (seed);
    cout << "seed: " << seed << endl;
    // rng.seed(10000); // warm it up
    int n_runs = 5, T = 10000;
    double delta = 0.01;
    double reg_val = 1.;
    double noise_std = 0.3;
    double bonus_scale = 1.;
    bool adaptive_ci = true;
    //double cor_gamma = 1./T, cor_beta = exp(1./log(T)), cor_lr0 = 20. / sqrt(T);

    std::vector<long> seeds(n_runs);
    std::generate(seeds.begin(), seeds.end(), [] ()
    {
        return rand();
    });

    // FiniteLinearRepresentation rep = make_random(20, 5, 6, true, noise_std, seed);
    // rep.save("linrep.json"); // save current model

    FiniteLinearRepresentation rep = flr_loadjson("linrep3.json", noise_std, seed);
    int dim = rep.features_dim();
    cout << "Dimension: " << dim << endl;

    std::vector<FiniteLinearRepresentation> reps;
    double MMM = rep.features_bound();
    for(int i = 1; i < dim; ++i)
    {
        FiniteLinearRepresentation rr = derank_hls(rep, i, false, true, true);
        // cout << i << ": " << rr.features_bound() << ", " << rr.param_bound() << std::endl;
        rr.normalize_features(MMM);
        // cout << i << ": " << rr.features_bound() << ", " << rr.param_bound() << std::endl;
        reps.push_back(rr);
        bool flag = rr.is_equal(rep);
        if (!flag) {
            std::cout << "Error: non realizable representation" << std::endl;
            exit(1);
        }
    }
    assert(rep.is_equal(rep));
    reps.push_back(rep);
    // cout << dim << ": " << rep.features_bound() << ", " << rep.param_bound() << std::endl;
    assert(reps.size() == dim);

    // //LEADER
    // vec2double regrets(n_runs), pseudo_regrets(n_runs);
    // #pragma omp parallel for
    // for (int i = 0; i < n_runs; ++i)
    // {
    //     std::vector<std::shared_ptr<ContRepresentation<int>>> lreps;
    //     for(auto& ll : reps)
    //     {
    //         auto tmp = std::make_shared<FiniteLinearRepresentation>(ll.copy(seeds[i]));
    //         lreps.push_back(tmp);
    //     }
    //     LEADER<int> localg(lreps, reg_val, noise_std, bonus_scale, delta/lreps.size(), adaptive_ci);
    //     ContBanditProblem<int> prb(*lreps[0], localg);
    //     prb.reset();
    //     prb.run(T);
    //     regrets[i] = prb.instant_regret;
    //     pseudo_regrets[i] = prb.exp_instant_regret;
    //     // delete localg;
    // }
    // // save_vector_csv(regrets, "LEADER_regrets.csv", EVERY, PREC);
    // save_vector_csv_gzip(regrets, "LEADER_regrets.csv.gz", EVERY, PREC);
    // // save_vector_csv(pseudo_regrets, "LEADER_pseudoregrets.csv", EVERY, PREC);
    // save_vector_csv_gzip(pseudo_regrets, "LEADER_pseudoregrets.csv.gz", EVERY, PREC);

    //just OFUL
    for(int j = 0; j < reps.size(); ++j)
    {
        vec2double regrets(n_runs), pseudo_regrets(n_runs);

        #pragma omp parallel for
        for (int i = 0; i < n_runs; ++i)
        {
            FiniteLinearRepresentation lrep = reps[j].copy(seeds[i]);
            OFUL<int> localg(lrep, reg_val, noise_std, bonus_scale, delta, adaptive_ci);
            ContBanditProblem<int> prb(lrep, localg);
            prb.reset();
            prb.run(T);
            regrets[i] = prb.instant_regret;
            pseudo_regrets[i] = prb.exp_instant_regret;
        }
        // save_vector_csv(regrets, "OFUL-rep"+std::to_string(j)+"_regrets.csv", EVERY, PREC);
        save_vector_csv_gzip(regrets, "OFUL-rep"+std::to_string(j)+"_regrets.csv.gz", EVERY, PREC);
        // save_vector_csv(pseudo_regrets, "OFUL-rep"+std::to_string(j)+"_pseudoregrets.csv", EVERY, PREC);
        save_vector_csv_gzip(pseudo_regrets, "OFUL-rep"+std::to_string(j)+"_pseudoregrets.csv.gz", EVERY, PREC);

#if 0
        #pragma omp parallel for
        for (int i = 0; i < n_runs; ++i)
        {
            std::vector<FiniteLinearRepresentation> llr{reps[j].copy(seeds[i])};
            LEADER ddd(llr, reg_val, 1, delta/llr.size(), adaptive_ci);
            LinBanditProblem prb(llr[0], ddd);
            prb.run(T);
            regrets[i] = prb.instant_regret;
            pseudo_regrets[i] = prb.exp_instant_regret;
        }
        save_vector_csv(regrets, "LEADER-rep"+std::to_string(j)+"_regrets.txt");
        save_vector_csv(pseudo_regrets, "LEADER-rep"+std::to_string(j)+"_pseudoregrets.txt");
#endif
    }
    return 0;
}


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/functional/partial.hpp>
#include <boost/hana/plus.hpp>
namespace hana = boost::hana;

constexpr auto increment = hana::partial(hana::plus, 1);
static_assert(increment(2) == 3, "");

int main() { }


#include <iostream>
#include <fstream>
using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <octomap/octomap.h>    // for octomap

#include <Eigen/Geometry>
#include <boost/format.hpp>  // for formating strings
#include <boost/timer/timer.hpp>
#include <cstdio>
int main( int argc, char** argv )
{
	boost::timer timer;
	freopen("octomap_output.txt","w", stdout);

    vector<cv::Mat> colorImgs, depthImgs;    // color image and depth image
    vector<Eigen::Isometry3d> poses;         // camera pose

    ifstream fin("./data/pose.txt");
    if (!fin)
    {
        cerr<<"cannot find pose file"<<endl;
        return 1;
    }

    for ( int i=0; i<5; i++ )
    {
        boost::format fmt( "./data/%s/%d.%s" ); //image file format
        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像

        double data[7] = {0};
        for ( int i=0; i<7; i++ )
        {
            fin>>data[i];
        }
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        Eigen::Isometry3d T(q);
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
        poses.push_back( T );
    }

    // join point cloud
    // camera intrinsic
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;

    cout<<"convert image to Octomap ..."<<endl;

    // octomap tree
    octomap::OcTree tree( 0.05 ); //  paramter is resolution

    for ( int i=0; i<5; i++ )
    {
        cout<<"converting image: "<<i+1<<endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];

        octomap::Pointcloud cloud;  // the point cloud in octomap

        for ( int v=0; v<color.rows; v++ )
            for ( int u=0; u<color.cols; u++ )
            {
                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // depth value
                if ( d==0 ) continue; // detect nothing
                if ( d >= 7000 ) continue; // depth too big, erase
                Eigen::Vector3d point;
                point[2] = double(d)/depthScale;
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy;
                Eigen::Vector3d pointWorld = T*point;
                // put word coordinate into point cloud
                cloud.push_back( pointWorld[0], pointWorld[1], pointWorld[2] );
            }
        // insert pointcloud into octomap, set original in order to calculate projection line
        tree.insertPointCloud( cloud, octomap::point3d( T(0,3), T(1,3), T(2,3) ) );
    }

    // update middle node and write
    tree.updateInnerOccupancy();
    cout<<"saving octomap ... "<<endl;
    tree.writeBinary( "octomap_output.bt" );
    cout<<" used time "<<timer.elapsed()<<endl;
    return 0;
}


#include <test_common.h>
#include <iostream>
#include <Eigen/Dense>

#include <igl/copyleft/cgal/peel_outer_hull_layers.h>
#include <igl/copyleft/cgal/remesh_self_intersections.h>
#include <igl/copyleft/cgal/RemeshSelfIntersectionsParam.h>
#include <igl/per_face_normals.h>
#include <igl/remove_unreferenced.h>
#include <igl/writeOBJ.h>

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

TEST_CASE("copyleft_cgal_peel_outer_hull_layers: TwoCubes", "[igl/copyleft/cgal]")
{
    Eigen::MatrixXd V;
    Eigen::MatrixXi F;
    igl::read_triangle_mesh(test_common::data_path("two-boxes-bad-self-union.ply"), V, F);
    REQUIRE (V.rows() == 486);
    REQUIRE (F.rows() == 708);

    typedef CGAL::Exact_predicates_exact_constructions_kernel K;
    typedef K::FT Scalar;
    typedef Eigen::Matrix<Scalar,
            Eigen::Dynamic,
            Eigen::Dynamic> MatrixXe;

    MatrixXe Vs;
    Eigen::MatrixXi Fs, IF;
    Eigen::VectorXi J, IM;
    igl::copyleft::cgal::RemeshSelfIntersectionsParam param;
    igl::copyleft::cgal::remesh_self_intersections(V, F, param, Vs, Fs, IF, J, IM);

    std::for_each(Fs.data(),Fs.data()+Fs.size(),
            [&IM](int & a){ a=IM(a); });
    MatrixXe Vt;
    Eigen::MatrixXi Ft;
    igl::remove_unreferenced(Vs,Fs,Vt,Ft,IM);
    const size_t num_faces = Ft.rows();

    Eigen::VectorXi I, flipped;
    size_t num_peels = igl::copyleft::cgal::peel_outer_hull_layers(Vt, Ft, I, flipped);

    Eigen::MatrixXd vertices(Vt.rows(), Vt.cols());
    std::transform(Vt.data(), Vt.data() + Vt.rows() * Vt.cols(),
            vertices.data(), [](Scalar v) { return CGAL::to_double(v); });
    igl::writeOBJ("debug.obj", vertices, Ft);

    REQUIRE (I.rows() == num_faces);
    REQUIRE (I.minCoeff() == 0);
    REQUIRE (I.maxCoeff() == 1);
}

TEST_CASE("PeelOuterHullLayers: CubeWithFold", "[igl/copyleft/cgal]")
{
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> V;
    Eigen::MatrixXi F;
    igl::read_triangle_mesh(test_common::data_path("cube_with_fold.ply"), V, F);

    typedef CGAL::Exact_predicates_exact_constructions_kernel K;
    typedef K::FT Scalar;
    typedef Eigen::Matrix<Scalar,
            Eigen::Dynamic,
            Eigen::Dynamic> MatrixXe;

    MatrixXe Vs;
    Eigen::MatrixXi Fs, IF;
    Eigen::VectorXi J, IM;
    igl::copyleft::cgal::RemeshSelfIntersectionsParam param;
    igl::copyleft::cgal::remesh_self_intersections(V, F, param, Vs, Fs, IF, J, IM);

    std::for_each(Fs.data(),Fs.data()+Fs.size(),
            [&IM](int & a){ a=IM(a); });
    MatrixXe Vt;
    Eigen::MatrixXi Ft;
    igl::remove_unreferenced(Vs,Fs,Vt,Ft,IM);

    Eigen::VectorXi I, flipped;
    size_t num_peels = igl::copyleft::cgal::peel_outer_hull_layers(Vt, Ft, I, flipped);
}


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

int main()
{
  const int size = 3, N = size * size;
  mtl::compressed2D<double>          A(N, N);

  laplacian_setup(A, size, size);
  std::cout<< "Laplacian_setup=\n" << A << "\n";

  diagonal_setup(A, 2.0);
  std::cout<< "diagonal_setup=\n" << A << "\n";

  mtl::dense2D<double>		     B(N, N);   //to expensive for sparse Matrix
  hessian_setup(B, 4.0);
  std::cout<< "hessian_setup=\n" << B << "\n";

  return 0;
}


#include <basix/e-lagrange.h>
#include <basix/quadrature.h>
#include <boost/program_options.hpp>
#include <cmath>
#include <dolfinx.h>
#include <dolfinx/io/XDMFFile.h>
#include <iostream>

#include <cublas_v2.h>
#include <cuda_profiler_api.h>

// Helper functions
#include <cuda/allocator.hpp>
#include <cuda/array.hpp>
#include <cuda/la.hpp>
#include <cuda/mass.hpp>
#include <cuda/scatter.hpp>
#include <cuda/transform.hpp>
#include <cuda/utils.hpp>
#include <operators.hpp>

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

void assert_cublas(cudaError_t e) {
  if (e != cudaSuccess)
    throw std::runtime_error(" Unable to allocate memoy - cublas error");
}
int main(int argc, char* argv[]) {

  po::options_description desc("Allowed options");
  desc.add_options()("help,h", "print usage message")(
      "size", po::value<std::size_t>()->default_value(32))(
      "degree", po::value<int>()->default_value(1))(
      "check", po::value<bool>()->default_value(false));

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).allow_unregistered().run(),
            vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << "\n";
    return 0;
  }

  const std::size_t Nx = vm["size"].as<std::size_t>();
  const int degree = vm["degree"].as<int>();
  const bool check = vm["check"].as<bool>();

  common::subsystem::init_logging(argc, argv);
  common::subsystem::init_mpi(argc, argv);
  {
    // MPI
    MPI_Comm mpi_comm{MPI_COMM_WORLD};
    int rank = utils::set_device(mpi_comm);

    // Create cublas handle
    cublasHandle_t handle;
    cublasCreate(&handle);

    // Read mesh and mesh tags
    std::array<std::array<double, 3>, 2> p = {{{0.0, 0.0, 0.0}, {1.0, 1.0, 1.0}}};
    std::array<std::size_t, 3> n = {Nx, Nx, Nx};
    auto mesh = std::make_shared<mesh::Mesh>(mesh::create_box(
        mpi_comm, p, n, mesh::CellType::hexahedron, mesh::GhostMode::none));

    // Create a Basix continuous Lagrange element of given degree
    basix::FiniteElement e = basix::element::create_lagrange(
        mesh::cell_type_to_basix_type(mesh::CellType::hexahedron), degree,
        basix::element::lagrange_variant::gll_warped, false);

    // Create a scalar function space
    std::shared_ptr<fem::FunctionSpace> V
        = std::make_shared<fem::FunctionSpace>(fem::create_functionspace(mesh, e, 1));
    auto idxmap = V->dofmap()->index_map;

    int ncells = mesh->topology().index_map(3)->size_local();
    int ndofs = e.dim();

    fem::Function<double> u(V);
    // Interpolate sin(2 \pi x[0]) in the scalar Lagrange finite element space
    constexpr double PI = xt::numeric_constants<double>::PI;
    u.interpolate([PI](auto&& x) { return PI * xt::row(x, 0); });

    CUDA::allocator<double> allocator{};
    la::Vector<double, decltype(allocator)> x(idxmap, 1, allocator);
    la::Vector<double, decltype(allocator)> y(idxmap, 1, allocator);
    std::fill(x.mutable_array().begin(), x.mutable_array().end(), 1);

    linalg::prefetch(0, x);
    linalg::prefetch(0, y);

    auto quad = basix::quadrature::type::gll;
    int qdegree = (degree > 1) ? degree + 1 : degree;
    MassOperator<double> op(V, e, quad, qdegree);

    double t = MPI_Wtime();
    op.apply(x, y);
    t = MPI_Wtime() - t;

    if (check) {
      la::Vector<double> x(idxmap, 1);
      la::Vector<double> y1(idxmap, 1);
      std::fill(x.mutable_array().begin(), x.mutable_array().end(), 1);
      MassOperatorCPU<double> cpu_op(V, degree);
      double t = MPI_Wtime();
      cpu_op(x, y1);
      t = MPI_Wtime() - t;
      std::cout << "Y norm: " << y1.norm() << std::endl;

      for (int i = 0; i < x.array().size(); i++) {
        double err = y1.array()[i] - y.array()[i];
        if (std::abs(err) > 1e-8)
          std::cout << y1.array()[i] - y.array()[i] << " ";
      }
      std::cout << "\n#Elapsed Time: " << t << std::endl;
    }

    std::cout << "X norm: " << x.norm() << std::endl;
    std::cout << "Y norm: " << y.norm() << std::endl;
    std::cout << "Number of cells: " << op.num_cells();
    std::cout << "\nNumber of dofs: " << op.num_dofs();
    std::cout << "\nNumber of quads: " << op.num_quads();
    std::cout << "\n#Elapsed Time: " << t;
    std::cout << "\nDOF/s: " << V->dofmap()->index_map->size_local() / t;
    std::cout << std::endl;
  }

  common::subsystem::finalize_mpi();
  return 0;
}


#ifndef _TAG_HPP
#define _TAG_HPP

#include <boost/math/quaternion.hpp>

class Tag
{
private:
   int _id;
   double _x;
   double _y;
   double _z;
   boost::math::quaternion<double> _orientation;

   static constexpr double CAMERA_HEIGHT = 0.195;
   static constexpr double CAMERA_OFFSET = -0.023;
public:
   const static int NEST_TAG_ID = 256;
   const static int CUBE_TAG_ID = 0;

   Tag(int id, double x, double y, double z, boost::math::quaternion<double> orientation);
   ~Tag() {}

   double Alignment() const;
   double Distance() const;
   double HorizontalDistance() const;
   double GetX() const { return _x; }
   double GetY() const { return _y; }
   double GetZ() const { return _z; }
   boost::math::quaternion<double> GetOrientation() const { return _orientation; }
   double GetYaw()   const;
   double GetPitch() const;
   int    GetId()    const { return _id; }
   bool   IsCube()   const;
   bool   IsNest()   const;

   friend std::ostream& operator<<(std::ostream& os, const Tag& tag);
};

#endif // _TAG_HPP


/*
*  ERD2.cpp
*
*      Author: gstoszek
*/

#include "ERD2.h"
#include "Centrality.h"
#include "../algebraic/CSRMatrix.h"
#include "../numerics/LAMG/Lamg.h"
#include "../auxiliary/Log.h"
#include <chrono>
#include <stdlib.h>
#include <cmath>
#include <armadillo>

namespace NetworKit {

  ERD2::ERD2(const Graph &G): Centrality(G, true) {
        n=G.upperNodeIdBound();
        /* EffectiveResistanceDistance-Matrix*/
        ERD.resize(n);
        /*Laplacian*/
        L.set_size(n,n);
        L.zeros();
        /* */
        vList.resize(n);
        vAdj_List.resize(n);

        G.forNodes([&](node v){
            ERD[v].resize(n);
            ERD[v][v]=0.;
            vAdj_List[v].resize(0);
            vList[v]=v;
            G.forNodes([&](node w){
              if(G.hasEdge(v,w)){
                /*instead of 1 one should use a function for weighted edges*/
                L(v,w)=-1.;
                L(v,v)+=1.;
                vAdj_List[v].push_back(w);
              }
            });
            nEdges+=L(v,v);
        });
      }
    /**************************************************************************/
    std::vector<std::vector<double>> ERD2::getERDMatrix() {
        return ERD;
    }
    /**************************************************************************/
    void ERD2::run(){
      /**Set LEVEL here**/
      upperLevelIdBound =1;
      count current_Level;
      current_Level=0;
      coarsed_List.resize(0);
      auto start = std::chrono::high_resolution_clock::now();
      while(current_Level<upperLevelIdBound){
        coarse_L(current_Level);
        current_Level+=1;
      }
      auto end = std::chrono::high_resolution_clock::now();
      std::chrono::duration<double> diff = end-start;
      std::cout<< "Coarsening finished in: " << diff.count() << "(s)" << "\n";
      /*invert*/
      start = std::chrono::high_resolution_clock::now();
      L=arma::pinv(L, 0.01);
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout<< "Pinv() calculated in: " << diff.count() << "(s)" << "\n";
      /*calculate initial ERD Matrix*/
      for(count i=0;i<vList.size();i++){
        for(count j=i+1;j<vList.size();j++){
          ERD[vList[i]][vList[j]]=L(i,i)+L(j,j)-2.*L(i,j);
          ERD[vList[j]][vList[i]]=ERD[vList[i]][vList[j]];
        }
      }
      for(count i=0;i<coarsed_List.size();i++){
        for(count j=0;j<ERD.size();j++){
          ERD[coarsed_List[i].first][j]=ERD[vAdj_List[coarsed_List[i].first][0]][j]+1.;
          ERD[j][coarsed_List[i].first]=ERD[coarsed_List[i].first][j];
        }
      }
    }
    /***************************************************************************/
    void uncoarse_L(){

    }

    /***************************************************************************/
    void ERD2::coarse_L(count current_Level){
      std::vector<node> coarse_List;
      coarse_List.resize(0);
      for(count i=0;i<L.n_rows;i++){
        if(L(i,i)==current_Level+1){
          coarse_List.push_back(i);
        }
      }
      for(count i=0;i<coarse_List.size();i++){
        for(count j=1;j<vAdj_List[coarse_List[i]].size();j++){
          if(L(vAdj_List[coarse_List[i]][0],vAdj_List[coarse_List[i]][j])!=-1){
            L(vAdj_List[coarse_List[i]][0],vAdj_List[coarse_List[i]][j])=-1.;
            L(vAdj_List[coarse_List[i]][j],vAdj_List[coarse_List[i]][0])=-1.;
            tM[vList[i]][vAdj_List[coarse_List[i]][j]];
            tM[vAdj_List[coarse_List[i]][j]][vList[i]];
            /*Diagonal*/
            L(vAdj_List[coarse_List[i]][0],vAdj_List[coarse_List[i]][0])+=1.;
          }
        }
        L(vAdj_List[coarse_List[i]][0],vAdj_List[coarse_List[i]][0])-=1.;
        coarsed_List.push_back(std::make_pair(vList[coarse_List[i]],current_Level));
      }
      arma::uvec indices(vList.size()-coarse_List.size());
      count k;
      count j;
      k=0;
      j=0;
      for(count i=0;i<vList.size();i++){
        if(coarse_List[k]==i){
          k++;
        }
        else{
          indices(j)=i;
          j++;
        }
      }
      for(count i=0;i<coarse_List.size();i++){
        vList.erase(vList.begin()+coarse_List[i]-i);
      }
      L=L.submat(indices, indices);
    }
    /**************************************************************************/
    /*
    void ERD2::sort_Level_List(std::vector<node> vLevel_List,count LevelId){
      std::vector<std::vector<node>> bucket_List;
      bucket_List.resize(upperLevelIdBound)
      bucket_List.resize(0);
      count k,
      k=0;
      while(k!=vLevel_List.size()){
        if(nAdj_List[vLevel_List[k]].second<upperNodeIdBound){
          bucket_List[nAdj_List[vLevel_List[k]].second-1].push_back vLevel_List[k];
        }
        else{
          bucket_List[upperNodeIdBound].push_back vLevel_List[k];
        }
        k++;
      }
      for(count i=0;i<bucket_List.size();i++){
        for(count j=0;j<bucket_List[i].size();j++){
          vLevel_List=bucket_List[i][j];
        }
      }
    }
    */
    /**************************************************************************/
    /*
    void ERD2::coarse(count samplesize, count LevelID){
      if(LevelID==3){
      }
      else{
        std::pair<node,node> coarsed_edge;
        std::vector<std::pair<bool,double>> Absorber_Edge_List;
        std::vector<std::pair<bool,double>> Submitter_Edge_List;
        Absorber_Edge_List.resize(Adj.size());
        Submitter_Edge_List.resize(Adj.size());
        coarsed_edge=select_edge();
        //coarsed_edge=random_edge();
        Absorber_Edge_List=Adj[coarsed_edge.first];
        Submitter_Edge_List=Adj[coarsed_edge.second];
        coarsening(coarsed_edge);
        coarse(1);
        uncoarsening(Absorber_Edge_List,Submitter_Edge_List,coarsed_edge);
      }
    }
    */
    /**************************************************************************/
    /*
    std::pair<node,node> ERD2::select_edge(){
      return make_pair(Matching_List[vLevel_List[0]].second,vLevel_List[0])
    }
    */
    /**************************************************************************/
    /*
    std::pair<node,node> ERD2::random_edge(){
      count Random_Edge;
      count i;
      count a;
      count b;

      a=1;
      b=nAdj_List.size()-1;

      i=a+(b-a)*0.5;
      Random_Edge= (std::rand() % nEdges)+1;
      while(!((Random_Edge>nAdj_List[i-1].second)&&(Random_Edge<=nAdj_List[i].second))){
        if(Random_Edge<=nAdj_List[i-1].second){
          b=i;
        }
        else{
          a=i;
        }
        i=a+(b-a)*0.5;
      }
      count k=0;
      count j=0;
      while(k<Random_Edge-nAdj_List[i-1].second){
        if(Adj[nAdj_List[i].first][j].first){
            k++;
        }
        j++;
      }
      j--;
      if(j>=G.upperNodeIdBound()){
        std::cout<<"<"<<nAdj_List[i].first<<","<<j<<"> mit "<<Random_Edge<<"\n";
        std::cout<<"No. of Edges="<<nEdges<<"\n";
        std::cout<<"i-1="<<i+1<<"-->"<<"v="<<nAdj_List[i-1].first<<" No.="<< nAdj_List[i-1].second<<"\n";
        std::cout<<"i="<<i<<"-->"<<"v="<<nAdj_List[i].first<<" No.="<< nAdj_List[i].second<<"\n";
        std::cout<<"i+1="<<i+1<<"-->"<<"v="<<nAdj_List[i+1].first<<" No.="<< nAdj_List[i+1].second<<"\n";
        count p;
        for(count k=0;k<Adj.size();k++){
          if(Adj[nAdj_List[i].first][k].first){
            p++;
          }
        }
        std::cout<<"p="<<p<<"\n";
      }
      return std::make_pair(nAdj_List[i].first,j);
    }
    */
    /**************************************************************************/
    /*
    void ERD2::coarsening_2(std::pair<node,node> coarsed_edge){
      std::cout << "coarsening! <a,s>=<" << coarsed_edge.first << "," << coarsed_edge.second <<">"<<"\n";
      vLevel_List.erase (vLevel_List.begin());
      Adj[coarsed_edge.first][coarsed_edge.second].first=false;
      Adj[coarsed_edge.second][coarsed_edge.first].first=false;
    }
    */
    /**************************************************************************/
    /**************************************************************************/
    /*void ERD2::coarsening(std::pair<node,node> coarsed_edge){
      count x;
      count y;
      std::vector<int> transfered_Edges;
      transfered_Edges.resize(Adj.size()+1);
      transfered_Edges[0]=0;
      for(count i=1;i<nAdj_List.size();i++){
        x=nAdj_List[i].first;
        if(Adj[coarsed_edge.second][x].first==true){
          if(Adj[coarsed_edge.first][x].first==false){
            Adj[coarsed_edge.first][x]=std::make_pair(true,Adj[coarsed_edge.second][x].second);
            Adj[x][coarsed_edge.first]=std::make_pair(true,Adj[coarsed_edge.second][x].second);
            transfered_Edges[x+1]++;
            transfered_Edges[coarsed_edge.first+1]++;
          }
          Adj[coarsed_edge.second][x]=std::make_pair(false,0.);
          Adj[x][coarsed_edge.second]=std::make_pair(false,0.);
          transfered_Edges[x+1]--;
          transfered_Edges[coarsed_edge.second+1]--;
        }
      }
      Adj[coarsed_edge.first][coarsed_edge.first]=std::make_pair(false,0);
      transfered_Edges[coarsed_edge.first+1]-=2;
      y=1;
      nAdj_List[1].second+=transfered_Edges[nAdj_List[1].first+1];
      for(count i=2;i<nAdj_List.size();i++){
          transfered_Edges[nAdj_List[i].first+1]+=transfered_Edges[nAdj_List[i-1].first+1];
          nAdj_List[i].second+=transfered_Edges[nAdj_List[i].first+1];
          if(nAdj_List[i].first==coarsed_edge.second){
            y=i;
          }
      }
      nAdj_List.erase(nAdj_List.begin()+y);
      nEdges=nAdj_List[nAdj_List.size()-1].second;
    }
    */
    /**************************************************************************/
    /*
    void ERD2::uncoarsening(std::vector<std::pair<bool,double>> Absorber_Edge_List,
      std::vector<std::pair<bool,double>> Submitter_Edge_List,
      std::pair<node,node> coarsed_edge){
        count x;

        first_join(coarsed_edge);
        nAdj_List.push_back(std::make_pair(coarsed_edge.second,1));
        for(count i=1; i<nAdj_List.size();i++){
          x=nAdj_List[i].first;
          if((Submitter_Edge_List[x].first) && (x!=coarsed_edge.first)){
            if(Absorber_Edge_List[x].first){
              edge_fire(std::make_pair(x,coarsed_edge.second));
            }
            else{
              ERD[coarsed_edge.second][x]=ERD[coarsed_edge.first]
            }
          }
          if((Adj[coarsed_edge.first][x].first)&&(Absorber_Edge_List[x].first!=true)){
            non_bridge_delete(std::make_pair(x,coarsed_edge.first));
          }
        }
      }
      */
    /**************************************************************************/
    /*
    void ERD2::first_join(std::pair<node,node> coarsed_edge){
      for(count i=1;i<nAdj_List.size();i++){
        ERD[nAdj_List[i].first][coarsed_edge.second]=ERD[nAdj_List[i].first][coarsed_edge.first]+1.;
        ERD[coarsed_edge.second][nAdj_List[i].first]=ERD[nAdj_List[i].first][coarsed_edge.second];
      }
      Adj[coarsed_edge.first][coarsed_edge.second]=std::make_pair(true,1.);
      Adj[coarsed_edge.second][coarsed_edge.first]=std::make_pair(true,1.);
    }
    */
    /**************************************************************************/
    /*
    void ERD2::edge_fire(std::pair<node,node> coarsed_edge){
      std::vector<std::vector<double>> ERD2;
      ERD2.resize(ERD.size());
      G.forNodes([&](node v){
        ERD2[v].resize(ERD.size());
      });
      count x;
      count y;

      for(count i=1;i<nAdj_List.size();i++){
        x=nAdj_List[i].first;
        for(count j=i+1;j<nAdj_List.size();j++){
          y=nAdj_List[j].first;
          ERD2[x][y]=ERD[x][coarsed_edge.second]-ERD[x][coarsed_edge.first];
          ERD2[x][y]-=ERD[coarsed_edge.second][y]-ERD[coarsed_edge.first][y];
          ERD2[x][y]*=ERD2[x][y];
          ERD2[x][y]/=4.*(1.+ERD[coarsed_edge.first][coarsed_edge.second]);
          ERD2[x][y]=ERD[x][y]-ERD2[x][y];
          ERD2[y][x]=ERD2[x][y];
        }
      }
      ERD=ERD2;
      Adj[coarsed_edge.first][coarsed_edge.second]=std::make_pair(true,1.);
      Adj[coarsed_edge.second][coarsed_edge.first]=std::make_pair(true,1.);
    }
    */
    /**************************************************************************/
    /*
    void ERD2::non_bridge_delete(std::pair<node,node> coarsed_edge){
      std::vector<std::vector<double>> ERD2;
      ERD2.resize(ERD.size());
      G.forNodes([&](node v){
        ERD2[v].resize(ERD.size());
      });
      count x;
      count y;

      for(count i=1;i<nAdj_List.size();i++){
        x=nAdj_List[i].first;
        for(count j=i+1;j<nAdj_List.size();j++){
          y=nAdj_List[j].first;
          ERD2[x][y]=ERD[x][coarsed_edge.second]-ERD[x][coarsed_edge.first];
          ERD2[x][y]-=ERD[coarsed_edge.second][y]-ERD[coarsed_edge.first][y];
          ERD2[x][y]*=ERD2[x][y];
          ERD2[x][y]/=4.*(1.-ERD[coarsed_edge.first][coarsed_edge.second]);
          ERD2[x][y]=ERD[x][y]+ERD2[x][y];
          ERD2[y][x]=ERD2[x][y];
        }
      }
      ERD=ERD2;
      Adj[coarsed_edge.first][coarsed_edge.second]=std::make_pair(false,0.);
      Adj[coarsed_edge.second][coarsed_edge.first]=std::make_pair(false,0.);
    }
    */
    /**************************************************************************/
} /* namespace NetworKit*/


// Copyright (c) 2016 Matt Overby
//
// MPM-OPTIMIZATION Uses the BSD 2-Clause License (http://www.opensource.org/licenses/BSD-2-Clause)
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright notice, this list of
// conditions and the following disclaimer.
// 2. 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.
// THIS SOFTWARE IS PROVIDED "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 UNIVERSITY OF MINNESOTA, DULUTH 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.

#ifndef INTERP_HPP
#define INTERP_HPP 1

#include <Eigen/Dense>
#include <vector>

namespace mpm
{

	// Cubic B-spline
	static inline double cspline(double x)
	{
		if (x==0.0) { x=1e-12; }
		x = fabs(x);
		if (x < 1.0) { return x*x*(x*0.5 - 1.0) + 2.0/3.0; }
		else if (x < 2.0) { return x*(x*(-x/6.0 + 1.0) - 2.0) + 4.0/3.0; }
		return 0.0;
	}

	// Slope of cubic spline
	static inline double d_cspline(double x)
	{
		if (x==0.0) { x=1e-12; }
		double abs_x = fabs(x);
		if (abs_x < 1.0) { return 1.5*x*abs_x - 2.0*x; }
		else if (x < 2.0) { return -x*abs_x*0.5 + 2.0*x - 2.0*x/abs_x; }
		return 0.0;
	}

	// Quadratic spline
	static inline double qspline( double x )
	{
		double fx = fabs(x);
		if (fx < 0.5) { return ( 0.75 - x*x ); }
		else if (fx < 1.5) { return ( 0.5*x*x - 1.5*fx + 9.0/8.0 ); }
		return 0.0;
	}

	// Slope of quadratic spline
	static inline double d_qspline( double x )
	{
		double fx = fabs(x);
		if (fx < 0.5) { return ( -2.0 * x ); }
		else if (fx < 1.5)
		{
			if (x < 0.0) { return fabs( fx - 1.5 ); }
			else { return (fx - 1.5); }
		}
		return 0.0;
	}

	static inline bool isreal(double n)
	{
		return !std::isnan(n) && !std::isinf(n);
	}

} // end namespace mpm

#endif


#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <math.h>
#include <Eigen/LU>

#include <utils/pseudo_inversion.h>
#include <utils/skew_symmetric.h>
#include <lwr_controllers/multi_task_priority_inverse_kinematics.h>

namespace lwr_controllers
{
	MultiTaskPriorityInverseKinematics::MultiTaskPriorityInverseKinematics() {}
	MultiTaskPriorityInverseKinematics::~MultiTaskPriorityInverseKinematics() {}

	bool MultiTaskPriorityInverseKinematics::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
	{
        KinematicChainControllerBase<hardware_interface::EffortJointInterface>::init(robot, n);

		jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
		id_solver_.reset(new KDL::ChainDynParam(kdl_chain_,gravity_));
		fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
		tau_cmd_.resize(kdl_chain_.getNrOfJoints());
		J_.resize(kdl_chain_.getNrOfJoints());
		J_star_.resize(kdl_chain_.getNrOfJoints());

		sub_command_ = nh_.subscribe("command", 1, &MultiTaskPriorityInverseKinematics::command, this);

		pub_error_ = nh_.advertise<std_msgs::Float64MultiArray>("error", 1000);
		pub_marker_ = nh_.advertise<visualization_msgs::MarkerArray>("marker",1000);

		return true;
	}

	void MultiTaskPriorityInverseKinematics::starting(const ros::Time& time)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    		joint_des_states_.q(i) = joint_msr_states_.q(i);
    		joint_des_states_.qdot(i) = joint_msr_states_.qdot(i);
    	}

    	I_ = Eigen::Matrix<double,7,7>::Identity(7,7);
    	e_dot_ = Eigen::Matrix<double,6,1>::Zero();

    	cmd_flag_ = 0;
	}

	void MultiTaskPriorityInverseKinematics::update(const ros::Time& time, const ros::Duration& period)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    	}

    	// clearing error msg before publishing
    	msg_err_.data.clear();

    	if (cmd_flag_)
    	{
    		// resetting P and qdot(t=0) for the highest priority task
    		P_ = I_;
    		SetToZero(joint_des_states_.qdot);

    		for (int index = 0; index < ntasks_; index++)
    		{
		    	// computing Jacobian
		    	jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_,links_index_[index]);

		    	// computing forward kinematics
		    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_,links_index_[index]);

		    	// setting marker parameters
		    	set_marker(x_,index,msg_id_);

		    	// computing end-effector position/orientation error w.r.t. desired frame
		    	x_err_ = diff(x_,x_des_[index]);

		    	for(int i = 0; i < e_dot_.size(); i++)
		    	{
		    		e_dot_(i) = x_err_(i);
	    			msg_err_.data.push_back(e_dot_(i));
		    	}

		    	// computing (J[i]*P[i-1])^pinv
		    	J_star_.data = J_.data*P_;
		    	pseudo_inverse(J_star_.data,J_pinv_);

		    	// computing q_dot (qdot(i) = qdot[i-1] + (J[i]*P[i-1])^pinv*(x_err[i] - J[i]*qdot[i-1]))
		    	joint_des_states_.qdot.data = joint_des_states_.qdot.data + J_pinv_*(e_dot_ - J_.data*joint_des_states_.qdot.data);

		    	// stop condition
		    	if (!on_target_flag_[index])
		    	{
			    	if (Equal(x_,x_des_[index],0.01))
			    	{
			    		ROS_INFO("Task %d on target",index);
			    		on_target_flag_[index] = true;
			    		if (index == (ntasks_ - 1))
			    			cmd_flag_ = 0;
			    	}
			    }

		    	// updating P_ (it mustn't make use of the damped pseudo inverse)
		    	pseudo_inverse(J_star_.data,J_pinv_,false);
		    	P_ = P_ - J_pinv_*J_star_.data;
		    }

		    // integrating q_dot -> getting q (Euler method)
		    for (int i = 0; i < joint_handles_.size(); i++)
		    	joint_des_states_.q(i) += period.toSec()*joint_des_states_.qdot(i);
    	}

    	// set controls for joints
    	for (int i = 0; i < joint_handles_.size(); i++)
    	{
    		tau_cmd_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),joint_des_states_.qdot(i) - joint_msr_states_.qdot(i),period);
    		joint_handles_[i].setCommand(tau_cmd_(i));
    	}

    	// publishing markers for visualization in rviz
    	pub_marker_.publish(msg_marker_);
    	msg_id_++;

	    // publishing error for all tasks as an array of ntasks*6
	    pub_error_.publish(msg_err_);
	    ros::spinOnce();

	}

	void MultiTaskPriorityInverseKinematics::command(const lwr_controllers::MultiPriorityTask::ConstPtr &msg)
	{
		if (msg->links.size() == msg->tasks.size()/6)
		{
			ntasks_ = msg->links.size();
			ROS_INFO("Number of tasks: %d",ntasks_);
			// Dynamically resize desired postures and links index when a message arrives
			x_des_.resize(ntasks_);
			links_index_.resize(ntasks_);
			on_target_flag_.resize(ntasks_);
			msg_marker_.markers.resize(ntasks_);
			msg_id_ = 0;

			for (int i = 0; i < ntasks_; i++)
			{
					if (msg->links[i] == -1)	// adjust index
						links_index_[i] = msg->links[i];
					else if (msg->links[i] >= 1 && msg->links[i] <=joint_handles_.size())
						links_index_[i] = msg->links[i] + 1;
					else
					{
						ROS_INFO("Links index must be within 1 and %ld. (-1 is end-effector)",joint_handles_.size());
						return;
					}

					x_des_[i] = KDL::Frame(
									KDL::Rotation::RPY(msg->tasks[i*6 + 3],
													   msg->tasks[i*6 + 4],
													   msg->tasks[i*6 + 5]),
									KDL::Vector(msg->tasks[i*6],
												msg->tasks[i*6 + 1],
												msg->tasks[i*6 + 2]));

					on_target_flag_[i] = false;
			}

			cmd_flag_ = 1;
		}
		else
		{
			ROS_INFO("The number of links index and tasks must be the same");
			ROS_INFO("Tasks parameters are [x,y,x,roll,pitch,yaw]");
			return;
		}

	}

	void MultiTaskPriorityInverseKinematics::set_marker(KDL::Frame x, int index, int id)
	{
				sstr_.str("");
				sstr_.clear();

				if (links_index_[index] == -1)
					sstr_<<"end_effector";
				else
					sstr_<<"link_"<<(links_index_[index]-1);

				msg_marker_.markers[index].header.frame_id = "world";
				msg_marker_.markers[index].header.stamp = ros::Time();
				msg_marker_.markers[index].ns = sstr_.str();
				msg_marker_.markers[index].id = id;
				msg_marker_.markers[index].type = visualization_msgs::Marker::SPHERE;
				msg_marker_.markers[index].action = visualization_msgs::Marker::ADD;
				msg_marker_.markers[index].pose.position.x = x.p(0);
				msg_marker_.markers[index].pose.position.y = x.p(1);
				msg_marker_.markers[index].pose.position.z = x.p(2);
				msg_marker_.markers[index].pose.orientation.x = 0.0;
				msg_marker_.markers[index].pose.orientation.y = 0.0;
				msg_marker_.markers[index].pose.orientation.z = 0.0;
				msg_marker_.markers[index].pose.orientation.w = 1.0;
				msg_marker_.markers[index].scale.x = 0.01;
				msg_marker_.markers[index].scale.y = 0.01;
				msg_marker_.markers[index].scale.z = 0.01;
				msg_marker_.markers[index].color.a = 1.0;
				msg_marker_.markers[index].color.r = 0.0;
				msg_marker_.markers[index].color.g = 1.0;
				msg_marker_.markers[index].color.b = 0.0;
	}
}

PLUGINLIB_EXPORT_CLASS(lwr_controllers::MultiTaskPriorityInverseKinematics, controller_interface::ControllerBase)