#ifndef BENCH_COMMON_BST_HPP
#define BENCH_COMMON_BST_HPP

#include "../src/ygg.hpp"
#include "benchmark.h"
#include "common.hpp"
#include "random.hpp"

#include <algorithm>
#include <boost/intrusive/set.hpp>
#include <cstdlib>
#include <draup.hpp>
#include <functional>
#include <limits>
#include <memory>
#include <random>
#include <unordered_set>
#include <vector>

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

#ifdef USEZIPF
#define DYN_GENERATOR UseZipf
#define PREFIX "<ZIPF>"
#else
#ifdef USESKEWED
#define DYN_GENERATOR UseSkewed
#define PREFIX "<SKEWED>"
#else
#define DYN_GENERATOR UseUniform

#ifdef PRESORT
#define PREFIX "<PRESORT>"
#else
#define PREFIX "<UNI>"
#endif
#endif
#endif

struct UseNone
{
	static constexpr bool enable = false;
};

struct UseUniform
{
	static constexpr bool enable = true;
	using Randomizer = UniformDistr;
	static constexpr int min = std::numeric_limits<int>::min();
	static constexpr int max = std::numeric_limits<int>::max();

	static Randomizer
	create(long unsigned int seed)
	{
		return UniformDistr(seed);
	}
};

struct UseZipf
{
	static constexpr bool enable = true;
	using Randomizer = ZipfDistr;
	static constexpr int min = std::numeric_limits<int>::min();
	static constexpr int max = 10000; // TODO is this sane?

	static Randomizer
	create(long unsigned int seed)
	{
		return ZipfDistr(seed, 1.0);
	}
};

struct UseSkewed
{
	static constexpr bool enable = true;
	using Randomizer = MaekinenSkewedDistr;
	static constexpr int min = std::numeric_limits<int>::min() + 1000;
	static constexpr int max =
	    static_cast<int>(std::numeric_limits<int>::max() * 0.8);

	static Randomizer
	create(long unsigned int seed)
	{
		return MaekinenSkewedDistr(seed, 3, 1000);
	}
};

template <class T, class Compare = decltype(std::less<T>{})>
void
presort(std::vector<T> & v, size_t shuffle_count, size_t seed,
        Compare cmp = std::less<T>{})
{
	std::mt19937 rng(seed);

	std::sort(v.begin(), v.end(), cmp);

	std::vector<size_t> indices(v.size());
	std::iota(indices.begin(), indices.end(), size_t{0});
	std::shuffle(indices.begin(), indices.end(), rng);

	T first_element = std::move(v[indices[0]]);
	for (size_t i = 1; i < shuffle_count; ++i) {
		v[indices[i - 1]] = std::move(v[indices[i]]);
	}
	v[indices[shuffle_count - 1]] = std::move(first_element);
}

struct DefaultBenchmarkOptions
{
	constexpr static bool distinct = false; // sets everything to be distinct

	constexpr static bool fixed_presort = false;
	constexpr static bool values_from_fixed = false;
	constexpr static bool need_nodes = false;
	constexpr static bool nodes_presort = false;
	constexpr static bool need_node_pointers = false;
	constexpr static bool node_pointers_distinct = false;

	constexpr static bool pointers_presort = false;
	constexpr static bool need_values = false;
	constexpr static bool values_presort = false;

	constexpr static size_t node_value_change_percentage = 0;
};

template <class Interface, class Experiment, class Options>
class BSTFixture : public benchmark::Fixture {
public:
	using NodeInterface = Interface;

	static std::string
	get_name()
	{
		auto experiment_c = Experiment{};
		std::string name = std::string("BST :: ") +
		                   boost::hana::to<char const *>(experiment_c) +
		                   std::string(" :: ") + Interface::get_name();
		return name;
	}

	void
	set_name(std::string name)
	{
		this->SetName(name.c_str());
	}

	BSTFixture() = default;
	void
	SetUp(const ::benchmark::State & state)
	{
		size_t fixed_count = static_cast<size_t>(state.range(0));
		size_t experiment_count = static_cast<size_t>(state.range(1));
		int seed = static_cast<int>(state.range(2));

		this->initialize(fixed_count, experiment_count, seed);
	}

	void
	initialize(size_t fixed_count, size_t experiment_count, int seed)
	{
		Interface::clear(this->t);
		this->papi.initialize();

		this->rng = std::mt19937(static_cast<unsigned long>(seed));

		auto main_rnd =
		    Options::MainRandomizer::create(static_cast<unsigned long>(seed));

		this->fixed_nodes.clear();
		this->fixed_values.clear();

		std::unordered_set<int> seen_values;
		for (size_t i = 0; i < fixed_count; ++i) {
			int val = main_rnd.generate(Options::MainRandomizer::min,
			                            Options::MainRandomizer::max);
			if (Options::distinct) {
				while (seen_values.find(val) != seen_values.end()) {
					val = main_rnd.generate(Options::MainRandomizer::min,
					                        Options::MainRandomizer::max);
				}
				seen_values.insert(val);
			}

			this->fixed_values.push_back(val);
		}

		if constexpr (Options::fixed_presort) {
			size_t presort_count = static_cast<size_t>(std::floor(
			    static_cast<double>(static_cast<double>(this->fixed_values.size()) *
			                        Options::fixed_presort_fraction)));
			presort(this->fixed_values, presort_count, this->rng());
		}

		for (int val : this->fixed_values) {
			this->fixed_nodes.push_back(Interface::create_node(val));
		}
		// DO NOT MERGE THESE LOOPS
		for (auto & n : this->fixed_nodes) {
			Interface::insert(this->t, n);
		}

		std::vector<int> shuffled_values;
		if (Options::values_from_fixed) {
			shuffled_values.insert(shuffled_values.begin(),
			                       this->fixed_values.begin(),
			                       this->fixed_values.end());
			std::shuffle(shuffled_values.begin(), shuffled_values.end(), this->rng);
		}

		if constexpr (Options::need_nodes) {
			seen_values.clear();
			auto rnd = Options::NodeRandomizer::create(rng());

			this->experiment_nodes.clear();
			for (size_t i = 0; i < experiment_count; ++i) {
				int val;

				if (Options::values_from_fixed) {
					size_t rand_index = static_cast<size_t>(
					    rnd.generate(0, static_cast<int>(this->fixed_values.size())));
					val = fixed_values[rand_index];
				} else {
					val = rnd.generate(Options::NodeRandomizer::min,
					                   Options::NodeRandomizer::max);
					if (Options::distinct) {
						while (seen_values.find(val) != seen_values.end()) {
							val = rnd.generate(Options::NodeRandomizer::min,
							                   Options::NodeRandomizer::max);
						}
						seen_values.insert(val);
					}
				}

				this->experiment_nodes.push_back(Interface::create_node(val));
			}
			if constexpr (Options::nodes_presort) {
				size_t presort_count = static_cast<size_t>(
				    std::floor(static_cast<double>(this->experiment_nodes.size()) *
				               Options::nodes_presort_fraction));
				presort(this->experiment_nodes, presort_count, this->rng(),
				        [](const typename Interface::Node & lhs,
				           const typename Interface::Node & rhs) {
					        return Interface::get_value(lhs) < Interface::get_value(rhs);
				        });
			}
		}

		if constexpr (Options::need_node_pointers) {
			auto rnd = Options::NodePointerRandomizer::create(rng());
			std::unordered_set<size_t> seen_indices;
			this->experiment_node_pointers.clear();

			for (size_t i = 0; i < experiment_count; ++i) {
				size_t rnd_index = static_cast<size_t>(
				    rnd.generate(0, static_cast<int>(this->fixed_nodes.size())));
				if (Options::distinct || Options::node_pointers_distinct) {
					while (seen_indices.find(rnd_index) != seen_indices.end()) {
						rnd_index = static_cast<size_t>(
						    rnd.generate(0, static_cast<int>(this->fixed_nodes.size())));
						assert(rnd_index < this->fixed_nodes.size());
					}

					seen_indices.insert(rnd_index);
				}
				auto node_ptr = &this->fixed_nodes[rnd_index];
				this->experiment_node_pointers.push_back(node_ptr);
			}

			if constexpr (Options::pointers_presort) {
				size_t presort_count = static_cast<size_t>(std::floor(
				    static_cast<double>(this->experiment_node_pointers.size()) *
				    Options::pointers_presort_fraction));
				presort(this->experiment_node_pointers, presort_count, this->rng(),
				        [](const typename Interface::Node * lhs,
				           const typename Interface::Node * rhs) {
					        return Interface::get_value(*lhs) <
					               Interface::get_value(*rhs);
				        });
			}
		}

		if constexpr (Options::need_values) {
			seen_values.clear();
			auto rnd = Options::ValueRandomizer::create(rng());

			this->experiment_values.clear();
			for (size_t i = 0; i < experiment_count; ++i) {
				int val;

				if (Options::values_from_fixed) {
					size_t rnd_index = static_cast<size_t>(
					    rnd.generate(0, static_cast<int>(this->fixed_values.size())));
					val = this->fixed_values[rnd_index];
				} else {
					int min = Options::ValueRandomizer::min;
					int max = Options::ValueRandomizer::max;

					if constexpr (Options::node_value_change_percentage > 0) {
						double node_value_max_delta =
						    ((max / 2) - (min / 2)) *
						    (static_cast<double>(Options::node_value_change_percentage) /
						     100.0);
						if (std::numeric_limits<double>::max() / ((max / 2) - (min / 2)) <
						    (static_cast<double>(Options::node_value_change_percentage) /
						     100.0)) {
							node_value_max_delta = std::numeric_limits<double>::max() / 2;
						}

						if (std::numeric_limits<int>::min() + node_value_max_delta <
						    Interface::get_value(*this->experiment_node_pointers[i])) {

							min = static_cast<int>(std::round(
							    Interface::get_value(*this->experiment_node_pointers[i]) -
							    node_value_max_delta));
						} else {
							min = std::numeric_limits<int>::min();
						}

						if (std::numeric_limits<int>::max() - node_value_max_delta >
						    Interface::get_value(*this->experiment_node_pointers[i])) {
							max = static_cast<int>(std::round(
							    Interface::get_value(*this->experiment_node_pointers[i]) +
							    node_value_max_delta));
						} else {
							max = std::numeric_limits<int>::max();
						}
					}

					val = rnd.generate(min, max);
					if (Options::distinct) {
						while (seen_values.find(val) != seen_values.end()) {
							val = rnd.generate(min, max);
						}
						seen_values.insert(val);
					}
				}

				this->experiment_values.push_back(val);
			}
			if constexpr (Options::values_presort) {
				size_t presort_count = static_cast<size_t>(std::floor(
				    this->experiment_values.size() * Options::values_presort_fraction));
				presort(this->fixed_values, presort_count, this->rng());
			}
		}
	}

	void
	TearDown(const ::benchmark::State & state)
	{
		(void)state;
		Interface::clear(this->t);
	}

	std::vector<int> fixed_values;
	std::vector<typename Interface::Node> fixed_nodes;

	std::vector<typename Interface::Node> experiment_nodes;
	std::vector<int> experiment_values;
	std::vector<typename Interface::Node *> experiment_node_pointers;

