// Copyright (c) Dietmar Wolz.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory.

// Eigen based implementation of multi objective
// Differential Evolution using the DE/all/1 strategy.
//
// Can switch to NSGA-II like population update via parameter 'nsga_update'.
// Then it works essentially like NSGA-II but instead of the tournament selection
// the whole population is sorted and the best individuals survive. To do this
// efficiently the crowd distance ordering is slightly inaccurate.
//
// Supports parallel fitness function evaluation.
//
// Features enhanced multiple constraint ranking (https://www.jstage.jst.go.jp/article/tjpnsec/11/2/11_18/_article/-char/en/)
// improving its performance in handling constraints for engineering design optimization.
//
// Enables the comparison of DE and NSGA-II population update mechanism with everything else
// kept completely identical.
//
// Uses the following deviation from the standard DE algorithm:
// a) oscillating CR/F parameters.
//
// You may keep parameters F and CR at their defaults since this implementation works well with the given settings for most problems,
// since the algorithm oscillates between different F and CR settings.
//
// For expensive objective functions (e.g. machine learning parameter optimization) use the workers
// parameter to parallelize objective function evaluation. The workers parameter is limited by the
// population size.

#include <Eigen/Core>
#include <iostream>
#include <float.h>
#include <stdint.h>
#include <ctime>
#include <random>
#include <queue>
#include <tuple>
#include "pcg_random.hpp"
#include "evaluator.h"

namespace mode_optimizer {

class MoDeOptimizer {

public:

    MoDeOptimizer(long runid_, Fitness *fitfun_, callback_type log_, int dim_,
    		int nobj_, int ncon_, int seed_,
            int popsize_, int maxEvaluations_, double F_, double CR_,
            double pro_c_, double dis_c_, double pro_m_, double dis_m_,
            bool nsga_update_, double pareto_update_, int log_period_) {
        // runid used to identify a specific run
        runid = runid_;
        // fitness function to minimize
        fitfun = fitfun_;
        // callback to log progress
        log = log_;
        // Number of objective variables/problem dimension
        dim = dim_;
        // Number of objectives
        nobj = nobj_;
        // Number of constraints
        ncon = ncon_;
        // Population size
        popsize = popsize_ > 0 ? popsize_ : 128;
        // maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_ > 0 ? maxEvaluations_ : 500000;
        // DE population update parameters, ignored if nsga_update == true
        F = F0 = F_ > 0 ? F_ : 0.5;
        CR = CR0 = CR_ > 0 ? CR_ : 0.9;
        // Number of iterations already performed.
        iterations = 0;
        // Number of evaluations already performed.
        n_evals = 0;
        // position of current x/y
        pos = 0;
        //std::random_device rd;
        rs = new pcg64(seed_);
        // NSGA population update parameters, ignored if nsga_update == false
   	    // usually use pro_c = 1.0, dis_c = 20.0, pro_m = 1.0, dis_m = 20.0.
    	pro_c = pro_c_;
    	dis_c = dis_c_;
    	pro_m = pro_m_;
    	dis_m = dis_m_;
        // if true, use NSGA population update, if false, use DE population update
        // Use DE update to diversify your results.
    	nsga_update = nsga_update_;
        // DE population update parameter. Only applied if nsga_update = false.
    	// Use the pareto front for population update
    	// with probability pareto_update, else use the whole population.
    	// If pareto_update == 0: use always the whole population.
        // usually should be 0, optimization can get stuck in local minima otherwise.
    	pareto_update = pareto_update_;
        // The log callback is called each log_period iterations
        log_period = log_period_;
        if (log_period <= 0)
            log_period = 1000;
        init();
    }

    ~MoDeOptimizer() {
        delete rs;
    }

    double rnd01() {
        return distr_01(*rs);
    }

    int rndInt(int max) {
        return (int) (max * distr_01(*rs));
    }

    vec nextX(int p) {
        if (p == 0) {
            iterations++;
        	if (iterations % log_period == 0) {
        		if (log(popX.cols(), popX.data(), popY.data()))
        			fitfun->setTerminate();
        	}
        }
    	if (nsga_update) {
    		vec x = vX.col(vp);
    		vp = (vp + 1) % popsize;
    		return x;
    	}
    	// use DE update strategy.
        if (p == 0) {
            CR = iterations % 2 == 0 ? 0.5 * CR0 : CR0;
            F = iterations % 2 == 0 ? 0.5 * F0 : F0;
        }
        int r3;
        if (rnd01() < pareto_update) {
			// sample from pareto front
            do {
            	r3 = rndInt(bestP.size());
            	r3 = bestP[r3];
            } while (r3 == p);
        } else {
			// sample from whole population
            do {
                r3 = rndInt(popsize);
            } while (r3 == p);
        }
        vec xp = popX.col(p);
        vec x3 = popX.col(r3);
        int r1, r2;
        do {
            r1 = rndInt(popsize);
        } while (r1 == p || r1 == r3);
        do {
            r2 = rndInt(popsize);
        } while (r2 == p || r2 == r3 || r2 == r1);
        vec x1 = popX.col(r1);
        vec x2 = popX.col(r2);
        vec x = x3 + (x1 - x2) * F;
        int r = rndInt(dim);
        for (int j = 0; j < dim; j++)
            if (j != r && rnd01() > CR)
                x[j] = xp[j];
        return fitfun->getClosestFeasible(x);
    }

    vec crowd_dist(mat& y) { // crowd distance for 1st objective
    	int n = y.cols();
    	vec y0 = y.row(0);
    	ivec si = sort_index(y0); // sort 1st objective
    	vec y0s = y0(si); // sorted y0
    	vec d(n-1);
        for (int i = 0; i < n-1; i++)
        	d(i) = y0s[i+1] - y0s[i]; // neighbor distance
        if (d.maxCoeff() == 0)
        	return zeros(n);
        vec dsum = zeros(n);
        for (int i = 0; i < n; i++) {
        	if (i > 0)
        		dsum(i) += d(i-1); // distance to left
        	if (i < n-1)
        		dsum(i) += d(i); //  distance to right
        }
        dsum(0) = DBL_MAX; // keep borders
        dsum(n-1) = DBL_MAX;
        vec ds(n);
        ds(si) = dsum;  // inverse order
        return ds;
    }

    bool is_dominated(const mat& y, int i, int index) {
    	for (int j = 0; j < y.rows(); j++)
    		if (y(j,i) < y(j,index))
    			return false;
    	return true;
    }

    vec pareto_levels(const mat& y) {
         int n = y.cols();
         ivec pareto(n);
         for (int i = 0; i < n; i++)
         	pareto(i) = i;
         vec domination = zeros(n);
         bool mask[n];
         for (int i = 0; i < n; i++)
        	 mask[i] = true;
         for (int index = 0; index < n;) {
    		for (int i = 0; i < n; i++) {
    			if (i != index && mask[i] && is_dominated(y, i, index))
    				mask[i] = false;
    		}
    		for (int i = 0; i < n; i++) {
    			if (mask[i])
    				domination[i] += 1;
    		}
    		index++;
    		while(!mask[index] && index < n)
    			index++;
         }
         return domination;
    }

    vec objranks(mat objs) {
    	imat ci(objs.cols(), objs.rows());
    	for (int i = 0; i < objs.rows(); i++)
    		ci.col(i) = sort_index(objs.row(i).transpose());
    	mat rank(objs.rows(), objs.cols());
    	for (int j = 0; j < objs.rows(); j++)
    		for (int i = 0; i < objs.cols(); i++) {
    			rank(j, ci(i,j)) = i;
    	}
    	return rank.colwise().sum();
    }

    vec ranks(mat cons) {
    	imat ci(cons.cols(), cons.rows());
    	for (int i = 0; i < cons.rows(); i++)
    		ci.col(i) = sort_index(cons.row(i).transpose());
    	mat rank(cons.rows(), cons.cols());
    	vec alpha = zeros(cons.rows());
    	for (int j = 0; j < cons.rows(); j++) {
    		for (int i = 0; i < cons.cols(); i++) {
    			if (cons(j,i) <= 0) {
    				rank(j, ci(i,j)) = 0;
    			} else {
    				rank(j, ci(i,j)) = i;
    				alpha[j]++;
    			}
    		}
    	}
    	for (int j = 0; j < cons.rows(); j++) {
    		for (int i = 0; i < cons.cols(); i++)
    			rank(j, ci(i,j)) *= alpha[j] / cons.rows();
    	}
    	return rank.colwise().sum();
    }

    vec pareto(const mat& ys) {
    	if (ncon == 0)
    		return pareto_levels(ys);
        int popn = ys.cols();
        mat yobj = ys(Eigen::seqN(0, nobj), Eigen::all);
        mat ycon = ys(Eigen::lastN(ncon), Eigen::all);
        vec csum = ranks(ycon);
        bool feasible[ys.cols()];
        bool hasFeasible = false;
        for (int i = 0; i < ys.cols(); i++) {
        	feasible[i] = ycon.col(i).maxCoeff() <= 0;
        	if (feasible[i])
        		hasFeasible = true;
        }
        if (hasFeasible)
        	csum += objranks(yobj);
		// first pareto front of feasible solutions
        vec domination = zeros(popn);
        std::vector<int> cyv;
        for (int i = 0; i < ys.cols(); i++) // collect feasibles
        	if (feasible[i]) cyv.push_back(i);
        ivec cy =  Eigen::Map<ivec, Eigen::Unaligned>(cyv.data(), cyv.size());
        if (hasFeasible) { // compute pareto levels only for feasible
        	vec ypar = pareto_levels(yobj(Eigen::all, cy));
        	domination(cy) += ypar;
        }
        // then constraint violations
        ivec ci = sort_index(csum);
        std::vector<int> civ;
        for (int i = 0; i < ci.size(); i++)
        	if (!feasible[ci(i)]) civ.push_back(ci(i));
        if (civ.size() > 0) {
        	ivec ci =  Eigen::Map<ivec, Eigen::Unaligned>(civ.data(), civ.size());
        	int maxcdom = ci.size();
        	// higher constraint violation level gets lower domination level assigned
        	for (int i = 0; i < ci.size(); i++)
        		domination(ci(i)) += maxcdom - i;
        	if (cy.size() > 0) { // priorize feasible solutions
            	for (int i = 0; i < cy.size(); i++)
            		domination(cy(i)) += maxcdom + 1;
        	}
        } // higher dominates lower
        return domination;
    }

    mat variation(const mat& x) {
    	int n2 = x.cols() / 2;
    	int n = 2 * n2;
        mat parent1 = x(Eigen::all, Eigen::seq(0, n2-1));
        mat parent2 = x(Eigen::all, Eigen::seq(n2, n-1));
        mat beta = mat(dim, n2);
        vec to1;
        if (pro_c < 1.0) {
        	to1 = uniformVec(dim, *rs);
        }
        for (int p = 0; p < n2; p++) {
			for (int i = 0; i < dim; i++) {
				if (rnd01() > 0.5 || (pro_c < 1.0 && to1(i) < pro_c))
					beta(i, p) = 1.0;
				else {
					double r = rnd01();
					if (r <= 0.5)
						beta(i, p) = pow(2 * r, 1.0 / (dis_c + 1.0));
					else
						beta(i, p) = pow(2 * r, -1.0 / (dis_c + 1.0));
					if (rnd01() > 0.5)
						beta(i, p) = -beta(i, p);
				}
			}
		}
        mat offspring1 = ((parent1 + parent2) * 0.5);
        mat offspring2 = mat(offspring1);
        mat delta = (beta.array() * (parent1 - parent2).array()).matrix() * 0.5;
        offspring1 += delta;
        offspring2 -= delta;
    	mat offspring = mat(dim, n);
    	offspring << offspring1, offspring2;

        double limit = pro_m / dim;
        vec scale = fitfun->scale();
        for (int p = 0; p < n; p++) {
            for (int i = 0; i < dim; i++) {
            	if (rnd01() < limit) { // site
            		double mu = rnd01();
        			double norm = fitfun->norm_i(i, offspring(i, p));
            		if (mu <= 0.5) // temp
            			offspring(i, p) += scale(i) *
                                (pow(2. * mu + (1. - 2. * mu) * pow(1. - norm, dis_m + 1.),
	                               1. / (dis_m + 1.)) - 1.);
            		else
            			offspring(i, p) += scale(i) *
                                (1. - pow(2. * (1. - mu) + 2. * (mu - 0.5) * pow(1. - norm, dis_m + 1.),
	                               1. / (dis_m + 1.)));
        		}
        	}
        }
        fitfun->setClosestFeasible(offspring);
    	return offspring;
    }

    void pop_update() {
    	vec domination = pareto(popY);
    	std::vector<vec> x;
    	std::vector<vec> y;
    	int maxdom = (int) domination.maxCoeff();
    	for (int dom = maxdom; dom >= 0; dom--) {
    		std::vector<int> level;
    		for (int i = 0; i < domination.size(); i++)
    			if (domination(i) == dom)
    				level.push_back(i);
            ivec domlevel =  Eigen::Map<ivec, Eigen::Unaligned>(level.data(), level.size());
			mat domx = popX(Eigen::all, domlevel);
			mat domy = popY(Eigen::all, domlevel);
			if (dom == maxdom) // store pareto front in bestP
				bestP = domlevel;

    		if ((int)(x.size() + domlevel.size()) <= popsize) {
				// whole level fits
    			for (int i = 0; i < domy.cols(); i++) {
    				x.push_back(domx.col(i));
    				y.push_back(domy.col(i));
    			}
    		} else {
    			std::vector<int> si;
    			si.push_back(0);
    			if (domy.cols() > 1) {
    				vec cd = crowd_dist(domy);
    				ivec si = sort_index(cd).reverse();
    				for (int i = 0; i < si.size(); i++) {
    					if (((int)x.size()) >= popsize)
    						break;
    					x.push_back(domx.col(si(i)));
    					y.push_back(domy.col(si(i)));
    				}
    			}
    			break;
    		}
    	}
    	for (int i = 0; i < popsize; i++) {
    		popX.col(i) = x[i];
       		popY.col(i) = y[i];
    	}
    	if (nsga_update)
    		vX = variation(popX(Eigen::all, Eigen::seqN(0, popsize)));
    }

