//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file steiner_strategy.hpp
 * @brief
 * @author Maciej Andrejczuk
 * @version 1.0
 * @date 2013-08-01
 */
#ifndef PAAL_STEINER_STRATEGY_HPP
#define PAAL_STEINER_STRATEGY_HPP

#include "paal/data_structures/bimap.hpp"
#include "paal/data_structures/subset_iterator.hpp"
#include "paal/iterative_rounding/steiner_tree/steiner_components.hpp"
#include "paal/utils/assign_updates.hpp"

#include <boost/graph/connected_components.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/range/as_array.hpp>
#include <boost/range/algorithm_ext/erase.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/range/algorithm/unique.hpp>
#include <boost/random/discrete_distribution.hpp>

#include <random>
#include <unordered_set>
#include <vector>

namespace paal {
namespace ir {

/**
 * Generates all the components possible.
 * It iterates over all subsets of terminals with no more than K elements.
 */
class steiner_tree_all_generator {
private:
    template<typename Metric>
    using MTraits = typename data_structures::metric_traits<Metric>;

public:
    /// Constructor.
    steiner_tree_all_generator(int K = 4) : m_component_max_size(K) {}

    /// Generates all possible components.
    template<typename Metric, typename Terminals>
    void gen_components(const Metric& cost_map, const Terminals& terminals,
            const Terminals& steiner_vertices,
            steiner_components<
                typename MTraits<Metric>::VertexType,
                typename MTraits<Metric>::DistanceType>& components) {

        using Vertex = typename MTraits<Metric>::VertexType;
        using Dist = typename MTraits<Metric>::DistanceType;
        std::vector<Vertex> current_terminals;
        gen_all_components<Vertex, Dist>(components, 0, terminals.size(),
            current_terminals, cost_map, terminals, steiner_vertices);
    }
private:
    template<typename Vertex, typename Dist, typename Metric, typename Terminals>
    void gen_all_components(steiner_components<Vertex, Dist>& components,
            int first_avail, int last, std::vector<Vertex>& curr,
            const Metric& cost_map, const Terminals& terminals,
            const Terminals& steiner_vertices) {

        if (curr.size() > 1) {
            steiner_component<Vertex, Dist> c(cost_map, curr, steiner_vertices);
            components.add(std::move(c));
        }
        if ((int) curr.size() >= m_component_max_size)
            return;
        for (int i = first_avail; i < last; ++i) {
            curr.push_back(terminals[i]);
            gen_all_components(components, i + 1, last, curr, cost_map,
                terminals, steiner_vertices);
            curr.pop_back();
        }
    }

    int m_component_max_size;
};

namespace detail {

template<typename Vertex>
struct vertex_filter {
    bool operator()(Vertex v) const { return vertices.find(v) == vertices.end(); }
    std::unordered_set<Vertex> vertices;
};

}// detail

/**
 * Generates all the components possible based on the underlying graph.
 * It iterates over all subsets of terminals with no more than K elements.
 */
template<typename Graph, typename Vertex, typename Terminals>
class steiner_tree_graph_all_generator {
private:
    template<typename Metric>
    using MTraits = typename data_structures::metric_traits<Metric>;

public:
    /// Constructor.
    steiner_tree_graph_all_generator(const Graph& graph,
        const Terminals& terminals, int K = 4) : m_component_max_size(K),
            m_index(terminals),
            m_terminals_graph(m_index.size()) {
        initialize_terminals_graph(graph, terminals);
    }

    /// Generates all possible components.
    template<typename Metric>
    void gen_components(const Metric& cost_map, const Terminals& terminals,
            const Terminals& steiner_vertices,
            steiner_components<
                typename MTraits<Metric>::VertexType,
                typename MTraits<Metric>::DistanceType>& components) {

        using Dist = typename MTraits<Metric>::DistanceType;
        merge_vertices<Dist>(cost_map);
        std::vector<Vertex> current_terminals;
        gen_all_components<Dist>(components, 0, terminals.size(),
            current_terminals, cost_map, terminals, steiner_vertices);
    }

private:
    using AuxGraph = boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS>;
    using VertexIndex = data_structures::bimap<Vertex>;

    int m_component_max_size;
    VertexIndex m_index;
    AuxGraph m_terminals_graph;

    void initialize_terminals_graph(const Graph& graph, const Terminals& terminals) {
        detail::vertex_filter<Vertex> filter;
        filter.vertices.insert(terminals.begin(), terminals.end());
        auto index = get(boost::vertex_index, graph);
        std::vector<int> components(num_vertices(graph));
        for (auto u : terminals) {
            for (auto v : terminals) {
                if (u == v) continue;
                filter.vertices.erase(u);
                filter.vertices.erase(v);
                boost::filtered_graph<Graph, boost::keep_all, decltype(filter)>
                    fg(graph, boost::keep_all{}, filter);
                boost::connected_components(fg, &components[0]);
                if (components[index[u]] == components[index[v]]) {
                    add_edge(get_idx(u), get_idx(v), m_terminals_graph);
                }
                filter.vertices.insert(u);
                filter.vertices.insert(v);
            }
        }
    }

    template<typename Dist, typename Metric>
    void merge_vertices(const Metric& cost_map) {
        auto range = vertices(m_terminals_graph);
        for (auto v_pair : data_structures::make_subsets_iterator_range<2>(
                range.first, range.second)) {
            auto i = std::get<0>(v_pair);
            auto j = std::get<1>(v_pair);
            if (cost_map(get_val(i), get_val(j)) == Dist{}) {
                merge_vertices(i, j);
                merge_vertices(j, i);
            }
        }
    }

    void merge_vertices(Vertex trg, Vertex src) {
        for (auto v : boost::as_array(adjacent_vertices(src,
                m_terminals_graph))) {
            if (trg != static_cast<Vertex>(v)) {
                add_edge(trg, v, m_terminals_graph);
            }
        }
    }

    auto get_idx(Vertex v) const -> decltype(m_index.get_idx(v)) {
        return m_index.get_idx(v);
    }

    auto get_val(int idx) const -> decltype(m_index.get_val(idx)) {
        return m_index.get_val(idx);
    }

    bool is_graph_component(const std::vector<Vertex>& comp) const {
        for (auto term_pair : data_structures::make_subsets_iterator_range<2>(
                comp.begin(), comp.end())) {
            if (!edge(
                    get_idx(std::get<0>(term_pair)),
                    get_idx(std::get<1>(term_pair)),
                    m_terminals_graph).second) {
                return false;
            }
        }
        return true;
    }

    template<typename Dist, typename Metric>
    void gen_all_components(steiner_components<Vertex, Dist>& components,
            int first_avail, int last, std::vector<Vertex>& curr,
            const Metric& cost_map, const Terminals& terminals,
            const Terminals& steiner_vertices) {

        // TODO implement a subset_iterator with K passed as a parameter
        // not as a template parameter (also in the gen_all_components method
        // in steiner_tree_all_generator)
        if (curr.size() > 1) {
            if (!is_graph_component(curr))
                return;
            steiner_component<Vertex, Dist> c(cost_map, curr, steiner_vertices);
            components.add(std::move(c));
        }
        if ((int) curr.size() >= m_component_max_size)
            return;
        for (int i = first_avail; i < last; ++i) {
            curr.push_back(terminals[i]);
            gen_all_components(components, i + 1, last, curr, cost_map,
                terminals, steiner_vertices);
            curr.pop_back();
        }
    }
};

/**
 * Makes a graph_all_generator object.
 */
template<typename Vertex, typename Graph, typename Terminals>
steiner_tree_graph_all_generator<Graph, Vertex, Terminals>
make_steiner_tree_graph_all_generator(const Graph& graph,
        const Terminals& terminals, int K = 4) {
    return steiner_tree_graph_all_generator<Graph, Vertex, Terminals>(
        graph, terminals, K);
}

/**
 * Generates specified number of components by selecting random elements.
 */
class steiner_tree_random_generator {
public:
    /// Constructor.
    steiner_tree_random_generator(int N = 100, int K = 3) :
            m_iterations(N), m_component_max_size(K) {
    }

    /// Generates a specified number of components by selecting random elements.
    template<typename Metric, typename Terminals>
    void gen_components(const Metric& cost_map, const Terminals & terminals,
            const Terminals& steiner_vertices,
            steiner_components<
                typename data_structures::metric_traits<Metric>::VertexType,
                typename data_structures::metric_traits<Metric>::DistanceType>& components) {

        using Vertex = typename data_structures::metric_traits<Metric>::VertexType;
        using Dist = typename data_structures::metric_traits<Metric>::DistanceType;
        if (terminals.size() < 2) {
            return;
        }
        for (int i = 0; i < m_iterations; ++i) {
            std::set<Vertex> curr;
            while ((int)curr.size() < m_component_max_size) {
                if (curr.size() > 1) {
                    int c =
                        (int)rand() %
                        m_component_max_size; // TODO: Is this fair probability?
                    if (c == 0) {
                        break;
                    }
                }
                int r = (int)rand() % terminals.size();
                curr.insert(terminals[r]);
            }
            std::vector<Vertex> elements(curr.begin(), curr.end());
            steiner_component<Vertex, Dist> c(cost_map, elements, steiner_vertices);
            components.add(std::move(c));
        }
        // TODO some terminals may not be in any component
    }

  private:
    int m_iterations;
    int m_component_max_size;
};

/**
 * Generates specified number of components by randomly selecting elements
 * with probability dependent on distance from vertices already selected.
 */
class steiner_tree_smart_generator {
    std::default_random_engine m_rng;
public:
    /// Constructor.
    steiner_tree_smart_generator(int N = 100, int K = 3, std::default_random_engine rng = std::default_random_engine{}) :
            m_iterations(N), m_component_max_size(K) {
    }

    /// Generates components.
    template<typename Metric, typename Terminals>
    void gen_components(const Metric& cost_map, const Terminals& terminals,
            const Terminals& steiner_vertices,
            steiner_components<
                    typename data_structures::metric_traits<Metric>::VertexType,
                    typename data_structures::metric_traits<Metric>::DistanceType>& components) {

        using Vertex = typename data_structures::metric_traits<Metric>::VertexType;
        using Dist = typename data_structures::metric_traits<Metric>::DistanceType;
        std::vector<Vertex> elements;
        std::vector<double> prob;
        for (Vertex start : terminals) {
            for (int i = 0; i < m_iterations; ++i) {
                elements.clear();
                elements.push_back(start);
                int limit = 2 + rand() % (m_component_max_size - 1);
                while ((int)elements.size() < limit) {
                    prob.resize(terminals.size());
                    for (int k = 0; k < (int)prob.size(); ++k) {
                        for (auto e : elements) {
                            if (e == terminals[k]) {
                                prob[k] = 0;
                                break;
                            }
                            int cost = cost_map(e, terminals[k]);
                            assert(cost > 0);
                            assign_max(prob[k], 1. / cost);
                        }
                    }
                    auto selected = boost::random::discrete_distribution<std::size_t>(prob)(m_rng);
                    if (selected == prob.size()) break;
                    elements.push_back(terminals[selected]);
                }
                boost::erase(elements, boost::unique<boost::return_found_end>(boost::sort(elements)));

                steiner_component<Vertex, Dist> c(cost_map, elements, steiner_vertices);
                components.add(std::move(c));
            }
        }
    }

  private:
    int m_iterations;
    int m_component_max_size;
};

} // ir
} // paal

#endif // PAAL_STEINER_STRATEGY_HPP


#include <vector>
#include <Eigen/Dense>
#include <cassert>
#include <numeric>
#include <math.h>
#include <errno.h>
#include <fstream>
#include <iostream>
#include <map>
#include <vector>
#include <algorithm>
#include <regex>
#include "MorrisMethod.h"
#include "Transformable.h"
#include "RunManagerAbstract.h"
#include "ParamTransformSeq.h"
#include "ModelRunPP.h"
#include "utilities.h"
#include "FileManager.h"
#include "Stats.h"
//#include "Ensemble.h"

using namespace std;
using namespace pest_utils;
using Eigen::MatrixXd;
using Eigen::VectorXd;

void MorrisObsSenFile::initialize(const vector<string> &_par_names_vec, const vector<string> &_obs_names_vec, double _no_data, const GsaAbstractBase *_gsa_abstract_base)
{
	par_names_vec =_par_names_vec;
	obs_names_vec = _obs_names_vec;
	gsa_abstract_base = _gsa_abstract_base;
	no_data = _no_data;
}