	typename Interface::Tree t;

	PapiMeasurements papi;
	std::mt19937 rng;
};

/*
 * Red-Black Tree Interface
 */
template <class MyTreeOptions>
class RBNode
    : public ygg::RBTreeNodeBase<RBNode<MyTreeOptions>, MyTreeOptions> {
private:
	int value;

public:
	RBNode(int value_in) : value(value_in){};

	void
	set_value(int new_value)
	{
		this->value = new_value;
	}

	int
	get_value() const
	{
		return this->value;
	}

	bool
	operator<(const RBNode<MyTreeOptions> & rhs) const
	{
		return this->value < rhs.value;
	}
};

template <class T>
bool
operator<(const RBNode<T> & lhs, int rhs)
{
	return lhs.get_value() < rhs;
}
template <class T>
bool
operator<(int lhs, const RBNode<T> & rhs)
{
	return lhs < rhs.get_value();
}

template <class MyTreeOptions>
class YggRBTreeInterface {
public:
	using Node = RBNode<MyTreeOptions>;
	using Tree = ygg::RBTree<Node, ygg::RBDefaultNodeTraits, MyTreeOptions>;

	static void
	insert(Tree & t, Node & n)
	{
		t.insert(n);
	}

	static std::string
	get_name()
	{
		std::string avc = "";
		if constexpr (MyTreeOptions::micro_avoid_conditionals) {
			avc = "avc";
		}

		std::string cc = "";
		if constexpr (MyTreeOptions::compress_color) {
			cc = ",cc";
		}

		std::string pf = "";
		if constexpr (MyTreeOptions::micro_prefetch) {
			pf = ",pf";
		}

		return std::string("RBTree[") + avc + cc + pf + std::string("]");
	}

	static int
	get_value(const Node & n)
	{
		return n.get_value();
	}

	static void
	set_value(Node & n, int val)
	{
		n.set_value(val);
	}

	static Node
	create_node(int val)
	{
		return Node(val);
	}

	static void
	clear(Tree & t)
	{
		t.clear();
	}
};

/*
 * Weight-Balanced Tree Interface
 */
template <class MyTreeOptions>
class WBNode
    : public ygg::WBTreeNodeBase<WBNode<MyTreeOptions>, MyTreeOptions> {
private:
	int value;

public:
	WBNode(int value_in) : value(value_in){};

	void
	set_value(int new_value)
	{
		this->value = new_value;
	}

	int
	get_value() const
	{
		return this->value;
	}

	bool
	operator<(const WBNode<MyTreeOptions> & rhs) const
	{
		return this->value < rhs.value;
	}
};

template <class T>
bool
operator<(const WBNode<T> & lhs, int rhs)
{
	return lhs.get_value() < rhs;
}
template <class T>
bool
operator<(int lhs, const WBNode<T> & rhs)
{
	return lhs < rhs.get_value();
}

template <class MyTreeOptions>
class YggWBTreeInterface {
public:
	using Node = WBNode<MyTreeOptions>;
	using Tree = ygg::WBTree<Node, ygg::WBDefaultNodeTraits, MyTreeOptions>;

	static void
	insert(Tree & t, Node & n)
	{
		t.insert(n);
	}

	static std::string
	get_name()
	{
		std::string sp_tp;
		if constexpr (MyTreeOptions::wbt_single_pass) {
			sp_tp = "SP";
		} else {
			sp_tp = "TP";
		}

		return std::string("WBTree[") + MyTreeOptions::wbt_delta_str() +
		       std::string(",") + MyTreeOptions::wbt_gamma_str() +
		       std::string(",") + sp_tp + std::string("]");
	}

	static int
	get_value(const Node & n)
	{
		return n.get_value();
	}

	static void
	set_value(Node & n, int val)
	{
		n.set_value(val);
	}

	static Node
	create_node(int val)
	{
		return Node(val);
	}

	static void
	clear(Tree & t)
	{
		t.clear();
	}
};

/*
 * Energy-Balanced Tree Interface
 */
template <class MyTreeOptions>
class ENode
    : public ygg::EnergyTreeNodeBase<ENode<MyTreeOptions>, MyTreeOptions> {
private:
	int value;

public:
	ENode(int value_in) : value(value_in){};

	void
	set_value(int new_value)
	{
		this->value = new_value;
	}

	int
	get_value() const
	{
		return this->value;
	}

	bool
	operator<(const ENode<MyTreeOptions> & rhs) const
	{
		return this->value < rhs.value;
	}
};

template <class T>
bool
operator<(const ENode<T> & lhs, int rhs)
{
	return lhs.get_value() < rhs;
}
template <class T>
bool
operator<(int lhs, const ENode<T> & rhs)
{
	return lhs < rhs.get_value();
}

template <class MyTreeOptions>
class YggEnergyTreeInterface {
public:
	using Node = ENode<MyTreeOptions>;
	using Tree = ygg::EnergyTree<Node, MyTreeOptions>;

	static void
	insert(Tree & t, Node & n)
	{
		t.insert(n);
	}

	static std::string
	get_name()
	{
		return "EnergyTree";
	}

	static int
	get_value(const Node & n)
	{
		return n.get_value();
	}

	static void
	set_value(Node & n, int val)
	{
		n.set_value(val);
	}

	static Node
	create_node(int val)
	{
		return Node(val);
	}

	static void
	clear(Tree & t)
	{
		t.clear();
	}
};

/*
 * Zip Tree Interface
 */
template <class MyTreeOptions>
class ZipNode
    : public ygg::ZTreeNodeBase<ZipNode<MyTreeOptions>, MyTreeOptions> {
private:
	int value;

public:
	ZipNode(int value_in) : value(value_in) { this->update_rank(); }

	void
	set_value(int new_value)
	{
		this->value = new_value;
		this->update_rank();
	}

	int
	get_value() const
	{
		return this->value;
	}

	bool
	operator<(const ZipNode<MyTreeOptions> & rhs) const
	{
		return this->value < rhs.value;
	}
};
template <class T>
bool
operator<(const ZipNode<T> & lhs, int rhs)
{
	return lhs.get_value() < rhs;
}
template <class T>
bool
operator<(int lhs, const ZipNode<T> & rhs)
{
	return lhs < rhs.get_value();
}

template <class MyTreeOptions>
class YggZTreeInterface {
public:
	using Node = ZipNode<MyTreeOptions>;

	using Tree =
	    ygg::ZTree<Node, ygg::ZTreeDefaultNodeTraits<Node>, MyTreeOptions>;

	static std::string
	get_name()
	{
		std::string randomness = "R"; // "Truly" random
		if (MyTreeOptions::ztree_use_hash) {
			randomness = "H"; // Hash
		}

		std::string universalize = "";
		if (MyTreeOptions::ztree_universalize_lincong) {
			universalize = ",UL";
		}
		if (MyTreeOptions::ztree_universalize_multiply) {
			universalize = ",UM";
		}

		return "ZipTree[" + randomness + universalize + "]";
	}

	static void
	insert(Tree & t, Node & n)
	{
		t.insert(n);
	}

	static int
	get_value(const Node & n)
	{
		return n.get_value();
	}

	static Node
	create_node(int val)
	{
		return Node(val);
	}

	static void
	clear(Tree & t)
	{
		t.clear();
	}
};
// Necessary for rank-by-hash
namespace std {
template <class T>
struct hash<ZipNode<T>>
{
	size_t
	operator()(const ZipNode<T> & n) const
	{
		return static_cast<size_t>(n.get_value());
	}
};
} // namespace std

/*
 * Boost::Intrusive::Set Interface
 */
class BoostSetInterface {
public:
	class Node
	    : public boost::intrusive::set_base_hook<boost::intrusive::link_mode<
	          boost::intrusive::link_mode_type::normal_link>> {
	public:
		int value;

		Node(int value_in) : value(value_in) {}

		bool
		operator<(const Node & rhs) const
		{
			return this->value < rhs.value;
		}
	};

	using Tree = boost::intrusive::multiset<Node>;

	static std::string
	get_name()
	{
		return "boost::intrusive::multiset";
	}

	static int
	get_value(const Node & n)
	{
		return n.value;
	}

	static void
	set_value(Node & n, int val)
	{
		n.value = val;
	}

	static void
	insert(Tree & t, Node & n)
	{
		t.insert(n);
	}

	static Node
	create_node(int val)
	{
		return Node(val);
	}

	static void
	clear(Tree & t)
	{
		t.clear();
	}
};

/*
 * Std::Set's interface
 */
class StdSetInterface {
public:
	using Node = decltype(std::multiset<int>().extract(0));
	using Tree = std::multiset<int>;

	static std::string
	get_name()
	{
		return "std::multiset";
	}

	static void
	insert(Tree & t, Node & n)
	{
		// TODO this is very unclean
		t.insert(std::move(n));
	}

	static int
	get_value(const Node & n)
	{
		return n.value();
	}

	static void
	insert(Tree & t, Node && n)
	{
		// TODO this is very unclean
		t.insert(std::move(n));
	}

	static Node
	create_node(int val)
	{
		std::multiset<int> donor;
		donor.insert(val);

		return donor.extract(donor.begin());
	}

	static void
	clear(Tree & t)
	{
		t.clear();
	}
};

using BasicTreeOptions = ygg::TreeOptions<ygg::TreeFlags::MULTIPLE>;
using ArithTreeOptions =
    ygg::TreeOptions<ygg::TreeFlags::MULTIPLE,
                     ygg::TreeFlags::MICRO_AVOID_CONDITIONALS>;

/* Variants of the red-black tree */
using RBColorCompressTreeOptions =
    ygg::TreeOptions<ygg::TreeFlags::MULTIPLE, ygg::TreeFlags::COMPRESS_COLOR>;
using RBPrefetchTreeOptions =
    ygg::TreeOptions<ygg::TreeFlags::MULTIPLE, ygg::TreeFlags::MICRO_PREFETCH>;

/* Variants of the zip tree */
using ZRandomTreeOptions =
    ygg::TreeOptions<ygg::TreeFlags::MULTIPLE,
                     ygg::TreeFlags::ZTREE_RANK_TYPE<std::uint8_t>>;
using ZHashTreeOptions =
    ygg::TreeOptions<ygg::TreeFlags::MULTIPLE, ygg::TreeFlags::ZTREE_USE_HASH>;
using ZUnivHashTreeOptions =
    ygg::TreeOptions<ygg::TreeFlags::MULTIPLE, ygg::TreeFlags::ZTREE_USE_HASH,
                     ygg::TreeFlags::ZTREE_RANK_HASH_UNIVERSALIZE_COEFFICIENT<
                         9859957398433823229ul>>;

/* Variants of the weight-balanced tree */
using WBTTwopassTreeOptions = ygg::TreeOptions<ygg::TreeFlags::MULTIPLE>;
using WBTSinglepassTreeOptions =
    ygg::TreeOptions<ygg::TreeFlags::MULTIPLE, ygg::TreeFlags::WBT_SINGLE_PASS>;