    vec ask(int &p) {
		p = pos;
		vec x = nextX(p);
		pos = (pos + 1) % popsize;
		return x;
    }

    int tell(const vec &y, const vec &x, int p) {
    	long unsigned int dp = 0;
    	for (; dp < vdone.size(); dp++)
    		if (!vdone[dp]) break;
    	nX.col(dp) = x;
       	nY.col(dp) = y;
       	vdone[dp] = true;
    	int ndone = 0;
    	for (long unsigned int i = 0; i < vdone.size(); i++)
    		if (vdone[i]) ndone++;
    	if (ndone >= popsize) {
			int p = popsize;
        	for (dp = 0; dp < vdone.size(); dp++) {
        		if (vdone[dp]) {
					popX.col(p) = nX.col(dp);
					popY.col(p) = nY.col(dp);
					vdone[dp] = false;
					if (p >= popY.cols())
						break;
					p++;
        		}
        	}
        	pop_update();
    	}
    	n_evals += 1;
//        if (n_evals % 1000 == 999)
//        	std::cout << popY << std::endl;
    	return stop;
    }

    void doOptimize() {
    	iterations = 0;
    	fitfun->resetEvaluations();
    	while (fitfun->evaluations() < maxEvaluations && !fitfun->terminate()) {
            for (int p = 0; p < popsize; p++) {
            	vec x = nextX(p);
            	popX.col(popsize + p) = x;
            	popY.col(popsize + p) = fitfun->eval(x);
            }
            pop_update();
     	}
    }

    void do_optimize_delayed_update(int workers) {
    	 iterations = 0;
    	 fitfun->resetEvaluations();
         workers = std::min(workers, popsize); // workers <= popsize
    	 evaluator eval(fitfun, nobj, workers);
    	 vec evals_x[popsize];
	     // fill eval queue with initial population
    	 for (int i = 0; i < workers; i++) {
    		 int p;
    		 vec x = ask(p);
    		 eval.evaluate(x, p);
    		 evals_x[p] = x;
    	 }
    	 while (fitfun->evaluations() < maxEvaluations && !fitfun->terminate()) {
    		 vec_id* vid = eval.result();
    		 vec y = vec(vid->_v);
    		 int p = vid->_id;
    		 delete vid;
    		 vec x = evals_x[p];
    		 tell(y, x, p); // tell evaluated x
    		 if (fitfun->evaluations() >= maxEvaluations)
    			 break;
    		 x = ask(p);
    		 eval.evaluate(x, p);
    		 evals_x[p] = x;
    	 }
	}

    void init() {
        popX = mat(dim, 2*popsize);
        popY = mat(nobj + ncon, 2*popsize);
        for (int p = 0; p < popsize; p++) {
            popX.col(p) = fitfun->sample(*rs);
            popY.col(p) = constant(nobj + ncon, DBL_MAX);
        }
        next_size = 2*popsize;
        vdone = std::vector<bool>(next_size, false);
		nX = mat(dim, next_size);
		nY = mat(nobj + ncon, next_size);
		vX = mat(popX);
		vp = 0;
		bestP = ivec(popsize);
		for (int i = 0; i < popsize; i++)
			bestP(i) = i;
    }

    mat getX() {
        return popX;
    }

    mat getY() {
        return popY;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

    Fitness* getFitfun() {
        return fitfun;
    }

    int getDim() {
        return dim;
    }

private:
    long runid;
    Fitness *fitfun;
    callback_type log;
    int popsize; // population size
    int dim;
    int nobj;
    int ncon;
    int maxEvaluations;
    double keep;
    double stopfitness;
    int iterations;
    int n_evals;
    ivec bestP;
    int stop;
    double F0;
    double CR0;
    double F;
    double CR;
	double pro_c;
	double dis_c;
	double pro_m;
	double dis_m;
    pcg64 *rs;
    mat popX;
    mat popY;
    mat nX;
    mat nY;
    mat vX;
    int vp;
    int next_size;
    std::vector<bool> vdone;
    int pos;
    bool nsga_update;
    double pareto_update;
    int log_period;
};
}

using namespace mode_optimizer;

extern "C" {
void optimizeMODE_C(long runid, callback_type func, callback_type log,
		int dim, int nobj, int ncon, int seed,
        double *lower, double *upper, int maxEvals,
		int popsize, int workers, double F, double CR,
	    double pro_c, double dis_c, double pro_m, double dis_m,
        bool nsga_update, double pareto_update, int log_period, double* res) {
    vec lower_limit(dim), upper_limit(dim);
    for (int i = 0; i < dim; i++) {
        lower_limit[i] = lower[i];
        upper_limit[i] = upper[i];
    }
    Fitness fitfun(func, dim, nobj + ncon, lower_limit, upper_limit);
    MoDeOptimizer opt(runid, &fitfun, log, dim, nobj, ncon,
    		seed, popsize, maxEvals, F, CR,
            pro_c, dis_c, pro_m, dis_m,
            nsga_update, pareto_update, log_period);
    try {
        if (workers <= 1)
            opt.doOptimize();
        else
            opt.do_optimize_delayed_update(workers);
        double* xdata = opt.getX().data();
        for (int i = 0; i < opt.getX().size(); i++)
            res[i] = xdata[i];
    } catch (std::exception &e) {
    	std::cout << e.what() << std::endl;
    }
   }
}


#pragma once
#include <boost/config.hpp>
#include <iostream>
#include <vector>
#include <algorithm>
#include <utility>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>

using namespace std;
using namespace boost;

    typedef adjacency_list <vecS, vecS, undirectedS> Graph;
typedef typename graph_traits< Graph >::adjacency_iterator adjacency_iterator;

void connected_components(const uint32_t n_ver, const uint32_t n_edg
          , const uint32_t * Eu, const uint32_t * Ev, const char * active_edge
		  , std::vector<uint32_t> & in_component, std::vector< std::vector<uint32_t> > & components, const uint32_t cutoff)
{   //C-style interface

	Graph G(n_ver);
	for (uint32_t i_edg = 0; i_edg < n_edg; i_edg++)
	{
		if (active_edge[i_edg] > 0)
		{
			add_edge(Eu[i_edg], Ev[i_edg], G);
		}
	}

	int n_com = connected_components(G, &in_component[0]);

	//cout << "Total number of components: " << n_com << endl;

	std::vector< std::vector<uint32_t> > components_tmp(n_com);
	for (uint32_t i_ver = 0; i_ver < n_ver; i_ver++)
	{
		components_tmp[in_component[i_ver]].push_back(i_ver);
	}

	//fuse components to preserve cutoff

	G = Graph(n_ver);
	for (uint32_t i_edg = 0; i_edg < n_edg; i_edg++)
	{
		if (active_edge[i_edg] == 0)
		{
			add_edge(Eu[i_edg], Ev[i_edg], G);
		}
	}

	typename graph_traits < Graph >::adjacency_iterator nei_ini, nei_end;
	boost::property_map<Graph, boost::vertex_index_t>::type vertex_index_map = get(boost::vertex_index, G);
	std::vector<int> is_fused(n_ver, 0);

	int n_com_final = n_com;
	for (int i_com = 0; i_com < n_com; i_com++)
	{
		if (components_tmp[i_com].size() < cutoff)
		{//components is too small
			//std::cout << i_com << " of size " << components_tmp[i_com].size() << " / " << cutoff << std::endl;
			int largest_neigh_comp_value = 0;
			int largest_neigh_comp_index = -1;
			for (int i_ver_com = 0; i_ver_com < components_tmp[i_com].size(); i_ver_com++)
			{	//std::cout << "	considering node" << components_tmp[i_com][i_ver_com] << std::endl;
				boost::tie(nei_ini, nei_end) = adjacent_vertices(vertex(components_tmp[i_com][i_ver_com], G), G);
				for (graph_traits < Graph >::adjacency_iterator nei_ite = nei_ini; nei_ite != nei_end; nei_ite++)
				{
					int candidate_comp = in_component[vertex_index_map(*nei_ite)];
					if ((candidate_comp == i_com) || (is_fused[candidate_comp]))
					{
						continue;
					}
					//std::cout << "		neighbors " << vertex_index_map(*nei_ite) << " in comp " << candidate_comp << "of size " << components_tmp[candidate_comp].size() << std::endl;
					if (components_tmp[candidate_comp].size() > largest_neigh_comp_value)
					{
						largest_neigh_comp_value = components_tmp[candidate_comp].size() ;
						largest_neigh_comp_index = candidate_comp;
					}
				}
			}
			if (largest_neigh_comp_index>0)
			{
				//std::cout << "best comp = " << largest_neigh_comp_index << " of size " << largest_neigh_comp_value << std::endl;
				//we now fuse the two connected components
				components_tmp[largest_neigh_comp_index].insert(components_tmp[largest_neigh_comp_index].end(), components_tmp[i_com].begin(), components_tmp[i_com].end());
				is_fused[i_com] = 1;
				n_com_final--;
			}
		}
	}

	components.resize(n_com_final);
	int i_com_index = 0;
	for (int i_com = 0; i_com < n_com; i_com++)
	{
		if (!is_fused[i_com])
		{
			components[i_com_index] = components_tmp[i_com];
			for (uint32_t i_ver_com = 0; i_ver_com < components_tmp[i_com].size(); i_ver_com++)
			{
				in_component[components_tmp[i_com][i_ver_com]] = i_com_index;
			}
			i_com_index++;
		}
	}

    return;
}


#include <geodesy/utm.h>
#include <geodesy/wgs84.h>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include "hdl_graph_slam/building.hpp"
#include <fstream>
#include <regex>
#include <string>
#include <cmath>
#include <sstream>
#include <cstdlib>
#include <iomanip>

#include <curlpp/cURLpp.hpp>
#include <curlpp/Easy.hpp>
#include <curlpp/Options.hpp>
#include <curlpp/Exception.hpp>

namespace hdl_graph_slam {
	class BuildingTools {
		public:
			static std::vector<Building> getBuildings(double lat, double lon, double rad, Eigen::Vector3d zero_utm);
			struct Node
			{
			    std::string id;
			    double lat;
			    double lon;
			};
			BuildingTools(void);
		private:

			static std::string downloadBuildings(double lat, double lon, double rad);
			static std::vector<Building> parseBuildings(std::string result, Eigen::Vector3d zero_utm);
			//static Building constructBuilding(std::string bstr);
			static pcl::PointCloud<PointT3>::Ptr buildPointCloud(std::vector<std::string> nd_refs, std::vector<Node> nodes,Eigen::Vector3d zero_utm);
			static Node getNode(std::string nd_ref, std::vector<Node> nodes);
			static PointT3 toUtm(Eigen::Vector3d pt, Eigen::Vector3d zero_utm);
			static pcl::PointCloud<PointT3>::Ptr interpolate(PointT3 a, PointT3 b);
	};
}


/// \file
/// Maintainer: Felice Serena
///
///

#include "cube_iterator.h"
#include <set>

#include <boost/test/unit_test.hpp>

namespace utf = boost::unit_test;

typedef std::vector<int> CellCoordinate;

using namespace MouseTrack::SpatialImpl;

BOOST_AUTO_TEST_CASE( cube_iterator_4d ) {
    /// Create all expected grid coordinates for four dimensions
    /// Let the iterator create all grid coordinates
    /// compare the two set
    const int cubeWidth = 3;
    std::multiset<CellCoordinate> expected;
    CellCoordinate tmp;
    tmp.resize(4);
    for(int a = 0; a < cubeWidth; ++a){
        tmp[0] = a;
        for(int b = 0; b < cubeWidth; ++b){
            tmp[1] = b;
            for(int c = 0; c < cubeWidth; ++c){
                tmp[2] = c;
                for(int d = 0; d < cubeWidth; ++d){
                    tmp[3] = d;
                    expected.insert(tmp);
                }
            }
        }
    }
    std::multiset<CellCoordinate> received;
    CubeIterator<4, CellCoordinate> it(cubeWidth);
    CubeIterator<4, CellCoordinate> end;
    for(; it != end; ++it){
        received.insert(*it);
    }

    BOOST_CHECK_MESSAGE(expected.size() == received.size(), "Expected and received sets have different cardinalities.");
    BOOST_CHECK_MESSAGE(expected == received, "Expected and received set do not contain same elements." );
}

BOOST_AUTO_TEST_CASE( cube_iterator_Xd ) {
    /// Create all expected grid coordinates for four dimensions
    /// Let the iterator create all grid coordinates
    /// compare the two set
    const int cubeWidth = 3;
    std::multiset<CellCoordinate> expected;
    CellCoordinate tmp;
    tmp.resize(4);
    for(int a = 0; a < cubeWidth; ++a){
        tmp[0] = a;
        for(int b = 0; b < cubeWidth; ++b){
            tmp[1] = b;
            for(int c = 0; c < cubeWidth; ++c){
                tmp[2] = c;
                for(int d = 0; d < cubeWidth; ++d){
                    tmp[3] = d;
                    expected.insert(tmp);
                }
            }
        }
    }
    std::multiset<CellCoordinate> received;
    CubeIterator<-1, CellCoordinate> it(cubeWidth, 4);
    CubeIterator<-1, CellCoordinate> end;
    for(; it != end; ++it){
        received.insert(*it);
    }

    BOOST_CHECK_MESSAGE(expected.size() == received.size(), "Expected and received sets have different cardinalities.");
    BOOST_CHECK_MESSAGE(expected == received, "Expected and received set do not contain same elements." );
}


#ifndef NUMERICAL_OPTIMIZATION_HPP
#define NUMERICAL_OPTIMIZATION_HPP

#include <mathtoolbox/bfgs.hpp>
#include <mathtoolbox/l-bfgs.hpp>
#include <functional>
#include <stdexcept>
#include <Eigen/Core>

namespace mathtoolbox
{
    namespace optimization
    {
        enum class Algorithm
        {
            Bfgs, LBfgs
        };