void MorrisObsSenFile::add_sen_run_pair(const std::string &par_name, double p1, Observations &obs1, double p2, Observations &obs2)
{
	assert (obs1.size() == obs2.size());

	// compute sensitivities of individual observations
	double isen;
	double del_par = p2 - p1;

	for (const auto &iobs : obs_names_vec)
	{
		isen = (obs2[iobs] - obs1[iobs]) / del_par;
		const auto id = make_pair(par_name, iobs);
		if (map_obs_stats.find(id) == map_obs_stats.end())
		{
			map_obs_stats[id] = RunningStats();
		}
		map_obs_stats[make_pair(par_name, iobs)].add(isen);
	}
}

void MorrisObsSenFile::calc_pooled_obs_sen(ofstream &fout_obs_sen, map<string, double> &obs_2_sen_weight,
	map<string, double> &par_2_sen_weight)
{
	fout_obs_sen << "par_name, n_samples, obs_name, mean, abs_mean, sigma, scaled_sen" << endl;
	for (const auto &ip : par_names_vec)
	{
		string ipar = ip;
		for (const auto &iobs : obs_names_vec)
		{
			double mean = no_data;
			double abs_mean = no_data;
			double sigma = no_data;
			int n_samples = 0;
			const auto id = make_pair(ipar, iobs);
			auto sen_itr = map_obs_stats.find(id);
			if (sen_itr != map_obs_stats.end() && sen_itr->second.comp_nsamples() > 0)
			{
				mean = sen_itr->second.comp_mean();
				abs_mean = sen_itr->second.comp_abs_mean();
				sigma = sen_itr->second.comp_sigma();
				n_samples = sen_itr->second.comp_nsamples();
			}
			string weighted_sen = "N/A";
			auto it_obs = obs_2_sen_weight.find(iobs);
			auto it_par = par_2_sen_weight.find(ipar);
			if (it_obs != obs_2_sen_weight.end() && it_par != par_2_sen_weight.end() && abs_mean != no_data)
			{
				stringstream sstr;
				double value = abs_mean * it_par->second / it_obs->second;
				sstr << value;
				weighted_sen = sstr.str();
			}
			fout_obs_sen << ipar << ", " << n_samples << ", " << iobs << ", " << mean << ", " << abs_mean << ", " << sigma << ", " << weighted_sen << endl;
		}
	}
}

void MorrisMethod::process_pooled_var_file()
{
	if (calc_obs_sen == true)
	{
		ifstream &fin = file_manager_ptr->open_ifile_ext("pgp");
		std::set<string> obs_group_names;
		for (const auto &imap : obs_info_ptr->groups)
			obs_group_names.insert(imap.first);

		string line;
		string cur_pool_grp;
		regex reg_reg("regex\\s*\\(\"(.+)\"\\)", regex_constants::icase);
		regex reg_grp("pool_group\\s*\\((.+)\\)", regex_constants::icase);
		cmatch mr;
		while (getline(fin, line))
		{
			if (regex_match(line.c_str(), mr, reg_reg))
			{
				regex inp_reg = regex(mr[1].str(), regex_constants::icase);

				for (auto itr = obs_group_names.begin(); itr != obs_group_names.end();)
				{
					auto  here = itr++;
					if (regex_match(*here, inp_reg))
					{
						group_2_pool_group_map[*here] = cur_pool_grp;
						obs_group_names.erase(here);
					}
				}

			}
			if (regex_match(line.c_str(), mr, reg_grp))
			{
				cur_pool_grp = mr[1];
			}
		}
	}
	file_manager_ptr->close_file("pgp");
}

MatrixXd MorrisMethod::create_P_star_mat(int k)
{
	MatrixXd b_mat = create_B_mat(k);
	MatrixXd j_mat = create_J_mat(k);
	MatrixXd d_mat = create_D_mat(k);
	MatrixXd x_vec = create_x_vec(k);
	MatrixXd p_mat = create_P_mat(k);
	b_star_mat = j_mat.col(0) * x_vec.transpose() + (delta/2.0)*((2.0 * b_mat - j_mat) * d_mat + j_mat) ;
	return b_star_mat;
}

MorrisMethod::MorrisMethod(Pest &_pest_scenario,
	FileManager &_file_manager, ObjectiveFunc *_obj_func_ptr,
	const ParamTransformSeq &_par_transform,
	int _p, int _r, double _delta,
	bool _calc_pooled_obs, bool _calc_morris_obs_sen, PARAM_DIST _par_dist, unsigned int _seed)
	: GsaAbstractBase(_pest_scenario, _file_manager, _obj_func_ptr, _par_transform,
		_par_dist, _seed),
	calc_obs_sen(_calc_pooled_obs), calc_morris_obs_sen(_calc_morris_obs_sen)
{
	initialize(_p, _r, _delta);
}

void MorrisMethod::initialize(int _p, int _r, double _delta)
{
	p = _p;
	r = _r;
	delta = _delta;
	//delta = p / (2.0 * (p - 1));
}

MatrixXd MorrisMethod::create_B_mat(int k)
{
	MatrixXd b_mat = MatrixXd::Constant(k+1, k, 0.0);
	int nrow = k+1;
	int ncol = k;
	for (int icol=0; icol<ncol; ++icol)
	{
		for (int irow=icol+1; irow<nrow; ++irow)
		{
			b_mat(irow, icol) = 1.0;
		}
	}
	return b_mat;
}

MatrixXd MorrisMethod::create_J_mat(int k)
{
	MatrixXd j_mat = MatrixXd::Constant(k+1, k, 1.0);
	return j_mat;
}

MatrixXd MorrisMethod::create_D_mat(int k)
{
	MatrixXd d_mat=MatrixXd::Constant(k, k, 0.0);
	for (int i=0; i<k; ++i) {
		d_mat(i,i) = rand_plus_minus_1();
	}
	return d_mat;
}

MatrixXd MorrisMethod::create_P_mat(int k)
{
	// generate and return a random permutation matrix
	MatrixXd p_mat=MatrixXd::Constant(k, k, 0.0);
	vector<int> rand_idx;
	rand_idx.reserve(k);
	for (int i=0; i<k; ++i) {
		rand_idx.push_back(i);
	}
	// Shuffle random index vector
	shuffle(rand_idx.begin(), rand_idx.end(), rand_engine);

	for(int irow=0; irow<k; ++irow)
	{
	  p_mat(irow, rand_idx[irow]) = 1;
	}

	return p_mat;
}

VectorXd MorrisMethod::create_x_vec(int k)
{
	// Warning Satelli's book is wrong.  Need to use Morris's original paper
	// to compute x.  Maximim value of any xi shoud be 1.0 - delta.
	VectorXd x(k);
	double rnum;
	// Used with the randon number generator below, max_num will produce a
	// random interger which varies between 0 and (p-2)/2.  This will in turn
	// produce the correct xi's which vary from {0, 1/(p-1), 2/(p-1), .... 1 - delta)
	// when divided by (p-1).  See Morris's paper for the derivaition.
	// Satelli's book has this equation wrong!
	int max_num = 1+(p-2)/2;
	for (int i=0; i<k; ++i)
	{
		rnum = rand_engine() % max_num;
		x(i) = rnum / (p - 1);
	}
	return x;
}

int MorrisMethod::rand_plus_minus_1(void)
{
	return (rand_engine() % 2 * 2) - 1;
}

Parameters MorrisMethod::get_numeric_parameters(int row)
{
	Parameters numeric_pars;
	auto e=adj_par_name_vec.end();
	size_t n_cols = b_star_mat.cols();
	for (int j=0; j<n_cols; ++j)
	{
		const string &p = adj_par_name_vec[j];
		auto it_lbnd = min_numeric_pars.find(p);
		assert(it_lbnd != min_numeric_pars.end());
		auto it_ubnd = max_numeric_pars.find(p);
		assert(it_ubnd != max_numeric_pars.end());
		numeric_pars[p] =  it_lbnd->second + (it_ubnd->second - it_lbnd->second) * b_star_mat(row, j);
	}
	return numeric_pars;
}

void MorrisMethod::assemble_runs(RunManagerAbstract &run_manager)
{
	map<int, Parameters> par_map;
	for (int tmp_r=0; tmp_r<r; ++tmp_r)
	{
		b_star_mat = create_P_star_mat(adj_par_name_vec.size());
		auto n_rows = b_star_mat.rows();
		int run_id;
		string par_name = "";
		for (int i=0; i<n_rows; ++i)
		{
			par_name.clear();
			//get control parameters
			Parameters pars = get_numeric_parameters(i);
			// convert control parameters to model parameters
			base_partran_seq_ptr->numeric2model_ip(pars);
			// convert control parameters to model parameters
			base_partran_seq_ptr->ctl2model_ip(pars);
			if (i>0)
			{
				par_name = adj_par_name_vec[i-1];
			}
			run_id = run_manager.add_run(pars, par_name, Parameters::no_data);
			base_partran_seq_ptr->model2ctl_ip(pars);
			par_map[run_id] =  pars;

		}
	}
	//write the parameter sequence to a csv file
	string filename = file_manager_ptr->get_base_filename() + ".sen.par.csv";
	ofstream fout(filename);
	vector<string> var_names = pest_scenario_ptr->get_ctl_ordered_par_names();
	fout << "run_id";
	for (auto pname : var_names)
		fout << "," << pname;
	fout << endl;
	for (auto &p : par_map)
	{
		fout << p.first;
		for (auto &pname : var_names)
			fout << "," << p.second.get_rec(pname);
		fout << endl;
	}
	/*ParameterEnsemble pe(pest_scenario_ptr);
	vector<string> real_names,var_names=pest_scenario_ptr->get_ctl_ordered_par_names();
	stringstream ss;
	map<int, string> real_name_map;
	for (auto &p : par_map)
	{
		ss.str("");
		ss << p.first;
		real_names.push_back(ss.str());
		real_name_map[p.first] = ss.str();
	}
	pe.set_trans_status(ParameterEnsemble::transStatus::CTL);
	pe.reserve(real_names, var_names);
	for (auto &p : par_map)
	{
		pe.update_real_ip(real_name_map[p.first], p.second.get_data_eigen_vec(var_names));
	}
	string filename = file_manager_ptr->get_base_filename() + ".sen.par.csv";
	pe.to_csv(filename);*/

}