using WBTTwopass32TreeOptions =
    ygg::TreeOptions<ygg::TreeFlags::MULTIPLE,
                     ygg::TreeFlags::WBT_DELTA_NUMERATOR<3>,
                     ygg::TreeFlags::WBT_DELTA_DENOMINATOR<1>,
                     ygg::TreeFlags::WBT_GAMMA_NUMERATOR<2>,
                     ygg::TreeFlags::WBT_GAMMA_DENOMINATOR<1>>;
using WBTSinglepass32TreeOptions =
    ygg::TreeOptions<ygg::TreeFlags::MULTIPLE, ygg::TreeFlags::WBT_SINGLE_PASS,
                     ygg::TreeFlags::WBT_DELTA_NUMERATOR<3>,
                     ygg::TreeFlags::WBT_DELTA_DENOMINATOR<1>,
                     ygg::TreeFlags::WBT_GAMMA_NUMERATOR<2>,
                     ygg::TreeFlags::WBT_GAMMA_DENOMINATOR<1>>;

using WBTSinglepassLWTreeOptions =
    ygg::TreeOptions<ygg::TreeFlags::MULTIPLE, ygg::TreeFlags::WBT_SINGLE_PASS,
                     ygg::TreeFlags::WBT_DELTA_NUMERATOR<3>,
                     ygg::TreeFlags::WBT_DELTA_DENOMINATOR<1>,
                     ygg::TreeFlags::WBT_GAMMA_NUMERATOR<4>,
                     ygg::TreeFlags::WBT_GAMMA_DENOMINATOR<3>>;

using WBTSinglepassBalTreeOptions =
    ygg::TreeOptions<ygg::TreeFlags::MULTIPLE, ygg::TreeFlags::WBT_SINGLE_PASS,
                     ygg::TreeFlags::WBT_DELTA_NUMERATOR<2>,
                     ygg::TreeFlags::WBT_DELTA_DENOMINATOR<1>,
                     ygg::TreeFlags::WBT_GAMMA_NUMERATOR<3>,
                     ygg::TreeFlags::WBT_GAMMA_DENOMINATOR<2>>;

using WBTSinglepassSuperBalTreeOptions =
    ygg::TreeOptions<ygg::TreeFlags::MULTIPLE, ygg::TreeFlags::WBT_SINGLE_PASS,
                     ygg::TreeFlags::WBT_DELTA_NUMERATOR<3>,
                     ygg::TreeFlags::WBT_DELTA_DENOMINATOR<2>,
                     ygg::TreeFlags::WBT_GAMMA_NUMERATOR<5>,
                     ygg::TreeFlags::WBT_GAMMA_DENOMINATOR<4>>;

using WBTSinglepassBalArithTreeOptions =
    ygg::TreeOptions<ygg::TreeFlags::MULTIPLE, ygg::TreeFlags::WBT_SINGLE_PASS,
                     ygg::TreeFlags::WBT_DELTA_NUMERATOR<2>,
                     ygg::TreeFlags::WBT_DELTA_DENOMINATOR<1>,
                     ygg::TreeFlags::WBT_GAMMA_NUMERATOR<3>,
                     ygg::TreeFlags::WBT_GAMMA_DENOMINATOR<2>,
                     ygg::TreeFlags::MICRO_AVOID_CONDITIONALS>;

#endif


#ifndef DECAF_BLOCK_HPP
#define DECAF_BLOCK_HPP

#include <vector>
#include <math.h>
#include <iostream>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/serialization.hpp>

namespace decaf {

template<int Dim>
class Block {

public:
    Block() : hasGridspace_(false), hasGlobalBBox_(false), hasGlobalExtends_(false),
        hasLocalBBox_(false), hasLocalExtends_(false), ghostSize_(0),
        hasOwnBBox_(false), hasOwnExtends_(false)
    {
        globalExtends_.resize(2*Dim);
        localExtends_.resize(2*Dim);
        ownExtends_.resize(2*Dim);

        globalBBox_.resize(2*Dim);
        localBBox_.resize(2*Dim);
        ownBBox_.resize(2*Dim);
    }

    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        ar & BOOST_SERIALIZATION_NVP(hasGridspace_);
        ar & BOOST_SERIALIZATION_NVP(gridspace_);
        ar & BOOST_SERIALIZATION_NVP(hasGlobalBBox_);
        ar & BOOST_SERIALIZATION_NVP(globalBBox_);
        ar & BOOST_SERIALIZATION_NVP(hasGlobalExtends_);
        ar & BOOST_SERIALIZATION_NVP(globalExtends_);
        ar & BOOST_SERIALIZATION_NVP(hasLocalBBox_);
        ar & BOOST_SERIALIZATION_NVP(localBBox_);
        ar & BOOST_SERIALIZATION_NVP(hasLocalExtends_);
        ar & BOOST_SERIALIZATION_NVP(localExtends_);
        ar & BOOST_SERIALIZATION_NVP(ghostSize_);
        ar & BOOST_SERIALIZATION_NVP(hasOwnBBox_);
        ar & BOOST_SERIALIZATION_NVP(ownBBox_);
        ar & BOOST_SERIALIZATION_NVP(hasOwnExtends_);
        ar & BOOST_SERIALIZATION_NVP(ownExtends_);
    }

    void setGridspace(float gridspace)
    {
        gridspace_ = gridspace;
        hasGridspace_ = true;
    }

    void setGhostSizes(unsigned int ghostSize)
    {
        ghostSize_ = ghostSize;
    }

    void setGlobalBBox(std::vector<float>& globalBBox)
    {
        assert(globalBBox.size() == Dim * 2);
        globalBBox_ =  globalBBox;
        hasGlobalBBox_ = true;
    }

    void setGlobalExtends(std::vector<unsigned int>& globalExtends)
    {
        assert(globalExtends.size() == Dim * 2);
        globalExtends_ =  globalExtends;
        hasGlobalExtends_ = true;
    }

    void setLocalBBox(std::vector<float>& localBBox)
    {
        assert(localBBox.size() == Dim * 2);
        localBBox_ = localBBox;
        hasLocalBBox_ = true;
    }

    void setLocalExtends(std::vector<unsigned int>& localExtends)
    {
        assert(localExtends.size() == Dim * 2);
        localExtends_ = localExtends;
        hasLocalExtends_ = true;
    }

    void setOwnBBox(std::vector<float>& ownBBox)
    {
        assert(ownBBox.size() == Dim * 2);
        ownBBox_ = ownBBox;
        hasOwnBBox_ = true;
    }

    void setOwnExtends(std::vector<unsigned int>& ownExtends)
    {
        assert(ownExtends.size() == Dim * 2);
        ownExtends_ = ownExtends;
        hasOwnExtends_ = true;
    }

    bool updateExtends()
    {
        if(!hasGridspace_)
            return false;
        if(hasGlobalBBox_)
        {
            globalExtends_.resize(2*Dim);
            for(unsigned int i = 0; i < Dim; i++)
            {
                globalExtends_[i] = 0;
                //Ceil because we count the number of cells
                globalExtends_[Dim+i] = (unsigned int)(ceil(globalBBox_[Dim+i] / gridspace_));
            }
            hasGlobalExtends_ = true;

        }

        if(hasLocalBBox_)
        {
            localExtends_.resize(2*Dim);
            for(unsigned int i = 0; i < Dim; i++)
            {
                // TODO : here should be another value because of ghost region
                localExtends_[i] = 0;
                //Ceil because we count the number of cells
                localExtends_[Dim+i] = (unsigned int)(ceil(localBBox_[Dim+i] / gridspace_));
            }
            hasLocalExtends_ = true;
        }

        if(hasOwnBBox_)
        {
            ownExtends_.resize(2*Dim);
            for(unsigned int i = 0; i < Dim; i++)
            {
                // TODO : here should be another value because of ghost region
                ownExtends_[i] = 0;
                //Ceil because we count the number of cells
                ownExtends_[Dim+i] = (unsigned int)(ceil(ownBBox_[Dim+i] / gridspace_));
            }
            hasOwnExtends_ = true;
        }

        return true;
    }

    bool isInGlobalBlock(float x, float y, float z) const
    {
        if(!hasGlobalBBox_) return false;
        return x >= globalBBox_[0] && x <= globalBBox_[0] + globalBBox_[3] &&
                y >= globalBBox_[1] && y <= globalBBox_[1] + globalBBox_[4] &&
                z >= globalBBox_[2] && z <= globalBBox_[2] + globalBBox_[5];
    }

    bool isInLocalBlock(float x, float y, float z) const
    {
        if(!hasLocalBBox_) return false;
        return x >= localBBox_[0] && x <= localBBox_[0] + localBBox_[3] &&
                y >= localBBox_[1] && y <= localBBox_[1] + localBBox_[4] &&
                z >= localBBox_[2] && z <= localBBox_[2] + localBBox_[5];
    }

    bool isInOwnBlock(float x, float y, float z) const
    {
        if(!hasOwnBBox_) return false;
        return x >= ownBBox_[0] && x <= ownBBox_[0] + ownBBox_[3] &&
                y >= ownBBox_[1] && y <= ownBBox_[1] + ownBBox_[4] &&
                z >= ownBBox_[2] && z <= ownBBox_[2] + ownBBox_[5];
    }

    bool isInGlobalBlock(unsigned int x, unsigned int y, unsigned int z) const
    {
        if(!hasGlobalExtends_) return false;
        return x >= globalExtends_[0] && x < globalExtends_[0] + globalExtends_[3] &&
                y >= globalExtends_[1] && y < globalExtends_[1] + globalExtends_[4] &&
                z >= globalExtends_[2] && z < globalExtends_[2] + globalExtends_[5];
    }

    bool isInLocalBlock(unsigned int x, unsigned int y, unsigned int z) const
    {
        if(!hasLocalExtends_) return false;
        return x >= localExtends_[0] && x < localExtends_[0] + localExtends_[3] &&
                y >= localExtends_[1] && y < localExtends_[1] + localExtends_[4] &&
                z >= localExtends_[2] && z < localExtends_[2] + localExtends_[5];
    }

    bool isInOwnBlock(unsigned int x, unsigned int y, unsigned int z) const
    {
        if(!hasOwnExtends_) return false;
        return x >= ownExtends_[0] && x < ownExtends_[0] + ownExtends_[3] &&
                y >= ownExtends_[1] && y < ownExtends_[1] + ownExtends_[4] &&
                z >= ownExtends_[2] && z < ownExtends_[2] + ownExtends_[5];
    }

    bool hasSameExtends(const Block<Dim> & other)
    {
        if(hasGlobalExtends_ && other.hasGlobalExtends_)
        {
            for(unsigned int i = 0; i < 2 * Dim; i++)
                if(globalExtends_[i] != other.globalExtends_[i])
                    return false;
        }

        if(hasLocalExtends_ && other.hasLocalExtends_)
        {
            for(unsigned int i = 0; i < 2 * Dim; i++)
                if(localExtends_[i] != other.localExtends_[i])
                    return false;
        }

        if(hasOwnExtends_ && other.hasOwnExtends_)
        {
            for(unsigned int i = 0; i < 2 * Dim; i++)
                if(ownExtends_[i] != other.ownExtends_[i])
                    return false;
        }

        return true;
    }