        enum class Type
        {
            Min, Max
        };

        struct Setting
        {
            Algorithm algorithm                                      = Algorithm::Bfgs;
            Eigen::VectorXd x_init                                   = Eigen::VectorXd(0);
            std::function<double(const Eigen::VectorXd&)> f          = nullptr;
            std::function<Eigen::VectorXd(const Eigen::VectorXd&)> g = nullptr;
            double epsilon                                           = 1e-05;
            unsigned int max_num_iterations                          = 1000;
            Type type                                                = Type::Min;
        };

        struct Result
        {
            Eigen::VectorXd x_star;
            unsigned        num_iterations;
        };

        inline Result RunOptimization(const Setting& input)
        {
            const auto f = (input.type == Type::Min) ? input.f : [&input](const Eigen::VectorXd& x) { return - input.f(x); };
            const auto g = (input.type == Type::Min) ? input.g : [&input](const Eigen::VectorXd& x) { return - input.g(x); };

            switch (input.algorithm) {
                case Algorithm::Bfgs:
                {
                    if (!input.f || !input.g || input.x_init.rows() == 0)
                    {
                        throw std::invalid_argument("Invalid setting.");
                    }

                    Result result;
                    RunBfgs(input.x_init, f, g, input.epsilon, input.max_num_iterations, result.x_star, result.num_iterations);
                    return result;
                }
                case Algorithm::LBfgs:
                {
                    if (!input.f || !input.g || input.x_init.rows() == 0)
                    {
                        throw std::invalid_argument("Invalid setting.");
                    }

                    Result result;
                    RunLBfgs(input.x_init, f, g, input.epsilon, input.max_num_iterations, result.x_star, result.num_iterations);
                    return result;
                }
            }
        }
    }
}

#endif // NUMERICAL_OPTIMIZATION_HPP


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/vision/sfm/pose/five_point_relative_pose.h"

#include <Eigen/Dense>
#include <glog/logging.h>

#include <cmath>
#include <ctime>
#include <vector>

#include "theia/math/matrix/gauss_jordan.h"
#include "theia/math/polynomial.h"
#include "theia/vision/sfm/pose/util.h"

namespace theia {

using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::Matrix;
using Eigen::RowVector3d;
using Eigen::RowVector4d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;

typedef Matrix<double, 3, 3, Eigen::RowMajor> RowMatrix3d;

namespace {
// Multiplies two polynomials over the same variable.
template <int n1, int n2>
Matrix<double, 1, n1 + n2 - 1> MultiplyPoly(const Matrix<double, 1, n1>& a,
                                            const Matrix<double, 1, n2>& b) {
  Matrix<double, 1, n1 + n2 - 1> poly = Matrix<double, 1, n1 + n2 - 1>::Zero();
  for (int i = 0; i < a.cols(); i++)
    for (int j = 0; j < b.cols(); j++)
      poly[i + j] += a[i] * b[j];

  return poly;
}

// Evaluates a given polynomial at the value x.
template <int n>
double EvaluatePoly(const Matrix<double, 1, n>& poly, double x) {
  double val = 0;
  for (int i = poly.cols() - 1; i > 0; i--) {
    val += poly[i];
    val *= x;
  }
  val += poly[0];
  return val;
}

// Multiply two degree one polynomials of variables x, y, z.
// E.g. p1 = a[0]x + a[1]y + a[2]z + a[3]
// x^2 y^2 z^2 xy xz yz x y z 1
Matrix<double, 1, 10> MultiplyDegOnePoly(const RowVector4d& a,
                                         const RowVector4d& b) {
  Matrix<double, 1, 10> output;
  output(0) = a(0) * b(0);
  output(1) = a(1) * b(1);
  output(2) = a(2) * b(2);
  output(3) = a(0) * b(1) + a(1) * b(0);
  output(4) = a(0) * b(2) + a(2) * b(0);
  output(5) = a(1) * b(2) + a(2) * b(1);
  output(6) = a(0) * b(3) + a(3) * b(0);
  output(7) = a(1) * b(3) + a(3) * b(1);
  output(8) = a(2) * b(3) + a(3) * b(2);
  output(9) = a(3) * b(3);
  return output;
}

// Multiply a 2 deg poly (in x, y, z) and a one deg poly.
// x^3 y^3 x^2y xy^2 x^2z x^2 y^2z y^2 xyz xy | z^2x zx x z^2y zy y z^3 z^2 z 1
// NOTE: after the | all are variables along z.
Matrix<double, 1, 20> MultiplyDegTwoDegOnePoly(const Matrix<double, 1, 10>& a,
                                               const RowVector4d& b) {
  Matrix<double, 1, 20> output;
  output(0) = a(0) * b(0);
  output(1) = a(1) * b(1);
  output(2) = a(0) * b(1) + a(3) * b(0);
  output(3) = a(1) * b(0) + a(3) * b(1);
  output(4) = a(0) * b(2) + a(4) * b(0);
  output(5) = a(0) * b(3) + a(6) * b(0);
  output(6) = a(1) * b(2) + a(5) * b(1);
  output(7) = a(1) * b(3) + a(7) * b(1);
  output(8) = a(3) * b(2) + a(4) * b(1) + a(5) * b(0);
  output(9) = a(3) * b(3) + a(6) * b(1) + a(7) * b(0);
  output(10) = a(2) * b(0) + a(4) * b(2);
  output(11) = a(4) * b(3) + a(8) * b(0) + a(6) * b(2);
  output(12) = a(6) * b(3) + a(9) * b(0);
  output(13) = a(2) * b(1) + a(5) * b(2);
  output(14) = a(5) * b(3) + a(8) * b(1) + a(7) * b(2);
  output(15) = a(7) * b(3) + a(9) * b(1);
  output(16) = a(2) * b(2);
  output(17) = a(2) * b(3) + a(8) * b(2);
  output(18) = a(8) * b(3) + a(9) * b(2);
  output(19) = a(9) * b(3);
  return output;
}

// Shorthand for multiplying the Essential matrix with its transpose according
// to Eq. 20 in Nister paper.
Matrix<double, 1, 10> EETranspose(
    const Matrix<double, 9, 4>& null_matrix, int i, int j) {
  return MultiplyDegOnePoly(null_matrix.row(3 * i), null_matrix.row(3 * j)) +
      MultiplyDegOnePoly(null_matrix.row(3 * i + 1),
                         null_matrix.row(3 * j + 1)) +
      MultiplyDegOnePoly(null_matrix.row(3 * i + 2),
                         null_matrix.row(3 * j + 2));
}

// Builds the 10x20 constraint matrix according to Section 3.2.2 of Nister
// paper. Constraints are built based on the singularity of the Essential
// matrix, and the trace equation (Eq. 6). This builds the 10x20 matrix such
// that the columns correspond to: x^3, yx^2, y^2x, y^3, zx^2, zyx, zy^2, z^2x,
// z^2y, z^3, x^2, yx, y^2, zx, zy, z^2, x, y, z, 1.
Matrix<double, 10, 20> BuildConstraintMatrix(
    const Matrix<double, 9, 4>& null_space) {
  Matrix<double, 10, 20> constraint_matrix;
  // Singularity constraint.
  constraint_matrix.row(0) =
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space.row(1), null_space.row(5)) -
          MultiplyDegOnePoly(null_space.row(2), null_space.row(4)),
          null_space.row(6)) +
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space.row(2), null_space.row(3)) -
          MultiplyDegOnePoly(null_space.row(0), null_space.row(5)),
          null_space.row(7)) +
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space.row(0), null_space.row(4)) -
          MultiplyDegOnePoly(null_space.row(1), null_space.row(3)),
          null_space.row(8));

  // Trace Constraint. Only need to compute the upper triangular part of the
  // symmetric polynomial matrix
  Matrix<double, 1, 10> symmetric_poly[3][3];
  symmetric_poly[0][0] = EETranspose(null_space, 0, 0);
  symmetric_poly[1][1] = EETranspose(null_space, 1, 1);
  symmetric_poly[2][2] = EETranspose(null_space, 2, 2);

  Matrix<double, 1, 10> half_trace = 0.5*(symmetric_poly[0][0] +
                                          symmetric_poly[1][1] +
                                          symmetric_poly[2][2]);

  symmetric_poly[0][0] -= half_trace;
  symmetric_poly[1][1] -= half_trace;
  symmetric_poly[2][2] -= half_trace;
  symmetric_poly[0][1] = EETranspose(null_space, 0, 1);
  symmetric_poly[0][2] = EETranspose(null_space, 0, 2);
  symmetric_poly[1][0] = symmetric_poly[0][1];
  symmetric_poly[1][2] = EETranspose(null_space, 1, 2);
  symmetric_poly[2][0] = symmetric_poly[0][2];
  symmetric_poly[2][1] = symmetric_poly[1][2];

  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      constraint_matrix.row(3*i + j + 1) =
          MultiplyDegTwoDegOnePoly(symmetric_poly[i][0],
                                   null_space.row(j)) +
          MultiplyDegTwoDegOnePoly(symmetric_poly[i][1],
                                   null_space.row(3 + j)) +
          MultiplyDegTwoDegOnePoly(symmetric_poly[i][2],
                                   null_space.row(6 + j));
    }
  }
  return constraint_matrix;
}

// Efficient nullspace extraction based on the idea of QR decomposition on the 5
// correspondences of the epipolar constraint. Instead of QR, we use
// Gauss-Jordan for the same effect.
Matrix<double, 9, 4> EfficientNullspaceExtraction(
    const Matrix<double, 5, 9>& constraint) {
  Matrix<double, 5, 9> constraint_copy(constraint);
  GaussJordan(&constraint_copy);
  Matrix<double, 4, 9> null_space;
  null_space << constraint_copy.rightCols(4).transpose(),
      -Matrix4d::Identity();
  return null_space.transpose();
}

void EfficientSVDDecomp(const Matrix3d& essential_mat,
                        Vector3d* null_space,
                        Matrix3d rotation[4],
                        Vector3d translation[4]) {
  Matrix3d d;
  d << 0, 1, 0,
      -1, 0, 0,
      0, 0, 1;

  const Vector3d& ea = essential_mat.row(0);
  const Vector3d& eb = essential_mat.row(1);
  const Vector3d& ec = essential_mat.row(2);

  // Generate cross products.
  Matrix3d cross_products;
  cross_products << ea.cross(eb), ea.cross(ec), eb.cross(ec);

  // Choose the cross product with the largest norm (for numerical accuracy).
  const Vector3d cf_scales(cross_products.col(0).squaredNorm(),
                           cross_products.col(1).squaredNorm(),
                           cross_products.col(2).squaredNorm());
  int max_index;
  cf_scales.maxCoeff(&max_index);

  // For index 0, 1, we want ea and for index 2 we want eb.
  const int max_e_index = max_index / 2;

  // Construct v of the SVD.
  Matrix3d v = Matrix3d::Zero();
  v.col(2) = cross_products.col(max_index).normalized();
  v.col(0) = essential_mat.row(max_e_index).normalized();
  v.col(1) = v.col(2).cross(v.col(0));

  // Construct U of the SVD.
  Matrix3d u = Matrix3d::Zero();
  u.col(0) = (essential_mat * v.col(0)).normalized();
  u.col(1) = (essential_mat * v.col(1)).normalized();
  u.col(2) = u.col(0).cross(u.col(1));

  // Possible rotation configurations.
  const RowMatrix3d ra =
      Eigen::Quaterniond(u * d * v.transpose()).normalized().toRotationMatrix();
  const RowMatrix3d rb = Eigen::Quaterniond(u * d.transpose() * v.transpose())
      .normalized().toRotationMatrix();

  // Scale t to be proper magnitude. Scale factor is derived from the fact that
  // U*diag*V^t = E. We simply choose to scale it such that the last terms will
  // be equal.
  const Vector3d t = u.col(2).normalized();
  const Vector3d t_neg = -t;

  // Copy the 4 possible decompositions into the output arrays.
  rotation[0] = ra;
  translation[0] = t;
  rotation[1] = ra;
  translation[1] = t_neg;
  rotation[2] = rb;
  translation[2] = t;
  rotation[3] = rb;
  translation[3] = t_neg;

  *null_space = v.col(2);
}

void DecomposeWithIdealCorrespondence(const Vector2d& image_point1,
                                      const Vector2d& image_point2,
                                      const Matrix3d& essential_mat,
                                      Matrix3d* rotation,
                                      Vector3d* translation) {
  const Vector3d image_point1_homog = image_point1.homogeneous();
  const Vector3d image_point2_homog = image_point2.homogeneous();

  // Map the image points to vectors.
  Matrix3d candidate_rotation[4];
  Vector3d candidate_translation[4];
  Vector3d null_space;
  EfficientSVDDecomp(essential_mat, &null_space, candidate_rotation,
                     candidate_translation);

  Matrix<double, 3, 4> projection_mat;
  projection_mat.block<3, 3>(0, 0) = candidate_rotation[0];
  projection_mat.block<3, 1>(0, 3) = candidate_translation[0];

  // Compute c.
  Matrix3d temp_diag = Matrix3d::Identity();
  temp_diag(2, 2) = 0.0;
  const Vector3d c =
      image_point2_homog.cross(temp_diag * essential_mat * image_point1_homog);

  // Compute C.
  const Vector4d C = projection_mat.transpose() * c;
  const Vector4d Q(image_point1_homog(0) * C(3),
                   image_point1_homog(1) * C(3),
                   image_point1_homog(2) * C(3),
                   -(image_point1_homog.dot(C.head<3>())));
  // We only care about the sign of the depth because it informs us of the
  // direction of the point (i.e. if it is in front of the camera). Use a
  // multiply instead of divide for speed.
  const double scaled_depth_1  = Q(2) * Q(3);
  const double scaled_depth_2 = projection_mat.row(2).dot(Q) * Q(3);

  // Create the twisted pair transformation.
  const Vector4d twisted_transformation(-2.0 * null_space(0),
                                        -2.0 * null_space(1),
                                        -2.0 * null_space(2),
                                        -1.0);

  // Determine the proper configuration for the R,t decomposition.
  int best_index;
  if (scaled_depth_1 > 0 && scaled_depth_2 > 0) {
    best_index = 0;
  } else if (scaled_depth_1 < 0 && scaled_depth_2 < 0) {
    best_index = 1;
  } else if (Q(2) * twisted_transformation.dot(Q) > 0) {
    best_index = 2;
  } else {
    best_index = 3;
  }

  *rotation = candidate_rotation[best_index];
  *translation = candidate_translation[best_index];
}

}  // namespace