void  MorrisMethod::calc_sen(RunManagerAbstract &run_manager, ModelRun model_run)
{
	ofstream &fout_morris = file_manager_ptr->open_ofile_ext("msn");
	ofstream &fout_raw = file_manager_ptr->open_ofile_ext("raw.csv");

	ModelRun run0 = model_run;
	ModelRun run1 = model_run;
	Parameters pars0;
	Observations obs0;
	Parameters pars1;
	Observations obs1;

	map<string, RunningStats > sen_map;
	map<string, RunningStats> obs_stats_map;

	for (auto &it_p : adj_par_name_vec)
	{
		sen_map[it_p] = RunningStats();
	}

	for (auto &it_obs : obs_name_vec)
	{
		obs_stats_map[it_obs] = RunningStats();
	}

	const vector<string> &run_mngr_obs_name_vec = run_manager.get_obs_name_vec();
	obs_sen_file.initialize(adj_par_name_vec, run_mngr_obs_name_vec, Observations::no_data, this);

	fout_raw << "parameter_name, phi_0, phi_1, par_0, par_1, elem_effect" << endl;
	int n_runs = run_manager.get_nruns();
	bool run0_ok = false;
	bool run1_ok = false;
	string par_name_1;
	double null_value;
	stringstream message;
	cout << endl;
	run1_ok = run_manager.get_run(0, pars1, obs1);
	base_partran_seq_ptr->model2numeric_ip(pars1);
	for (int i_run=1; i_run<n_runs; ++i_run)
	{
		std::cout << string(message.str().size(), '\b');
		message.str("");
		message << "processing run " << i_run+1 << " / " << n_runs;
		std::cout << message.str();

		run0_ok = run1_ok;
		pars0 = pars1;
		obs0 = obs1;
		run1_ok = run_manager.get_run(i_run, pars1, obs1, par_name_1, null_value);
		base_partran_seq_ptr->model2numeric_ip(pars1);
		// Add run0 to obs_stats
		if (run0_ok)
		{
			for (const auto &i_obs : run_mngr_obs_name_vec)
			{
				auto it = obs0.find(i_obs);
				if (it != obs0.end() && it->second != Observations::no_data)
				{
					obs_stats_map[i_obs].add(obs0[i_obs]);
				}
			}
		}

		if (run0_ok && run1_ok && !par_name_1.empty())
		{
			Parameters tmp_ctl_par = base_partran_seq_ptr->numeric2ctl_cp(pars0);
			run0.update_ctl(tmp_ctl_par, obs0);
			double phi0 = run0.get_phi(DynamicRegularization::get_zero_reg_instance());
			tmp_ctl_par = base_partran_seq_ptr->numeric2ctl_cp(pars1);
			run1.update_ctl(tmp_ctl_par, obs1);
			double phi1 = run1.get_phi(DynamicRegularization::get_zero_reg_instance());
			double p0 = pars0[par_name_1];
			double p1 = pars1[par_name_1];
			// compute standard Morris Sensitivity on the global objective function
			double sen = (phi1 - phi0) / delta;
			fout_raw << par_name_1 << ",  " << phi1 << ",  " << phi0 << ",  " << p1 << ",  " << p0 << ", " << sen << endl;

			const auto &it_senmap = sen_map.find(par_name_1);
			if (it_senmap != sen_map.end())
			{
				it_senmap->second.add(sen);
			}

			//Compute sensitvities of indiviual observations
			obs_sen_file.add_sen_run_pair(par_name_1, p0, obs0, p1, obs1);
		}
	}
	// Add final run to obs_stats
	if (run1_ok)
	{
		for (const auto &i_obs : run_mngr_obs_name_vec)
		{
			auto it = obs1.find(i_obs);
			if (it != obs1.end() && it->second != Observations::no_data)
			{
				obs_stats_map[i_obs].add(obs0[i_obs]);
			}
		}
	}
	cout << endl;
	cout << "writing output files" << endl;
	// write standard Morris Sensitivity for the global objective function
	fout_morris << "parameter_name, n_samples, sen_mean, sen_mean_abs, sen_std_dev" << endl;
	for (const auto &it_par : adj_par_name_vec)
	{
		const auto &it_senmap = sen_map.find(it_par);
		if (it_senmap != sen_map.end())
		{
			fout_morris << it_par << ", " << it_senmap->second.comp_nsamples() << ", " << it_senmap->second.comp_mean() << ", " << it_senmap->second.comp_abs_mean() << ", " << sqrt(it_senmap->second.comp_var()) << endl;
		}
	}
	if (calc_morris_obs_sen)
	{
		// write standard Morris Sensitivity for individual observations
		ofstream &fout_mis = file_manager_ptr->open_ofile_ext("mio");
		calc_morris_obs(fout_mis, obs_sen_file);
		file_manager_ptr->close_file("mio");
	}

	if (calc_obs_sen)
	{
		ofstream &fout_mos = file_manager_ptr->open_ofile_ext("mos");
		////compute pooled standard deviations
		map<string, double> obs_2_sen_weight;
		{
			//Compute Pooled Standard Deviations
			map<string, vector<RunningStats> > tmp_pool_grps;
			for (const auto &it : obs_stats_map)
			{
				const string &obs_name = it.first;
				const string &obs_group = obs_info_ptr->get_group(obs_name);
				auto it_pg = group_2_pool_group_map.find(obs_group);
				if (it_pg != group_2_pool_group_map.end())
				{
					const string &pool_group = it_pg->second;
					if (tmp_pool_grps.find(pool_group) == tmp_pool_grps.end())
					{
						tmp_pool_grps[pool_group] = vector<RunningStats>();
					}
					tmp_pool_grps[pool_group].push_back(it.second);
				}
			}
			////compute pooled standard deviations
			map<string, double> obs_pooled_grp_std_dev;
			for (const auto &i_pgrp : tmp_pool_grps)
			{
				const string &pool_group = i_pgrp.first;
				double var_sum = 0;
				long int weight_sum = 0;
				for (const auto &istat : i_pgrp.second)
				{
					long int weight = istat.comp_nsamples() - 1;
					var_sum += weight * istat.comp_var();
					weight_sum += weight;
				}
				if (weight_sum > 0)
				{
					obs_pooled_grp_std_dev[pool_group] = sqrt(var_sum / weight_sum);
				}
			}
			for (const auto &iobs : obs_name_vec)
			{
				const string &obs_name = iobs;
				const string &obs_group = obs_info_ptr->get_group(obs_name);
				auto it_pg = group_2_pool_group_map.find(obs_group);
				if (it_pg != group_2_pool_group_map.end())
				{
					const string &pool_group = it_pg->second;
					if (obs_pooled_grp_std_dev.find(pool_group) != obs_pooled_grp_std_dev.end())
					{
						obs_2_sen_weight[iobs] = obs_pooled_grp_std_dev[pool_group];
					}
				}
			}
		}

		//compute parameter standard deviations
		map<string, double> par_std_dev;
		par_std_dev = calc_parameter_unif_std_dev();
		obs_sen_file.calc_pooled_obs_sen(fout_mos, obs_2_sen_weight, par_std_dev);
		file_manager_ptr->close_file("mos");
	}
	file_manager_ptr->close_file("msn");
	file_manager_ptr->close_file("raw");
}

void MorrisMethod::calc_morris_obs(ostream &fout, MorrisObsSenFile &morris_sen_file)
{
	// write standard Morris Sensitivity
	fout << "observation_name,parameter_name,n_samples,sen_mean,sen_mean_abs,sen_std_dev" << endl;

	for (const auto &i_obs : morris_sen_file.obs_names_vec)
	{
		//fout << "Method of Morris for observation: " << i_obs << endl;
		//fout << "parameter_name, n_samples, sen_mean, sen_mean_abs, sen_std_dev" << endl;
		//fout << i_obs;
		for (const auto &i_par : morris_sen_file.par_names_vec)
		{
			const auto &it_senmap = morris_sen_file.map_obs_stats.find(make_pair(i_par, i_obs));
			if (it_senmap != morris_sen_file.map_obs_stats.end())
			{
				fout << i_obs <<"," << i_par << "," << it_senmap->second.comp_nsamples() << "," << it_senmap->second.comp_mean() << "," << it_senmap->second.comp_abs_mean() << "," << sqrt(it_senmap->second.comp_var()) << endl;
			}
		}
		//fout << endl;
	}
}

MorrisMethod::~MorrisMethod(void)
{
}


/*
 * Copyright 2010, 2011, 2012
 * François Bleibel,
 * Olivier Stasse,
 * Florent Lamiraux
 * Nicolas Mansard
 *
 * CNRS/AIST
 *
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* --- SOT --- */
//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command.h>

#include <Eigen/LU>

#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/feature-point6d.hh>

using namespace std;
using namespace dynamicgraph;
using namespace dynamicgraph::sot;

#include <sot/core/factory.hh>
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoint6d, "FeaturePoint6d");

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

const FeaturePoint6d::ComputationFrameType
    FeaturePoint6d::COMPUTATION_FRAME_DEFAULT = FRAME_DESIRED;

FeaturePoint6d::FeaturePoint6d(const string &pointName)
    : FeatureAbstract(pointName), computationFrame_(COMPUTATION_FRAME_DEFAULT),
      positionSIN(NULL, "sotFeaturePoint6d(" + name +
                            ")::input(matrixHomo)::position"),
      velocitySIN(NULL,
                  "sotFeaturePoint6d(" + name + ")::input(vector)::velocity"),
      articularJacobianSIN(NULL, "sotFeaturePoint6d(" + name +
                                     ")::input(matrix)::Jq"),
      error_th_(), R_(), Rref_(), Rt_(), Rreft_(), P_(3, 3), Pinv_(3, 3),
      accuracy_(1e-8) {
  jacobianSOUT.addDependency(positionSIN);
  jacobianSOUT.addDependency(articularJacobianSIN);

  errorSOUT.addDependency(positionSIN);

  signalRegistration(positionSIN << articularJacobianSIN);
  signalRegistration(errordotSOUT << velocitySIN);
  errordotSOUT.setFunction(
      boost::bind(&FeaturePoint6d::computeErrordot, this, _1, _2));
  errordotSOUT.addDependency(velocitySIN);
  errordotSOUT.addDependency(positionSIN);
  errordotSOUT.addDependency(errorSOUT);

  // Commands
  //
  {
    using namespace dynamicgraph::command;
    std::string docstring;
    // Set computation frame
    docstring = "Set computation frame\n"
                "\n"
                "  Input:\n"
                "    a string: 'current' or 'desired'.\n"
                "      If 'current', the error is defined as the rotation "
                "vector (VectorUTheta)\n"
                "      corresponding to the position of the reference in the "
                "current frame:\n"
                "                         -1 *\n"
                "        error = utheta (M  M )\n"
                "      If 'desired',      *-1\n"
                "        error = utheta (M   M)\n";
    addCommand("frame",
               new dynamicgraph::command::Setter<FeaturePoint6d, std::string>(
                   *this, &FeaturePoint6d::computationFrame, docstring));
    docstring = "Get frame of computation of the error\n"
                "\n"
                "  See command 'frame' for definition.\n";
    addCommand("getFrame",
               new dynamicgraph::command::Getter<FeaturePoint6d, std::string>(
                   *this, &FeaturePoint6d::computationFrame, docstring));
    addCommand(
        "keep",
        makeCommandVoid0(
            *this, &FeaturePoint6d::servoCurrentPosition,
            docCommandVoid0(
                "modify the desired position to servo at current pos.")));
  }
}

void FeaturePoint6d::addDependenciesFromReference(void) {
  assert(isReferenceSet());
  errorSOUT.addDependency(getReference()->positionSIN);
  jacobianSOUT.addDependency(getReference()->positionSIN);
}