    bool doExtendsInclude(const Block<Dim> & other)
    {
        if(hasGlobalExtends_ && other.hasGlobalExtends_)
        {
            for(unsigned int i = 0; i < Dim; i++)
                if(globalExtends_[i] > other.globalExtends_[i])
                    return false;
            for(unsigned int i = Dim; i < 2*Dim; i++)
                if(globalExtends_[i-Dim]+globalExtends_[i] < other.globalExtends_[i-Dim] + other.globalExtends_[i])
                    return false;
        }

        if(hasLocalExtends_ && other.hasLocalExtends_)
        {
            for(unsigned int i = 0; i < Dim; i++)
                if(localExtends_[i] > other.localExtends_[i])
                    return false;
            for(unsigned int i = Dim; i < 2*Dim; i++)
                if(localExtends_[i-Dim]+localExtends_[i] < other.localExtends_[i-Dim] + other.localExtends_[i])
                    return false;
        }

        if(hasOwnExtends_ && other.hasOwnExtends_)
        {
            for(unsigned int i = 0; i < Dim; i++)
                if(ownExtends_[i] > other.ownExtends_[i])
                    return false;
            for(unsigned int i = Dim; i < 2*Dim; i++)
                if(ownExtends_[i-Dim]+ownExtends_[i] < other.ownExtends_[i-Dim] + other.ownExtends_[i])
                    return false;
        }

        return true;
    }

    void makeUnion(const Block<Dim> & other)
    {
        makeExtendsUnion(other);
        makeBBoxUnion(other);
    }

    void makeUnion(const Block<Dim> & other, Block<Dim> & result)
    {
        makeExtendsUnion(other, result);
        makeBBoxUnion(other, result);
    }

    void makeExtendsUnion(const Block<Dim> & other)
    {
        for(unsigned i = 0; i < Dim; i++)
        {
            if(hasGlobalExtends_ && other.hasGlobalExtends_)
            {
                unsigned int origin = std::min(globalExtends_[i], other.globalExtends_[i]);
                unsigned int delta = std::max(
                            globalExtends_[i]+globalExtends_[Dim+i],
                            other.globalExtends_[i]+other.globalExtends_[Dim+i]
                            ) - origin;
                globalExtends_[i] = origin;
                globalExtends_[Dim+i] = delta;
            }

            if(hasLocalExtends_ && other.hasLocalExtends_)
            {
                unsigned int origin = std::min(localExtends_[i], other.localExtends_[i]);
                unsigned int delta = std::max(
                            localExtends_[i]+localExtends_[Dim+i],
                            other.localExtends_[i]+other.localExtends_[Dim+i]
                            ) - origin;
                localExtends_[i] = origin;
                localExtends_[Dim+i] = delta;
            }

            if(hasOwnExtends_ && other.hasOwnExtends_)
            {
                unsigned int origin = std::min(ownExtends_[i], other.ownExtends_[i]);
                unsigned int delta = std::max(
                            ownExtends_[i]+ownExtends_[Dim+i],
                            other.ownExtends_[i]+other.ownExtends_[Dim+i]
                            ) - origin;
                ownExtends_[i] = origin;
                ownExtends_[Dim+i] = delta;
            }

        }
    }

    // Used during the merge
    void makeBBoxUnion(const Block<Dim> & other)
    {
        for(unsigned i = 0; i < Dim; i++)
        {
            if(hasGlobalBBox_ && other.hasGlobalBBox_)
            {
                float dMax = std::max(globalBBox_[i] + globalBBox_[Dim+i], other.globalBBox_[i] + other.globalBBox_[Dim+i]);
                globalBBox_[i] = std::min(globalBBox_[i], other.globalBBox_[i]);
                globalBBox_[Dim+i] = dMax - globalBBox_[i];
            }

            if(hasLocalBBox_ && other.hasLocalBBox_)
            {
                float dMax = std::max(localBBox_[i] + localBBox_[Dim+i], other.localBBox_[i] + other.localBBox_[Dim+i]);
                localBBox_[i] = std::min(localBBox_[i], other.localBBox_[i]);
                localBBox_[Dim+i] = dMax - localBBox_[i];
            }

            if(hasOwnBBox_ && other.hasOwnBBox_)
            {
                float dMax = std::max(ownBBox_[i] + ownBBox_[Dim+i], other.ownBBox_[i] + other.ownBBox_[Dim+i]);
                ownBBox_[i] = std::min(ownBBox_[i], other.ownBBox_[i]);
                ownBBox_[Dim+i] = dMax - ownBBox_[i];
            }
        }
    }

    void makeExtendsUnion(const Block<Dim> & other, Block<Dim> & result)
    {
        for(unsigned i = 0; i < Dim; i++)
        {
            if(hasGlobalExtends_ && other.hasGlobalExtends_)
            {
                unsigned int origin = std::min(globalExtends_[i], other.globalExtends_[i]);
                unsigned int delta = std::max(
                            globalExtends_[i]+globalExtends_[Dim+i],
                            other.globalExtends_[i]+other.globalExtends_[Dim+i]
                            ) - origin;
                result.hasGlobalExtends_ = true;
                result.globalExtends_[i] = origin;
                result.globalExtends_[Dim+i] = delta;
            }

            if(hasLocalExtends_ && other.hasLocalExtends_)
            {
                unsigned int origin = std::min(localExtends_[i], other.localExtends_[i]);
                unsigned int delta = std::max(
                            localExtends_[i]+localExtends_[Dim+i],
                            other.localExtends_[i]+other.localExtends_[Dim+i]
                            ) - origin;
                result.hasLocalExtends_ = true;
                result.localExtends_[i] = origin;
                result.localExtends_[Dim+i] = delta;
            }

            if(hasOwnExtends_ && other.hasOwnExtends_)
            {
                unsigned int origin = std::min(ownExtends_[i], other.ownExtends_[i]);
                unsigned int delta = std::max(
                            ownExtends_[i]+ownExtends_[Dim+i],
                            other.ownExtends_[i]+other.ownExtends_[Dim+i]
                            ) - origin;
                result.hasOwnExtends_ = true;
                result.ownExtends_[i] = origin;
                result.ownExtends_[Dim+i] = delta;
            }
        }
    }

    void makeBBoxUnion(const Block<Dim> & other, Block<Dim> & result)
    {
        for(unsigned i = 0; i < Dim; i++)
        {
            if(hasGlobalBBox_ && other.hasGlobalBBox_)
            {
                result.hasGlobalBBox_ = true;
                float dMax = std::max(globalBBox_[i] + globalBBox_[Dim+i], other.globalBBox_[i] + other.globalBBox_[Dim+i]);
                result.globalBBox_[i] = std::min(globalBBox_[i], other.globalBBox_[i]);
                result.globalBBox_[Dim+i] = dMax - globalBBox_[i];
            }

            if(hasLocalBBox_ && other.hasLocalBBox_)
            {
                result.hasLocalBBox_ = true;
                float dMax = std::max(localBBox_[i] + localBBox_[Dim+i], other.localBBox_[i] + other.localBBox_[Dim+i]);
                result.localBBox_[i] = std::min(localBBox_[i], other.localBBox_[i]);
                result.localBBox_[Dim+i] = dMax - localBBox_[i];
            }

            if(hasOwnBBox_ && other.hasOwnBBox_)
            {
                result.hasOwnBBox_ = true;
                float dMax = std::max(ownBBox_[i] + ownBBox_[Dim+i], other.ownBBox_[i] + other.ownBBox_[Dim+i]);
                result.ownBBox_[i] = std::min(ownBBox_[i], other.ownBBox_[i]);
                result.ownBBox_[Dim+i] = dMax - ownBBox_[i];
            }
        }
    }

    // Expect the position in global space
    // Return the position in the own index
    bool getOwnPositionIndex(float* pos, unsigned int* index)
    {
        // TODO : test if the position is in the space

        // Move the absolute coordinates to the own coordinates
        for(unsigned int i = 0; i <  Dim; i++)
        {
            float offsetPos = pos[i] - ownBBox_[i];
            index[i] = (unsigned int)(floor(offsetPos / gridspace_));
        }

        return true;
    }

    // Expect the position index in Own index
    // Return a position in the global space
    bool getOwnPositionValue(unsigned int* index, float* pos)
    {
        assert(hasOwnBBox_);
        for(unsigned int i = 0; i <  Dim; i++)
        {
            // 0.5 to get to the middle of the cell
            pos[i] = ownBBox_[i] + ((float)index[i] + 0.5) * gridspace_;
        }
        return true;
    }

    // Expect the position in global space
    // Return the position in the local index
    bool getLocalPositionIndex(float* pos, unsigned int* index)
    {
        // TODO : test if the position is in the space

        // Move the absolute coordinates to the own coordinates
        for(unsigned int i = 0; i <  Dim; i++)
        {
            float offsetPos = pos[i] - localBBox_[i];
            index[i] = (unsigned int)(floor(offsetPos / gridspace_));
        }

        return true;
    }

    // Expect the position index in Local index
    // Return a position in the global space
    bool getLocalPositionValue(unsigned int* index, float* pos)
    {
        assert(hasLocalBBox_);
        for(unsigned int i = 0; i <  Dim; i++)
        {
            // 0.5 to get to the middle of the cell
            pos[i] = localBBox_[i] + ((float)index[i] + 0.5) * gridspace_;
        }
        return true;
    }

    // Expect the position in global space
    // Return the position in the local index
    bool getGlobalPositionIndex(float* pos, unsigned int* index)
    {
        // TODO : test if the position is in the space

        // Move the absolute coordinates to the own coordinates
        for(unsigned int i = 0; i <  Dim; i++)
        {
            float offsetPos = pos[i] - globalBBox_[i];
            index[i] = (unsigned int)(floor(offsetPos / gridspace_));
        }

        return true;
    }

    // Expect the position index in Global index
    // Return a position in the global space
    bool getGlobalPositionValue(unsigned int* index, float* pos)
    {
        assert(hasGlobalBBox_);
        for(unsigned int i = 0; i <  Dim; i++)
        {
            // 0.5 to get to the middle of the cell
            pos[i] = globalBBox_[i] + ((float)index[i] + 0.5) * gridspace_;
        }
        return true;
    }

    void buildGhostRegions(unsigned int ghostSize)
    {
        assert(hasLocalExtends_ && hasLocalBBox_);

        ghostSize_ = ghostSize;

        //The ownBox is becomming is becomming the localBox and we extend
        //the localBox with the ghost region

        setOwnBBox(localBBox_);
        setOwnExtends(localExtends_);

        //Updating the info of the localbox
        //Checking the minimum dimension if we don't go bellow 0
        for(unsigned int i = 0; i < Dim; i++)
        {
            if(localExtends_[i] >= ghostSize_)
                //We can safely extend the region
                localExtends_[i] -= ghostSize_;
            else
                localExtends_[i] = 0;
        }

        //Checking if with the ghost region we don't go outside of the global box
        for(unsigned int i = Dim; i < 2*Dim; i++)
        {
            if(localExtends_[i-Dim] + localExtends_[i] + ghostSize_ <= globalExtends_[i])
                localExtends_[i] += ghostSize_;
            else
                localExtends_[i] += globalExtends_[i] - localExtends_[i-Dim] - localExtends_[i];
        }

        for(unsigned int j = 0; j < Dim; j++)
            localBBox_[j] = globalBBox_[j] + (float)localExtends_[j] * gridspace_;
        for(unsigned int j = Dim; j < 2*Dim; j++)
            localBBox_[j] = (float)(localExtends_[j]) * gridspace_;

    }