// Implementation of Nister from "An Efficient Solution to the Five-Point
// Relative Pose Problem"
bool FivePointRelativePose(const Vector2d image1_points[5],
                           const Vector2d image2_points[5],
                           std::vector<Matrix3d>* rotation,
                           std::vector<Vector3d>* translation) {
  // Step 1. Create the 5x9 matrix containing epipolar constraints.
  //   Essential matrix is a linear combination of the 4 vectors spanning the
  //   null space of this matrix (found by SVD).
  Matrix<double, 5, 9> epipolar_constraint;
  for (int i = 0; i < 5; i++) {
    // Fill matrix with the epipolar constraint from q'_t*E*q = 0. Where q is
    // from the first image, and q' is from the second. Eq. 8 in the Nister
    // paper.
    epipolar_constraint.row(i) <<
        image1_points[i].x() * image2_points[i].x(),
        image1_points[i].y() * image2_points[i].x(),
        image2_points[i].x(),
        image1_points[i].x() * image2_points[i].y(),
        image1_points[i].y() * image2_points[i].y(),
        image2_points[i].y(),
        image1_points[i].x(),
        image1_points[i].y(),
        1.0;
  }

  // Solve for right null space of the 5x9 matrix. NOTE: We use a
  // super-efficient method that is a variation of the QR decomposition
  // described in the Nister paper.  by roughly 5x.
  Matrix<double, 9, 4> null_space =
      EfficientNullspaceExtraction(epipolar_constraint);

  // Step 2. Expansion of the epipolar constraints Eq. 5 and 6 from Nister
  // paper.
  Matrix<double, 10, 20> constraint_matrix = BuildConstraintMatrix(null_space);

  // Step 3. Gauss-Jordan Elimination with partial pivoting on constraint
  // matrix.
  GaussJordan(&constraint_matrix);

  // Step 4. Expand determinant polynomial of 3x3 polynomial B.
  // Create matrix B. Horribly ugly, but not sure if there's a better way to do
  // it!
  RowVector4d b11(constraint_matrix(4, 12),
                  constraint_matrix(4, 11) - constraint_matrix(5, 12),
                  constraint_matrix(4, 10) - constraint_matrix(5, 11),
                  -constraint_matrix(5, 10));
  RowVector4d b12(constraint_matrix(4, 15),
                  constraint_matrix(4, 14) - constraint_matrix(5, 15),
                  constraint_matrix(4, 13) - constraint_matrix(5, 14),
                  -constraint_matrix(5, 13));
  Matrix<double, 1, 5> b13;
  b13 << constraint_matrix(4, 19),
      constraint_matrix(4, 18) - constraint_matrix(5, 19),
      constraint_matrix(4, 17) - constraint_matrix(5, 18),
      constraint_matrix(4, 16) - constraint_matrix(5, 17),
      -constraint_matrix(5, 16);
  RowVector4d b21(constraint_matrix(6, 12),
                  constraint_matrix(6, 11) - constraint_matrix(7, 12),
                  constraint_matrix(6, 10) - constraint_matrix(7, 11),
                  -constraint_matrix(7, 10));
  RowVector4d b22(constraint_matrix(6, 15),
                  constraint_matrix(6, 14) - constraint_matrix(7, 15),
                  constraint_matrix(6, 13) - constraint_matrix(7, 14),
                  -constraint_matrix(7, 13));
  Matrix<double, 1, 5> b23;
  b23 << constraint_matrix(6, 19),
      constraint_matrix(6, 18) - constraint_matrix(7, 19),
      constraint_matrix(6, 17) - constraint_matrix(7, 18),
      constraint_matrix(6, 16) - constraint_matrix(7, 17),
      -constraint_matrix(7, 16);
  RowVector4d b31(constraint_matrix(8, 12),
                  constraint_matrix(8, 11) - constraint_matrix(9, 12),
                  constraint_matrix(8, 10) - constraint_matrix(9, 11),
                  -constraint_matrix(9, 10));
  RowVector4d b32(constraint_matrix(8, 15),
                  constraint_matrix(8, 14) - constraint_matrix(9, 15),
                  constraint_matrix(8, 13) - constraint_matrix(9, 14),
                  -constraint_matrix(9, 13));
  Matrix<double, 1, 5> b33;
  b33 << constraint_matrix(8, 19),
      constraint_matrix(8, 18) - constraint_matrix(9, 19),
      constraint_matrix(8, 17) - constraint_matrix(9, 18),
      constraint_matrix(8, 16) - constraint_matrix(9, 17),
      -constraint_matrix(9, 16);

  // Eq. 24.
  Matrix<double, 1, 8> p1 = MultiplyPoly(b12, b23) - MultiplyPoly(b13, b22);
  // Eq. 25.
  Matrix<double, 1, 8> p2 = MultiplyPoly(b13, b21) - MultiplyPoly(b11, b23);
  // Eq. 26.
  Matrix<double, 1, 7> p3 = MultiplyPoly(b11, b22) - MultiplyPoly(b12, b21);

  // Eq. 27. Form determinant of B as a 10th degree polynomial.
  Matrix<double, 1, 11> n = MultiplyPoly(p1, b31) + MultiplyPoly(p2, b32) +
                            MultiplyPoly(p3, b33);

  // Step 5. Extract real roots of the 10th degree polynomial.
  Eigen::VectorXd roots;
  FindRealPolynomialRoots(n.transpose().reverse(), &roots);

  rotation->reserve(roots.size());
  translation->reserve(roots.size());
  static const double kTolerance = 1e-12;
  for (int i = 0; i < roots.size(); i++) {
    // We only want non-zero roots
    if (fabs(roots(i)) < kTolerance)
      continue;

    double x = EvaluatePoly(p1, roots(i)) / EvaluatePoly(p3, roots(i));
    double y = EvaluatePoly(p2, roots(i)) / EvaluatePoly(p3, roots(i));
    Matrix<double, 9, 1> temp_sum =
        x * null_space.col(0) + y * null_space.col(1) +
        roots(i) * null_space.col(2) + null_space.col(3);
    // Need to do it like this because temp_sum is a row vector and recasting
    // it as a 3x3 will load it column-major.
    Matrix3d candidate_essential_mat;
    candidate_essential_mat << temp_sum.head<3>().transpose(),
        temp_sum.segment(3, 3).transpose(), temp_sum.tail(3).transpose();

    Matrix3d rotation_soln;
    Vector3d translation_soln;
    // Decompose into R, t using the first point correspondence.
    DecomposeWithIdealCorrespondence(image1_points[0], image2_points[0],
                                     candidate_essential_mat, &rotation_soln,
                                     &translation_soln);
    rotation->push_back(rotation_soln);
    translation->push_back(translation_soln);
  }
  return (roots.size() > 0);
}

}  // namespace theia


#pragma once
#include <vector>
#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Matrix6d PoseCovariance;             // x,y,z,roll,pitch,yaw
typedef Eigen::Matrix3d PosisitonCovariance; // x,y,z
typedef Eigen::Vector3d Position;            // x,y,z
typedef Eigen::Quaterniond Quat;             // x,y,z,w
typedef Eigen::Vector3d Euler;               // roll,pitch,yaw

namespace amcl_3d
{
struct State
{
    Position position; // x,y,z
    Quat quat;         // x,y,z,w
    double weight;
};

struct MeasurementState
{
    double raw_weight_avg;
};

struct AugmentedMCLParam
{
    double w_fast;
    double w_slow;
    double alpha_fast;
    double alpha_slow;
    double noise_x_var;
    double noise_y_var;
    double noise_z_var;
    double noise_roll_var;
    double noise_pitch_var;
    double noise_yaw_var;
};

struct ResampleTimingParam
{
    double ess_ratio_threshold;
};

struct InitialPoseParam
{
    double initial_particle_num;
};

struct KLDSamplingParam
{
    size_t min_particle_num;
    size_t max_particle_num;
    double delta;
    double epsilon;
    double x_bin_width;
    double y_bin_width;
    double z_bin_width;
};

struct AmclParam
{
    AugmentedMCLParam augmented_mcl;
    ResampleTimingParam resample_timing;
    KLDSamplingParam kld_sampling;
    InitialPoseParam init_pose;
};

typedef std::vector<State> Particles;

} // namespace amcl_3d


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