void FeaturePoint6d::removeDependenciesFromReference(void) {
  assert(isReferenceSet());
  errorSOUT.removeDependency(getReference()->positionSIN);
  jacobianSOUT.removeDependency(getReference()->positionSIN);
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
void FeaturePoint6d::computationFrame(const std::string &inFrame) {
  if (inFrame == "current")
    computationFrame_ = FRAME_CURRENT;
  else if (inFrame == "desired")
    computationFrame_ = FRAME_DESIRED;
  else {
    std::string msg("FeaturePoint6d::computationFrame: " + inFrame +
                    ": invalid argument,\n"
                    "expecting 'current' or 'desired'");
    throw ExceptionFeature(ExceptionFeature::GENERIC, msg);
  }
}

/// \brief Get computation frame
std::string FeaturePoint6d::computationFrame() const {
  switch (computationFrame_) {
  case FRAME_CURRENT:
    return "current";
  case FRAME_DESIRED:
    return "desired";
  }
  assert(false && "Case not handled");
  return "Case not handled";
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

unsigned int &FeaturePoint6d::getDimension(unsigned int &dim, int time) {
  sotDEBUG(25) << "# In {" << endl;

  const Flags &fl = selectionSIN.access(time);

  dim = 0;
  for (int i = 0; i < 6; ++i)
    if (fl(i))
      dim++;

  sotDEBUG(25) << "# Out }" << endl;
  return dim;
}

/** Compute the interaction matrix from a subset of
 * the possible features.
 */
Matrix &FeaturePoint6d::computeJacobian(Matrix &J, int time) {
  sotDEBUG(15) << "# In {" << endl;

  const Matrix &Jq = articularJacobianSIN(time);
  const int &dim = dimensionSOUT(time);
  const Flags &fl = selectionSIN(time);

  sotDEBUG(25) << "dim = " << dimensionSOUT(time) << " time:" << time << " "
               << dimensionSOUT.getTime() << " " << dimensionSOUT.getReady()
               << endl;
  sotDEBUG(25) << "selec = " << selectionSIN(time) << " time:" << time << " "
               << selectionSIN.getTime() << " " << selectionSIN.getReady()
               << endl;

  sotDEBUG(15) << "Dimension=" << dim << std::endl;

  const Matrix::Index cJ = Jq.cols();
  J.resize(dim, cJ);
  Matrix LJq(6, cJ);

  if (FRAME_CURRENT == computationFrame_) {
    /* The Jacobian on rotation is equal to Jr = - hdRh Jr6d.
     * The Jacobian in translation is equalt to Jt = [hRw(wthd-wth)]x Jr - Jt.
     */
    const MatrixHomogeneous &wMh = positionSIN(time);
    MatrixRotation wRh;
    wRh = wMh.linear();
    MatrixRotation wRhd;
    Vector hdth(3), Rhdth(3);

    if (isReferenceSet()) {
      const MatrixHomogeneous &wMhd = getReference()->positionSIN(time);
      wRhd = wMhd.linear();
      for (unsigned int i = 0; i < 3; ++i)
        hdth(i) = wMhd(i, 3) - wMh(i, 3);
    } else {
      wRhd.setIdentity();
      for (unsigned int i = 0; i < 3; ++i)
        hdth(i) = -wMh(i, 3);
    }
    Rhdth = (wRh.inverse()) * hdth;
    MatrixRotation hdRh;
    hdRh = (wRhd.inverse()) * wRh;

    Matrix Lx(6, 6);
    for (unsigned int i = 0; i < 3; i++) {
      for (unsigned int j = 0; j < 3; j++) {
        if (i == j) {
          Lx(i, j) = -1;
        } else {
          Lx(i, j) = 0;
        }
        Lx(i + 3, j) = 0;
        Lx(i + 3, j + 3) = -hdRh(i, j);
      }
    }
    const double &X = Rhdth(0), &Y = Rhdth(1), &Z = Rhdth(2);
    Lx(0, 4) = -Z;
    Lx(0, 5) = Y;
    Lx(1, 3) = Z;
    Lx(1, 5) = -X;
    Lx(2, 3) = -Y;
    Lx(2, 4) = X;
    Lx(0, 3) = 0;
    Lx(1, 4) = 0;
    Lx(2, 5) = 0;
    sotDEBUG(15) << "Lx= " << Lx << endl;

    LJq = Lx * Jq;
  } else {
    /* The Jacobian in rotation is equal to Jr = hdJ = hdRh Jr.
     * The Jacobian in translation is equal to Jr = hdJ = hdRh Jr. */
    const MatrixHomogeneous &wMh = positionSIN(time);
    MatrixRotation wRh;
    wRh = wMh.linear();
    MatrixRotation hdRh;

    if (isReferenceSet()) {
      const MatrixHomogeneous &wMhd = getReference()->positionSIN(time);
      MatrixRotation wRhd;
      wRhd = wMhd.linear();
      hdRh = (wRhd.inverse()) * wRh;
    } else {
      hdRh = wRh;
    }

    LJq.fill(0);
    for (unsigned int i = 0; i < 3; i++)
      for (unsigned int j = 0; j < cJ; j++) {
        for (unsigned int k = 0; k < 3; k++) {
          LJq(i, j) += hdRh(i, k) * Jq(k, j);
          LJq(i + 3, j) += hdRh(i, k) * Jq(k + 3, j);
        }
      }
  }

  /* Select the active line of Jq. */
  unsigned int rJ = 0;
  for (unsigned int r = 0; r < 6; ++r)
    if (fl(r)) {
      for (unsigned int c = 0; c < cJ; ++c)
        J(rJ, c) = LJq(r, c);
      rJ++;
    }

  sotDEBUG(15) << "# Out }" << endl;
  return J;
}

#define SOT_COMPUTE_H1MH2(wMh, wMhd, hMhd)                                     \
  {                                                                            \
    MatrixHomogeneous hMw;                                                     \
    hMw = wMh.inverse(Eigen::Affine);                                          \
    sotDEBUG(15) << "hMw = " << hMw << endl;                                   \
    hMhd = hMw * wMhd;                                                         \
    sotDEBUG(15) << "hMhd = " << hMhd << endl;                                 \
  }

/** Compute the error between two visual features from a subset
 * a the possible features.
 */
Vector &FeaturePoint6d::computeError(Vector &error, int time) {
  sotDEBUGIN(15);

  const Flags &fl = selectionSIN(time);
  const MatrixHomogeneous &wMh = positionSIN(time);
  sotDEBUG(15) << "wMh = " << wMh << endl;

  /* Computing only translation:                                        *
   * trans( hMw wMhd ) = htw + hRw wthd                                 *
   *                   = -hRw wth + hrW wthd                            *
   *                   = hRw ( wthd - wth )                             *
   * The second line is obtained by writting hMw as the inverse of wMh. */

  MatrixHomogeneous hMhd;
  if (isReferenceSet()) {
    const MatrixHomogeneous &wMhd = getReference()->positionSIN(time);
    sotDEBUG(15) << "wMhd = " << wMhd << endl;
    switch (computationFrame_) {
    case FRAME_CURRENT:
      SOT_COMPUTE_H1MH2(wMh, wMhd, hMhd);
      break;
    case FRAME_DESIRED:
      SOT_COMPUTE_H1MH2(wMhd, wMh, hMhd);
      break; // Compute hdMh indeed.
    };
  } else {
    switch (computationFrame_) {
    case FRAME_CURRENT:
      hMhd = wMh.inverse();
      break;
    case FRAME_DESIRED:
      hMhd = wMh;
      break; // Compute hdMh indeed.
    };
  }

  sotDEBUG(25) << "dim = " << dimensionSOUT(time) << " time:" << time << " "
               << dimensionSOUT.getTime() << " " << dimensionSOUT.getReady()
               << endl;
  sotDEBUG(25) << "selec = " << selectionSIN(time) << " time:" << time << " "
               << selectionSIN.getTime() << " " << selectionSIN.getReady()
               << endl;

  error.resize(dimensionSOUT(time));
  unsigned int cursor = 0;
  for (unsigned int i = 0; i < 3; ++i) {
    if (fl(i))
      error(cursor++) = hMhd(i, 3);
  }

  if (fl(3) || fl(4) || fl(5)) {
    MatrixRotation hRhd;
    hRhd = hMhd.linear();
    error_th_.fromRotationMatrix(hRhd);
    for (unsigned int i = 0; i < 3; ++i) {
      if (fl(i + 3))
        error(cursor++) = error_th_.angle() * error_th_.axis()(i);
    }
  }

  sotDEBUGOUT(15);
  return error;
}

void FeaturePoint6d::inverseJacobianRodrigues() {
  const double &r1 = error_th_.angle() * error_th_.axis()(0);
  const double &r2 = error_th_.angle() * error_th_.axis()(1);
  const double &r3 = error_th_.angle() * error_th_.axis()(2);
  double r1_2 = r1 * r1;
  double r2_2 = r2 * r2;
  double r3_2 = r3 * r3;
  double r1_3 = r1 * r1_2;
  double r2_3 = r2 * r2_2;
  double r3_3 = r3 * r3_2;
  double r1_4 = r1_2 * r1_2;
  double r2_4 = r2_2 * r2_2;
  double r3_4 = r3_2 * r3_2;
  double norm_2 = r3_2 + r2_2 + r1_2;

  if (norm_2 < accuracy_) {
    P_.setIdentity();
  } else {
    // This code has been generated by maxima software
    P_(0, 0) =
        ((r3_2 + r2_2) * sqrt(norm_2) * sin(sqrt(norm_2)) + r1_2 * r3_2 +
         r1_2 * r2_2 + r1_4) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(0, 1) =
        -(r1 * r2 * sqrt(norm_2) * sin(sqrt(norm_2)) +
          (r3_3 + (r2_2 + r1_2) * r3) * cos(sqrt(norm_2)) - r3_3 -
          r1 * r2 * r3_2 + (-r2_2 - r1_2) * r3 - r1 * r2_3 - r1_3 * r2) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(0, 2) =
        -(r1 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) +
          (-r2 * r3_2 - r2_3 - r1_2 * r2) * cos(sqrt(norm_2)) - r1 * r3_3 +
          r2 * r3_2 + (-r1 * r2_2 - r1_3) * r3 + r2_3 + r1_2 * r2) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(1, 0) =
        -(r1 * r2 * sqrt(norm_2) * sin(sqrt(norm_2)) +
          ((-r2_2 - r1_2) * r3 - r3_3) * cos(sqrt(norm_2)) + r3_3 -
          r1 * r2 * r3_2 + (r2_2 + r1_2) * r3 - r1 * r2_3 - r1_3 * r2) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(1, 1) =
        ((r3_2 + r1_2) * sqrt(norm_2) * sin(sqrt(norm_2)) + r2_2 * r3_2 + r2_4 +
         r1_2 * r2_2) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(1, 2) =
        -(r2 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) +
          (r1 * r3_2 + r1 * r2_2 + r1_3) * cos(sqrt(norm_2)) - r2 * r3_3 -
          r1 * r3_2 + (-r2_3 - r1_2 * r2) * r3 - r1 * r2_2 - r1_3) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(2, 0) =
        -(r1 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) +
          (r2 * r3_2 + r2_3 + r1_2 * r2) * cos(sqrt(norm_2)) - r1 * r3_3 -
          r2 * r3_2 + (-r1 * r2_2 - r1_3) * r3 - r2_3 - r1_2 * r2) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(2, 1) =
        -(r2 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) +
          (-r1 * r3_2 - r1 * r2_2 - r1_3) * cos(sqrt(norm_2)) - r2 * r3_3 +
          r1 * r3_2 + (-r2_3 - r1_2 * r2) * r3 + r1 * r2_2 + r1_3) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(2, 2) =
        ((r2_2 + r1_2) * sqrt(norm_2) * sin(sqrt(norm_2)) + r3_4 +
         (r2_2 + r1_2) * r3_2) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
  }
  Pinv_ = P_.inverse();
}

Vector &FeaturePoint6d::computeErrordot(Vector &errordot, int time) {
  if (isReferenceSet()) {
    const Vector &velocity = getReference()->velocitySIN(time);
    const MatrixHomogeneous &M = positionSIN(time);
    const MatrixHomogeneous &Mref = getReference()->positionSIN(time);
    // Linear velocity if the reference frame
    v_(0) = velocity(0);
    v_(1) = velocity(1);
    v_(2) = velocity(2);
    // Angular velocity if the reference frame
    omega_(0) = velocity(3);
    omega_(1) = velocity(4);
    omega_(2) = velocity(5);
    R_ = M.linear();
    t_ = M.translation();
    Rt_ = R_.transpose();
    Rref_ = Mref.linear();
    tref_ = Mref.translation();
    Rreft_ = Rref_.transpose();
    errorSOUT.recompute(time);
    inverseJacobianRodrigues();
    switch (computationFrame_) {
    case FRAME_CURRENT:
      // \dot{e}_{t} = R^{T} v
      errordot_t_ = Rt_ * v_;
      // \dot{e}_{\theta} = P^{-1}(e_{theta})R^{*T}\omega
      Rreftomega_ = Rreft_ * omega_;
      errordot_th_ = Pinv_ * Rreftomega_;
      break;
    case FRAME_DESIRED:
      errordot_t_ = Rreft_ * (omega_.cross(tref_ - t_) - v_);
      errordot_th_ = -Pinv_ * (Rt_ * omega_);
      break;
    }
  } else {
    errordot_t_.setZero();
    errordot_th_.setZero();
  }

  const Flags &fl = selectionSIN(time);
  errordot.resize(dimensionSOUT(time));
  unsigned int cursor = 0;
  for (unsigned int i = 0; i < 3; ++i) {
    if (fl(i)) {
      errordot(cursor++) = errordot_t_(i);
    }
  }

  if (fl(3) || fl(4) || fl(5)) {
    for (unsigned int i = 0; i < 3; ++i) {
      if (fl(i + 3)) {
        errordot(cursor++) = errordot_th_(i);
      }
    }
  }

  return errordot;
}

/* Modify the value of the reference (sdes) so that it corresponds
 * to the current position. The effect on the servo is to maintain the
 * current position and correct any drift. */
void FeaturePoint6d::servoCurrentPosition(void) {
  sotDEBUGIN(15);

  if (!isReferenceSet()) {
    sotERROR << "The reference is not set, this function should not be called"
             << std::endl;
    throw ExceptionFeature(
        ExceptionFeature::GENERIC,
        "The reference is not set, this function should not be called");
  }
  getReference()->positionSIN = positionSIN.accessCopy();

  sotDEBUGOUT(15);
}

static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
void FeaturePoint6d::display(std::ostream &os) const {
  os << "Point6d <" << name << ">: (";

  try {
    const Flags &fl = selectionSIN.accessCopy();
    bool first = true;
    for (int i = 0; i < 6; ++i)
      if (fl(i)) {
        if (first) {
          first = false;
        } else {
          os << ",";
        }
        os << featureNames[i];
      }
    os << ") ";
  } catch (ExceptionAbstract e) {
    os << " selectSIN not set.";
  }
}