    bool hasGhostRegions(){ return ghostSize_ > 0; }

    void printExtend(const std::vector<unsigned int>& extend) const
    {
        std::cout<<"["<<extend[0]<<","<<extend[1]<<","<<extend[2]<<"]"
                <<"["<<extend[0]+extend[3]<<","<<extend[1]+extend[4]
                <<","<<extend[2]+extend[5]<<"]"<<std::endl;
    }

    void printBox(const std::vector<float>& box) const
    {
        std::cout<<"["<<box[0]<<","<<box[1]<<","<<box[2]<<"]"
                <<"["<<box[0]+box[3]<<","<<box[1]+box[4]<<","
                                                       <<box[2]+box[5]<<"]"<<std::endl;
    }

    void printBoxes() const
    {
        if(hasGlobalBBox_)
        {
            std::cout<<"Global bounding box : ";
            printBox(globalBBox_);
        }
        if(hasLocalBBox_)
        {
            std::cout<<"Local bounding box : ";
            printBox(localBBox_);
        }
        if(hasOwnBBox_)
        {
            std::cout<<"Own bounding box : ";
            printBox(ownBBox_);
        }
    }

    void printExtends() const
    {
        if(hasGlobalExtends_)
        {
            std::cout<<"Global Extends : ";
            printExtend(globalExtends_);
        }
        if(hasLocalExtends_)
        {
            std::cout<<"Local Extends : ";
            printExtend(localExtends_);
        }
        if(hasOwnExtends_)
        {
            std::cout<<"Own Extends : ";
            printExtend(ownExtends_);
        }

    }

    float* getGlobalBBox()
    {
        if(hasGlobalBBox_)
            return &globalBBox_[0];
        else
            return NULL;
    }

    float* getLocalBBox()
    {
        if(hasLocalBBox_)
            return &localBBox_[0];
        else
            return NULL;
    }

    float* getOwnBBox()
    {
        if(hasOwnBBox_)
            return &ownBBox_[0];
        else
            return NULL;
    }

    unsigned int* getGlobalExtends()
    {
        if(hasGlobalExtends_)
            return &globalExtends_[0];
        else
            return NULL;
    }

    unsigned int* getLocalExtends()
    {
        if(hasLocalExtends_)
            return &localExtends_[0];
        else
            return NULL;
    }

    unsigned int* getOwnExtends()
    {
        if(hasOwnExtends_)
            return &ownExtends_[0];
        else
            return NULL;
    }

    float getGridspace()
    {
        if(hasGridspace_)
            return gridspace_;
        else
            return 0.0f;
    }

    unsigned int getGhostsize()
    {
        return ghostSize_;
    }

    bool hasGridspace_;                     //Size of a cell
    float gridspace_;

    //Global bounding box of the domain/simulation
    bool hasGlobalBBox_;
    std::vector<float> globalBBox_;         //Min, delta for the complete domain
    bool hasGlobalExtends_;
    std::vector<unsigned int> globalExtends_;//Number of cells in each dimensions

    //Local domain manage withing the process with Ghost region
    bool hasLocalBBox_;
    std::vector<float> localBBox_;          //Min, delta for the localdomain domain with ghost
    bool hasLocalExtends_;
    std::vector<unsigned int> localExtends_;//Start offset + Number of cells in each dimensions with ghost

    //Local domain manage within the process without Ghost region
    unsigned int ghostSize_;                //Width of the ghost region in the 3 dimensions
    bool hasOwnBBox_;
    std::vector<float> ownBBox_;            //Min, delta without ghost region
    bool hasOwnExtends_;
    std::vector<unsigned int> ownExtends_;  //Start offset + Number of cells in each dimensions without ghost

};

}// namespace

#endif


#ifndef __CONTROLLER_TEMPLATE__
#define __CONTROLLER_TEMPLATE__

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <boost/thread.hpp>
#include <urdf/model.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <eigen_conversions/eigen_msg.h>
#include <eigen_conversions/eigen_kdl.h>
#include <kdl_conversions/kdl_msg.h>
// #include <kdl/chainiksolvervel_wdls.hpp>
#include <kdl/chainiksolvervel_pinv_nso.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/kdl.hpp>
#include <kdl/frames.hpp>
#include <geometry_msgs/WrenchStamped.h>
#include <visualization_msgs/Marker.h>
#include <actionlib/server/simple_action_server.h>
#include <utils/TwistController.hpp>
#define NUM_ARMS 2

namespace cartesian_controllers{

/**
  Defines the basic cartesian controller interface to be instantiated in the
  joint controller level.
**/
class ControllerBase
{
public:
  ControllerBase(){}
  virtual ~ControllerBase(){}

  /**
    Method for computing the desired joint states given the control algorithm.

    @param current_state Current joint states.
    @param dt Elapsed time since last control loop.

    @return Desired joint states.
  **/
  virtual sensor_msgs::JointState updateControl(const sensor_msgs::JointState &current_state, ros::Duration dt) = 0;
};

/**
  Defines the interface for all the cartesian controllers, allowing for
  an easier embedding in the PR2 realtime loop.
**/
template <class ActionClass, class ActionFeedback, class ActionResult>
class ControllerTemplate : public ControllerBase
{
public:
  ControllerTemplate();
  virtual ~ControllerTemplate()
  {
  }

protected:
  /**
    Return the last controlled joint state. If the controller does not have
    an active actionlib goal, it will set the references of the joint controller
    to the last desired position (and null velocity).

    @param current The current joint state.
    @return The last commanded joint state before the actionlib goal was
    preempted or completed.
  **/
  sensor_msgs::JointState lastState(const sensor_msgs::JointState &current);

  /**
    Return the last controlled joint state for the joints that are not in the given arm.
    If the controller does not have an active actionlib goal, it will set the references of the joint controller
    to the last desired position (and null velocity). The current joint state will only be
    changed for the given arm.

    @param current The current joint state.
    @param arm The index of the joint chain to be considered.
    @return The last commanded joint state before the actionlib goal was
    preempted or completed.
  **/
  sensor_msgs::JointState lastState(const sensor_msgs::JointState &current, int arm);

  /**
    Initialize the kinematic chain and joint arrays for an arm defined by its end-effector link.
    The kinematic chain is assumed to start at chain_base_link_.

    @param end_effector_link The final link of the kinematic chain.
    @param chain The kinematic chain to be initialized.
    @param joint_positions The joint positions array of the kinematic chain.
    @param joint_velocities The joint velocities array of the kinematic chain.
    @param actuated_joint_names The list of joint names of the kinematic chain.
  **/
  void initializeArm(std::string end_effector_link, KDL::Chain &chain, KDL::JntArray &joint_positions, KDL::JntArrayVel &joint_velocities, std::vector<std::string> &actuated_joint_names);

  /**
    Initialize the kinematic solvers to be used with a kinematic chain.

    @param The chain for which we want to initialized the solvers.
    @param fkpos Positional forward kinematics solver.
    @param fkvel Velocity forward kinematics solver.
    @param ikpos Positional inverse kinematics solver.
    @param ikvel Velocity inverse kinematics solver.
    @param jac_solver The jacobian solver.
  **/
  void initializeSolvers(const KDL::Chain &chain, boost::shared_ptr<KDL::ChainFkSolverPos_recursive> &fkpos, boost::shared_ptr<KDL::ChainFkSolverVel_recursive> &fkvel, boost::shared_ptr<KDL::ChainIkSolverVel_pinv_nso> &ikvel, boost::shared_ptr<KDL::ChainIkSolverPos_LMA> &ikpos, boost::shared_ptr<KDL::ChainJntToJacSolver> &jac_solver);

  /**
    Initializes the wrench vector, subscriber and publisher for a given wrench topic.

    @param measured_wrench The eigen vector where the six-dimensional wrench is going to be stored.
    @param ft_sub The ros subscriber for the sensor.
    @param ft_pub The ros publisher that allows for modified measurements to be published. The ros publisher will publish in ft_topic_name/corrected.
    @param ft_topic_name The ros topic where the original wrench measurements are published.
  **/
  void initializeWrenchComms(Eigen::Matrix<double, 6, 1> &measured_wrench, ros::Subscriber &ft_sub, ros::Publisher &ft_pub, std::string ft_topic_name);

  /**
    Initializes the twist controller by translating the desired gains from the given frame id to the base link.

    @param comp_gains The gains in frame_id.
    @param base_link The base link name.
    @param frame_id The frame where the gains are expressed.
  **/
  void initTwistController(const std::vector<double> &comp_gains, const std::string &base_link, const std::string &frame_id);

  /**
    Provides access to the measured wrench in a given frame.

    @param arm_index The index of the arm measuring the wrench.
    @param frame The desired wrench frame.
    @return The wrench in the given frame
  **/
  Eigen::Matrix<double, 6, 1> wrenchInFrame(int arm_index, const std::string &frame);

  /**
    Goal callback method to be implemented in the cartesian controllers.
  **/
  virtual void goalCB() = 0;

  /**
    Preempt callback method to be implemented in the cartesian controllers.
  **/
  virtual void preemptCB() = 0;

  /**
    Method that manages the starting of the actionlib server of each cartesian
    controller.
  **/
  void startActionlib();

  /**
    Load the parameters that are common to all the cartesian controllers.

    @return False if an error occurs, True otherwise.
  **/
  bool loadGenericParams();

  /**
    Method to be implemented in the cartesian controllers that loads controller
    specific parameters.
  **/
  virtual bool loadParams() = 0;

  /**
    Obtains wrench measurments for a force torque sensor.

    @param msg The force torque message from the sensor node.
  **/
  void forceTorqueCB(const geometry_msgs::WrenchStamped::ConstPtr &msg);

  /**
    Gets the actuated joint limits from the URDF description of the robot
    for a given kinematic chain.

    @param chain The kinematic chain for which the limits are going to be found.
    @param min_limits The minimum position limits of the joint.
    @param max_limits The maximum position limits of the joint.
  **/
  void getJointLimits(const KDL::Chain &chain, KDL::JntArray &min_limits, KDL::JntArray &max_limits);

  /**
    Check if a chain has the given joint_name.

    @param chain The kinematic chain where to look for the joint.
    @param joint_name The joint name we wish to check.
  **/
  bool hasJoint(const KDL::Chain &chain, const std::string &joint_name);

  /**
    Fills in the joint arrays with the state of a given kinematic chain.
    The joint state might include joints outside of the kinematic chain, so there is
    the need to process it.

    @param current_state The robot joint state.
    @param chain The target kinematic chain.
    @param positions The joint positions of the kinematic chain.
    @param velocities The joint velocities of the kinematic chain.
    @return True if the full joint chain was found in the current state, false otherwise.
  **/
  bool getChainJointState(const sensor_msgs::JointState &current_state, const KDL::Chain &chain, KDL::JntArray &positions, KDL::JntArrayVel &velocities);