/*
Copyright (C) 2005, 2006, 2007, 2008, 2009 StatPro Italia srl
Copyright (C) 2015 CompatibL

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

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

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

// Based on piecewiseyieldcurve.cpp from test-suite.
#ifndef cl_adjoint_piecewice_yied_curve_impl
#define cl_adjoint_piecewice_yied_curve_impl
#pragma once

#include "adjointpiecewiseyieldcurvetest.hpp"
#include "utilities.hpp"
#include "adjointtestutilities.hpp"
#include "adjointtestbase.hpp"
#include <ql/time/schedule.hpp>
#include <boost/make_shared.hpp>
#include <ql/quantlib.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;

#define OUTPUT_FOLDER_NAME "AdjointPiecewiseYieldCurve"

namespace
{
    enum
    {
#if defined CL_GRAPH_GEN
        // Number of points for dependency plots.
        pointNo = 45,
        // Number of points for performance plot.
        iterNo = 45,
        // Step for portfolio size for performance testing .
        step = 1,
#else
        // Number of points for dependency plots.
        pointNo = 20,
        // Number of points for performance plot.
        iterNo = 1,
        // Step for portfolio size for performance testing .
        step = 1,
#endif
        // Defines performance accuracy. Its value is a minimum number
        // of calling of O(1) complexity methods per one performance test.
        iterNumFactor = 10,
        maxSize = 50,
    };

    struct Datum
    {
        Integer n_;
        TimeUnit units_;
        Rate rate_;
    };

    struct SwapData
    {
        SwapData():
        calendar_(Japan())
        , settlementDays_(2)
        , today_(Date(4, October, 2007))
        , settlement_(calendar_.advance(today_, settlementDays_, Days))
        , fixedLegConvention_(Unadjusted)
        , fixedLegFrequency_ (Annual)
        , fixedLegDayCounter_( Thirty360())
        , swapData_(maxSize)
        , rates_(1)
        , instruments_(1)
        , termStructure_()
        {
            Settings::instance().evaluationDate() = today_;

            for (Integer i = 0; i < maxSize; i++)
            {
                swapData_[i] = { i + 1, Years, (4.5 + i*0.03) / 100 };
            }
        }

        // Calculate total NPV of swaps based on input rates using Japanese Yen LIBOR rate.
        // Japanese Yen LIBOR rate can be considered as the interbank cost of borrowing funds in Japanese yens.
        Real calculateSwapNPV(Real swapRate, Integer id)
        {
            // Create ibor index using 6 Month JPY Libor.
            boost::shared_ptr<IborIndex> index = boost::make_shared<JPYLibor>(6 * Months);

            rates_[0] = boost::make_shared<SimpleQuote>(swapRate);
            instruments_[0] = boost::make_shared<SwapRateHelper>(Handle<Quote>(rates_[0]), swapData_[id].n_*swapData_[id].units_,
                                                                 calendar_,
                                                                 fixedLegFrequency_, fixedLegConvention_,
                                                                 fixedLegDayCounter_, index);

            // Initialize yield term structure.
            termStructure_ = boost::make_shared<PiecewiseYieldCurve<Discount, LogLinear>>(
                settlement_, instruments_,
                Actual360(),
                1.0e-12);

            RelinkableHandle<YieldTermStructure> curveHandle;
            curveHandle.linkTo(termStructure_);

            // Create ibor index using 6 Month JPY Libor.
            boost::shared_ptr<IborIndex> jpylibor6m = boost::make_shared<JPYLibor>(6 * Months, curveHandle);

                Period tenor = swapData_[id].n_*swapData_[id].units_;

            // Create vanilla swap
            VanillaSwap swap = MakeVanillaSwap(tenor, jpylibor6m, 0.0)
                .withEffectiveDate(settlement_)
                .withFixedLegDayCount(fixedLegDayCounter_)
                .withFixedLegTenor(Period(fixedLegFrequency_))
                .withFixedLegConvention(fixedLegConvention_)
                .withFixedLegTerminationDateConvention(fixedLegConvention_)
                .withFixedLegCalendar(calendar_)
                .withFloatingLegCalendar(calendar_);

            return swap.NPV();
        }

        // Global variables.
        Calendar calendar_;
        Natural settlementDays_;
        Date today_, settlement_;
        BusinessDayConvention fixedLegConvention_;
        Frequency fixedLegFrequency_;
        DayCounter fixedLegDayCounter_;

        std::vector<boost::shared_ptr<SimpleQuote> > rates_;
        std::vector<boost::shared_ptr<RateHelper> > instruments_;
        boost::shared_ptr<YieldTermStructure> termStructure_;

        std::vector<Datum> swapData_;

        // Cleanup.
        SavedSettings backup_;
        IndexHistoryCleaner cleaner_;
    };

    // Struct for plots recording.
    struct RateSens
    {
        static std::deque<std::string > get_columns()
        {
            static std::deque<std::string > columns =
            {
                "Rate", ""
            };

            return columns;
        }

        template <typename stream_type>
        friend inline stream_type&
            operator << (stream_type& stm, RateSens& v)
        {
                stm << v.rate_
                    << ";" << v.sensitivity_
                    << std::endl;
                return stm;
            }

        Real rate_;
        Real sensitivity_;
    };

    struct TestData
        : public SwapData
    {
        struct Test
        : public cl::AdjointTest<Test>
        {
            Test(Size size, TestData* data)
            : size_(size)
            , data_(data)
            , rates_(size)
            , totalNPV_()
            {
                setLogger(&data_->outPerform_);

                for (Size i = 0; i < size_; i++)
                {
                    rates_[i] = data_->swapData_[i].rate_;
                }

            }

            Size indepVarNumber() { return size_; }

            Size depVarNumber() { return 1; }

            Size minPerfIteration() { return iterNumFactor; }

            void recordTape()
            {
                cl::Independent(rates_);
                calculateNPV();
                f_ = std::make_unique<cl::tape_function<double>>(rates_, totalNPV_);
            }

            // Calculates price of portfolio and each option.
            void calculateNPV()
            {
                totalNPV_.resize(1, 0);
                for (Size i = 0; i < size_; i++)
                    totalNPV_[0] += data_->calculateSwapNPV(rates_[i], i);
            }

            // Calculates derivatives using finite difference method.
            void calcAnalytical()
            {
                double h = 1.0e-4;
                analyticalResults_.resize(size_);
                std::vector<Real> npvs(size_);
                for (Size i = 0; i < size_; i++)
                {
                    npvs[i] = data_->calculateSwapNPV(rates_[i], i);
                }
                for (Size i = 0; i < size_; i++)
                {
                    //Evaluate derivative using finite difference.
                    analyticalResults_[i] = (data_->calculateSwapNPV(rates_[i] + h, i) - npvs[i]) / h;
                }
            }

            double relativeTol() const { return 1e-2; }

            double absTol() const { return 1e-4; }

            Size size_;
            TestData* data_;
            std::vector<cl::tape_double> rates_;
            std::vector<cl::tape_double> totalNPV_;
        };

        TestData()
            : SwapData()
            , outPerform_(OUTPUT_FOLDER_NAME
            , { { "filename", "AdjointPerformance" }
            , { "not_clear", "Not" }
            , { "line_box_width", "-5" }
            , { "title", "Swap NPV differentiation performance with respect to rate" }
            , { "ylabel", "Time (s)" }
            , { "xlabel", "Number of rates" }
            , { "cleanlog", "true" }
            , { "smooth", "default" }
        })
            , outAdjoint_(OUTPUT_FOLDER_NAME
            , { { "filename", "Adjoint" }
            , { "not_clear", "Not" }
            , { "title", "Swap NPV adjoint differentiation performance with respect to rate" }
            , { "ylabel", "Time (s)" }
            , { "xlabel", "Number of rates" }
            , { "cleanlog", "false" }
            , { "smooth", "default" }
        })
            , outSize_(OUTPUT_FOLDER_NAME
            , { { "filename", "TapeSize" }
            , { "not_clear", "Not" }
            , { "title", "Tape size dependence on number of rates" }
            , { "ylabel", "Size (MB)" }
            , { "xlabel", "Number of rates" }
            , { "cleanlog", "false" }
        })
            , out_(OUTPUT_FOLDER_NAME "//output", {
            { "filename", "StrikeDependence" }
            , { "not_clear", "Not" }
            , { "title", "Swap NPV sensitivity dependence on rate" }
            , { "ylabel", "Swap NPV" }
            , { "xlabel", "Rate" }
            , { "cleanlog", "false" }
        })
        {
        }

        bool makeOutput()
        {
            bool ok = true;
            if (pointNo > 0)
            {
                ok &= recordDependencePlot();
            }
            ok &= cl::recordPerformance(*this, iterNo, step);
            return ok;
        }

        std::shared_ptr<Test> getTest(size_t size)
        {
            return std::make_shared<Test>(size, this);
        }

        // Makes plots for strike sensitivity dependence.
        bool recordDependencePlot()
        {
            std::vector<RateSens> outData(pointNo);
            auto test = getTest(pointNo);
            bool ok = test->testReverse();
            for (Size i = 0; i < pointNo; i++)
            {
                outData[i] = { test->rates_[i], test->data_->calculateSwapNPV(test->rates_[i], i) };
            }
            out_ << outData;
            return ok;
        }

        cl::tape_empty_test_output outPerform_;
        cl::tape_empty_test_output outAdjoint_;
        cl::tape_empty_test_output outSize_;
        cl::tape_empty_test_output out_;
    };

    typedef TestData::Test PiecewiceYieldCurveTest;
}
#endif


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

#ifndef BOOST_COMPUTE_RANDOM_DISCRETE_DISTRIBUTION_HPP
#define BOOST_COMPUTE_RANDOM_DISCRETE_DISTRIBUTION_HPP

#include <boost/compute/command_queue.hpp>
#include <boost/compute/function.hpp>
#include <boost/compute/algorithm/accumulate.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/detail/literal.hpp>
#include <boost/compute/types/fundamental.hpp>

namespace boost {
namespace compute {

/// \class discrete_distribution
/// \brief Produces random integers on the interval [0, n), where
/// probability of each integer is given by the weight of the ith
/// integer divided by the sum of all weights.
///
/// The following example shows how to setup a discrete distribution to
/// produce 0 and 1 with equal probability
///
/// \snippet test/test_discrete_distribution.cpp generate
///
template<class IntType = uint_>
class discrete_distribution
{
public:
    typedef IntType result_type;

    /// Creates a new discrete distribution with weights given by
    /// the range [\p first, \p last)
    template<class InputIterator>
    discrete_distribution(InputIterator first, InputIterator last)
        : m_n(std::distance(first, last)),
          m_probabilities(std::distance(first, last))
    {
        double sum = 0;

        for(InputIterator iter = first; iter!=last; iter++)
        {
            sum += *iter;
        }

        for(size_t i=0; i<m_n; i++)
        {
            m_probabilities[i] = m_probabilities[i-1] + first[i]/sum;
        }
    }

    /// Destroys the discrete_distribution object.
    ~discrete_distribution()
    {
    }

    /// Returns the value of n
    result_type n() const
    {
        return m_n;
    }

    /// Returns the probabilities
    ::std::vector<double> probabilities() const
    {
        return m_probabilities;
    }

    /// Generates uniformily distributed integers and stores
    /// them to the range [\p first, \p last).
    template<class OutputIterator, class Generator>
    void generate(OutputIterator first,
                  OutputIterator last,
                  Generator &generator,
                  command_queue &queue)
    {
        std::string source = "inline uint scale_random(uint x)\n";

        source = source +
            "{\n" +
            "float rno = convert_float(x) / UINT_MAX;\n";
        for(size_t i=0; i<m_n; i++)
        {
            source = source +
                "if(rno <= " + detail::make_literal<float>(m_probabilities[i]) + ")\n" +
                "   return " + detail::make_literal(i) + ";\n";
        }

        source = source +
            "return " + detail::make_literal(m_n - 1) + ";\n" +
            "}\n";

        BOOST_COMPUTE_FUNCTION(IntType, scale_random, (const uint_ x), {});

        scale_random.set_source(source);

        generator.generate(first, last, scale_random, queue);
    }

private:
    size_t m_n;
    ::std::vector<double> m_probabilities;
};

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_RANDOM_UNIFORM_INT_DISTRIBUTION_HPP


#ifndef POLYGONAL_BASIS_HPP
#define POLYGONAL_BASIS_HPP

#include <polyfem/Mesh2D.hpp>
#include <polyfem/ElementBases.hpp>
#include <polyfem/ElementAssemblyValues.hpp>
#include <polyfem/InterfaceData.hpp>

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

namespace polyfem
{

	class PolygonalBasis2d
	{
	public:

		// Compute the integral constraints for each basis of the mesh. This step is PDE-dependent.
		//
		// @param[in]  mesh             Input surface mesh
		// @param[in]  n_bases          Number of bases/nodes in the mesh
		// @param[in]  values           Shape functions for the PDE field evaluated over each
		//                              elements
		// @param[in]  gvalues          Shape functions for the geometric mapping evaluated over
		//                              each elements
		// @param[out] basis_integrals  Integral constraints over polygon that each basis of the
		//                              mesh must verify
		//
		static void compute_integral_constraints(
			const std::string &assembler_name,
			const Mesh2D &mesh,
			const int n_bases,
			const std::vector< ElementBases > &bases,
			const std::vector< ElementBases > &gbases,
			Eigen::MatrixXd &basis_integrals);

		///
		/// @brief         Build bases over the remaining polygons of a mesh.
		///
		/// @param[in]     n_samples_per_edge    { Number of collocation samples per polygon edge
		///                                      (including endpoints) }
		/// @param[in]     mesh                  { Input surface mesh }
		/// @param[in]     n_bases               { Total number of bases functions in the mesh }
		/// @param[in]     quadrature_order      { Quadrature order for the polygons }
		/// @param[in]     integral_constraints  { Order of the integral constraints (0 = no
		///                                      constraints, 1 = linear, 2 = quadratic) }
		/// @param[in,out] bases                 { List of the different basis (shape functions)
		///                                      used to discretize the PDE }
		/// @param[in]     gbases                { List of the different basis used to discretize
		///                                      the geometry of the mesh }
		/// @param[in]     poly_edge_to_data     { Additional data computed for edges at the
		///                                      interface with a polygon }
		/// @param[out]    mapped_boundary       { Map element id -> #S x dim polyline formed by the
		///                                      collocation points on the boundary of the polygon.
		///                                      The collocation points are mapped through the
		///                                      geometric mapping of the element across the edge,
		///                                      so this polyline may differ from the original
		///                                      polygon. }
		/// @param[in]  element_types  { Per-element tag indicating the type of each element (see Mesh.hpp) }
		/// @param[in]  values         { Per-element shape functions for the PDE, evaluated over the element,
		///                            used for the system matrix assembly (used for linear reproduction) }
		/// @param[in]  gvalues        { Per-element shape functions for the geometric mapping, evaluated over
		///                            the element (get boundary of the polygon) }
		///
		static int build_bases(
			const std::string &assembler_name,
			const int n_samples_per_edge,
			const Mesh2D &mesh,
			const int n_bases,
			const int quadrature_order,
			const int integral_constraints,
			std::vector< ElementBases > &bases,
			const std::vector< ElementBases > &gbases,
			const  std::map<int, InterfaceData> &poly_edge_to_data,
			std::map<int, Eigen::MatrixXd> &mapped_boundary);
	};
}
#endif //POLYGONAL_BASIS_HPP


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

#include <math.h>

#include "flame/config.h"

BOOST_AUTO_TEST_CASE(config_getset)
{
    Config C;

    C.set<double>("hello", 4.2);

    BOOST_CHECK_CLOSE(C.get<double>("hello"), 4.2, 0.1);

    BOOST_CHECK_THROW(C.get<double>("world"), key_error);

    BOOST_CHECK_CLOSE(C.get<double>("world", 5.2), 5.2, 0.1);
}

static const char config_print_stmt_input[] =
"X = 14;\n"
"print(X);\n"
"Y = \"test\";\n"
"print(Y);\n"
"foo : bar, x=\"y\";\n"
"baz : LINE = (foo);\n";

BOOST_AUTO_TEST_CASE(config_print_stmt)
{
    std::ostringstream strm;
    GLPSParser parse;
    parse.setPrinter(&strm);

    flame::auto_ptr<Config> conf(parse.parse_byte(config_print_stmt_input, sizeof(config_print_stmt_input)-1));

    BOOST_CHECK_EQUAL(strm.str(), "On line 2 : 14\n" "On line 4 : \"test\"\n");
}


#include "profile.hpp"
#include "profile_python.hpp"
#include <boost/python.hpp>
#include <pyublas/numpy.hpp>
#include "optimization.hpp"
#include "rootfinder.hpp"

namespace gd {
using namespace boost::python;

// in km/s^2 Msun/kpc
const double GravConst = 4.30200406461e-06;

boost::python::tuple ProfileModel::Lmax_and_rcirc_at_E_(double E) {
	double Lmax, rcirc;
	Lmax_and_rcirc_at_E(E, Lmax, rcirc);
	return boost::python::make_tuple(Lmax, rcirc);
}
void ProfileModel::Lmax_and_rcirc_at_E(double E, double& Lmax, double& rcirc) {
	/*	def f(r, E=E):
			r = abs(r)
			return -r**2*(2*(E-self.potentialr(r)))
		rLmax, Lmaxsq = scipy.optimize.fmin(f, rtry, full_output=True, disp=False)[:2]
		return sqrt(abs(Lmaxsq)), abs(rLmax[0])
	*/
	auto f = [&](double logr) -> double {
		//r = fabs(r);
		double r = pow(10, logr);
		//printf("r = %f logr\n ", r);
		return -r*r*(2*(E-this->potentialr(r)));
	};
	MinimizerNLopt2<> opt(f);
	//double lower_bounds[1] = { 0};
	//nlopt_set_lower_bounds(opt.opt, lower_bounds);
	//opt.x = 1.;
	rcirc = pow(10, opt.optimize());
	//printf("r = %f Ekin = %f\n", r, 2*(E-this->potentialr(r)));
	Lmax = sqrt(2*(E-this->potentialr(rcirc)))*rcirc;
}
double ProfileModel::Lmax_at_E(double E) {
	double Lmax, rcirc;
	Lmax_and_rcirc_at_E(E, Lmax, rcirc);
	return Lmax;
}
double ProfileModel::rcirc_at_E(double E) {
	double Lmax, rcirc;
	Lmax_and_rcirc_at_E(E, Lmax, rcirc);
	return rcirc;
}
double ProfileModel::rmax_at_E(double E, double rcirc) {
	auto f = [&](double r) -> double {
		return E - this->potentialr(r);
	};
	double scale = 1.5;
	double r_far_way = rcirc * scale;
	assert(f(rcirc) >= 0);
	while(f(r_far_way) > 0) {
		r_far_way *= scale;
	}
	RootFinderGSL<> rootfinder(f);
	double rmax = rootfinder.findRoot(rcirc, r_far_way);
	return rmax;

	/*auto f = [&](double r) -> double {
		return E - this->potentialr(r);
	};
	auto root = [&](double x, double * y, double *dy) -> void {
		double r = pow(10, x) + rcirc;
		// x = log10(r-rcirc)
		// dx = 1/(r-rcirc) / log(10) dr
		*y = f(r);
		double gradient = -this->dphidr(r) * (r-rcirc) * log(10);
		*dy = gradient;
		//*
		double r1 = r;
		double dx = 1e-4;
		double r2 = pow(10, x+dx)+rcirc;
		double y1 = f(r1);
		double y2 = f(r2);
		printf("rmax grad: logr=%f r=%10.5f: erad=%10.5f [%10.5f %10.5f] (%f) \n", x, r, *y, (y2-y1)/dx, gradient, this->potentialr(r));
		//return g
		/**/
	/*};

	// find apo or peri center
	RootFinderGSLDerivative<> rootfinderFirst(root);
	double rstart = pow(10, rootfinderFirst.findRoot(1, 0, 1e-5))+rcirc; //pow(10, rootfinderFirst.findRoot(0));
	double scale = 1.+1e-4;
	assert(f(rstart*scale) < 0);
	assert(f(rstart/scale) > 0);
	return rstart;*/
}