/* Copyright (c) 2017, Waterloo Autonomous Vehicles Laboratory (WAVELab),
 * Waterloo Intelligent Systems Engineering Lab (WISELab),
 * University of Waterloo.
 *
 * Refer to the accompanying LICENSE file for license information.
 *
 * ############################################################################
 ******************************************************************************
 |                                                                            |
 |                         /\/\__/\_/\      /\_/\__/\/\                       |
 |                         \          \____/          /                       |
 |                          '----________________----'                        |
 |                              /                \                            |
 |                            O/_____/_______/____\O                          |
 |                            /____________________\                          |
 |                           /    (#UNIVERSITY#)    \                         |
 |                           |[**](#OFWATERLOO#)[**]|                         |
 |                           \______________________/                         |
 |                            |_""__|_,----,_|__""_|                          |
 |                            ! !                ! !                          |
 |                            '-'                '-'                          |
 |       __    _   _  _____  ___  __  _  ___  _    _  ___  ___   ____  ____   |
 |      /  \  | | | ||_   _|/ _ \|  \| |/ _ \| \  / |/ _ \/ _ \ /     |       |
 |     / /\ \ | |_| |  | |  ||_||| |\  |||_|||  \/  |||_||||_|| \===\ |====   |
 |    /_/  \_\|_____|  |_|  \___/|_| \_|\___/|_|\/|_|\___/\___/ ____/ |____   |
 |                                                                            |
 ******************************************************************************
 * ############################################################################
 *
 * File: pose_cov_comp.cpp
 * Desc: File containing the impl. for pose composition with uncertainty
 * Auth: Chunshang Li and Jordan Hu
 *
 * ############################################################################
 */

#include "wave/utils/pose_cov_comp.hpp"
#include <Eigen/Dense>

namespace wave {

PoseWithCovariance::PoseWithCovariance() {
    this->position.setZero();
    this->rotation_matrix.setIdentity();
    this->covariance.setZero();
}

PoseWithCovariance::PoseWithCovariance(Vector6 &p, Matrix6x6 &cov) {
    Vector3 ypr;

    ypr << p[5], p[4], p[3];

    this->position = p.block<3, 1>(0, 0);
    this->rotation_matrix = pose_comp::yprToRotMatrix(ypr);
    this->covariance = cov;
}

PoseWithCovariance::PoseWithCovariance(Vector3 &p,
                                       Matrix3x3 &r,
                                       Matrix6x6 &cov) {
    this->position = p;
    this->rotation_matrix = r;
    this->covariance = cov;
}

Vector3 PoseWithCovariance::getPosition() const {
    return this->position;
}

Vector3 PoseWithCovariance::getYPR() const {
    Vector3 ypr = pose_comp::rotMatrixToYPR(this->rotation_matrix);
    return ypr;
}

Eigen::Quaterniond PoseWithCovariance::getQuaternion() const {
    Eigen::Quaterniond q = pose_comp::rotMatrixToQuat(this->rotation_matrix);

    q.normalize();

    return q;
}

Vector7 PoseWithCovariance::getPoseQuaternion() const {
    Vector7 pose;
    Eigen::Quaterniond q = this->getQuaternion();
    Vector4 q_coeffs;

    q_coeffs << q.coeffs().w(), q.coeffs().x(), q.coeffs().y(), q.coeffs().z();
    pose << this->position, q_coeffs;

    return pose;
}

Eigen::Affine3d PoseWithCovariance::getTransformMatrix() const {
    Eigen::Affine3d T_m;

    T_m.translation() = this->position;
    T_m.linear() = this->rotation_matrix;

    return T_m;
}

PoseWithCovariance composePose(PoseWithCovariance &p1, PoseWithCovariance &p2) {
    PoseWithCovariance r;  // store all the results

    // Use Eigen Transform to transform poses
    // This is the same as implementing Equation (5.5)
    Eigen::Affine3d T_p1, T_p2, T_r;
    T_p1 = p1.getTransformMatrix();
    T_p2 = p2.getTransformMatrix();
    T_r = T_p1 * T_p2;

    r.position = T_r.translation();
    r.rotation_matrix = T_r.rotation();

    // compute the covariances
    // p6 = [x y z roll pitch yaw]', transformation using YPR
    // p7 = [x y z qr, qx, qy, qz]', transformation using Quaternion, qr is qw

    // get covariances from PoseWithCovariance objects
    Matrix6x6 cov_p1 = p1.covariance, cov_p2 = p2.covariance;

    // pR7 is the composed pose in p7 form
    // p17 is the p1 in p7 form, p27 is p2 in p7 form, p16 is p1 in p6 form,
    // p26 is p2 in p6 form
    Vector7 pR7, p17, p27;
    Vector6 p16, p26;

    pR7 = r.getPoseQuaternion();
    p17 = p1.getPoseQuaternion();
    p27 = p2.getPoseQuaternion();

    p16 << p1.getPosition(), pose_comp::quatToYPR(p1.getQuaternion());
    p26 << p2.getPosition(), pose_comp::quatToYPR(p2.getQuaternion());

    // Equation (5.3)
    Matrix6x7 jacobian_p7_to_p6 = jacobian_p7_to_p6_wrt_p(pR7);
    Matrix7x7 jacobian_p7_p7_composition =
      jacobian_p7_p7_Composition_wrt_p1(p17, p27);
    Matrix7x6 jacobian_p6_to_p7 = jacobian_p6_to_p7_wrt_p(p16);
    Matrix6x6 dfpc_dp =
      jacobian_p7_to_p6 * jacobian_p7_p7_composition * jacobian_p6_to_p7;

    // Equation (5.4)
    jacobian_p7_p7_composition = jacobian_p7_p7_Composition_wrt_p2(p17, p27);
    jacobian_p6_to_p7 = jacobian_p6_to_p7_wrt_p(p26);
    Matrix6x6 dfpc_dq =
      jacobian_p7_to_p6 * jacobian_p7_p7_composition * jacobian_p6_to_p7;

    // Equation (5.2)
    // putting everything together
    Matrix6x6 final_cov = dfpc_dp * cov_p1 * dfpc_dp.transpose() +
                          dfpc_dq * cov_p2 * dfpc_dq.transpose();

    r.covariance = final_cov;

    return r;
}

/// the jacobian of quaternion normalization function
/// quat in the form of [qr, qx, qt, qz]
/// Equation (1.7)
Matrix4x4 jacobian_Quat_Norm_wrt_q(const Vector4 &q) {
    const double &qr = q(0), &qx = q(1), &qy = q(2), &qz = q(3);
    Matrix4x4 m;

    // See Equation (1.7)
    double k = 1 / pow(qr * qr + qx * qx + qy * qy + qz * qz, 1.5);

    // clang-format off
    m << qx * qx + qy * qy + qz * qz, -qr * qx, -qr * qy, -qr * qz,
        -qx * qr, qr * qr + qy * qy + qz * qz, -qx * qy, -qx * qz,
        -qy * qr, -qy * qx, qr * qr + qx * qx + qz * qz, -qy * qz,
        -qz * qr, -qz * qx, -qz * qy, qr * qr + qx * qx + qy * qy;
    // clang-format on

    m = m * k;

    return m;
}

// the jacobian of normalized quaternion to rpy function
// Equation (2.9) to Equation (2.10)
Matrix3x4 jacobian_Quat_Norm_to_Rpy_wrt_q(const Vector4 &q) {
    const double &qr = q(0), &qx = q(1), &qy = q(2), &qz = q(3);

    Matrix3x4 m;

    // Equation (2.9)
    double delta = qr * qy - qx * qz;

    // handle special (rare) cases for when |delta| = 0.5
    if (fabs(delta - 0.5) < 1e-10) {  // delta = 0.5
                                      // clang-format off
        m << (2*qx)/(qr*qr + qx*qx), -(2*qr)/(qr*qr + qx*qx), 0, 0,
                                    0,                     0, 0, 0,
                                    0,                     0, 0, 0;

        // clang-format on

    } else if (fabs(delta + 0.5) < 1e-10) {  // delta = -0.5
                                             // clang-format off
        m << -(2*qx)/(qr*qr + qx*qx), (2*qr)/(qr*qr + qx*qx), 0, 0,
                                     0,                    0, 0, 0,
                                     0,                    0, 0, 0;
                                             // clang-format on
    } else {
        // Equation (2.10)
        // Jacobian obtained symbolically from SymPy by taking the derivative
        // of this below
        //     roll  = [[        2*(qr*qz + qx*qy)/(-2*qy*qy - 2*qz*qz + 1)],
        //     pitch =  [                           asin(2*qr*qy - 2*qx*qz)],
        //     yaw   =  [atan((2*qr*qx + 2*qy*qz)/(-2*qx*qx - 2*qy*qy + 1))]]
        // with respect to qr, qx, qy, qz using SymPy

        // just for convenience
        auto sq = [](double x) { return x * x; };

        // clang-format off
        m <<
            2*qz*(-2*qy*qy - 2*qz*qz + 1)/(sq(2*qr*qz + 2*qx*qy) + sq(-2*qy*qy - 2*qz*qz + 1)),
            2*qy*(-2*qy*qy - 2*qz*qz + 1)/(sq(2*qr*qz + 2*qx*qy) + sq(-2*qy*qy - 2*qz*qz + 1)),
            2*qx*(-2*qy*qy - 2*qz*qz + 1)/(sq(2*qr*qz + 2*qx*qy) + sq(-2*qy*qy - 2*qz*qz + 1)) - 4*qy*(-2*qr*qz - 2*qx*qy)/(sq(2*qr*qz + 2*qx*qy) + sq(-2*qy*qy - 2*qz*qz + 1)),
            2*qr*(-2*qy*qy - 2*qz*qz + 1)/(sq(2*qr*qz + 2*qx*qy) + sq(-2*qy*qy - 2*qz*qz + 1)) - 4*qz*(-2*qr*qz - 2*qx*qy)/(sq(2*qr*qz + 2*qx*qy) + sq(-2*qy*qy - 2*qz*qz + 1)),

            2*qy/sqrt(-sq(2*qr*qy - 2*qx*qz) + 1),
            -2*qz/sqrt(-sq(2*qr*qy - 2*qx*qz) + 1),
            2*qr/sqrt(-sq(2*qr*qy - 2*qx*qz) + 1),
            -2*qx/sqrt(-sq(2*qr*qy - 2*qx*qz) + 1),

            2*qx*(-2*qx*qx - 2*qy*qy + 1)/(sq(2*qr*qx + 2*qy*qz) + sq(-2*qx*qx - 2*qy*qy + 1)),
            2*qr*(-2*qx*qx - 2*qy*qy + 1)/(sq(2*qr*qx + 2*qy*qz) + sq(-2*qx*qx - 2*qy*qy + 1)) - 4*qx*(-2*qr*qx - 2*qy*qz)/(sq(2*qr*qx + 2*qy*qz) + sq(-2*qx*qx - 2*qy*qy + 1)),
            -4*qy*(-2*qr*qx - 2*qy*qz)/(sq(2*qr*qx + 2*qy*qz) + sq(-2*qx*qx - 2*qy*qy + 1)) + 2*qz*(-2*qx*qx - 2*qy*qy + 1)/(sq(2*qr*qx + 2*qy*qz) + sq(-2*qx*qx - 2*qy*qy + 1)),
            2*qy*(-2*qx*qx - 2*qy*qy + 1)/(sq(2*qr*qx + 2*qy*qz) + sq(-2*qx*qx - 2*qy*qy + 1));

        // clang-format on
    }

    return m;
}

/// the jacobian of p7 to p6 conversion
/// Equation (2.12)
Matrix6x7 jacobian_p7_to_p6_wrt_p(const Vector7 &p) {
    Matrix6x7 r = Matrix6x7::Zero();

    // Equation (2.12)
    // bottom left 3x3 is zero
    // top right 3x4 is zero
    // top left 3x3 is identity
    r(0, 0) = 1;
    r(1, 1) = 1;
    r(2, 2) = 1;

    // This function takes the un-normalized quaternion
    Matrix4x4 jacobian_quat_norm =
      jacobian_Quat_Norm_wrt_q(p.block<4, 1>(3, 0));

    Matrix3x4 jacobian_quat_norm_to_rpy =
      jacobian_Quat_Norm_to_Rpy_wrt_q(p.block<4, 1>(3, 0));

    r.block<3, 4>(3, 3) = jacobian_quat_norm_to_rpy * jacobian_quat_norm;

    return r;
}

// jacobian of composing a point to a p7
// Equation (3.8)
Matrix3x7 jacobian_p7_Point_Composition_wrt_p(const Vector7 &p,
                                              const Vector3 &a) {
    Matrix3x7 m = Matrix3x7::Zero();

    // The 3x3 on the top left is the identity matrix
    m.block<3, 3>(0, 0) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;

    // Equation (3.9)
    double ax = a(0), ay = a(1), az = a(2);
    double qr = p(3), qx = p(4), qy = p(5), qz = p(6);

    // clang-format off
    m.block<3, 4>(0, 3) <<
        -qz*ay+qy*az, qy*ay+qz*az, -2*qy*ax+qx*ay+qr*az, -2*qz*ax-qr*ay+qx*az,
        qz*ax-qx*az, qy*ax-2*qx*ay-qr*az, qx*ax+qz*az, qr*ax-2*qz*ay+qy*az,
        -qy*ax+qx*ay, qz*ax+qr*ay-2*qx*az, -qr*ax+qz*ay-2*qy*az, qx*ax+qy*ay;
    // clang-format on

    m.block<3, 4>(0, 3) *= 2;

    // applying jacobian normalization
    Matrix4x4 jacobian_quat_norm =
      jacobian_Quat_Norm_wrt_q(p.block<4, 1>(3, 0));
    m.block<3, 4>(0, 3) *= jacobian_quat_norm;

    return m;
}

// jacobian of the composition of p7 poses
// Equation (5.8)
Matrix7x7 jacobian_p7_p7_Composition_wrt_p1(const Vector7 &p1,
                                            const Vector7 &p2) {
    Matrix7x7 m = Matrix7x7::Zero();

    Vector4 q2 = p2.block<4, 1>(3, 0);
    double qr2 = q2(0), qx2 = q2(1), qy2 = q2(2), qz2 = q2(3);

    // clang-format off
    m.block<4, 4>(3, 3) << qr2, -qx2, -qy2, -qz2,
                           qx2, qr2, qz2, -qy2,
                           qy2, -qz2, qr2, qx2,
                           qz2, qy2, -qx2, qr2;
    // clang-format on

    // Note: this quaternion normalization jacobian matrix is not present in
    // the book's formulation, but it should be there if a jacobian
    // normalization is performed on the input
    Matrix4x4 jacobian_quat_norm =
      jacobian_Quat_Norm_wrt_q(p1.block<4, 1>(3, 0));
    m.block<4, 4>(3, 3) *= jacobian_quat_norm;

    m.block<3, 7>(0, 0) =
      jacobian_p7_Point_Composition_wrt_p(p1, p2.block<3, 1>(0, 0));

    return m;
}

// jacobian of composing a point to a p7
// Equation (3.10)
Matrix3x3 jacobian_p7_Point_Composition_wrt_a(const Vector7 &p,
                                              const Vector3 &a) {
    Matrix3x3 m = Matrix3x3::Zero();

    double qr = p(3), qx = p(4), qy = p(5), qz = p(6);

    // clang-format off
    m << 0.5 - qy*qy - qz*qz , qx*qy - qr*qz, qr*qy + qx*qz,
         qr*qz + qx*qy, 0.5 - qx*qx - qz*qz, qy*qz - qr*qx,
         qx*qz - qr*qy, qr*qx + qy*qz, 0.5 - qx*qx - qy*qy;
    // clang-format on

    m *= 2;

    // To avoid warnings
    (void) a;

    return m;
}

// jacobian of the composition of p7 poses
// Equation (5.9)
Matrix7x7 jacobian_p7_p7_Composition_wrt_p2(const Vector7 &p1,
                                            const Vector7 &p2) {
    Matrix7x7 m = Matrix7x7::Zero();

    Vector4 q1 = p1.block<4, 1>(3, 0);
    double qr1 = q1(0), qx1 = q1(1), qy1 = q1(2), qz1 = q1(3);

    // clang-format off
    m.block<4, 4>(3, 3) << qr1, -qx1, -qy1, -qz1,
                           qx1, qr1, -qz1, qy1,
                           qy1, qz1, qr1, -qx1,
                           qz1, -qy1, qx1, qr1;
    // clang-format on

    // Note: this quaternion normalization jacobian matrix is not present in
    // the book's formulation, but it should be there if a jacobian
    // normalization is performed on the input
    Matrix4x4 jacobian_quat_norm =
      jacobian_Quat_Norm_wrt_q(p2.block<4, 1>(3, 0));
    m.block<4, 4>(3, 3) *= jacobian_quat_norm;

    m.block<3, 3>(0, 0) =
      jacobian_p7_Point_Composition_wrt_a(p1, p2.block<3, 1>(0, 0));

    return m;
}

// jacobian of converting a p6 to a p7
// Equation (2.8)
Matrix7x6 jacobian_p6_to_p7_wrt_p(const Vector6 &p) {
    Matrix7x6 m = Matrix7x6::Zero();

    // top left corner is a identity matrix
    // the bottom left 4x3 and top right 3x3 is zero
    m.block<3, 3>(0, 0) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;

    double ccc, ccs, csc, scc, ssc, sss, scs, css;
    double roll = p(5), pitch = p(4), yaw = p(3);
    ccc = cos(roll / 2) * cos(pitch / 2) * cos(yaw / 2);
    ccs = cos(roll / 2) * cos(pitch / 2) * sin(yaw / 2);
    csc = cos(roll / 2) * sin(pitch / 2) * cos(yaw / 2);
    scs = sin(roll / 2) * cos(pitch / 2) * sin(yaw / 2);
    css = cos(roll / 2) * sin(pitch / 2) * sin(yaw / 2);
    scc = sin(roll / 2) * cos(pitch / 2) * cos(yaw / 2);
    ssc = sin(roll / 2) * sin(pitch / 2) * cos(yaw / 2);
    sss = sin(roll / 2) * sin(pitch / 2) * sin(yaw / 2);

    // clang-format off
    m.block<4, 3>(3, 3) <<
        (ssc - ccs) / 2.0, (scs - csc) / 2.0, (css - scc) / 2.0,
        -(csc + scs) / 2.0, -(ssc + ccs) / 2.0, (ccc + sss) / 2.0,
        (scc - css) / 2.0, (ccc - sss) / 2.0, (ccs - ssc) / 2.0,
        (ccc + sss) / 2.0, -(css + scc) / 2.0, -(csc + scs) / 2.0;
    // clang-format on

    return m;
}
}  // namespace wave