  /**
    Get the value for indexing the joint positions for the given joint name.

    @param joint_names The vector with the available joint names.
    @param name The name of the query joint.
    @return the index of the queried joint in the joint names vector.
    @throws logic_error if the name does not exist in the joint_names vector.
  **/
  int getJointIndex(const std::vector<std::string> &joint_names, const std::string &name);

  /**
    Fills a marker with the given initial and end point. Clears existing points.

    @param initial_point Initial marker point.
    @param final_point Final marker point.
    @param marker The marker object.
  **/
  void getMarkerPoints(const Eigen::Vector3d &initial_point, const Eigen::Vector3d &final_point, visualization_msgs::Marker &marker);

  /**
    Convert a geometry msgs vector to an std vector.

    @param in The geometry msgs vector.
    @param out The converted std vector.
  **/
  void vectorMsgToStd(const geometry_msgs::Vector3 &in, std::vector<double> &out);

  /**
    Convert an std vector to a geometry msgs vector.

    @param in The std vector.
    @param out The converted geometry msgs vector.
  **/
  void vectorStdToMsg(const std::vector<double> &in, geometry_msgs::Vector3 &out);

  /**
    Wraps the ROS NodeHandle getParam method with an error message.

    @param param_name The name of the parameter address in the parameter server.
    @param var The variable where to store the parameter.
  **/
  bool getParam(const std::string &param_name, std::string &var);
  bool getParam(const std::string &param_name, double &var);
  bool getParam(const std::string &param_name, std::vector<double> &var);
  bool getParam(const std::string &param_name, int &var);
  bool getParam(const std::string &param_name, bool &var);

protected:
  // Robot related
  sensor_msgs::JointState robot_state;
  std::vector<KDL::JntArray> joint_positions_;
  std::vector<KDL::JntArrayVel> joint_velocities_;
  std::vector<std::vector<std::string> > actuated_joint_names_; // list of actuated joints per arm

  // KDL::ChainIkSolverVel_wdls *ikvel_;
  std::vector<boost::shared_ptr<KDL::ChainIkSolverVel_pinv_nso> > ikvel_;
  std::vector<boost::shared_ptr<KDL::ChainIkSolverPos_LMA> > ikpos_;
  std::vector<boost::shared_ptr<KDL::ChainFkSolverPos_recursive> > fkpos_;
  std::vector<boost::shared_ptr<KDL::ChainFkSolverVel_recursive> > fkvel_;
  std::vector<boost::shared_ptr<KDL::ChainJntToJacSolver> > jac_solver_;
  std::vector<KDL::Chain> chain_;
  urdf::Model model_;
  std::vector<std::string> end_effector_link_;
  std::vector<std::string> ft_topic_name_;
  std::vector<std::string> ft_frame_id_;
  std::vector<std::string> ft_sensor_frame_;
  std::string base_link_, chain_base_link_;
  double eps_; // ikSolverVel epsilon
  double alpha_; // ikSolverVel alpha
  int maxiter_; // ikSolverVel maxiter
  double nso_weights_;
  double feedback_hz_;
  bool has_state_;
  sensor_msgs::JointState last_state_;

  //Actionlib
  boost::shared_ptr<actionlib::SimpleActionServer<ActionClass> > action_server_;
  ActionFeedback feedback_;
  ActionResult result_;
  std::string action_name_;

  boost::thread feedback_thread_;
  boost::mutex reference_mutex_;

  // ROS
  ros::NodeHandle nh_;
  std::vector<ros::Subscriber> ft_sub_;
  std::vector<ros::Publisher> ft_pub_;
  tf::TransformListener listener_;

  std::vector<Eigen::Matrix<double, 6, 1> > measured_wrench_;
  double force_d_;
  boost::shared_ptr<TwistController> twist_controller_;
  tf::TransformBroadcaster broadcaster_;
};

// Implementing the template class in its header file saves some headaches
// later on: http://stackoverflow.com/questions/8752837/undefined-reference-to-template-class-constructor

template <class ActionClass, class ActionFeedback, class ActionResult>
ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::ControllerTemplate() :
 joint_positions_(NUM_ARMS),
 joint_velocities_(NUM_ARMS),
 ikvel_(NUM_ARMS),
 ikpos_(NUM_ARMS),
 fkpos_(NUM_ARMS),
 fkvel_(NUM_ARMS),
 jac_solver_(NUM_ARMS),
 chain_(NUM_ARMS),
 end_effector_link_(NUM_ARMS),
 ft_topic_name_(NUM_ARMS),
 ft_frame_id_(NUM_ARMS),
 ft_sensor_frame_(NUM_ARMS),
 ft_sub_(NUM_ARMS),
 ft_pub_(NUM_ARMS),
 actuated_joint_names_(NUM_ARMS),
 measured_wrench_(NUM_ARMS)
{
  nh_ = ros::NodeHandle("~");

  if(!loadGenericParams())
  {
    ros::shutdown();
    exit(0);
  }

  for (int i = 0; i < NUM_ARMS; i++)
  {
    initializeArm(end_effector_link_[i], chain_[i], joint_positions_[i], joint_velocities_[i], actuated_joint_names_[i]);
    initializeSolvers(chain_[i], fkpos_[i], fkvel_[i], ikvel_[i], ikpos_[i], jac_solver_[i]);
    initializeWrenchComms(measured_wrench_[i], ft_sub_[i], ft_pub_[i], ft_topic_name_[i]);
  }

  has_state_ = false;
}

template <class ActionClass, class ActionFeedback, class ActionResult>
void ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::initializeArm(std::string end_effector_link, KDL::Chain &chain, KDL::JntArray &joint_positions, KDL::JntArrayVel &joint_velocities, std::vector<std::string> &actuated_joint_names)
{
  KDL::Tree tree;
  KDL::Joint kdl_joint;
  kdl_parser::treeFromUrdfModel(model_, tree); // convert URDF description of the robot into a KDL tree
  tree.getChain(chain_base_link_, end_effector_link, chain);
  joint_positions.resize(chain.getNrOfJoints());
  joint_velocities.q.resize(chain.getNrOfJoints());
  joint_velocities.qdot.resize(chain.getNrOfJoints());

  for (unsigned int i = 0; i < chain.getNrOfSegments(); i++)
  {
    kdl_joint = chain.getSegment(i).getJoint();

    if (kdl_joint.getTypeName() == "None")
    {
      continue;
    }

    actuated_joint_names.push_back(kdl_joint.getName());
  }
}

template <class ActionClass, class ActionFeedback, class ActionResult>
void ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::initializeSolvers(const KDL::Chain &chain, boost::shared_ptr<KDL::ChainFkSolverPos_recursive> &fkpos, boost::shared_ptr<KDL::ChainFkSolverVel_recursive> &fkvel, boost::shared_ptr<KDL::ChainIkSolverVel_pinv_nso> &ikvel, boost::shared_ptr<KDL::ChainIkSolverPos_LMA> &ikpos, boost::shared_ptr<KDL::ChainJntToJacSolver> &jac_solver)
{
  KDL::JntArray min_limits, max_limits;
  KDL::JntArray optimal_values, weights;

  getJointLimits(chain, min_limits, max_limits);
  ROS_DEBUG("Min limits rows: %d, min limits columns: %d", min_limits.rows(), min_limits.columns());
  optimal_values.resize(chain.getNrOfJoints());
  weights.resize(chain.getNrOfJoints());

  ROS_DEBUG("Joint limits: ");
  for (unsigned int i = 0; i < chain.getNrOfJoints(); i++) // define the optimal joint values as the one that's as far away from joint limits as possible
  {
    optimal_values(i) = (min_limits(i) + max_limits(i))/2;

    ROS_DEBUG("Joint: %d, min_limit: %.2f, max_limit: %.2f, optimal_value: %.2f", i, min_limits(i), max_limits(i), optimal_values(i));

    if (min_limits(i) == max_limits(i)) // Do not weight in joints with no limits in the nullspace optimization method
    {
      weights(i) = 0;
    }
    else
    {
      weights(i) = nso_weights_;
    }

    ROS_DEBUG("Weight: %.2f\n\n", weights(i));
  }

  fkpos.reset(new KDL::ChainFkSolverPos_recursive(chain));
  fkvel.reset(new KDL::ChainFkSolverVel_recursive(chain));
  // ikvel = new KDL::ChainIkSolverVel_wdls(chain, eps_);
  // ikvel = new KDL::ChainIkSolverVel_pinv_nso(chain, eps_);
  ikvel.reset(new KDL::ChainIkSolverVel_pinv_nso(chain, optimal_values, weights, eps_, maxiter_, alpha_));
  ikpos.reset(new KDL::ChainIkSolverPos_LMA(chain));
  jac_solver.reset(new KDL::ChainJntToJacSolver(chain));
}

template <class ActionClass, class ActionFeedback, class ActionResult>
void ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::getJointLimits(const KDL::Chain &chain, KDL::JntArray &min_limits, KDL::JntArray &max_limits)
{
  KDL::Joint kdl_joint;
  boost::shared_ptr<const urdf::Joint> urdf_joint;
  int j = 0;

  min_limits.resize(chain.getNrOfJoints());
  max_limits.resize(chain.getNrOfJoints());

  for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) // get joint limits
  {
    kdl_joint = chain.getSegment(i).getJoint();

    if (kdl_joint.getTypeName() == "None")
    {
      continue;
    }

    urdf_joint = model_.getJoint(kdl_joint.getName());
    min_limits(j) = urdf_joint->limits->lower;
    max_limits(j) = urdf_joint->limits->upper;
    j++;
  }
}

template <class ActionClass, class ActionFeedback, class ActionResult>
bool ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::getChainJointState(const sensor_msgs::JointState &current_state, const KDL::Chain &chain, KDL::JntArray &positions, KDL::JntArrayVel &velocities)
{
  unsigned int processed_joints = 0;
  for (unsigned long i = 0; i < current_state.name.size(); i++)
  {
    if (hasJoint(chain, current_state.name[i]))
    {
      positions(processed_joints) = current_state.position[i];
      velocities.q(processed_joints) = current_state.position[i];
      velocities.qdot(processed_joints) = current_state.velocity[i];
      processed_joints++;
    }
  }

  if (processed_joints != chain.getNrOfJoints())
  {
    ROS_ERROR("Failed to acquire chain joint state");
    return false;
  }

  return true;
}

template <class ActionClass, class ActionFeedback, class ActionResult>
void ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::initializeWrenchComms(Eigen::Matrix<double, 6, 1> &measured_wrench, ros::Subscriber &ft_sub, ros::Publisher &ft_pub, std::string ft_topic_name)
{
  // Subscribe to force and torque measurements
  measured_wrench << 0, 0, 0, 0, 0, 0;
  ft_sub = nh_.subscribe(ft_topic_name, 1, &ControllerTemplate::forceTorqueCB, this); // we will pass the topic name to the subscriber to allow the proper wrench vector to be filled.
  ft_pub = nh_.advertise<geometry_msgs::WrenchStamped>(ft_topic_name + "/converted", 1);
}