boost::python::tuple ProfileModel::get_apo_peri(double E, double L, double rmin, double rcirc, double rmax) {

	auto ekinrad = [&](double r) -> double { // kinetic energy in radial direction
		return (-L*L/(2*r*r) - (this->potentialr(r) - E));
	};
	auto root = [&](double x, double * y, double *dy) -> void {
		if(x < 0) {
			//*y =FP_INFINITE;// inf;
			//*dy = FP_INFINITE;//inf;
			//return;
			//printf("grad: x=%f\n", x);
			//x = -x;
		}
		double r = pow(10, x);
		*y = ekinrad(r);
		double gradient = (-this->dphidr(r) + L*L/pow(r, 3)) * r * log(10);
		*dy = gradient;
		//*
		double r1 = r;
		double dx = 1e-4;
		double r2 = pow(10, x+dx);
		double y1 = ekinrad(r1);
		double y2 = ekinrad(r2);
		//printf("grad: logr=%f r=%10.5f: erad=%10.5f [%10.5f %10.5f] (%f) \n", x, r, *y, (y2-y1)/dx, gradient, this->potentialr(r));
		//return g
		/**/
	};

	// find apo or peri center
	//RootFinderGSLDerivative<> rootfinderFirst(root);
	//double rstart = pow(10, rootfinderFirst.findRoot(1)); //pow(10, rootfinderFirst.findRoot(0));

	double scale = (1+1e-4);
	double apo, peri;
	RootFinderGSL<> rootfinder(ekinrad);
	//printf("found a valid r %f (E=%f L=%f Eleft=%f (%f %f))\n", rstart, E, L, ekinrad(rstart), ekinrad(rstart*scale), ekinrad(rstart/scale));
	//printf("rmin : %f ekinrad: %f %f %f\n", rmin, ekinrad(rmin), ekinrad(rmin*scale), ekinrad(rmin/scale));
	//printf("rcirc: %f ekinrad: %f %f %f\n", rcirc, ekinrad(rcirc), ekinrad(rcirc*scale), ekinrad(rcirc/scale));
	//printf("rmax : %f ekinrad: %f %f %f\n", rmax, ekinrad(rmax), ekinrad(rmax*scale), ekinrad(rmax/scale));
	int n;
	n = 0;
	assert(ekinrad(rcirc) >= 0);
	while((n < 10) & (ekinrad(rmin) > 0)) {
		rmin /= scale;
		n++;
	}
	if(ekinrad(rmin) > 0) {
		throw std::range_error("rmin cannot be properly found");
	}

	n = 0;
	while((n < 10) & (ekinrad(rmax) > 0)) {
		rmax *= scale;
		n++;
	}
	if(ekinrad(rmax) > 0) {
		throw std::range_error("rmax cannot be properly found");
	}

	peri = rootfinder.findRoot(rmin, rcirc);
	apo = rootfinder.findRoot(rcirc, rmax);

		//printf("finding apocenter [%f]\n", rstart);
		//printf("found apocenter [%f]\n", apo);
		//printf("finding pericenter [%f] (%f %f %f)\n", rstart, ekinrad(1e-6), ekinrad(rstart/scale/scale), ekinrad(rstart*scale*scale));
		//printf("found pericenter [%f]\n", peri);
	scale = (1+1e-5);
	while(ekinrad(peri) < 0) {
		peri *= scale;
	}
	while(ekinrad(apo) < 0) {
		apo /= scale;
	}
	return boost::python::make_tuple(apo/scale, peri*scale);

	/*
	# just find apo or pericenter
	rstart = self.findR_at_EL(E, L, rtry)
	try:
		rstart = rstart[0]
	except:
		pass
	s = (1+1e-5) # scale factor for testing apo/peri
	r = rstart
	#print rstart, ekinrad(rstart/s), ekinrad(rstart*s), -L**2/(2*r**2) - (self.potentialr(r) - E), -L**2/(2*r**2) , (self.potentialr(r) - E)
	if (ekinrad(rstart/s) < 0) and (ekinrad(rstart*s) > 0): # we found pericenter
		rp = rstart
		ra = brentq(ekinrad, rstart*s, 1e9)
	else: # we found apocenter
		ra = rstart
		rp = brentq(ekinrad, 1e-9, rstart/s)

	# sanity checks
	assert ekinrad(ra*s) < 0, "available energy ar r > r_apo should be negative"
	assert ekinrad(rp/s) < 0, "available energy ar r < r_peri should be negative"
	assert ekinrad(ra/s) > 0, "available energy ar r < r_apo should be positive"
	assert ekinrad(rp*s) > 0, "available energy ar r > r_peri should be positive"
	assert ra > rp, "apocenter should be larger than pericenter"
	return ra/s, rp*s*/
}

void py_export_profile() {
	class_< ProfileModel, boost::noncopyable >("ProfileModel", no_init)
		.def("get_apo_peri", (&ProfileModel::get_apo_peri))
		.def("Lmax_at_E", (&ProfileModel::Lmax_at_E))
		.def("Lmax_and_rcirc_at_E", (&ProfileModel::Lmax_and_rcirc_at_E_))
		.def("rcirc_at_E", (&ProfileModel::rcirc_at_E))
		.def("rmax_at_E", (&ProfileModel::rmax_at_E))

		;
	class_<ProfileModel1C, bases<ProfileModel>, boost::noncopyable  >("ProfileModel1C", init<Profile*>())
		.def("densityr", (&ProfileModel1C::densityr))
		.def("dphidr", (&ProfileModel1C::dphidr))
		.def("potentialr", (&ProfileModel1C::potentialr))
		;
	class_<ProfileModel2C, bases<ProfileModel>, boost::noncopyable  >("ProfileModel2C", init<Profile*,Profile*>())
		.def("densityr", (&ProfileModel2C::densityr))
		.def("dphidr", (&ProfileModel2C::dphidr))
		.def("potentialr", (&ProfileModel2C::potentialr))
		;

	class_< Profile, boost::noncopyable >("Profile", no_init)
		.def("densityr", pure_virtual(&Profile::densityr))
		;
	class_< Density, boost::noncopyable >("Density", no_init)
		.def("densityr", pure_virtual(&Density::densityr))
		;
	//py_export_profile_profile<Plummer, init<double, double, double>>("Plummer");
	//init<double, double, double>((boost::python::arg("b"), boost::python::arg("M"), boost::python::arg("G")))
	py_export_profile_profile_kw<Plummer>("Plummer", init<double, double, double>((boost::python::arg("M"), boost::python::arg("b"), boost::python::arg("G")=GravConst)));
	/*class_<Plummer, bases<Profile, Density> >("Plummer", init<double, double, double>())
		.def("potentialr", (&Plummer::potentialr))
		.def("densityr", (&Plummer::densityr))
		.def("densityR", (&Plummer::densityR))
		.def("dphidr", (&Plummer::dphidr))
		//.def("dphidr2", (&Plummer::dphidr2))
		;
	*/
	//py_export_profile_profile_kw<ProjectedExponential>("ProjectedExponential", init<double, double, double>((boost::python::arg("M"), boost::python::arg("scale"), boost::python::arg("G")=GravConst)));
	py_export_profile_profile_kw<ProjectedExponential>("ProjectedExponential", init<double, double, double>());

	class_<TestCase, bases<Profile, Density> >("TestCase", init<double, double, double>())
		.def("potentialr", (&TestCase::potentialr))
		.def("densityr", (&TestCase::densityr))
		.def("densityR", (&TestCase::densityR))
		.def("dphidr", (&TestCase::dphidr))
		//.def("dphidr2", (&TestCase::dphidr2))
		;
	class_<LogarithmicProfile, bases<Profile, Density> >("LogarithmicProfile", init<double, double>())
		.def("densityr", (&LogarithmicProfile::densityr))
		.def("densityR", (&LogarithmicProfile::densityR))
		.def("dphidr", (&LogarithmicProfile::dphidr))
		//.def("dphidr2", (&LogarithmicProfile::dphidr2))
	;
	class_<Isochrone, bases<Profile, Density> >("Isochrone", init<double, double, double>())
		.def("densityr", (&Isochrone::densityr))
		.def("densityR", (&Isochrone::densityR))
		.def("dphidr", (&Isochrone::dphidr))
		//.def("dphidr2", (&Isochrone::dphidr2))
		.def("potentialr", (&Isochrone::potentialr))
		;
	class_<NullProfile, bases<Profile, Density> >("NullProfile", init<>())
		.def("densityr", (&NullProfile::densityr))
		.def("densityR", (&NullProfile::densityR))
		.def("dphidr", (&NullProfile::dphidr))
		//.def("dphidr2", (&NullProfile::dphidr2))
	;
	class_<Hernquist, bases<Profile, Density> >("Hernquist", init<double, double, double>())
		.def("densityr", (&Hernquist::densityr))
		.def("densityR", (&Hernquist::densityR))
		.def("dphidr", (&Hernquist::dphidr))
		.def("potentialr", (&Hernquist::potentialr))
		;
	class_<Jaffe, bases<Profile, Density> >("Jaffe", init<double, double, double>())
		.def("densityr", (&Jaffe::densityr))
		.def("densityR", (&Jaffe::densityR))
		.def("dphidr", (&Jaffe::dphidr))
		.def("potentialr", (&Jaffe::potentialr))
		;
	class_<Einasto, bases<Profile, Density> >("Einasto", init<double, double, double, double>())
		.def("densityr", (&Einasto::densityr))
		.def("densityR", (&Einasto::densityR))
		.def("dphidr", (&Einasto::dphidr))
		.def("potentialr", (&Einasto::potentialr))
		;
	class_<Burkert, bases<Profile, Density> >("Burkert", init<double, double, double>())
		.def("densityr", (&Burkert::densityr))
		.def("densityR", (&Burkert::densityR))
		.def("dphidr", (&Burkert::dphidr))
		.def("potentialr", (&Burkert::potentialr))
		;
	class_<NFW, bases<Profile, Density> >("NFW", init<double, double, double, double>())
		.def("densityr", (&NFW::densityr))
		.def("potentialr", (&NFW::potentialr))
		.def("dphidr", (&NFW::dphidr))
		.def_readonly("c", &NFW::c)
		.def_readonly("mass200", &NFW::mass200)
		.def_readonly("r200", &NFW::r200)
		.def_readonly("rs", &NFW::rs)
		.def_readonly("rho0", &NFW::rho0);
	;
	class_<NFWCut, bases<Density> >("NFWCut", init<double, double, double>())
		.def("densityr", (&NFWCut::densityr))
		.def_readwrite("rs", &NFWCut::rs)
		.def_readwrite("rte", &NFWCut::rte)
		.def_readwrite("rho0", &NFWCut::rho0)
		;
	class_<TwoSlopeDensity, bases<Density> >("TwoSlopeDensity", init<double, double, double, double, double>())
		.def("densityr", (&TwoSlopeDensity::densityr))
		.def_readwrite("alpha", &TwoSlopeDensity::alpha)
		.def_readwrite("beta", &TwoSlopeDensity::beta)
		.def_readwrite("rho0", &TwoSlopeDensity::rho0)
		.def_readwrite("rs", &TwoSlopeDensity::rs)
		.def_readwrite("gamma", &TwoSlopeDensity::gamma)
		;
	class_<TwoSlopeDensityCut, bases<Density> >("TwoSlopeDensityCut", init<double, double, double, double, double, double>())
		.def("densityr", (&TwoSlopeDensityCut::densityr))
		.def_readwrite("alpha", &TwoSlopeDensityCut::alpha)
		.def_readwrite("beta", &TwoSlopeDensityCut::beta)
		.def_readwrite("rho0", &TwoSlopeDensityCut::rho0)
		.def_readwrite("rs", &TwoSlopeDensityCut::rs)
		.def_readwrite("gamma", &TwoSlopeDensityCut::gamma)
		.def_readwrite("rte", &TwoSlopeDensityCut::rte)
		;
	class_<BrokenPowerLawDensitySoft3, bases<Density> >("BrokenPowerLawDensitySoft3", init<double, double, double, double, double, double, double, double>())
		.def("densityr", (&BrokenPowerLawDensitySoft3::densityr))
		.def_readwrite("rho0", &BrokenPowerLawDensitySoft3::rho0)
		.def_readwrite("s1", &BrokenPowerLawDensitySoft3::s1)
		.def_readwrite("s2", &BrokenPowerLawDensitySoft3::s2)
		.def_readwrite("s3", &BrokenPowerLawDensitySoft3::s3)
		.def_readwrite("gamma1", &BrokenPowerLawDensitySoft3::gamma1)
		.def_readwrite("gamma2", &BrokenPowerLawDensitySoft3::gamma2)
		.def_readwrite("rs1", &BrokenPowerLawDensitySoft3::rs1)
		.def_readwrite("rs2", &BrokenPowerLawDensitySoft3::rs2)
		;

}

}