namespace wave {
namespace pose_comp {

Vector3 quatToYPR(const Eigen::Quaterniond &q) {
    Vector3 v;
    double qr = q.coeffs().w(), qx = q.coeffs().x(), qy = q.coeffs().y(),
           qz = q.coeffs().z();
    double delta = qr * qy - qx * qz;  // discriminant of normalized quaternion
    double roll, pitch, yaw;

    // Check for special cases when abs(delta) =~ 0.5 (Equation (2.10))
    if (fabs(delta - 0.5) < 1e-6) {
        yaw = -2 * atan2(qx, qr);
        pitch = M_PI / 2;
        roll = 0;

    } else if (fabs(delta + 0.5) < 1e-6) {
        yaw = 2 * atan2(qx, qr);
        pitch = -M_PI / 2;
        roll = 0;
    } else {
        yaw = atan2(2 * (qr * qz + qx * qy), (1 - 2 * (qy * qy + qz * qz)));
        pitch = asin(2 * delta);
        roll = atan2(2 * (qr * qx + qy * qz), (1 - 2 * (qx * qx + qy * qy)));
    }

    v(0) = yaw;
    v(1) = pitch;
    v(2) = roll;

    return v;
}

Eigen::Quaterniond yprToQuat(const Vector3 &ypr) {
    Eigen::Quaterniond q;
    double qr, qx, qy, qz;
    double half_y = ypr(0) / 2, half_p = ypr(1) / 2, half_r = ypr(2) / 2;
    double cy = cos(half_y), cp = cos(half_p), cr = cos(half_r),
           sy = sin(half_y), sp = sin(half_p), sr = sin(half_r);

    qr = cr * cp * cy + sr * sp * sy;
    qx = sr * cp * cy - cr * sp * sy;
    qy = cr * sp * cy + sr * cp * sy;
    qz = cr * cp * sy - sr * sp * cy;

    q.coeffs() << qx, qy, qz, qr;

    return q;
}

Matrix3x3 yprToRotMatrix(const Vector3 &ypr) {
    double y = ypr(0), p = ypr(1), r = ypr(2);
    Matrix3x3 rotMatrix;
    double cy = cos(y), cp = cos(p), cr = cos(r), sy = sin(y), sp = sin(p),
           sr = sin(r);

    rotMatrix << cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr,
      sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, -sp, cp * sr,
      cp * cr;

    return rotMatrix;
}

Vector3 rotMatrixToYPR(const Matrix3x3 &p) {
    Vector3 ypr;
    double yaw, pitch, roll;
    double p11 = p(0, 0), p12 = p(0, 1), p13 = p(0, 2), p21 = p(1, 0),
           p22 = p(1, 1), p23 = p(1, 2), p31 = p(2, 0), p32 = p(2, 1),
           p33 = p(2, 2);

    pitch = atan2(-p31, sqrt(p11 * p11 + p21 * p21));

    if (fabs(pitch + M_PI * 1.0 / 2) < 1e-6) {
        yaw = atan2(-p23, -p13);
        roll = 0;
    } else if (fabs(pitch - M_PI * 1.0 / 2) < 1e-6) {
        yaw = atan2(p23, p13);
        roll = 0;
    } else {
        yaw = atan2(p21, p11);
        roll = atan2(p32, p33);
    }

    ypr << yaw, pitch, roll;

    // To avoid warnings
    (void) p12;
    (void) p22;

    return ypr;
}

Eigen::Quaterniond rotMatrixToQuat(const Matrix3x3 &p) {
    Eigen::Quaterniond q;
    Vector3 ypr;

    ypr = rotMatrixToYPR(p);
    q = yprToQuat(ypr);

    return q;
}
}  // namespace pose_comp
}  // namespace wave


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

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GELSD_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GELSD_HPP

#include <algorithm>