template <class ActionClass, class ActionFeedback, class ActionResult>
void ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::initTwistController(const std::vector<double> &comp_gains, const std::string &base_link, const std::string &frame_id)
{
  std::vector<double> linear_gains(3), ang_gains(3);
  for (int i = 0; i < 3; i++)
  {
    linear_gains[i] = comp_gains[i];
    ang_gains[i] = comp_gains[i + 3];
  }

  geometry_msgs::Vector3Stamped lin_gains_msg, ang_gains_msg;

  vectorStdToMsg(linear_gains, lin_gains_msg.vector);
  vectorStdToMsg(ang_gains, ang_gains_msg.vector);

  lin_gains_msg.header.frame_id = frame_id;
  // ang_gains_msg.header.frame_id = ft_frame_id_[surface_arm_];

  try
  {
    lin_gains_msg.header.stamp = ros::Time(0);
    ang_gains_msg.header.stamp = ros::Time(0);
    listener_.transformVector(base_link, lin_gains_msg, lin_gains_msg);
    vectorMsgToStd(lin_gains_msg.vector, linear_gains);
    Eigen::Matrix<double, 6, 1> gains;
    for (int i = 0; i < 3; i++)
    {
      gains[i] = linear_gains[i];
      gains[i + 3] = ang_gains[i];
    }

    twist_controller_.reset(new TwistController(gains));
  }
  catch (tf::TransformException ex)
  {
    ROS_ERROR("TF exception in %s: %s", action_name_.c_str(), ex.what());
    action_server_->setAborted();
    return;
  }
}

template <class ActionClass, class ActionFeedback, class ActionResult>
sensor_msgs::JointState ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::lastState(const sensor_msgs::JointState &current, int arm)
{
  sensor_msgs::JointState temp_state;

  temp_state = last_state_;

  for (unsigned long j = 0; j < temp_state.velocity.size(); j++)
  {
    if (hasJoint(chain_[arm], temp_state.name[j]))
    {
      temp_state.position[j] = current.position[j];
      temp_state.velocity[j] = current.velocity[j];
    }
  }

  return temp_state;
}

template <class ActionClass, class ActionFeedback, class ActionResult>
sensor_msgs::JointState ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::lastState(const sensor_msgs::JointState &current)
{
  if(!has_state_)
  {
    last_state_ = current;
    for (unsigned long i = 0; i < last_state_.velocity.size(); i++)
    {
      last_state_.velocity[i] = 0.0;
    }

    has_state_ = true;
  }

  return last_state_;
}

template <class ActionClass, class ActionFeedback, class ActionResult>
void ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::forceTorqueCB(const geometry_msgs::WrenchStamped::ConstPtr &msg)
{
  KDL::Wrench wrench_kdl;
  KDL::Frame sensor_to_grasp_frame_kdl, sensor_frame_kdl, desired_kdl;
  geometry_msgs::PoseStamped sensor_to_grasp_frame, sensor_frame, desired;
  geometry_msgs::WrenchStamped converted_wrench;
  tf::Transform converted_wrench_frame;
  int sensor_num = -1;

  boost::lock_guard<boost::mutex> guard(reference_mutex_);
  tf::wrenchMsgToKDL(msg->wrench, wrench_kdl);

  for (int i = 0; i < NUM_ARMS; i++)
  {
    if (msg->header.frame_id == ft_sensor_frame_[i])
    {
      sensor_num = i;
      break;
    }
  }

  if (sensor_num == -1)
  {
    ROS_ERROR("Got wrench message from sensor %s, which was not defined in the config file.", msg->header.frame_id.c_str());
    return;
  }

  converted_wrench = *msg;
  sensor_to_grasp_frame.header.frame_id = msg->header.frame_id;
  sensor_to_grasp_frame.header.stamp = ros::Time(0); // To enable transform with the latest ft data available
  sensor_to_grasp_frame.pose.position.x = 0;
  sensor_to_grasp_frame.pose.position.y = 0;
  sensor_to_grasp_frame.pose.position.z = 0;
  sensor_to_grasp_frame.pose.orientation.x = 0;
  sensor_to_grasp_frame.pose.orientation.y = 0;
  sensor_to_grasp_frame.pose.orientation.z = 0;
  sensor_to_grasp_frame.pose.orientation.w = 1;
  converted_wrench.header.frame_id = ft_frame_id_[sensor_num];

  try
  {
    // obtain a vector from the wrench frame id to the desired ft frame
    listener_.transformPose(ft_frame_id_[sensor_num], sensor_to_grasp_frame, sensor_to_grasp_frame);
    // listener_.transformPose(base_link_, sensor_frame, sensor_frame);
  }
  catch (tf::TransformException ex)
  {
    ROS_ERROR("TF exception in %s: %s", action_name_.c_str(), ex.what());
  }

  tf::poseMsgToKDL(sensor_to_grasp_frame.pose, sensor_to_grasp_frame_kdl);
  wrench_kdl = sensor_to_grasp_frame_kdl*wrench_kdl;
  tf::wrenchKDLToMsg(wrench_kdl, converted_wrench.wrench);
  tf::wrenchKDLToEigen(wrench_kdl, measured_wrench_[sensor_num]);
  ft_pub_[sensor_num].publish(converted_wrench);

  // HACK: PR2 had a broken force torque sensor. To allow dual-arm operations to
  // work smoothly, I'm doing this =x
  measured_wrench_[0] = measured_wrench_[sensor_num];
  measured_wrench_[1] = measured_wrench_[sensor_num];
}

template <class ActionClass, class ActionFeedback, class ActionResult>
Eigen::Matrix<double, 6, 1> ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::wrenchInFrame(int arm_index, const std::string &frame)
{
  KDL::Wrench wrench_kdl;
  geometry_msgs::PoseStamped sensor_to_desired_frame, desired_frame_to_base;
  KDL::Frame sensor_to_desired_frame_kdl, desired_frame_to_base_kdl;
  Eigen::Matrix<double, 6, 1> converted_wrench;

  // HACK: Always use arm 0, as arm 1 has a broken sensor
  tf::wrenchEigenToKDL(measured_wrench_[0], wrench_kdl);
  sensor_to_desired_frame.header.frame_id = ft_frame_id_[0];
  sensor_to_desired_frame.header.stamp = ros::Time(0);
  sensor_to_desired_frame.pose.position.x = 0;
  sensor_to_desired_frame.pose.position.y = 0;
  sensor_to_desired_frame.pose.position.z = 0;
  sensor_to_desired_frame.pose.orientation.x = 0;
  sensor_to_desired_frame.pose.orientation.y = 0;
  sensor_to_desired_frame.pose.orientation.z = 0;
  sensor_to_desired_frame.pose.orientation.w = 1;
  desired_frame_to_base = sensor_to_desired_frame;
  desired_frame_to_base.header.frame_id = frame;

  try
  {
    // obtain a vector from the wrench frame id to the desired ft frame
    listener_.transformPose(frame, sensor_to_desired_frame, sensor_to_desired_frame);
    listener_.transformPose(base_link_, desired_frame_to_base, desired_frame_to_base);
  }
  catch (tf::TransformException ex)
  {
    ROS_ERROR("TF exception in %s: %s", action_name_.c_str(), ex.what());
  }

  tf::poseMsgToKDL(sensor_to_desired_frame.pose, sensor_to_desired_frame_kdl);
  tf::poseMsgToKDL(desired_frame_to_base.pose, desired_frame_to_base_kdl);
  wrench_kdl = sensor_to_desired_frame_kdl*wrench_kdl;
  // wrench_kdl = desired_frame_to_base_kdl.M*(sensor_to_desired_frame_kdl*wrench_kdl);
  tf::wrenchKDLToEigen(wrench_kdl, converted_wrench);
  return converted_wrench;
}

template <class ActionClass, class ActionFeedback, class ActionResult>
void ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::startActionlib()
{
  // Initialize actionlib server
  action_server_ = boost::shared_ptr<actionlib::SimpleActionServer<ActionClass> >(new actionlib::SimpleActionServer<ActionClass>(nh_, action_name_, false));

  // Register callbacks
  action_server_->registerGoalCallback(boost::bind(&ControllerTemplate::goalCB, this));
  action_server_->registerPreemptCallback(boost::bind(&ControllerTemplate::preemptCB, this));

  action_server_->start();

  ROS_INFO("%s initialized successfully!", action_name_.c_str());
}

template <class ActionClass, class ActionFeedback, class ActionResult>
bool ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::hasJoint(const KDL::Chain &chain, const std::string &joint_name)
{
  for (unsigned int i = 0; i < chain.getNrOfSegments(); i++)
  {
    if(chain.segments[i].getJoint().getName() == joint_name)
    {
      return true;
    }
  }

  return false;
}

template <class ActionClass, class ActionFeedback, class ActionResult>
int ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::getJointIndex(const std::vector<std::string> &joint_names, const std::string &name)
{
  for (unsigned long i = 0; i < joint_names.size(); i++)
  {
    if (joint_names[i] == name)
    {
      return i;
    }
  }

  throw std::logic_error("getJointIndex: Tried to query a joint name that is not present in the joint names vector.");
}

template <class ActionClass, class ActionFeedback, class ActionResult>
void ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::getMarkerPoints(const Eigen::Vector3d &initial_point, const Eigen::Vector3d &final_point, visualization_msgs::Marker &marker)
{
  geometry_msgs::Point point;

  marker.points.clear();
  tf::pointEigenToMsg(initial_point, point);
  marker.points.push_back(point);
  tf::pointEigenToMsg(final_point, point);
  marker.points.push_back(point);
}

template <class ActionClass, class ActionFeedback, class ActionResult>
void ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::vectorMsgToStd(const geometry_msgs::Vector3 &in, std::vector<double> &out)
{
  if (out.size() != 3)
  {
    throw std::logic_error("vectorMsgToStd: out must be dimension 3");
  }

  out[0] = in.x;
  out[1] = in.y;
  out[2] = in.z;
}

template <class ActionClass, class ActionFeedback, class ActionResult>
void ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::vectorStdToMsg(const std::vector<double> &in, geometry_msgs::Vector3 &out)
{
  if (in.size() != 3)
  {
    throw std::logic_error("vectorStdToMsg: in must be dimension 3");
  }

  out.x = in[0];
  out.y = in[1];
  out.z = in[2];
}

template <class ActionClass, class ActionFeedback, class ActionResult>
bool ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::getParam(const std::string &param_name, std::string &var)
{
  if (!nh_.getParam(param_name, var))
  {
    ROS_ERROR("Missing ROS parameter %s!", param_name.c_str());
    return false;
  }

  return true;
}

template <class ActionClass, class ActionFeedback, class ActionResult>
bool ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::getParam(const std::string &param_name, double &var)
{
  if (!nh_.getParam(param_name, var))
  {
    ROS_ERROR("Missing ROS parameter %s!", param_name.c_str());
    return false;
  }

  return true;
}

template <class ActionClass, class ActionFeedback, class ActionResult>
bool ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::getParam(const std::string &param_name, std::vector<double> &var)
{
  if (!nh_.getParam(param_name, var))
  {
    ROS_ERROR("Missing ROS parameter %s!", param_name.c_str());
    return false;
  }

  return true;
}

template <class ActionClass, class ActionFeedback, class ActionResult>
bool ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::getParam(const std::string &param_name, int &var)
{
  if (!nh_.getParam(param_name, var))
  {
    ROS_ERROR("Missing ROS parameter %s!", param_name.c_str());
    return false;
  }

  return true;
}