// Copyright (c) 2021 Stig Rune Sellevag
//
// This file is distributed under the MIT License. See the accompanying file
// LICENSE.txt or http://www.opensource.org/licenses/mit-license.php for terms
// and conditions.

#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 5054)
#endif

#include <scilib/mdarray.h>
#include <scilib/linalg.h>
#include <chrono>
#include <iostream>
#include <valarray>
#include <numeric>
#include <Eigen/Dense>

using Timer = std::chrono::duration<double, std::micro>;

void print(int n, const Timer& t_eigen, const Timer& t_sci, const Timer& t_val)
{
    std::cout << "Dot product:\n"
              << "------------\n"
              << "size =            " << n << '\n'
              << "scilib/eigen =    " << t_sci.count() / t_eigen.count() << "\n"
              << "scilib/valarray = " << t_sci.count() / t_val.count()
              << "\n\n";
}

void benchmark(int n)
{
    Eigen::VectorXd aa(n);
    Eigen::VectorXd ab(n);

    aa.fill(1.0);
    ab.fill(2.0);

    auto t1 = std::chrono::high_resolution_clock::now();
    double dot_eigen;
    for (int it = 0; it < 10000; ++it) {
        dot_eigen = aa.dot(ab);
    }
    auto t2 = std::chrono::high_resolution_clock::now();
    Timer t_eigen = t2 - t1;
    (void) dot_eigen; // ignore unused result

    Sci::Vector<double> na(n);
    Sci::Vector<double> nb(n);
    na = 1.0;
    nb = 2.0;
    t1 = std::chrono::high_resolution_clock::now();
    double sci;
    for (int it = 0; it < 10000; ++it) {
        sci = Sci::Linalg::dot(na.view(), nb.view());
    }
    t2 = std::chrono::high_resolution_clock::now();
    Timer t_sci = t2 - t1;
    (void) sci;

    std::valarray<double> va(1.0, n);
    std::valarray<double> vb(2.0, n);
    t1 = std::chrono::high_resolution_clock::now();
    double val;
    for (int it = 0; it < 10000; ++it) {
        val = std::inner_product(std::begin(va), std::end(va), std::begin(vb),
                                 0.0);
    }
    t2 = std::chrono::high_resolution_clock::now();
    Timer t_val = t2 - t1;
    (void) val;

    print(n, t_eigen, t_sci, t_val);
}

int main()
{
    int n = 10;
    benchmark(n);

    n = 100;
    benchmark(n);

    n = 1000;
    benchmark(n);

    n = 10000;
    benchmark(n);

    n = 100000;
    benchmark(n);
}

#ifdef _MSC_VER
#pragma warning(pop)
#endif


#include "testsuite.h"
#include <blitz/array.h>
#include <blitz/array.cc>
#include <blitz/tinyvec2.cc>
#include <blitz/tinymat2.h>
#include <blitz/tinymat2.cc>

BZ_USING_NAMESPACE(blitz)

// Tests that operations on multicomponent arrays work as
// expected. This is a bit tricky because now they involve two
// recursive ET applications.

static const double eps = 0.0001;

#define ISCLOSE(a, b) BZTEST(fabs((a)-(b))<eps)

typedef TinyVector<double,2> tv;
typedef TinyMatrix<double,2,2> tmt;

// types to test
typedef Array<double, 1> a1;
typedef Array<tv, 1> a1v;
typedef Array<tmt, 1> a1m;
typedef TinyVector<tv, 5> tvv;
typedef TinyMatrix<tv, 2,2> tmv;
// and the hardcore multi-level multicomponent types
typedef Array<TinyVector<tv, 2>, 1> a1vv;
typedef Array<TinyMatrix<tv, 2,2>, 1> a1mv;

int main()
{
  const int sz=5;

  // create operands
  a1 a1A(sz), a1B(sz), a1C(sz);
  a1v a1vA(sz), a1vB(sz), a1vC(sz);
  a1m a1mA(sz), a1mB(sz), a1mC(sz);
  tvv tvvA, tvvB, tvvC;
  tmv tmvA, tmvB, tmvC;
  a1vv a1vvA(sz), a1vvB(sz), a1vvC(sz);
  a1mv a1mvA(sz), a1mvB(sz), a1mvC(sz);

  // fill them with data
  a1A=1,2,3,4,5;
  a1vA=tv(1,-1),tv(2,-2),tv(3,-3),tv(4),tv(5);
  a1mA=tmt(1),tmt(2),tmt(3),tmt(4),tmt(5);
  a1mA(2)=0,1,2,3;
  tvvA=tv(1,-1),tv(2,-2),tv(3,-3),tv(4),tv(5);
  tmvA=tv(1,-1),tv(2,-2),tv(3,-3),tv(4);
  a1vvA=TinyVector<TinyVector<double,2>,2>(tv(1,-1),tv(2,-2)),
    TinyVector<TinyVector<double,2>,2>(tv(3,-3),tv(3,-3)),
    TinyVector<TinyVector<double,2>,2>(tv(4,-4),tv(5,-5)),
    TinyVector<TinyVector<double,2>,2>(tv(6,6),tv(7,7)),
    TinyVector<TinyVector<double,2>,2>(-8);
  a1mvA=TinyMatrix<TinyVector<double,2>,2,2>(tv(1,-1));
  a1mvA(1)=2;
  a1mvA(2)=tv(1,-1),tv(2,-2),tv(3,-3),tv(4);
  a1mvA(3)(1,1)[1]=-42;

  // test that at least a subset was initialized correctly
  ISCLOSE(a1A(2),3);
  ISCLOSE(a1vA(2)(1),-3);
  ISCLOSE(a1mA(2)(1,0),2);
  ISCLOSE(tvvA(2)(1),-3);
  ISCLOSE(a1vvA(2)(1)(0),5);
  ISCLOSE(a1mvA(2)(1,0)(1),-3);

  cout << a1A << endl;
  cout << a1vA << endl;
  cout << a1mA << endl;
  cout << tvvA << endl;
  cout << tmvA << endl;
  cout << a1vvA << endl;
  cout << a1mvA << endl;

  // evaluate a complicated expression to exercise unary, binary,
  // constant, and funcs

  a1B = 2*(-a1A)+sqrt(a1A*a1A);
  a1vB = 2*(-a1vA)+sqrt(a1vA*a1vA);
  a1mB = 2*(-a1mA)+sqrt(a1mA*a1mA);
  tvvB = 2*(-tvvA)+sqrt(tvvA*tvvA);
  tmvB = 2*(-tmvA)+sqrt(tmvA*tmvA);
  a1vvB = 2*(-a1vvA)+sqrt(a1vvA*a1vvA);
  a1mvB = 2*(-a1mvA)+sqrt(a1mvA*a1mvA);

  cout << "\nTesting element-wise expression evaluation:\n";
  cout << a1B << endl;
  cout << a1vB << endl;
  cout << a1mB << endl;
  cout << tvvB << endl;
  cout << tmvB << endl;
  cout << a1vvB << endl;
  cout << a1mvB << endl;

  // test results (we are not testing reductions here so we loop over
  // elements)
  for(int i=0; i<sz; ++i) {
    ISCLOSE ( a1B(i), -a1A(i) );

    for(int j=0; j<2; ++j) {
      ISCLOSE(a1vB(i)(j), a1vA(i)(j)>0?-a1vA(i)(j):-3*a1vA(i)(j));
      ISCLOSE(tvvB(i)(j), tvvA(i)(j)>0?-tvvA(i)(j):-3*tvvA(i)(j));
      for(int k=0; k<2; ++k) {
	ISCLOSE(a1mB(i)(j,k),a1mA(i)(j,k)>0?-a1mA(i)(j,k):-3*a1mA(i)(j,k));
	ISCLOSE(a1vvB(i)(j)(k),a1vvA(i)(j)(k)>0?-a1vvA(i)(j)(k):-3*a1vvA(i)(j)(k));
	if(i<2) {
	  ISCLOSE(tmvB(j,k)(i),tmvA(j,k)(i)>0?-tmvA(j,k)(i):-3*tmvA(j,k)(i));
	}
	for(int l=0; l<2; ++l) {
	  ISCLOSE(a1mvB(i)(j,k)(l),a1mvA(i)(j,k)(l)>0?-a1mvA(i)(j,k)(l):-3*a1mvA(i)(j,k)(l));
	}
      }
    }
  }

  // also test that the update versions of the operators work
  a1B -= 2*(-a1A)+sqrt(a1A*a1A);
  a1vB -= 2*(-a1vA)+sqrt(a1vA*a1vA);
  a1mB -= 2*(-a1mA)+sqrt(a1mA*a1mA);
  tvvB -= 2*(-tvvA)+sqrt(tvvA*tvvA);
  tmvB -= 2*(-tmvA)+sqrt(tmvA*tmvA);
  a1vvB -= 2*(-a1vvA)+sqrt(a1vvA*a1vvA);
  a1mvB -= 2*(-a1mvA)+sqrt(a1mvA*a1mvA);

  for(int i=0; i<sz; ++i) {
    ISCLOSE ( a1B(i), 0 );

    for(int j=0; j<2; ++j) {
      ISCLOSE(a1vB(i)(j), 0);
      ISCLOSE(tvvB(i)(j), 0);
      for(int k=0; k<2; ++k) {
	ISCLOSE(a1mB(i)(j,k), 0);
	ISCLOSE(a1vvB(i)(j)(k), 0);
	if(i<2) {
	  ISCLOSE(tmvB(j,k)(i), 0 );
	}
	for(int l=0; l<2; ++l) {
	  ISCLOSE(a1mvB(i)(j,k)(l), 0);
	}
      }
    }
  }

  cout << "\nTesting scalar wrapper:\n";

  // now we also want to test the "scalar" wrapper.
  a1vB = a1vA*scalar(tv(2,-2));
  tmt sc;
  sc=1,2,3,4;
  a1mB = a1mA*scalar(sc);

  cout << a1vB << endl;
  cout << a1mB << endl;

  ISCLOSE(a1vB(1)(1),4);
  ISCLOSE(a1vB(4)(1),-10);
  ISCLOSE(a1mB(1)(0,1),4);
  ISCLOSE(a1mB(2)(1,1),12);

  return 0;
}


/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */

/*
    MiniBPM
    A fixed-tempo BPM estimator for music audio
    Copyright 2012 Particular Programs Ltd.

    This program is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public License as
    published by the Free Software Foundation; either version 2 of the
    License, or (at your option) any later version.  See the file
    COPYING included with this distribution for more information.

    Alternatively, if you have a valid commercial licence for MiniBPM
    obtained by agreement with the copyright holders, you may
    redistribute and/or modify it under the terms described in that
    licence.

    If you wish to distribute code using MiniBPM under terms other
    than those of the GNU General Public License, you must obtain a
    valid commercial licence before doing so.
*/

#include "../src/MiniBpm.cpp"

using breakfastquay::FourierFilterbank;
using breakfastquay::Autocorrelation;
using breakfastquay::ACFCombFilter;
using breakfastquay::MiniBPM;

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(TestFourierFilterbank)

#define FFT(n) FourierFilterbank(n, 2, 0, 1, false)
#define COMPARE_ZERO(x) BOOST_CHECK_SMALL(x, 1e-10)
#define COMPARE_ALL_ZERO(a) \
    for (int cmp_i = 0; cmp_i < (int)(sizeof(a)/sizeof(a[0])); ++cmp_i) { \
        COMPARE_ZERO(a[cmp_i]); \
    }

BOOST_AUTO_TEST_CASE(outputSize)
{
    int size = FFT(4).getOutputSize();
    BOOST_CHECK_EQUAL(size, 3);
    size = FourierFilterbank(4, 4, 1, 1, false).getOutputSize();
    BOOST_CHECK_EQUAL(size, 1);
    size = FourierFilterbank(4, 4, 1, 2, false).getOutputSize();
    BOOST_CHECK_EQUAL(size, 2);
    size = FourierFilterbank(512, 44100, 0, 440, true).getOutputSize();
    BOOST_CHECK_EQUAL(size, 6);
}

BOOST_AUTO_TEST_CASE(dc)
{
    double in[] = { 1, 1, 1, 1 };
    double mag[3];
    FFT(4).forwardMagnitude(in, mag);
    BOOST_CHECK_EQUAL(mag[0], 4.0);
    COMPARE_ZERO(mag[1]);
    COMPARE_ZERO(mag[2]);
}

BOOST_AUTO_TEST_CASE(sine)
{
    double in[] = { 0, 1, 0, -1 };
    double mag[3];
    FFT(4).forwardMagnitude(in, mag);
    COMPARE_ZERO(mag[0]);
    BOOST_CHECK_EQUAL(mag[1], 2.0);
    COMPARE_ZERO(mag[2]);
}

BOOST_AUTO_TEST_CASE(sineSubset)
{
    double in[] = { 0, 1, 0, -1 };
    double mag[1];
    FourierFilterbank(4, 4, 1, 1, false).forwardMagnitude(in, mag);
    BOOST_CHECK_EQUAL(mag[0], 2.0);
}

BOOST_AUTO_TEST_CASE(cosine)
{
    double in[] = { 1, 0, -1, 0 };
    double mag[3];
    FFT(4).forwardMagnitude(in, mag);
    COMPARE_ZERO(mag[0]);
    BOOST_CHECK_EQUAL(mag[1], 2.0);
    COMPARE_ZERO(mag[2]);
}

BOOST_AUTO_TEST_CASE(sineCosine)
{
    double in[] = { 0.5, 1, -0.5, -1 };
    double mag[3];
    FFT(4).forwardMagnitude(in, mag);
    COMPARE_ZERO(mag[0]);
    BOOST_CHECK_CLOSE(mag[1], sqrt(5.0), 1e-10);
    COMPARE_ZERO(mag[2]);
}