#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>
#include <boost/numeric/bindings/lapack/ilaenv.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

    namespace lapack {

        namespace detail {

            inline void gelsd(const integer_t m, const integer_t n, const integer_t nrhs,
                              float *a, const integer_t lda, float *b, const integer_t ldb,
                              float *s, const float rcond, integer_t *rank, float *work,
                              const integer_t lwork, integer_t *iwork, integer_t *info)
            {
                LAPACK_SGELSD(&m, &n, &nrhs, a, &lda, b, &ldb, s,
                              &rcond, rank, work, &lwork, iwork, info);
            }

            inline void gelsd(const integer_t m, const integer_t n, const integer_t nrhs,
                              double *a, const integer_t lda, double *b, const integer_t ldb,
                              double *s, const double rcond, integer_t *rank, double *work,
                              const integer_t lwork, integer_t *iwork, integer_t *info)
            {
                LAPACK_DGELSD(&m, &n, &nrhs, a, &lda, b, &ldb, s,
                              &rcond, rank, work, &lwork, iwork, info);
            }

            inline void gelsd(const integer_t m, const integer_t n, const integer_t nrhs,
                              traits::complex_f *a, const integer_t lda, traits::complex_f *b,
                              const integer_t ldb, float *s, const float rcond, integer_t *rank,
                              traits::complex_f *work, const integer_t lwork, float *rwork,
                              integer_t *iwork, integer_t *info)
            {
                LAPACK_CGELSD(&m, &n, &nrhs, traits::complex_ptr(a),
                              &lda, traits::complex_ptr(b), &ldb, s,
                              &rcond, rank, traits::complex_ptr(work),
                              &lwork, rwork, iwork, info);
            }

            inline void gelsd(const integer_t m, const integer_t n, const integer_t nrhs,
                              traits::complex_d *a, const integer_t lda, traits::complex_d *b,
                              const integer_t ldb, double *s, const double rcond, integer_t *rank,
                              traits::complex_d *work, const integer_t lwork, double *rwork,
                              integer_t *iwork, integer_t *info)
            {
                LAPACK_ZGELSD(&m, &n, &nrhs, traits::complex_ptr(a),
                              &lda, traits::complex_ptr(b), &ldb, s,
                              &rcond, rank, traits::complex_ptr(work),
                              &lwork, rwork, iwork, info);
            }

            // gelsd for real type
            template <typename MatrA, typename MatrB, typename VecS, typename Work>
            int gelsd(MatrA& A, MatrB& B, VecS& s, Work& work)
            {
                typedef typename MatrA::value_type val_t;
                typedef typename traits::type_traits<val_t>::real_type real_t;

                const std::ptrdiff_t m = traits::matrix_size1(A);
                const std::ptrdiff_t n = traits::matrix_size2(A);
                const std::ptrdiff_t nrhs = traits::matrix_size2(B);
                const std::ptrdiff_t maxmn = std::max(m, n);
                const std::ptrdiff_t minmn = std::min(m, n);

                // sanity checks
                assert(m >= 0 && n >= 0);
                assert(nrhs >= 0);
                assert(traits::leading_dimension(A) >= std::max<std::ptrdiff_t>(1, m));
                assert(traits::leading_dimension(B) >= std::max<std::ptrdiff_t>(1, maxmn));
                assert(traits::vector_size(work) >= 1);
                assert(traits::vector_size(s) >= std::max<std::ptrdiff_t>(1, minmn));

                integer_t info;
                const real_t rcond = -1;    // use machine precision
                integer_t rank;

                // query for maximum size of subproblems
                const integer_t smlsiz = ilaenv(9, "GELSD", "");
                const integer_t nlvl = static_cast<integer_t>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

                traits::detail::array<integer_t> iwork(3*minmn*nlvl + 11*minmn);

                detail::gelsd(traits::matrix_size1(A),
                              traits::matrix_size2(A),
                              traits::matrix_size2(B),
                              traits::matrix_storage(A),
                              traits::leading_dimension(A),
                              traits::matrix_storage(B),
                              traits::leading_dimension(B),
                              traits::vector_storage(s),
                              rcond,
                              &rank,
                              traits::vector_storage(work),
                              traits::vector_size(work),
                              traits::vector_storage(iwork),
                              &info);

                return info;
            }

            // gelsd for complex type
            template <typename MatrA, typename MatrB, typename VecS,
                        typename Work, typename RWork>
            int gelsd(MatrA& A, MatrB& B, VecS& s, Work& work, RWork& rwork)
            {
                typedef typename MatrA::value_type val_t;
                typedef typename traits::type_traits<val_t>::real_type real_t;

                const std::ptrdiff_t m = traits::matrix_size1(A);
                const std::ptrdiff_t n = traits::matrix_size2(A);
                const std::ptrdiff_t nrhs = traits::matrix_size2(B);
                const std::ptrdiff_t maxmn = std::max(m, n);
                const std::ptrdiff_t minmn = std::min(m, n);

                // sanity checks
                assert(m >= 0 && n >= 0);
                assert(nrhs >= 0);
                assert(traits::leading_dimension(A) >= std::max<std::ptrdiff_t>(1, m));
                assert(traits::leading_dimension(B) >= std::max<std::ptrdiff_t>(1, maxmn));
                assert(traits::vector_size(work) >= 1);
                assert(traits::vector_size(s) >= std::max<std::ptrdiff_t>(1, minmn));

                integer_t info;
                const real_t rcond = -1;    // use machine precision
                integer_t rank;

                // query for maximum size of subproblems
                const integer_t smlsiz = ilaenv(9, "GELSD", "");
                const integer_t nlvl = static_cast<integer_t>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

                traits::detail::array<integer_t> iwork(3*minmn*nlvl + 11*minmn);

                detail::gelsd(traits::matrix_size1(A),
                    traits::matrix_size2(A),
                    traits::matrix_size2(B),
                    traits::matrix_storage(A),
                    traits::leading_dimension(A),
                    traits::matrix_storage(B),
                    traits::leading_dimension(B),
                    traits::vector_storage(s),
                    rcond,
                    &rank,
                    traits::vector_storage(work),
                    traits::vector_size(work),
                    traits::vector_storage(rwork),
                    traits::vector_storage(iwork),
                    &info);

                return info;
            }

            template <int N>
            struct Gelsd { };

            // specialization for gelsd real flavors (sgelsd, dgelsd)
            template <>
            struct Gelsd<1>
            {
                template <typename MatrA, typename MatrB, typename VecS>
                int operator() (MatrA& A, MatrB& B, VecS& s, minimal_workspace) const
                {
                    typedef typename MatrA::value_type val_t;

                    const std::ptrdiff_t m = traits::matrix_size1(A);
                    const std::ptrdiff_t n = traits::matrix_size2(A);
                    const std::ptrdiff_t nrhs = traits::matrix_size2(B);

                    const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, nrhs);   // maxmnr = maxmn > nrhs ? maxmn : nrhs

                    // query for maximum size of subproblems
                    const integer_t smlsiz = ilaenv(9, "GELSD", "");
                    const integer_t nlvl = static_cast<integer_t>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

                    const integer_t lwork = 12*minmn + 2*minmn*smlsiz + 8*minmn*nlvl +
                                      minmn*nrhs + (smlsiz+1)*(smlsiz+1);

                    traits::detail::array<val_t> work(lwork);

                    return gelsd(A, B, s, work);
                }

                template <typename MatrA, typename MatrB, typename VecS>
                int operator() (MatrA& A, MatrB& B, VecS& s, optimal_workspace) const
                {
                    typedef typename MatrA::value_type val_t;
                    typedef typename traits::type_traits<val_t>::real_type real_t;

                    //const std::ptrdiff_t m = traits::matrix_size1(A);
                    //const std::ptrdiff_t n = traits::matrix_size2(A);
                    //const std::ptrdiff_t nrhs = traits::matrix_size2(B);

                    //const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, nrhs);   // maxmnr = maxmn > nrhs ? maxmn : nrhs

                    val_t temp_work;
                    integer_t temp_iwork;

                    const real_t rcond = -1;
                    integer_t rank;
                    integer_t info;

                    // query for optimal workspace size
                    detail::gelsd(traits::matrix_size1(A),
                                  traits::matrix_size2(A),
                                  traits::matrix_size2(B),
                                  traits::matrix_storage(A),
                                  traits::leading_dimension(A),
                                  traits::matrix_storage(B),
                                  traits::leading_dimension(B),
                                  traits::vector_storage(s),
                                  rcond,
                                  &rank,
                                  &temp_work,   //traits::vector_storage(work),
                                  -1,           //traits::vector_size(work),
                                  &temp_iwork,
                                  &info);

                    assert(info == 0);

                    const integer_t lwork = traits::detail::to_int(temp_work);

                    traits::detail::array<val_t> work(lwork);

                    return gelsd(A, B, s, work);
                }

                template <typename MatrA, typename MatrB, typename VecS, typename Work>
                int operator() (MatrA& A, MatrB& B, VecS& s, detail::workspace1<Work> workspace) const
                {
                    //const std::ptrdiff_t m = traits::matrix_size1(A);
                    //const std::ptrdiff_t n = traits::matrix_size2(A);
                    //const std::ptrdiff_t nrhs = traits::matrix_size2(B);

                    //const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, nrhs);   // maxmnr = maxmn > nrhs ? maxmn : nrhs

                    typedef typename traits::vector_traits<VecS>::value_type value_type ;
                    return gelsd(A, B, s, workspace.select(value_type()));
                }
            };

            // specialization for gelsd (cgelsd, zgelsd)
            template <>
            struct Gelsd<2>
            {
                template <typename MatrA, typename MatrB, typename VecS>
                int operator() (MatrA& A, MatrB& B, VecS& s, minimal_workspace) const
                {
                    typedef typename MatrA::value_type val_t;
                    typedef typename traits::type_traits<val_t>::real_type real_t;

                    const std::ptrdiff_t m = traits::matrix_size1(A);
                    const std::ptrdiff_t n = traits::matrix_size2(A);
                    const std::ptrdiff_t nrhs = traits::matrix_size2(B);

                    const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, nrhs);   // maxmnr = maxmn > nrhs ? maxmn : nrhs

                    // query for maximum size of subproblems
                    const integer_t smlsiz = ilaenv(9, "GELSD", "");
                    const integer_t nlvl = static_cast<integer_t>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

                    traits::detail::array<val_t> work(2*minmn + minmn*nrhs);

                    const std::ptrdiff_t rwork_size = 10*minmn + 2*minmn*smlsiz + 8*minmn*nlvl +
                                           3*smlsiz*nrhs + (smlsiz+1)*(smlsiz+1);

                    traits::detail::array<real_t> rwork(std::max<std::ptrdiff_t>(1, rwork_size));

                    return gelsd(A, B, s, work, rwork);
                }

                template <typename MatrA, typename MatrB, typename VecS>
                int operator() (MatrA& A, MatrB& B, VecS& s, optimal_workspace) const
                {
                    typedef typename MatrA::value_type val_t;
                    typedef typename traits::type_traits<val_t>::real_type real_t;

                    const std::ptrdiff_t m = traits::matrix_size1(A);
                    const std::ptrdiff_t n = traits::matrix_size2(A);
                    const std::ptrdiff_t nrhs = traits::matrix_size2(B);

                    const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, nrhs);   // maxmnr = maxmn > nrhs ? maxmn : nrhs

                    val_t temp_work;
                    real_t temp_rwork;
                    integer_t temp_iwork;

                    const real_t rcond = -1;
                    integer_t rank;
                    integer_t info;

                    // query for optimal workspace size
                    detail::gelsd(traits::matrix_size1(A),
                                  traits::matrix_size2(A),
                                  traits::matrix_size2(B),
                                  traits::matrix_storage(A),
                                  traits::leading_dimension(A),
                                  traits::matrix_storage(B),
                                  traits::leading_dimension(B),
                                  traits::vector_storage(s),
                                  rcond,
                                  &rank,
                                  &temp_work,   //traits::vector_storage(work),
                                  -1,           //traits::vector_size(work),
                                  &temp_rwork,
                                  &temp_iwork,
                                  &info);

                    assert(info == 0);

                    const integer_t lwork = traits::detail::to_int(temp_work);

                    traits::detail::array<val_t> work(lwork);

                    // query for maximum size of subproblems
                    const integer_t smlsiz = ilaenv(9, "GELSD", "");
                    const integer_t nlvl = static_cast<integer_t>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

                    const std::ptrdiff_t rwork_size = 10*minmn + 2*minmn*smlsiz + 8*minmn*nlvl +
                                            3*smlsiz*nrhs + (smlsiz+1)*(smlsiz+1);

                    traits::detail::array<real_t> rwork(std::max<std::ptrdiff_t>(1, rwork_size));

                    return gelsd(A, B, s, work, rwork);
                }

                template <typename MatrA, typename MatrB, typename VecS, typename Work, typename RWork>
                int operator() (MatrA& A, MatrB& B, VecS& s, detail::workspace2<Work, RWork> workspace) const
                {
                    //const std::ptrdiff_t m = traits::matrix_size1(A);
                    //const std::ptrdiff_t n = traits::matrix_size2(A);
                    //const std::ptrdiff_t nrhs = traits::matrix_size2(B);

                    //const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, nrhs);   // maxmnr = maxmn > nrhs ? maxmn : nrhs

                    typedef typename traits::vector_traits<VecS>::value_type    value_type ;
                    typedef typename traits::type_traits<value_type>::real_type real_type ;
                    return gelsd(A, B, s, workspace.select(value_type()), workspace.select(real_type()));
                }
            };

        } // detail

        // gelsd
        // Parameters:
        //  A:          matrix of coefficients
        //  B:          matrix of solutions (stored column-wise)
        //  s:          vector to store singular values on output, length >= max(1, min(m,n))
        //  workspace:  either optimal, minimal, or user supplied
        //
        template <typename MatrA, typename MatrB, typename VecS, typename Work>
        int gelsd(MatrA& A, MatrB& B, VecS& s, Work workspace)
        {
            typedef typename MatrA::value_type val_t;

            return detail::Gelsd<n_workspace_args<val_t>::value>() (A, B, s, workspace);
        }

        // gelsd, no singular values are returned
        // Parameters:
        //  A:          matrix of coefficients
        //  B:          matrix of solutions (stored column-wise)
        //  workspace:  either optimal, minimal, or user supplied
        //
        template <typename MatrA, typename MatrB, typename Work>
        int gelsd(MatrA& A, MatrB& B, Work workspace)
        {
            typedef typename MatrA::value_type val_t;
            typedef typename traits::type_traits<val_t>::real_type real_t;

            const std::ptrdiff_t m = traits::matrix_size1(A);
            const std::ptrdiff_t n = traits::matrix_size2(A);

            const std::ptrdiff_t s_size = std::max<std::ptrdiff_t>(1, std::min(m,n));
            traits::detail::array<real_t> s(s_size);

            return detail::Gelsd<n_workspace_args<val_t>::value>() (A, B, s, workspace);
        }

    } // lapack

}}}

#endif


/*

Copyright (c) 2005-2021, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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 University of Oxford 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 HOLDER 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.

*/

/*
Megan E. Marsh, Raymond J. Spiteri
Numerical Simulation Laboratory
University of Saskatchewan
December 2011
Partial support provided by research grants from the National
Science and Engineering Research Council (NSERC) of Canada
and the MITACS/Mprime Canadian Network of Centres of Excellence.
*/

#ifndef _GRL2IVPODESOLVER_HPP_
#define _GRL2IVPODESOLVER_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include "AbstractOneStepIvpOdeSolver.hpp"