template <class ActionClass, class ActionFeedback, class ActionResult>
bool ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::getParam(const std::string &param_name, bool &var)
{
  if (!nh_.getParam(param_name, var))
  {
    ROS_ERROR("Missing ROS parameter %s!", param_name.c_str());
    return false;
  }

  return true;
}

template <class ActionClass, class ActionFeedback, class ActionResult>
bool ControllerTemplate<ActionClass, ActionFeedback, ActionResult>::loadGenericParams()
{
  for (int i = 0; i < NUM_ARMS; i++)
  {
    if(!getParam("/common/end_effector_link_name/arm_" + std::to_string(i + 1), end_effector_link_[i]))
    {
      return false;
    }

    if (!getParam("/common/force_torque_frame/arm_" + std::to_string(i + 1), ft_frame_id_[i])) // this is the frame where we want to transform the force/torque data
    {
      return false;
    }

    if (!getParam("/common/force_torque_sensor_frame/arm_" + std::to_string(i + 1), ft_sensor_frame_[i]))
    {
      return false;
    }

    if (!getParam("/common/force_torque_topic/arm_" + std::to_string(i + 1), ft_topic_name_[i]))
    {
      return false;
    }
  }

  if (!getParam("/common/base_link_name", base_link_))
  {
    return false;
  }

  if (!getParam("/common/chain_base_link_name", chain_base_link_))
  {
    return false;
  }

  if (!getParam("/common/solver/epsilon", eps_))
  {
    return false;
  }

  if (!getParam("/common/solver/alpha", alpha_))
  {
    return false;
  }

  if (!getParam("/common/solver/maxiter", maxiter_))
  {
    return false;
  }

  if (!getParam("/common/solver/nso_weights", nso_weights_))
  {
    return false;
  }

  if (!getParam("/common/feedback_rate", feedback_hz_))
  {
    return false;
  }

  if(!model_.initParam("/robot_description")){
      ROS_ERROR("ERROR getting robot description (/robot_description)");
      return false;
  }

  return true;
}
}
#endif


// -*- C++ -*-
//
// Copyright Sylvain Bougerel 2009 - 2012.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file COPYING or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_TEST_DYN_LINK
#define SPATIAL_ENABLE_ASSERT // detect interal issues that should not occur

#include <boost/test/unit_test.hpp>
#include "../../src/box_multiset.hpp"
#include "spatial_test_types.hpp"

using namespace spatial;

BOOST_AUTO_TEST_CASE( test_box_constructors )
{
  box_multiset<2, int2> boxes;
  box_multiset<0, int2> runtime_boxes;
}

BOOST_AUTO_TEST_CASE( test_box_copy_assignment )
{
  box_multiset<2, int2> boxes;
  boxes.insert(zeros);
  boxes.insert(ones);
  boxes.insert(twos);
  box_multiset<2, int2> copy(boxes);
  BOOST_CHECK_EQUAL(boxes.size(), copy.size());
  BOOST_CHECK(*boxes.begin() == *copy.begin());
  boxes = copy;
  BOOST_CHECK_EQUAL(boxes.size(), copy.size());
  BOOST_CHECK(*boxes.begin() == *copy.begin());
}

BOOST_AUTO_TEST_CASE( test_zero_box_copy_assignment )
{
  box_multiset<0, int2> boxes;
  boxes.insert(zeros);
  boxes.insert(ones);
  boxes.insert(twos);
  box_multiset<0, int2> copy(boxes);
  BOOST_CHECK_EQUAL(boxes.size(), copy.size());
  BOOST_CHECK(*boxes.begin() == *copy.begin());
  boxes = copy;
  BOOST_CHECK_EQUAL(boxes.size(), copy.size());
  BOOST_CHECK(*boxes.begin() == *copy.begin());
}


#pragma once

#include <supereight/tracking/track_data.hpp>

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

namespace se {
namespace tracking {

template<typename Derived, template<typename...> class BufferT>
class TrackerBase {
public:
    TrackerBase(const std::vector<int>& pyramid,
        Eigen::Vector2i computation_size, Eigen::Matrix4f init_pose);

    Eigen::Matrix4f getPose() const { return pose_; };
    Eigen::Matrix4f getInitPose() const { return init_pose_; };

    void setPose(const Eigen::Matrix4f& pose) {
        pose_ = pose;
        pose_.block<3, 1>(0, 3) += init_pose_.block<3, 1>(0, 3);
    }

    bool track(const Eigen::Vector4f& k, se::Image<float>& input_depth,
        float icp_threshold, const Eigen::Matrix4f& render_pose,
        const se::Image<Eigen::Vector3f>& rendered_vertex,
        const se::Image<Eigen::Vector3f>& rendered_normal);

    void renderTrack(
        unsigned char* out, const Eigen::Vector2i& output_size) const;

private:
    Derived* self = static_cast<Derived*>(this);

protected:
    std::vector<int> pyramid_;
    Eigen::Vector2i computation_size_;

    Eigen::Matrix4f pose_;
    Eigen::Matrix4f init_pose_;

    const Image<Eigen::Vector3f>* reference_vertex_;
    const Image<Eigen::Vector3f>* reference_normal_;
    Eigen::Matrix4f reference_view_;

    std::vector<BufferT<float>> scaled_depth_;
    std::vector<BufferT<Eigen::Vector3f>> vertex_;
    std::vector<BufferT<Eigen::Vector3f>> normal_;

    BufferT<float> reduction_output_;
    BufferT<TrackData> tracking_result_;
};

template<typename Derived, template<typename...> class BufferT>
TrackerBase<Derived, BufferT>::TrackerBase(const std::vector<int>& pyramid,
    Eigen::Vector2i computation_size, Eigen::Matrix4f init_pose)
    : pyramid_{pyramid}, computation_size_{computation_size}, pose_{init_pose},
      init_pose_{init_pose}, reduction_output_{8 * 32, 1},
      tracking_result_{computation_size} {
    int downsample = 1;
    for (auto iteration : pyramid_) {
        std::ignore                      = iteration;
        Eigen::Vector2i downsampled_size = computation_size_ / downsample;

        scaled_depth_.push_back(BufferT<float>(downsampled_size));

        vertex_.push_back(BufferT<Eigen::Vector3f>(downsampled_size));
        normal_.push_back(BufferT<Eigen::Vector3f>(downsampled_size));

        downsample *= 2;
    }
}

template<typename Derived, template<typename...> class BufferT>
bool TrackerBase<Derived, BufferT>::track(const Eigen::Vector4f& k,
    se::Image<float>& input_depth, float icp_threshold,
    const Eigen::Matrix4f& render_pose,
    const se::Image<Eigen::Vector3f>& rendered_vertex,
    const se::Image<Eigen::Vector3f>& rendered_normal) {
    self->copyInput_(scaled_depth_[0].accessor(), input_depth);

    // Half sample input depth maps into the pyramid levels
    for (std::size_t i = 1; i < pyramid_.size(); ++i) {
        self->halfSample_(scaled_depth_[i].accessor(),
            scaled_depth_[i - 1].accessor(), e_delta * 3, 1);
    }

    for (std::size_t i = 0; i < pyramid_.size(); ++i) {
        Eigen::Matrix4f invK = getInverseCameraMatrix(k / float(1 << i));
        self->depthToVertex_(
            vertex_[i].accessor(), scaled_depth_[i].accessor(), invK);

        if (k.y() < 0)
            self->template vertexToNormal_<true>(
                normal_[i].accessor(), vertex_[i].accessor());
        else
            self->template vertexToNormal_<false>(
                normal_[i].accessor(), vertex_[i].accessor());
    }

    Eigen::Matrix4f old_pose = pose_;

    reference_vertex_ = &rendered_vertex;
    reference_normal_ = &rendered_normal;
    reference_view_   = getCameraMatrix(k) * render_pose.inverse();

    for (int level = pyramid_.size() - 1; level >= 0; --level) {
        Eigen::Vector2i local_size(computation_size_.x() / (int) pow(2, level),
            computation_size_.y() / (int) pow(2, level));

        for (int i = 0; i < pyramid_[level]; ++i) {
            self->track_(vertex_[level].accessor(), normal_[level].accessor(),
                dist_threshold, normal_threshold);

            self->reduce_(local_size);

            if (self->updatePose_(icp_threshold)) break;
        }
    }

    return self->checkPose_(old_pose, track_threshold);
}

template<typename Derived, template<typename...> class BufferT>
void TrackerBase<Derived, BufferT>::renderTrack(
    unsigned char* out, const Eigen::Vector2i& output_size) const {
    self->renderTrack_(out, output_size);
}

} // namespace tracking
} // namespace se

#define tracker_header__(b) #b
#define tracker_header_(b) tracker_header__(tracker_##b.hpp)
#define tracker_header(b) tracker_header_(b)

#include tracker_header(SE_TRACKER)


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE DynamicSliceTest
#include <boost/test/framework.hpp>
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <numeric>
#include <poplar/Type.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <popops/DynamicSlice.hpp>
#include <popops/DynamicSliceInternal.hpp>
#include <sstream>
#include <vector>

using namespace poplar;
using namespace popops;
using namespace poplibs_support;

void checkPlanner(Type dType, unsigned numEntries, unsigned embeddingSize,
                  const std::vector<std::size_t> numLookups,
                  const sliceInternal::Partition &expectedPartition) {
  BOOST_TEST_MESSAGE(
      "Test " << boost::unit_test::framework::current_test_case().p_name);

  // Always check with the number of tiles on ipu1
  auto device = createTestDevice(TEST_TARGET, 1, 1216);
  Graph graph(device.getTarget());
  auto plan = popops::embedding::plan(graph, dType, numEntries, embeddingSize,
                                      numLookups, {});
  const auto &e = expectedPartition;
  const auto &p = plan.getImpl();
  BOOST_CHECK_EQUAL(e.lookupSplit, p.partition.lookupSplit);
  BOOST_CHECK_EQUAL(e.slicedDimSplit, p.partition.slicedDimSplit);
  BOOST_CHECK_EQUAL(e.unslicedDimSplit, p.partition.unslicedDimSplit);
  BOOST_CHECK_EQUAL(e.unslicedGrainSize, p.partition.unslicedGrainSize);
}

BOOST_AUTO_TEST_CASE(BigEmbedding) {
  checkPlanner(HALF, 100000, 200, {1440}, {1, 12, 100, 2});
}
BOOST_AUTO_TEST_CASE(ManyLookups) {
  checkPlanner(HALF, 1000, 200, {18000}, {12, 1, 100, 2});
}
BOOST_AUTO_TEST_CASE(VeryManyLookups) {
  checkPlanner(HALF, 1000, 200, {40000}, {12, 1, 100, 2});
}
BOOST_AUTO_TEST_CASE(LargeAndManyLookups) {
  checkPlanner(HALF, 100000, 200, {11520}, {1, 12, 100, 2});
}
BOOST_AUTO_TEST_CASE(Square) {
  checkPlanner(HALF, 1000, 1000, {1000}, {1, 2, 500, 2});
}
BOOST_AUTO_TEST_CASE(Small) {
  checkPlanner(FLOAT, 50, 50, {20}, {2, 5, 50, 1});
}