BOOST_AUTO_TEST_CASE(nyquist)
{
    double in[] = { 1, -1, 1, -1 };
    double mag[3];
    FFT(4).forwardMagnitude(in, mag);
    COMPARE_ZERO(mag[0]);
    COMPARE_ZERO(mag[1]);
    BOOST_CHECK_EQUAL(mag[2], 4.0);
}

BOOST_AUTO_TEST_CASE(dirac)
{
    double in[] = { 1, 0, 0, 0 };
    double mag[3];
    FFT(4).forwardMagnitude(in, mag);
    BOOST_CHECK_EQUAL(mag[0], 1.0);
    BOOST_CHECK_EQUAL(mag[1], 1.0);
    BOOST_CHECK_EQUAL(mag[2], 1.0);
}

BOOST_AUTO_TEST_CASE(forwardArrayBounds)
{
    double in[] = { 1, 1, -1, -1 };
    double mag[] = { 999, 999, 999, 999, 999 };
    FFT(4).forwardMagnitude(in, mag + 1);
    BOOST_CHECK_EQUAL(mag[0], 999.0);
    BOOST_CHECK_EQUAL(mag[4], 999.0);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(TestAutocorrelation)

BOOST_AUTO_TEST_CASE(unnormalised)
{
    double in[] = { 1,0,0, 1,0,0, 1,0,0, 1,0,0 };
    double out[] = { 0,0,0, 0,0,0, 0,0,0, 0,0,0 };
    Autocorrelation(12, 12).acf(in, out);
    BOOST_CHECK_EQUAL(out[0], 4.0);
    BOOST_CHECK_EQUAL(out[1], 0.0);
    BOOST_CHECK_EQUAL(out[2], 0.0);
    BOOST_CHECK_EQUAL(out[3], 3.0);
    BOOST_CHECK_EQUAL(out[4], 0.0);
    BOOST_CHECK_EQUAL(out[5], 0.0);
    BOOST_CHECK_EQUAL(out[6], 2.0);
    BOOST_CHECK_EQUAL(out[7], 0.0);
    BOOST_CHECK_EQUAL(out[8], 0.0);
    BOOST_CHECK_EQUAL(out[9], 1.0);
    BOOST_CHECK_EQUAL(out[10], 0.0);
    BOOST_CHECK_EQUAL(out[11], 0.0);
}

BOOST_AUTO_TEST_CASE(normalisedUnity)
{
    double in[] = { 1,0,0, 1,0,0, 1,0,0, 1,0,0 };
    double out[] = { 0,0,0, 0,0,0, 0,0,0, };
    Autocorrelation(12, 9).acfUnityNormalised(in, out);
    BOOST_CHECK_EQUAL(out[0], 1.0);
    BOOST_CHECK_EQUAL(out[1], 0.0);
    BOOST_CHECK_EQUAL(out[2], 0.0);
    BOOST_CHECK_EQUAL(out[3], 1.0);
    BOOST_CHECK_EQUAL(out[4], 0.0);
    BOOST_CHECK_EQUAL(out[5], 0.0);
    BOOST_CHECK_EQUAL(out[6], 1.0);
    BOOST_CHECK_EQUAL(out[7], 0.0);
    BOOST_CHECK_EQUAL(out[8], 0.0);
}

BOOST_AUTO_TEST_CASE(bpmLagConversion)
{
    int lag = Autocorrelation::bpmToLag(120.0, 2.0);
    BOOST_CHECK_EQUAL(lag, 1);
    lag = Autocorrelation::bpmToLag(130.0, 2.0);
    BOOST_CHECK_EQUAL(lag, 1);
    lag = Autocorrelation::bpmToLag(130.0, 100.0);
    BOOST_CHECK_EQUAL(lag, 46);

    double bpm = Autocorrelation::lagToBpm(1, 2.0);
    BOOST_CHECK_EQUAL(bpm, 120.0);
    bpm = Autocorrelation::lagToBpm(50, 100.0);
    BOOST_CHECK_EQUAL(bpm, 120.0);
    bpm = Autocorrelation::lagToBpm(46, 100.0);
    BOOST_CHECK_CLOSE(bpm, 130.0, 0.5);
    bpm = Autocorrelation::lagToBpm(0.5, 1.0);
    BOOST_CHECK_CLOSE(bpm, 120.0, 0.1);

    bpm = Autocorrelation::lagToBpm(1, 1.0);
    BOOST_CHECK_EQUAL(bpm, 60.0);
    bpm = Autocorrelation::lagToBpm(2, 1.0);
    BOOST_CHECK_EQUAL(bpm, 30.0);
    bpm = Autocorrelation::lagToBpm(3, 1.0);
    BOOST_CHECK_EQUAL(bpm, 20.0);
    bpm = Autocorrelation::lagToBpm(4, 1.0);
    BOOST_CHECK_EQUAL(bpm, 15.0);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(TestACFCombFilter)

BOOST_AUTO_TEST_CASE(contributingRange)
{
    int base, count;
    ACFCombFilter::getContributingRange(1, 1, base, count);
    BOOST_CHECK_EQUAL(base, 1);
    BOOST_CHECK_EQUAL(count, 1);
    ACFCombFilter::getContributingRange(1, 2, base, count);
    BOOST_CHECK_EQUAL(base, 2);
    BOOST_CHECK_EQUAL(count, 1);
    ACFCombFilter::getContributingRange(1, 4, base, count);
    BOOST_CHECK_EQUAL(base, 3);
    BOOST_CHECK_EQUAL(count, 3);
    ACFCombFilter::getContributingRange(1, 8, base, count);
    BOOST_CHECK_EQUAL(base, 6);
    BOOST_CHECK_EQUAL(count, 6);
}

BOOST_AUTO_TEST_CASE(contributingRangeCoverage)
{
    const int n = 64;
    const int maxMultiple = 16;
    int *target = new int[n];

    // Need to ensure that each lag contributes to no more than one
    // target lag, at each multiple
    for (int multiple = 1; multiple <= maxMultiple; multiple = multiple * 2) {
	for (int i = 1; i < n; ++i) {
	    target[i] = -1;
	}
	for (int i = 1; i < n; ++i) {
	    int base, count;
	    ACFCombFilter::getContributingRange(i, multiple, base, count);
	    for (int j = base; j < base + count; ++j) {
		BOOST_CHECK_GE(j, i);
		if (j >= 0 && j < n) {
		    if (target[j] != -1) {
			BOOST_CHECK_EQUAL(target[j], i);
		    }
		    target[j] = i;
		}
	    }
	}
    }

    // and that each lag is a source for only one multiple, for a
    // given target
    for (int i = 1; i < n; ++i) {
	for (int j = 1; j < n; ++j) {
	    target[j] = -1;
	}
	for (int multiple = 1; multiple <= maxMultiple; multiple = multiple * 2) {
	    int base, count;
	    ACFCombFilter::getContributingRange(i, multiple, base, count);
	    for (int j = base; j < base + count; ++j) {
		BOOST_CHECK_GE(j, i);
		if (j >= 0 && j < n) {
		    if (target[j] != -1) {
			BOOST_CHECK_EQUAL(target[j], multiple);
		    }
		    target[j] = multiple;
		}
	    }
	}
    }

    delete[] target;
}

#define REFINER(n) ACFCombFilter(4, 10, 20, n)

BOOST_AUTO_TEST_CASE(refineSingleElement)
{
    double acf[] = { 0.0, 1.0 };
    double bpm = REFINER(1).refine(1, acf, 2);
    BOOST_CHECK_EQUAL(bpm, 60);
}

BOOST_AUTO_TEST_CASE(refineEmpty)
{
    double acf[] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
    double bpm = REFINER(1).refine(1, acf, 8);
    BOOST_CHECK_EQUAL(bpm, 60);
}

BOOST_AUTO_TEST_CASE(refineTrivial)
{
    double acf[] = { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
    double bpm = REFINER(1).refine(1, acf, 8);
    BOOST_CHECK_EQUAL(bpm, 60);
}

BOOST_AUTO_TEST_CASE(refineConsistent)
{
    double acf[] = { 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0 };
    double bpm = REFINER(1).refine(1, acf, 8);
    BOOST_CHECK_EQUAL(bpm, 60);
}

BOOST_AUTO_TEST_CASE(refineAdjusting)
{
    double acf[] = { 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 };
    double bpm = REFINER(1).refine(1, acf, 8);
    BOOST_CHECK_LT(bpm, 60);
    BOOST_CHECK_GT(bpm, 50);
}

BOOST_AUTO_TEST_SUITE_END()

static float *tap(double duration, double rate, double bpm, int &len)
{
    double bps = bpm / 60.0;
    len = int(duration * rate + 0.5);
    float *data = new float[len];
    for (int i = 0; i < len; ++i) {
	data[i] = 0.f;
    }
    int beats = int(bps * duration + 0.5);
    for (int i = 0; i < beats; ++i) {
	int ix = (i / bps) * rate;
	if (ix < len) data[ix] = 1.f;
    }
    return data;
}

BOOST_AUTO_TEST_SUITE(TestMiniBpm)

BOOST_AUTO_TEST_CASE(tapper)
{
    // Testing our test harness
    int len = 0;
    float *data = tap(2.0, 20.0, 240.0, len);
    BOOST_CHECK_EQUAL(len, 40);
    for (int i = 0; i < 8; ++i) {
	BOOST_CHECK_EQUAL(data[i * 5], 1.f);
	for (int j = 1; j < 5; ++j) {
	    BOOST_CHECK_EQUAL(data[i * 5 + j], 0.f);
	}
    }
    delete[] data;
}

BOOST_AUTO_TEST_CASE(complete_120bpm_8000)
{
    float rate = 8000.f;
    int len;
    float *data = tap(6.0, rate, 120.0, len);
    double bpm = MiniBPM(rate).estimateTempoOfSamples(data, len);
    delete[] data;
    BOOST_CHECK_CLOSE(bpm, 120.0, 0.25);
}

BOOST_AUTO_TEST_CASE(complete_120bpm_44100)
{
    float rate = 44100.f;
    int len;
    float *data = tap(6.0, rate, 120.0, len);
    double bpm = MiniBPM(rate).estimateTempoOfSamples(data, len);
    delete[] data;
    BOOST_CHECK_CLOSE(bpm, 120.0, 0.25);
}

BOOST_AUTO_TEST_CASE(complete_120bpm_48000)
{
    float rate = 48000.f;
    int len;
    float *data = tap(6.0, rate, 120.0, len);
    double bpm = MiniBPM(rate).estimateTempoOfSamples(data, len);
    delete[] data;
    BOOST_CHECK_CLOSE(bpm, 120.0, 0.25);
}

BOOST_AUTO_TEST_CASE(buffered_120bpm_44100)
{
    float rate = 44100.f;
    int len;
    float *data = tap(6.0, rate, 120.0, len);
    MiniBPM mbpm(rate);
    int blockSize = 1000;
    for (int i = 0; i < len/blockSize; ++i) {
	int thisBlock = blockSize;
	if (i * blockSize + thisBlock > len) {
	    thisBlock = len - i * blockSize;
	}
	mbpm.process(data + i * blockSize, thisBlock);
    }
    delete[] data;
    double bpm = mbpm.estimateTempo();
    BOOST_CHECK_CLOSE(bpm, 120.0, 0.25);
}

BOOST_AUTO_TEST_CASE(minmax)
{
    MiniBPM mbpm(44100);
    double min = 0.0, max = 0.0;
    mbpm.getBPMRange(min, max);
    BOOST_CHECK_NE(min, 0.0);
    BOOST_CHECK_NE(max, 0.0);
    BOOST_CHECK_GT(max, min);
    BOOST_CHECK_GT(min, 30.0);
    BOOST_CHECK_GT(max, 30.0);
    BOOST_CHECK_LT(min, 240.0);
    BOOST_CHECK_LT(max, 240.0);
    mbpm.setBPMRange(70, 140);
    mbpm.getBPMRange(min, max);
    BOOST_CHECK_EQUAL(min, 70);
    BOOST_CHECK_EQUAL(max, 140);
}

BOOST_AUTO_TEST_CASE(complete_240bpm_outOfRange)
{
    // 240bpm is too fast for default range; should return 120
    float rate = 44100.f;
    int len;
    float *data = tap(6.0, rate, 240.0, len);
    double bpm = MiniBPM(rate).estimateTempoOfSamples(data, len);
    delete[] data;
    BOOST_CHECK_CLOSE(bpm, 120.0, 0.25);
}

BOOST_AUTO_TEST_CASE(complete_120bpm_outOfRange)
{
    // adjust range to make 120bpm also too fast; should return 60
    float rate = 44100.f;
    int len;
    float *data = tap(6.0, rate, 120.0, len);
    MiniBPM mbpm(rate);
    mbpm.setBPMRange(30, 90);
    double bpm = mbpm.estimateTempoOfSamples(data, len);
    delete[] data;
    BOOST_CHECK_CLOSE(bpm, 60.0, 0.25);
}

BOOST_AUTO_TEST_CASE(candidates)
{
    float rate = 44100.f;
    int len;
    float *data = tap(6.0, rate, 120.0, len);
    MiniBPM mbpm(rate);
    double bpm = mbpm.estimateTempoOfSamples(data, len);
    double min, max;
    mbpm.getBPMRange(min, max);
    std::vector<double> candidates = mbpm.getTempoCandidates();
    delete[] data;
    BOOST_CHECK_CLOSE(bpm, 120.0, 0.25);
    BOOST_CHECK(candidates.size() > 1);
    BOOST_CHECK_CLOSE(candidates[0], 120.0, 0.25);
    BOOST_CHECK_CLOSE(candidates[1], 60.0, 0.25);
    for (int i = 0; i < candidates.size(); ++i) {
	BOOST_CHECK_GE(candidates[i], min);
	BOOST_CHECK_LE(candidates[i], max);
    }
}

BOOST_AUTO_TEST_SUITE_END()