/**
 * A concrete one step ODE solver class that employs the GRL2 second-order solver.
 * Method is described in J. Sundnes, R. Artebrant, O. Skavhaug, and A. Tveito.
 * A second-order algorithm for solving dynamic cell membrane equations.
 * IEEE Trans. Biomed. Eng., 56(10):2546-2548, 2009.
 */
class GRL2IvpOdeSolver : public AbstractOneStepIvpOdeSolver
{
private:

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the abstract IVP Solver, never used directly - boost uses this.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        // This calls serialize on the base class.
        archive & boost::serialization::base_object<AbstractOneStepIvpOdeSolver>(*this);
    }

    /** Working memory for the solver */
    std::vector<double> mEvalF;

    /** Working memory for the solver */
    std::vector<double> mPartialF;

    /** Working memory for the solver */
    std::vector<double> mTemp;

    /** Working memory for the solver */
    std::vector<double> mYinit;

protected:

    /**
     * Calculate the solution to the ODE system at the next timestep.
     *
     * @param pAbstractOdeSystem  the ODE system to solve
     * @param timeStep  dt
     * @param time  the current time
     * @param rCurrentYValues  the current (initial) state
     * @param rNextYValues  the state at the next timestep
     */
    void CalculateNextYValue(AbstractOdeSystem* pAbstractOdeSystem,
                             double timeStep,
                             double time,
                             std::vector<double>& rCurrentYValues,
                             std::vector<double>& rNextYValues);

public:

    /**
     * Constructor.
     */
    GRL2IvpOdeSolver()
    {}
};

#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(GRL2IvpOdeSolver)

#endif //_GRL2IVPODESOLVER_HPP_


#include "./pybind11/include/pybind11/pybind11.h"
#include "./pybind11/include/pybind11/stl_bind.h"
#include "./pybind11/include/pybind11/numpy.h"

#include <algorithm>
#include <dogm/dogm.h>
#include <dogm/dogm_types.h>
#include <dogm/mapping/laser_to_meas_grid.h>
#include <iostream>

#include <Eigen/Dense>

namespace py = pybind11;

PYBIND11_MAKE_OPAQUE(std::vector<float>);
PYBIND11_MAKE_OPAQUE(std::vector<dogm::GridCell>);

namespace dogm {

  // https://stackoverflow.com/questions/48982143/returning-and-passing-around-raw-pod-pointers-arrays-with-python-c-and-pyb
  template <class T> class ptr_wrapper
  {
  public:
    ptr_wrapper() : ptr(nullptr) {}
    ptr_wrapper(T* ptr) : ptr(ptr) {}
    ptr_wrapper(const ptr_wrapper& other) : ptr(other.ptr) {}
    T& operator* () const { return *ptr; }
    T* operator->() const { return  ptr; }
    T* get() const { return ptr; }
    void destroy() { delete ptr; }
    T& operator[](std::size_t idx) const { return ptr[idx]; }
  private:
    T* ptr;
  };

  ptr_wrapper<MeasurementCell> wrap_laser_grid( LaserMeasurementGrid& lmg, const std::vector<float>& measurements ) {
    ptr_wrapper<MeasurementCell> ptr(lmg.generateGrid( measurements ));
    return ptr;
  }

  void wrap_dogm_grid( DOGM& grid, ptr_wrapper<MeasurementCell>& ptr, float x, float y, float yaw, float dt, bool device){
    grid.updateGrid( ptr.get(), x, y, yaw, dt, device );
  }

  float pignisticTransformation(float free_mass, float occ_mass) {
    return occ_mass + 0.5f * (1.0f - occ_mass - free_mass);
  }

  void hsvToRGB(float hue, float saturation, float value, float &r, float &g, float &b) {

    if (saturation == 0.0f) {
      r = g = b = value;
    } else {
      int i = static_cast<int>(hue * 6.0f);
      float f = (hue * 6.0f) - i;
      float p = value * (1.0f - saturation);
      float q = value * (1.0f - saturation * f);
      float t = value * (1.0f - saturation * (1.0f - f));
      int res = i % 6;

      switch (res) {
        case 0:
          r = value;
          g = t;
          b = p;
          break;
        case 1:
          r = q;
          g = value;
          b = p;
          break;
        case 2:
          r = p;
          g = value;
          b = t;
          break;
        case 3:
          r = p;
          g = q;
          b = value;
          break;
        case 4:
          r = t;
          g = p;
          b = value;
          break;
        case 5:
          r = value;
          g = p;
          b = q;
          break;
        default:
          r = g = b = value;
      }
    }
  }

  py::array_t<float> render_dynamic_occupancy_grid( const DOGM& grid, double occupancy_threshold,
                                                            double dynamic_threshold, double max_velocity ) {
    auto width = grid.getGridSize();
    auto height = grid.getGridSize();
    auto depth = 3;
    float *grid_ptr = new float[ depth * width * height ];

    const auto grid_cells = grid.getGridCells();
    #pragma omp parallel for
    for( int y = 0; y < height; y++ ) {
      for( int x = 0; x < width; x++ ) {
        auto cell = grid_cells[y*width + x];
        float occ = pignisticTransformation(cell.free_mass, cell.occ_mass);

        Eigen::Vector2f vel;
        vel << cell.mean_x_vel, cell.mean_y_vel;

        Eigen::Matrix2f covar;
        covar << cell.var_x_vel, cell.covar_xy_vel, cell.covar_xy_vel, cell.var_y_vel;

        auto mdist = vel.transpose() * covar.inverse() * vel;

        float r, g, b;
        if (occ >= occupancy_threshold && mdist >= dynamic_threshold ) {
          float angle = atan2(cell.mean_y_vel, cell.mean_x_vel) + M_PI;

          auto value = std::min(1.0, sqrt(cell.mean_x_vel * cell.mean_x_vel +
                                          cell.mean_y_vel * cell.mean_y_vel) / max_velocity );

          hsvToRGB(angle / M_PI, 1.0f, value, r, g, b);
        } else {
          r = g = b = 1.0f - occ;
        }
        grid_ptr[ depth * ( y*width + x ) ] = r;
        grid_ptr[ depth * ( y*width + x ) + 1 ] = g;
        grid_ptr[ depth * ( y*width + x ) + 2 ] = b;
      }
    }

    // Create a Python object that will free the allocated
    // memory when destroyed:
    py::capsule delete_fn(grid_ptr, [](void *p) {
      float *ptr = reinterpret_cast<float *>(p);
      delete[] ptr;
    });

    return py::array_t<float>(
            {height, width, depth}, // shape
            {depth*width*sizeof(float), depth*sizeof(float), sizeof(float)}, // C-style contiguous strides
            grid_ptr, // the data pointer
            delete_fn); // numpy array references this parent
  }

  py::array_t<float> render_occupancy_grid( const DOGM& grid ) {
    auto width = grid.getGridSize();
    auto height = grid.getGridSize();
    float *grid_ptr = new float[ width * height ];

    const auto grid_cells = grid.getGridCells();
  #pragma omp parallel for
    for( int y = 0; y < height; y++ ) {
      for( int x = 0; x < width; x++ ) {
        auto cell = grid_cells[y*width + x];
        float prob = pignisticTransformation( cell.free_mass, cell.occ_mass );
        grid_ptr[ y*width + x ] = 1.0f - prob;
      }
    }

    // Create a Python object that will free the allocated
    // memory when destroyed:
    py::capsule delete_fn(grid_ptr, [](void *p) {
      float *ptr = reinterpret_cast<float *>(p);
      delete[] ptr;
    });

    return py::array_t<float>(
            {height, width}, // shape
            {width*sizeof(float), sizeof(float)}, // C-style contiguous strides
            grid_ptr, // the data pointer
            delete_fn); // numpy array references this parent
  }

  PYBIND11_MODULE(dogm_py, m) {
    // bind a vector of floats to pass into the laser measurement grid
    py::bind_vector<std::vector<float>>(m, "VectorFloat", py::buffer_protocol());

    m.doc() = "Python bindings for the Dynamic Occupancy Grid Map Library";

    py::class_<ptr_wrapper<MeasurementCell>>(m, "MeasurementCellPtr")
      .def(py::init<>(), "Wrapper for CUDA pointer returned by LaserMeasurementGrid" );

    m.def( "generateMeasurements", &wrap_laser_grid, py::arg( "laser_measurement_grid"),
           py::arg("measurements") );

    m.def( "updateGrid", &wrap_dogm_grid, "Wrapper function for grid update",
           py::arg("grid"), py::arg("measurement_ptr"), py::arg("x"),
           py::arg("y"), py::arg("yaw"),
           py::arg("dt"), py::arg("device"));

    py::class_<LaserMeasurementGrid::Params>(m, "LaserMeasurementGridParams")
            .def(py::init<float, float, float, float>(),
                 R"lmgPARAM(
    struct Params {
      float max_range;
      float resolution;
      float fov;
      float angle_increment;
    };
                        )lmgPARAM", py::arg("max_range"), py::arg("resolution"),
                        py::arg("fov"), py::arg("angle_increment"))
            .def_readwrite("max_range", &LaserMeasurementGrid::Params::max_range)
            .def_readwrite("resolution", &LaserMeasurementGrid::Params::resolution)
            .def_readwrite("fov", &LaserMeasurementGrid::Params::fov)
            .def_readwrite("angle_increment", &LaserMeasurementGrid::Params::angle_increment)
            ;

    py::class_<LaserMeasurementGrid>(m, "LaserMeasurementGrid")
            .def(py::init<const LaserMeasurementGrid::Params&, float, float>(),
                 py::arg("params"), py::arg("size"), py::arg("resolution"))
            .def("generateGrid", &LaserMeasurementGrid::generateGrid, "Create a grid representation of supplied laser measurements",
                 py::arg("measurements"));

    py::class_<DOGM::Params>(m, "DOGMParams")
            .def(py::init<float, float, int, int, float, float, float, float, float, float>(),
                    R"DogmPARAM(
    struct Params {
      float size;                          // Grid size [m]
      float resolution;                    // Grid cell size [m/cell]
      int particle_count;                  // Number of persistent particles
      int new_born_particle_count;         // Number of birth particles
      float persistence_prob;              // Probability of persistence
      float stddev_process_noise_position; // Process noise position
      float stddev_process_noise_velocity; // Process noise velocity
      float birth_prob;                    // Probability of birth
      float stddev_velocity;               // Velocity to sample birth particles from (normal distribution) [m/s]
      float init_max_velocity;             // Velocity to sample the initial particles from (uniform distribution) [m/s]
    }
                        )DogmPARAM", py::arg("size"),py::arg("resolution"),py::arg("particle_count"),
                   py::arg("new_born_particle_count"),py::arg("persistance_prob"),py::arg("stddev_process_noise_position"),
                   py::arg("stddev_process_noise_velocity"), py::arg("birth_prob"), py::arg("stddev_velocity"),
                   py::arg("init_max_velocity"));

    // define a vector of GridCells for return types
    py::class_<GridCell>(m, "GridCell")
            .def(py::init<>())
            .def_readwrite("occ_mass", &GridCell::occ_mass)
            .def_readwrite("free_mass", &GridCell::free_mass);

    py::bind_vector<std::vector<GridCell>>(m, "VectorGridCell");

    py::class_<DOGM>(m, "DOGM")
            .def(py::init<const DOGM::Params&>(), py::arg("params") )
            .def("updateGrid", &DOGM::updateGrid, "Update the current state with constructed measurement grid",
                 py::arg("cellData"), py::arg("x"), py::arg("y"), py::arg("yaw"), py::arg("dt"), py::arg("device"))
            .def("getGridCells", &DOGM::getGridCells)
            .def("getMeasurementCells", &DOGM::getMeasurementCells)
            .def("getParticles", &DOGM::getParticles)
            .def("getGridSize", &DOGM::getGridSize)
            .def("getResolution", &DOGM::getResolution)
            .def("getPositionX", &DOGM::getPositionX)
            .def("getPositionY", &DOGM::getPositionY)
            .def("getYaw", &DOGM::getYaw)
            .def("getIteration", &DOGM::getIteration);

    m.def( "renderOccupancyGrid", &render_occupancy_grid, "Convert the occupancy grid into a numpy array",
           py::arg("grid"));

    m.def( "renderDynamicOccupancyGrid", &render_dynamic_occupancy_grid, "Convert the occupancy grid into a RGB numpy array encoding of dynamic elements",
           py::arg("grid"), py::arg("occupancy_threshold"), py::arg("dynamic_threshold"), py::arg("max_velocity"));

  }

}