/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 *  @file testDiscreteFactorGraph.cpp
 *  @date Feb 14, 2011
 *  @author Duy-Nguyen Ta
 */

#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteEliminationTree.h>
#include <gtsam/discrete/DiscreteBayesTree.h>
#include <gtsam/inference/BayesNet.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/map.hpp>
using namespace boost::assign;

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST_UNSAFE(DiscreteFactorGraph, debugScheduler) {
  DiscreteKey PC(0, 4), ME(1, 4), AI(2, 4), A(3, 3);

  DiscreteFactorGraph graph;
  graph.add(AI, "1 0 0 1");
  graph.add(AI, "1 1 1 0");
  graph.add(A & AI, "1 1 1 0   1 1 1 1   0 1 1 1");
  graph.add(ME, "0 1 0 0");
  graph.add(ME, "1 1 1 0");
  graph.add(A & ME, "1 1 1 0   1 1 1 1   0 1 1 1");
  graph.add(PC, "1 0 1 0");
  graph.add(PC, "1 1 1 0");
  graph.add(A & PC, "1 1 1 0   1 1 1 1   0 1 1 1");
  graph.add(ME & AI, "0 1 1 1   1 0 1 1   1 1 0 1  1 1 1 0");
  graph.add(PC & ME, "0 1 1 1   1 0 1 1   1 1 0 1  1 1 1 0");
  graph.add(PC & AI, "0 1 1 1   1 0 1 1   1 1 0 1  1 1 1 0");

  // Check MPE.
  auto actualMPE = graph.optimize();
  DiscreteValues mpe;
  insert(mpe)(0, 2)(1, 1)(2, 0)(3, 0);
  EXPECT(assert_equal(mpe, actualMPE));
}

/* ************************************************************************* */
/// Test the () operator of DiscreteFactorGraph
TEST_UNSAFE( DiscreteFactorGraph, DiscreteFactorGraphEvaluationTest) {

  // Three keys P1 and P2
  DiscreteKey P1(0,2), P2(1,2), P3(2,3);

  // Create the DiscreteFactorGraph
  DiscreteFactorGraph graph;
  graph.add(P1, "0.9 0.3");
  graph.add(P2, "0.9 0.6");
  graph.add(P1 & P2, "4 1 10 4");

  // Instantiate DiscreteValues
  DiscreteValues values;
  values[0] = 1;
  values[1] = 1;

  // Check if graph evaluation works ( 0.3*0.6*4 )
  EXPECT_DOUBLES_EQUAL( .72, graph(values), 1e-9);

  // Creating a new test with third node and adding unary and ternary factors on it
  graph.add(P3, "0.9 0.2 0.5");
  graph.add(P1 & P2 & P3, "1 2 3 4 5 6 7 8 9 10 11 12");

  // Below values lead to selecting the 8th index in the ternary factor table
  values[0] = 1;
  values[1] = 0;
  values[2] = 1;

  // Check if graph evaluation works (0.3*0.9*1*0.2*8)
  EXPECT_DOUBLES_EQUAL( 4.32, graph(values), 1e-9);

  // Below values lead to selecting the 3rd index in the ternary factor table
  values[0] = 0;
  values[1] = 1;
  values[2] = 0;

  // Check if graph evaluation works (0.9*0.6*1*0.9*4)
  EXPECT_DOUBLES_EQUAL( 1.944, graph(values), 1e-9);

  // Check if graph product works
  DecisionTreeFactor product = graph.product();
  EXPECT_DOUBLES_EQUAL( 1.944, product(values), 1e-9);
}

/* ************************************************************************* */
TEST(DiscreteFactorGraph, test) {
  // Declare keys and ordering
  DiscreteKey C(0, 2), B(1, 2), A(2, 2);

  // A simple factor graph (A)-fAC-(C)-fBC-(B)
  // with smoothness priors
  DiscreteFactorGraph graph;
  graph.add(A & C, "3 1 1 3");
  graph.add(C & B, "3 1 1 3");

  // Test EliminateDiscrete
  Ordering frontalKeys;
  frontalKeys += Key(0);
  DiscreteConditional::shared_ptr conditional;
  DecisionTreeFactor::shared_ptr newFactor;
  boost::tie(conditional, newFactor) = EliminateDiscrete(graph, frontalKeys);

  // Check Conditional
  CHECK(conditional);
  Signature signature((C | B, A) = "9/1 1/1 1/1 1/9");
  DiscreteConditional expectedConditional(signature);
  EXPECT(assert_equal(expectedConditional, *conditional));

  // Check Factor
  CHECK(newFactor);
  DecisionTreeFactor expectedFactor(B & A, "10 6 6 10");
  EXPECT(assert_equal(expectedFactor, *newFactor));

  // Test using elimination tree
  Ordering ordering;
  ordering += Key(0), Key(1), Key(2);
  DiscreteEliminationTree etree(graph, ordering);
  DiscreteBayesNet::shared_ptr actual;
  DiscreteFactorGraph::shared_ptr remainingGraph;
  boost::tie(actual, remainingGraph) = etree.eliminate(&EliminateDiscrete);

  // Check Bayes net
  DiscreteBayesNet expectedBayesNet;
  expectedBayesNet.add(signature);
  expectedBayesNet.add(B | A = "5/3 3/5");
  expectedBayesNet.add(A % "1/1");
  EXPECT(assert_equal(expectedBayesNet, *actual));

  // Test eliminateSequential
  DiscreteBayesNet::shared_ptr actual2 = graph.eliminateSequential(ordering);
  EXPECT(assert_equal(expectedBayesNet, *actual2));

  // Test mpe
  DiscreteValues mpe;
  insert(mpe)(0, 0)(1, 0)(2, 0);
  auto actualMPE = graph.optimize();
  EXPECT(assert_equal(mpe, actualMPE));
  EXPECT_DOUBLES_EQUAL(9, graph(mpe), 1e-5);  // regression

  // Test sumProduct alias with all orderings:
  auto mpeProbability = expectedBayesNet(mpe);
  EXPECT_DOUBLES_EQUAL(0.28125, mpeProbability, 1e-5);  // regression

  // Using custom ordering
  DiscreteBayesNet bayesNet = graph.sumProduct(ordering);
  EXPECT_DOUBLES_EQUAL(mpeProbability, bayesNet(mpe), 1e-5);

  for (Ordering::OrderingType orderingType :
       {Ordering::COLAMD, Ordering::METIS, Ordering::NATURAL,
        Ordering::CUSTOM}) {
    auto bayesNet = graph.sumProduct(orderingType);
    EXPECT_DOUBLES_EQUAL(mpeProbability, bayesNet(mpe), 1e-5);
  }
}

/* ************************************************************************* */
TEST_UNSAFE(DiscreteFactorGraph, testMaxProduct) {
  // Declare a bunch of keys
  DiscreteKey C(0, 2), A(1, 2), B(2, 2);

  // Create Factor graph
  DiscreteFactorGraph graph;
  graph.add(C & A, "0.2 0.8 0.3 0.7");
  graph.add(C & B, "0.1 0.9 0.4 0.6");

  // Created expected MPE
  DiscreteValues mpe;
  insert(mpe)(0, 0)(1, 1)(2, 1);

  // Do max-product with different orderings
  for (Ordering::OrderingType orderingType :
       {Ordering::COLAMD, Ordering::METIS, Ordering::NATURAL,
        Ordering::CUSTOM}) {
    DiscreteLookupDAG dag = graph.maxProduct(orderingType);
    auto actualMPE = dag.argmax();
    EXPECT(assert_equal(mpe, actualMPE));
    auto actualMPE2 = graph.optimize();  // all in one
    EXPECT(assert_equal(mpe, actualMPE2));
  }
}

/* ************************************************************************* */
TEST(DiscreteFactorGraph, marginalIsNotMPE) {
  // Declare 2 keys
  DiscreteKey A(0, 2), B(1, 2);

  // Create Bayes net such that marginal on A is bigger for 0 than 1, but the
  // MPE does not have A=0.
  DiscreteBayesNet bayesNet;
  bayesNet.add(B | A = "1/1 1/2");
  bayesNet.add(A % "10/9");

  // The expected MPE is A=1, B=1
  DiscreteValues mpe;
  insert(mpe)(0, 1)(1, 1);

  // Which we verify using max-product:
  DiscreteFactorGraph graph(bayesNet);
  auto actualMPE = graph.optimize();
  EXPECT(assert_equal(mpe, actualMPE));
  EXPECT_DOUBLES_EQUAL(0.315789, graph(mpe), 1e-5);  // regression

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
  // Optimize on BayesNet maximizes marginal, then the conditional marginals:
  auto notOptimal = bayesNet.optimize();
  EXPECT(graph(notOptimal) < graph(mpe));
  EXPECT_DOUBLES_EQUAL(0.263158, graph(notOptimal), 1e-5);  // regression
#endif
}

/* ************************************************************************* */
TEST(DiscreteFactorGraph, testMPE_Darwiche09book_p244) {
  // The factor graph in Darwiche09book, page 244
  DiscreteKey A(4, 2), C(3, 2), S(2, 2), T1(0, 2), T2(1, 2);

  // Create Factor graph
  DiscreteFactorGraph graph;
  graph.add(S, "0.55 0.45");
  graph.add(S & C, "0.05 0.95 0.01 0.99");
  graph.add(C & T1, "0.80 0.20 0.20 0.80");
  graph.add(S & C & T2, "0.80 0.20 0.20 0.80 0.95 0.05 0.05 0.95");
  graph.add(T1 & T2 & A, "1 0 0 1 0 1 1 0");
  graph.add(A, "1 0");  // evidence, A = yes (first choice in Darwiche)

  DiscreteValues mpe;
  insert(mpe)(4, 0)(2, 1)(3, 1)(0, 1)(1, 1);
  EXPECT_DOUBLES_EQUAL(0.33858, graph(mpe), 1e-5);  // regression
  // You can check visually by printing product:
  // graph.product().print("Darwiche-product");

  // Check MPE.
  auto actualMPE = graph.optimize();
  EXPECT(assert_equal(mpe, actualMPE));

  // Check Bayes Net
  Ordering ordering;
  ordering += Key(0), Key(1), Key(2), Key(3), Key(4);
  auto chordal = graph.eliminateSequential(ordering);
  EXPECT_LONGS_EQUAL(5, chordal->size());
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
  auto notOptimal = chordal->optimize();  // not MPE !
  EXPECT(graph(notOptimal) < graph(mpe));
#endif

  // Let us create the Bayes tree here, just for fun, because we don't use it
  DiscreteBayesTree::shared_ptr bayesTree =
      graph.eliminateMultifrontal(ordering);
  //  bayesTree->print("Bayes Tree");
  EXPECT_LONGS_EQUAL(2, bayesTree->size());
}

#ifdef OLD

/* ************************************************************************* */
/**
 * Key type for discrete conditionals
 * Includes name and cardinality
 */
class Key2 {
private:
std::string wff_;
size_t cardinality_;
public:
/** Constructor, defaults to binary */
Key2(const std::string& name, size_t cardinality = 2) :
wff_(name), cardinality_(cardinality) {
}
const std::string& name() const {
  return wff_;
}

/** provide streaming */
friend std::ostream& operator <<(std::ostream &os, const Key2 &key);
};

struct Factor2 {
std::string wff_;
Factor2() :
wff_("@") {
}
Factor2(const std::string& s) :
wff_(s) {
}
Factor2(const Key2& key) :
wff_(key.name()) {
}

friend std::ostream& operator <<(std::ostream &os, const Factor2 &f);
friend Factor2 operator -(const Key2& key);
};

std::ostream& operator <<(std::ostream &os, const Factor2 &f) {
os << f.wff_;
return os;
}

/** negation */
Factor2 operator -(const Key2& key) {
return Factor2("-" + key.name());
}

/** OR */
Factor2 operator ||(const Factor2 &factor1, const Factor2 &factor2) {
return Factor2(std::string("(") + factor1.wff_ + " || " + factor2.wff_ + ")");
}

/** AND */
Factor2 operator &&(const Factor2 &factor1, const Factor2 &factor2) {
return Factor2(std::string("(") + factor1.wff_ + " && " + factor2.wff_ + ")");
}

/** implies */
Factor2 operator >>(const Factor2 &factor1, const Factor2 &factor2) {
return Factor2(std::string("(") + factor1.wff_ + " >> " + factor2.wff_ + ")");
}

struct Graph2: public std::list<Factor2> {

/** Add a factor graph*/
//  void operator +=(const Graph2& graph) {
//    for(const Factor2& f: graph)
//        push_back(f);
//  }
friend std::ostream& operator <<(std::ostream &os, const Graph2& graph);

};

/** Add a factor */
//Graph2 operator +=(Graph2& graph, const Factor2& factor) {
//  graph.push_back(factor);
//  return graph;
//}
std::ostream& operator <<(std::ostream &os, const Graph2& graph) {
for(const Factor2& f: graph)
os << f << endl;
return os;
}

/* ************************************************************************* */
TEST(DiscreteFactorGraph, Sugar)
{
Key2 M("Mythical"), I("Immortal"), A("Mammal"), H("Horned"), G("Magical");

// Test this desired construction
Graph2 unicorns;
unicorns += M >> -A;
unicorns += (-M) >> (-I && A);
unicorns += (I || A) >> H;
unicorns += H >> G;

// should be done by adapting boost::assign:
// unicorns += (-M) >> (-I && A), (I || A) >> H , H >> G;

cout << unicorns;
}
#endif

/* ************************************************************************* */
TEST(DiscreteFactorGraph, Dot) {
  // Create Factor graph
  DiscreteFactorGraph graph;
  DiscreteKey C(0, 2), A(1, 2), B(2, 2);
  graph.add(C & A, "0.2 0.8 0.3 0.7");
  graph.add(C & B, "0.1 0.9 0.4 0.6");

  string actual = graph.dot();
  string expected =
      "graph {\n"
      "  size=\"5,5\";\n"
      "\n"
      "  var0[label=\"0\"];\n"
      "  var1[label=\"1\"];\n"
      "  var2[label=\"2\"];\n"
      "\n"
      "  factor0[label=\"\", shape=point];\n"
      "  var0--factor0;\n"
      "  var1--factor0;\n"
      "  factor1[label=\"\", shape=point];\n"
      "  var0--factor1;\n"
      "  var2--factor1;\n"
      "}\n";
  EXPECT(actual == expected);
}

/* ************************************************************************* */
TEST(DiscreteFactorGraph, DotWithNames) {
  // Create Factor graph
  DiscreteFactorGraph graph;
  DiscreteKey C(0, 2), A(1, 2), B(2, 2);
  graph.add(C & A, "0.2 0.8 0.3 0.7");
  graph.add(C & B, "0.1 0.9 0.4 0.6");

  vector<string> names{"C", "A", "B"};
  auto formatter = [names](Key key) { return names[key]; };
  string actual = graph.dot(formatter);
  string expected =
      "graph {\n"
      "  size=\"5,5\";\n"
      "\n"
      "  var0[label=\"C\"];\n"
      "  var1[label=\"A\"];\n"
      "  var2[label=\"B\"];\n"
      "\n"
      "  factor0[label=\"\", shape=point];\n"
      "  var0--factor0;\n"
      "  var1--factor0;\n"
      "  factor1[label=\"\", shape=point];\n"
      "  var0--factor1;\n"
      "  var2--factor1;\n"
      "}\n";
  EXPECT(actual == expected);
}

/* ************************************************************************* */
// Check markdown representation looks as expected.
TEST(DiscreteFactorGraph, markdown) {
  // Create Factor graph
  DiscreteFactorGraph graph;
  DiscreteKey C(0, 2), A(1, 2), B(2, 2);
  graph.add(C & A, "0.2 0.8 0.3 0.7");
  graph.add(C & B, "0.1 0.9 0.4 0.6");

  string expected =
      "`DiscreteFactorGraph` of size 2\n"
      "\n"
      "factor 0:\n"
      "|C|A|value|\n"
      "|:-:|:-:|:-:|\n"
      "|0|0|0.2|\n"
      "|0|1|0.8|\n"
      "|1|0|0.3|\n"
      "|1|1|0.7|\n"
      "\n"
      "factor 1:\n"
      "|C|B|value|\n"
      "|:-:|:-:|:-:|\n"
      "|0|0|0.1|\n"
      "|0|1|0.9|\n"
      "|1|0|0.4|\n"
      "|1|1|0.6|\n\n";
  vector<string> names{"C", "A", "B"};
  auto formatter = [names](Key key) { return names[key]; };
  string actual = graph.markdown(formatter);
  EXPECT(actual == expected);

  // Make sure values are correctly displayed.
  DiscreteValues values;
  values[0] = 1;
  values[1] = 0;
  EXPECT_DOUBLES_EQUAL(0.3, graph[0]->operator()(values), 1e-9);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


#ifndef RESOLVER_DATA_TYPE_HPP
#define RESOLVER_DATA_TYPE_HPP

#include <cstdint>
#include <cmath>
#include <utility>
#include <vector>
#include <queue>
#include <unordered_set>
#include <unordered_map>
#include <boost/noncopyable.hpp>
#include <boost/format.hpp>
#include <boost/functional/hash/extensions.hpp>
#include <opencv2/core/core.hpp>

enum struct AllDirection { Same, Up, UpperRight, Right, DownerRight, Down, DownerLeft, Left, UpperLeft };

struct point_type
{
	int x;
	int y;

	inline std::string const str() const
	{
		return (boost::format("(%1%,%2%)") % this->x % this->y).str();
	}
	inline uint16_t num() const
	{
		return this->x * 16 + this->y;
	}

	friend inline bool operator== (point_type const& lhs, point_type const& rhs)
	{
		return lhs.x == rhs.x && lhs.y == rhs.y;
	}
	friend inline bool operator!= (point_type const& lhs, point_type const& rhs)
	{
		return lhs.x != rhs.x || lhs.y != rhs.y;
	}
	friend inline bool operator< (point_type const& lhs, point_type const& rhs)
	{
		return (lhs.x == rhs.x) ? lhs.y < rhs.y : lhs.x < rhs.x;
	}
	friend inline point_type const operator- (point_type const& lhs, point_type const& rhs)
	{
		return point_type{ lhs.x - rhs.x, lhs.y - rhs.y };
	}
	friend inline point_type const operator+ (point_type const& lhs, point_type const& rhs)
	{
		return point_type{ lhs.x + rhs.x, lhs.y + rhs.y };
	}

	inline int manhattan(point_type const& other) const
	{
		return std::abs(this->x - other.x) + std::abs(this->y - other.y);
	}
	inline int manhattan_pow(point_type const& other) const
	{
		return std::pow(std::abs(this->x - other.x) + std::abs(this->y - other.y), 2.0);
	}

	template<class T = double>
	inline T euclid(point_type const& other) const
	{
		return std::sqrt<T>(
			std::pow<T>(this->x - other.x, 2) +
			std::pow<T>(this->y - other.y, 2)
			);
	}

	inline point_type const up() const
	{
		return point_type{ this->x, this->y - 1 };
	}
	inline point_type const right() const
	{
		return point_type{ this->x + 1, this->y };
	}
	inline point_type const down() const
	{
		return point_type{ this->x, this->y + 1 };
	}
	inline point_type const left() const
	{
		return point_type{ this->x - 1, this->y };
	}

	inline AllDirection direction(point_type const& point) const
	{
		point_type diff = *this - point;
		if (diff.x < 0) {
			if (diff.y < 0) {
				return AllDirection::DownerRight;
			}
			else if (diff.y > 0) {
				return AllDirection::UpperRight;
			}
			else {
				return AllDirection::Right;
			}
		}
		else if (diff.x > 0) {
			if (diff.y < 0) {
				return AllDirection::DownerLeft;
			}
			else if (diff.y > 0) {
				return AllDirection::UpperLeft;
			}
			else {
				return AllDirection::Left;
			}
		}
		else {
			if (diff.y < 0) {
				return AllDirection::Down;
			}
			else if (diff.y > 0) {
				return AllDirection::Up;
			}
			else {
				return AllDirection::Same;
			}
		}
	}

	friend std::size_t hash_value(point_type const& point)
	{
		std::size_t result = 0;
		boost::hash_combine(result, point.x);
		boost::hash_combine(result, point.y);
		return result;
	}
};

typedef cv::Vec3b					pixel_type;
typedef std::vector<uint8_t>				unfold_image_type;
typedef cv::Mat_<cv::Vec3b>				image_type;

// [i][j]の位置に分割された画像(cv::Mat_<cv::Vec3b>)が入っている．
typedef std::vector<std::vector<image_type>> split_image_type;

struct question_data //: private boost::noncopyable
{
	int problem_id;
	std::string player_id;

	std::pair<int, int> size;
	int selectable;
	int cost_select;
	int cost_change;
	std::vector<std::vector<point_type>> block;

	question_data(
		int const problem_id,
		std::string const& player_id,
		std::pair<int, int> const& size,
		int const selectable,
		int const cost_select,
		int const cost_change,
		std::vector<std::vector<point_type>> const& block
		)
		: problem_id(problem_id), player_id(player_id), size(size), selectable(selectable), cost_select(cost_select), cost_change(cost_change), block(block)
	{
	}

	//    question_data(question_data&& other)
	//    {
	//        *this = std::move(other);
	//    }

	question_data()
	{
	}

	//    question_data& operator=(question_data&& other)
	//    {
	//        this->problem_id = other.problem_id;
	//        this->player_id = other.player_id;
	//        this->size = std::move(other.size);
	//        this->selectable = other.selectable;
	//        this->cost_select = other.cost_select;
	//        this->cost_change = other.cost_change;
	//        this->block = std::move(other.block);
	//        return *this;
	//    }

	question_data clone() const
	{
		return question_data{
			problem_id,
			player_id,
			size,
			selectable,
			cost_select,
			cost_change,
			block
		};
	}
};

struct question_raw_data : private boost::noncopyable
{
	std::pair<int, int> split_num; // x * y
	int selectable_num;
	std::pair<int, int> cost; // 選択コスト / 交換コスト
	std::pair<int, int> size; // x * y
	int max_brightness; // 最大輝度
	image_type pixels;

	question_raw_data()
	{
	}
	question_raw_data(question_raw_data&& other)
	{
		*this = std::move(other);
	}
	question_raw_data& operator=(question_raw_data&& other)
	{
		this->split_num = std::move(other.split_num);
		this->selectable_num = other.selectable_num;
		this->cost = std::move(other.cost);
		this->size = std::move(other.size);
		this->max_brightness = other.max_brightness;
		this->pixels = std::move(other.pixels);
		return *this;
	}
};

struct answer_atom
{
	point_type position;
	std::string actions;
};
struct answer_type
{
	std::vector<answer_atom> list;

	std::string serialize() const
	{
		std::string answer_string;

		// 選択回数を数える
		int const select_num = list.size();

		// 1行目 選択回数
		answer_string = (boost::format("%d\r\n") % select_num).str();

		for (answer_atom const& line : list)
		{
			// 3n+2行目 選択画像位置
			answer_string += (boost::format("%X%X\r\n") % line.position.x % line.position.y).str();

			// 交換回数を数える
			int const change_num = line.actions.size();

			// 3n+3行目 交換回数
			answer_string += (boost::format("%d\r\n") % change_num).str();

			// 3n+4行目 交換操作
			for (char const direction : line.actions) answer_string.push_back(direction);
			answer_string += "\r\n";
		}

		return std::move(answer_string);
	}

	int get_score(int cost_select, int cost_change)
	{
		int score = 0;
		score += list.size() * cost_select;
		for (auto const& atom : list) {
			score += atom.actions.size() * cost_change;
		}
		return score;
	}
};

struct step_type {
	bool forward;
	answer_type answer;
	point_type selecting;
	point_type selecting_cur;
	std::vector<std::vector<point_type>> matrix;

	friend bool operator== (step_type const& lhs, step_type const& rhs)
	{
		return lhs.forward == rhs.forward && lhs.selecting == rhs.selecting && lhs.matrix == rhs.matrix;
	}
};

template<class T>
struct direction_type
{
	// 所謂CSSなどの順番に記述
	T up;
	T right;
	T down;
	T left;
};

// ostream に吐けると便利だよね
template<typename CharT, typename Traits>
std::basic_ostream<CharT, Traits>&
operator<< (std::basic_ostream<CharT, Traits>& os, point_type const& point)
{
	os << point.str();
	return os;
}

//sort_algorithm
typedef std::vector<std::vector<std::vector<std::vector<direction_type<uint64_t>>>>> compared_type;
typedef std::vector<std::vector<direction_type<point_type>>> adjacent_type;

//yrange2
struct answer_type_y{
	std::vector<std::vector<point_type>> points;
	uint_fast64_t score;
	cv::Mat mat_image;

	friend inline bool operator== (answer_type_y const& lhs, answer_type_y const& rhs)
	{
		return lhs.points == rhs.points && lhs.score == rhs.score;
	}
};

//yrange5
struct point_score{
	point_type point;
	uint_fast64_t score;
	bool operator<(const point_score& right) const {
		return score == right.score ? point < right.point : score < right.score;
	}
};

namespace std
{
	template <>
	struct hash<point_type>
	{
		std::size_t operator() (point_type const& point) const
		{
			return hash_value(point);
		}
	};

	template<>
	struct hash<std::vector<std::vector<point_type>>>
	{
		std::size_t operator() (std::vector<std::vector<point_type>> const& matrix) const
		{
			std::size_t result = 0;
			boost::hash_combine(result, matrix.size());
			for (auto const& row : matrix) for (auto const& point : row) {
				boost::hash_combine(result, point);
			}
			return result;
		}
	};

	template <>
	struct hash<step_type>
	{
		std::size_t operator() (step_type const& step) const
		{
			std::size_t result = 0;
			boost::hash_combine(result, step.forward);
			boost::hash_combine(result, step.selecting);
			boost::hash_combine(result, step.matrix);
			return result;
		}
	};
}

enum direction {
	up, right, down, left
};

#endif


/*
 * Copyright 2017 Yu Kunlin <yukunlin@mail.ustc.edu.cn>
 */
#include <pgslam/pgslam.h>
#include <pgslam/kdtree2d.h>

#include <float.h>
#include <sys/time.h>
#include <Eigen/Eigen>

#include <iomanip>
#include <iostream>
#include <sstream>

using pgslam::Pose2D;
using pgslam::Echo;
using pgslam::LaserScan;
using pgslam::GraphSlam;
using pgslam::Slam;

Pose2D::Pose2D() {
  this->x_ = 0;
  this->y_ = 0;
  this->theta_ = 0;
}

Pose2D::Pose2D(double x, double y, double theta) {
  x_ = x;
  y_ = y;
  theta_ = theta;
  while (theta_ < -M_PI) theta_ += 2 * M_PI;
  while (theta_ >  M_PI) theta_ -= 2 * M_PI;
}

void Pose2D::set_x(double x) { x_ = x; }

void Pose2D::set_y(double y) { y_ = y; }

void Pose2D::set_theta(double theta) {
  theta_ = theta;
  while (theta_ < -M_PI) theta_ += 2 * M_PI;
  while (theta_ >  M_PI) theta_ -= 2 * M_PI;
}

double Pose2D::x() const { return x_; }
double Pose2D::y() const { return y_; }
double Pose2D::theta () const { return theta_; }

Pose2D Pose2D::operator *(Pose2D p) const {
  Eigen::Vector2d v = p.pos() + p.ToRotation() * pos();
  return Pose2D(v.x(), v.y(), theta_ + p.theta_);
}

Pose2D Pose2D::inverse() const {
  Eigen::Vector2d v = ToRotation().inverse() * pos();
  return Pose2D (-v.x(), -v.y(), -theta_);
}

Eigen::Vector2d Pose2D::pos() const {
  return Eigen::Vector2d(x_, y_);
}

Eigen::Rotation2D<double> Pose2D::ToRotation() const {
  return Eigen::Rotation2D<double>(theta_);
}

Eigen::Transform<double, 2, Eigen::Affine> Pose2D::ToTransform() const {
  Eigen::Transform<double, 2, Eigen::Affine> transform;
  transform.setIdentity();
  transform.translate(pos());
  transform.rotate(ToRotation());
  return transform;
}

std::string Pose2D::ToJson() const {
  std::stringstream ss;
  ss << setiosflags(std::ios::fixed) << std::setprecision(4);
  ss << "{\"x\":" << x_;
  ss << ",\"y\":"  << y_;
  ss << ",\"theta\":" << theta_ << "}";
  return ss.str();
}

Echo::Echo(double range, double angle, double intensity, int64_t time_stamp) {
  range_ = range;
  angle_ = angle;
  intensity_ = intensity;
  time_stamp_ = time_stamp;
}

double Echo::range() const { return range_; }
double Echo::angle() const { return angle_; }
double Echo::intensity() const { return intensity_; }
int64_t Echo::time_stamp() const { return time_stamp_; }

Eigen::Vector2d Echo::point() const {
  double x = range_ * cos(angle_);
  double y = range_ * sin(angle_);
  return Eigen::Vector2d(x, y);
}

LaserScan::LaserScan(std::vector<Echo> echos) {
  points_.resize(Eigen::NoChange, echos.size());
  for (size_t i = 0; i < echos.size(); i++)
    points_.col(i) = echos[i].point();
  world_transformed_flag_ = false;

  match_threshold_ = 0.1;
  dist_threshold_ = 1.0;
}

LaserScan::LaserScan(std::vector<Echo> echos, Pose2D pose) {
  points_.resize(Eigen::NoChange, echos.size());
  for (size_t i = 0; i < echos.size(); i++)
    points_.col(i) = echos[i].point();
  pose_ = pose;
  world_transformed_flag_ = false;

  match_threshold_ = 0.1;
  dist_threshold_ = 1.0;
}

Pose2D LaserScan::pose() const {
  return pose_;
}

void LaserScan::set_pose(Pose2D pose) {
  pose_ = pose;
  world_transformed_flag_ = false;
}

const Eigen::Matrix2Xd& LaserScan::points() {
  UpdateToWorld();
  return points_world_;
}

void LaserScan::UpdateToWorld() {
  if (world_transformed_flag_) return;

  points_world_.resize(Eigen::NoChange, points_.cols());

  max_x_ = 0.0;
  min_x_ = 0.0;
  max_y_ = 0.0;
  min_y_ = 0.0;

  auto t = pose_.ToTransform();
  for (size_t i = 0; i < points_.cols(); i++) {
    Eigen::Vector2d p = t * points_.col(i);
    points_world_.col(i) = p;
    if (p.x() > max_x_) max_x_ = p.x();
    if (p.x() < min_x_) min_x_ = p.x();
    if (p.y() > max_y_) max_y_ = p.y();
    if (p.y() < min_y_) min_y_ = p.y();
  }

  world_transformed_flag_ = true;
}

Pose2D LaserScan::ICP(const LaserScan &scan, double *ratio) {
  Pose2D reference_pose = scan.pose() * pose_.inverse();

  // interpolate
  size_t interpolate_num = 7;
  Eigen::Matrix2Xd points_ref;
  points_ref.resize(Eigen::NoChange, points_.cols() * interpolate_num);
  for (size_t i = 0; i < points_.cols() - 1; i++) {
    for (size_t j = 0; j < interpolate_num; j++) {
      Eigen::Vector2d curr = points_.col(i + 0);
      Eigen::Vector2d next = points_.col(i + 1);
      double gain = static_cast<double>(j) / interpolate_num;
      points_ref.col(interpolate_num * i + j) = (next - curr) * gain + curr;
    }
  }

  // construct kd tree
  kd_tree_2d::KDTree2D tree;
  tree.Construct(points_ref);

  // iterate
  Pose2D pose = reference_pose;
  for (int i = 0; i < 100; i++) {
    Eigen::Matrix2Xd points = pose.ToTransform() * scan.points_;

    // store the closest point
    Eigen::Matrix2Xd near = points;
    // to trace some points have a same closest point
    std::vector<std::vector<int>> trace_back(points_ref.cols());
    // true: effective point; false: non-effective point;
    std::vector<bool> mask(points.cols());

    // search and save nearest point
    int match_count = 0;
    for (size_t i = 0; i < points.cols(); i++) {
      Eigen::Vector2d point = points.col(i);

      size_t index = tree.NearestIndex(points.col(i));

      trace_back[index].push_back(i);
      Eigen::Vector2d closest = points_ref.col(index);

      double distance = (point - closest).norm();
      if (distance < match_threshold_)
        match_count++;
      if (distance < dist_threshold_) {
        near.col(i) = closest;
        mask[i] = true;
      } else {
        mask[i] = false;
      }
    }
    if (ratio != nullptr)
      *ratio = static_cast<double>(match_count) / points.cols();

    // disable the points have one same nearest point
    for (size_t i = 0; i < trace_back.size(); i++)
      if (trace_back[i].size() > 3) {
        for (size_t j = 0; j < trace_back[i].size(); j++) {
          mask[trace_back[i][j]] = false;
          near.col(trace_back[i][j]) = points.col(trace_back[i][j]);
        }
      }

    // disable the farest 10% point
    std::vector<double> max_distance;
    std::vector<int>    max_index;
    max_distance.resize(points.cols() / 10, 0.0);
    max_index.resize(points.cols() / 10, 0);
    for (size_t i = 0; i < points.cols(); i++) {
      double distance = (points.col(i) - near.col(i)).norm();
      for (size_t j = 1; j < max_distance.size(); j++) {
        if (distance > max_distance[j]) {
          max_distance[j-1] = max_distance[j];
          max_index[j-1] = max_index[j];
          if (j == max_distance.size()-1) {
            max_distance[j] = distance;
            max_index[j] = i;
          }
        } else {
          max_distance[j-1] = distance;
          max_index[j-1] = i;
          break;
        }
      }
    }
    for (size_t i = 1; i < max_index.size(); i++)
      mask[max_index[i]] = false;

    // calc center
    Eigen::Vector2d center(0, 0);
    int count = 0;
    for (size_t i = 0; i < points.cols(); i++)
      if (mask[i]) {
        center += points.col(i);
        count++;
      }
    if (count == 0) {
      std::cout << "Error: no valid point, return reference pose." << std::endl;
      if (ratio != nullptr)
        *ratio = 0.0;
      return reference_pose;
    }
    center /= count;

    // calc the tramsform
    Eigen::Vector2d move = Eigen::Vector2d(0.0, 0.0);
    double rot = 0.0;
    for (size_t i = 0; i < points.cols(); i++) {
      if (mask[i] == false) continue;
      Eigen::Vector2d delta = near.col(i) - points.col(i);
      double length = delta.norm();
      if (length > 0) {
        delta.normalize();
        delta *= (length < 0.05) ? length : sqrt(length * 20) / 20;
      }
      move += delta;
      Eigen::Vector2d p = points.col(i) - center;
      Eigen::Vector2d q = near.col(i) - center;
      if (p.norm() < DBL_EPSILON * 2) continue;

      rot += (p.x() * q.y() - p.y() * q.x()) / p.norm() / sqrt(p.norm());
    }
    move /= count;
    rot /= count;

    // speed up
    move *= 2.0;
    rot *= 1.0;

    // update pose
    Pose2D pose_delta = pose * Pose2D(move.x(), move.y(), rot) * pose.inverse();
    pose = pose_delta * pose;
    if (pose_delta.pos().norm() < 0.001 && pose_delta.theta() < 0.001)
      break;
  }
  return pose;
}

double LaserScan::max_x_in_world() {
  UpdateToWorld();
  return max_x_;
}

double LaserScan::min_x_in_world() {
  UpdateToWorld();
  return min_x_;
}

double LaserScan::max_y_in_world() {
  UpdateToWorld();
  return max_y_;
}

double LaserScan::min_y_in_world() {
  UpdateToWorld();
  return min_y_;
}

#ifdef USE_ISAM
GraphSlam::GraphSlam() {
  slam_ = new isam::Slam();
}

bool GraphSlam::check(size_t id) {
  // size enough
  if (id < pose_nodes_.size()) {
    if (pose_nodes_[id] == NULL) {  // already been removed
      pose_nodes_[id] = new isam::Pose2d_Node();
      return true;
    } else {
      return false;  // still there
    }
  }

  // create new space
  int size = pose_nodes_.size();
  pose_nodes_.resize(id + 1);
  for (size_t i = size; i < id + 1; i++)
    pose_nodes_[i] = new isam::Pose2d_Node();
  return true;
}

void GraphSlam::remove(size_t node_id) {
  slam_->remove_node(pose_nodes_[node_id]);
  delete pose_nodes_[node_id];
  pose_nodes_[node_id] = NULL;
  slam_->batch_optimization();
}

void GraphSlam::clear() {
  delete slam_;
  slam_ = new isam::Slam();
  std::vector<isam::Pose2d_Node*>().swap(pose_nodes_);
}

std::vector<std::pair<size_t, Pose2D>> GraphSlam::nodes() {
  std::vector<std::pair<size_t, Pose2D>> pose_id;
  // store in response
  for (size_t i = 0; i < pose_nodes_.size(); i++) {
    if (pose_nodes_[i] == NULL) continue;
    Pose2D pose;
    pose.set_x(pose_nodes_[i]->value().x());
    pose.set_y(pose_nodes_[i]->value().y());
    pose.set_theta(pose_nodes_[i]->value().t());
    pose_id.push_back(std::pair<size_t, Pose2D>(i, pose));
  }
  return pose_id;
}

std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d>> GraphSlam::factors() {
  std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d>> factors;
  const std::list<isam::Factor*> factors_ = slam_->get_factors();
  for (std::list<isam::Factor*>::const_iterator it = factors_.begin();
      it != factors_.end(); it++) {
    std::vector<isam::Node*> nodes = (*it)->nodes();
    if (nodes.size() == 1) continue;
    Eigen::Vector2d first;
    first.x() = ((isam::Pose2d_Node*)nodes[0])->value().x();
    first.y() = ((isam::Pose2d_Node*)nodes[0])->value().y();
    Eigen::Vector2d second;
    second.x() = ((isam::Pose2d_Node*)nodes[1])->value().x();
    second.y() = ((isam::Pose2d_Node*)nodes[1])->value().y();
    factors.push_back(std::make_pair(first, second));
  }
  return factors;
}

void GraphSlam::AddPose2dFactor(size_t node_id, Pose2D pose_ros, double cov) {
  isam::Pose2d pose(pose_ros.x(), pose_ros.y(), pose_ros.theta());
  if (cov <= 0) {
    cov = 1.0;
  }

  // check new node
  bool ret = check(node_id);
  if (ret) slam_->add_node(pose_nodes_[node_id]);

  // add factor
  isam::Noise noise = isam::Information(cov * isam::eye(3));
  isam::Pose2d_Factor * factor =
    new isam::Pose2d_Factor(pose_nodes_[node_id], pose, noise);
  slam_->add_factor(factor);
  // slam_->batch_optimization();
}

void GraphSlam::AddPose2dPose2dFactor(size_t node_id_ref,
    size_t node_id, Pose2D pose_ros, double cov) {
  isam::Pose2d pose(pose_ros.x(), pose_ros.y(), pose_ros.theta());
  if (cov <= 0) {
    cov = 1.0;
  }

  // check new node
  bool ret = check(node_id_ref);
  if (ret) slam_->add_node(pose_nodes_[node_id_ref]);
  ret = check(node_id);
  if (ret) slam_->add_node(pose_nodes_[node_id]);

  // add factor
  isam::Noise noise = isam::Information(cov * isam::eye(3));
  isam::Pose2d_Pose2d_Factor * factor =
    new isam::Pose2d_Pose2d_Factor(pose_nodes_[node_id_ref],
        pose_nodes_[node_id], pose, noise);
  slam_->add_factor(factor);
  // slam_->batch_optimization();
}

void GraphSlam::Optimization() {
  slam_->batch_optimization();
}
#endif

Slam::Slam() {
  keyscan_threshold_ = 0.4;
  factor_threshold_ = 0.9;
}

void Slam::set_keyscan_threshold(double keyscan_threshold) {
  this->keyscan_threshold_ = keyscan_threshold;
  if (keyscan_threshold_ * 2 > factor_threshold_)
    factor_threshold_ = keyscan_threshold_ * 2;
}

void Slam::set_factor_threshold(double factor_threshold) {
  this->factor_threshold_ = factor_threshold;
  if (keyscan_threshold_ * 2 > factor_threshold_)
    keyscan_threshold_ = factor_threshold_/2;
}

Pose2D Slam::pose() const {
  return pose_;
}

const std::vector<LaserScan> & Slam::scans() {
  return scans_;
}

#ifdef USE_ISAM
std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d>> Slam::factors() {
  return graph_slam_.factors();
}
#endif

Pose2D Slam::EncoderToPose2D(double left, double right, double tread) {
  double theta = (right - left) / tread;
  double theta_2 = theta / 2.0;
  double arc = (right + left) / 2.0;
  double radius = arc / theta;
  double secant = 2 * sin(theta_2) * radius;
  if (theta == 0)
    secant = arc;
  double x = secant * cos(theta_2);
  double y = secant * sin(theta_2);

  return Pose2D(x, y, theta);
}

void Slam::UpdatePoseWithPose(Pose2D pose) {
  this->pose_ = pose * this->pose_;
}

void Slam::UpdatePoseWithEncoder(double left, double right, double tread) {
  pose_ = EncoderToPose2D(left, right, tread) * pose_;
  if (pose_update_callback)
    pose_update_callback(pose_);
}

void Slam::UpdatePoseWithLaserScan(const LaserScan &_scan) {
  LaserScan scan = _scan;
  scan.set_pose(pose_);

  // first scan
  if (scans_.empty()) {
    scans_.push_back(scan);
#ifdef USE_ISAM
    graph_slam_.AddPose2dFactor(0, pose_, 1);
#endif
    std::cout << "add key scan " << scans_.size() << ": "
      << pose_.ToJson() << std::endl;
    if (map_update_callback)
      map_update_callback();
    return;
  }

  // search for the closest scan
  LaserScan *closest_scan = &(scans_[0]);
  double min_dist = DBL_MAX;
  for (size_t i = 0; i < scans_.size(); i++) {
    double dist = (scans_[i].pose().pos() -
        scan.pose().pos()).norm();
    double delta_theta = fabs(scans_[i].pose().theta() -
        scan.pose().theta());;
    while (delta_theta < -M_PI) delta_theta += 2 * M_PI;
    while (delta_theta >  M_PI) delta_theta -= 2 * M_PI;
    delta_theta *= keyscan_threshold_ / (M_PI_4 * 3.0);

    dist = sqrt(dist * dist + delta_theta * delta_theta);
    if (dist < min_dist) {
      min_dist = dist;
      closest_scan = &(scans_[i]);
    }
  }

  if (min_dist < keyscan_threshold_) {
    // update pose
    double ratio;
    Pose2D pose_delta = closest_scan->ICP(scan, &ratio);
    pose_ = pose_delta * closest_scan->pose();
  } else {
    // add key scan
#ifdef USE_ISAM
    size_t constrain_count = 0;
    for (size_t i = 0; i < scans_.size(); i++) {
      double distance = (pose_.pos() - scans_[i].pose().pos()).norm();
      if (distance < factor_threshold_) {
        constrain_count++;
        double ratio;
        Pose2D pose_delta = scans_[i].ICP(scan, &ratio);
        graph_slam_.AddPose2dPose2dFactor(i, scans_.size(), pose_delta, ratio);
      }
    }
    if (constrain_count > 1)
      graph_slam_.Optimization();

    auto nodes = graph_slam_.nodes();
    for (size_t i = 0; i < nodes.size(); i++) {
      // update pose of node
      for (size_t j = 0; j < scans_.size(); j++) {
        if (j != nodes[i].first) continue;
        scans_[j].set_pose(nodes[i].second);
        break;
      }
      // add the new scan with pose
      if (nodes[i].first == scans_.size()) {
        pose_ = nodes[i].second;
        scan.set_pose(nodes[i].second);
        scans_.push_back(scan);
      }
    }
#else
    scans_.push_back(scan);
#endif
    std::cout << "add key scan " << scans_.size() << ": "
      << pose_.ToJson() << std::endl;

    if (map_update_callback)
      map_update_callback();
  }
  if (pose_update_callback)
    pose_update_callback(pose_);
}

void Slam::RegisterPoseUpdateCallback(std::function<void(Pose2D)> f) {
  pose_update_callback = f;
}
void Slam::RegisterMapUpdateCallback(std::function<void(void)> f) {
  map_update_callback = f;
}


/*
################################################################################
#                                                                              #
#                                                                              #
#                                                                              #
################################################################################
*/

#include "MolecularModelCVode.h"
#include "ODE_system.h"
#include "Param.h"

#include <iostream>
#include <string>
#include <algorithm> // min

#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

#define SEC_PER_DAY 86400

namespace po = boost::program_options;
typedef MolecularModelCVode<CancerVCT::ODE_system> QSP;
using CancerVCT::Param;

int main(int argc, char* argv[])
{
	std::string _inputParam;
	std::string _outPath;
	std::string _outfile;
	// command line options
	try {
		po::options_description desc("Allowed options");
		desc.add_options()
			("help,h", "produce help message")
			("input-file,i", po::value<std::string>(&_inputParam), "parameter file name")
			("output-path,o", po::value<std::string>(&_outPath)->default_value("output"), "output file path")
			("output-file-name,n", po::value<std::string>(&_outfile)->default_value("solution.csv"), "output file name")
			;
		po::variables_map vm;
		po::store(po::parse_command_line(argc, argv, desc), vm);
		po::notify(vm);

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

		if (!vm.count("input-file"))
		{
			std::cout << desc << "\n";
			std::cerr << "no input file specified!\n";
			return 1;
		}

	}catch (std::exception& e) {
		std::cerr << "error: " << e.what() << "\n";
		return 1;
	}

	// output directory
	boost::filesystem::path pOut(_outPath);
	boost::filesystem::create_directories(pOut);// create output directory

	// parameters
	Param params;
	params.initializeParams(_inputParam);

	CancerVCT::ODE_system::setup_class_parameters(params);
	// simulation object
	QSP model;
	// update variables from parameter file
	model.getSystem()->setup_instance_tolerance(params);
	model.getSystem()->setup_instance_varaibles(params);
	model.getSystem()->eval_init_assignment();

	//model.getSystem()->updateVar();

	// output file
	std::string output_file_name = _outPath + "/" + _outfile;
	std::ofstream f;
	f.open(output_file_name, std::ios::trunc);

	// header
	f << "t" << model.getSystem()->getHeader() << std::endl;

	// solve ODE system
	double t_start = params.getVal(0) * SEC_PER_DAY;
	double t_step = params.getVal(1) * SEC_PER_DAY ;
	int nrStep = int(params.getVal(2));
	auto t_end = t_start + t_step * nrStep;

	f << t_start << model << std::endl;

	while (t_start < t_end)
	{
		double t_remaining = t_end - t_start;
		double t_step_sim = std::min(t_remaining, t_step);

		model.solve(t_start, t_step_sim);

		t_start += t_step_sim;
		f << t_start << model << std::endl;
		//std::cout << t_start / SEC_PER_DAY << std::endl;
	}
	f.close();
	//std::cout << "End of simulation" << std::endl;

	return 0;
}


/*
 * EigenMatrixBaseAddonsTest.cpp
 *
 *  Created on: Sep 23, 2015
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, Autonomous Systems Lab
 */

#include "grid_map_core/grid_map_core.hpp"

// gtest
#include <gtest/gtest.h>

// Eigen
#include <Eigen/Core>

using namespace std;
using namespace Eigen;

TEST(EigenMatrixBaseAddons, numberOfFinites)
{
  Eigen::Matrix3f matrix(Eigen::Matrix3f::Ones());
  matrix(0, 0) = NAN;
  matrix(1, 0) = NAN;
  EXPECT_EQ(7, matrix.numberOfFinites());

  Matrix<double, 13, 10> matrix2;
  matrix2.setOnes();
  EXPECT_EQ(matrix2.rows() * matrix2.cols(), matrix2.numberOfFinites());

  Matrix<double, 13, 10> matrix3;
  matrix3.setConstant(NAN);
  matrix3.col(3).setConstant(0.0);
  EXPECT_EQ(matrix3.rows(), matrix3.numberOfFinites());
}

TEST(EigenMatrixBaseAddons, sumOfFinites)
{
  Matrix<double, 7, 18> matrix;
  matrix.setRandom();
  EXPECT_NEAR(matrix.sum(), matrix.sumOfFinites(), 1e-10);
  double finiteSum = matrix.sum() - matrix(0, 0) - matrix(1, 2) - matrix(3, 6) - matrix(6, 12);
  matrix(0, 0) = NAN;
  matrix(1, 2) = NAN;
  matrix(3, 6) = NAN;
  matrix(6, 12) = NAN;
  EXPECT_NEAR(finiteSum, matrix.sumOfFinites(), 1e-10);
  matrix.setConstant(NAN);
  EXPECT_TRUE(std::isnan(matrix.sumOfFinites()));
  matrix(5, 7) = 1.0;
  EXPECT_NEAR(1.0, matrix.sumOfFinites(), 1e-10);
}

TEST(EigenMatrixBaseAddons, meanOfFinites)
{
  Eigen::Matrix3f matrix(Eigen::Matrix3f::Ones());
  matrix(0, 0) = NAN;
  matrix(1, 1) = NAN;
  EXPECT_DOUBLE_EQ(1.0, matrix.meanOfFinites());

  Matrix<double, 13, 10> matrix2;
  matrix2.setRandom();
  EXPECT_NEAR(matrix2.mean(), matrix2.meanOfFinites(), 1e-10);
}

TEST(EigenMatrixBaseAddons, minCoeffOfFinites)
{
  Matrix<double, 7, 18> matrix;
  matrix.setRandom();
  double min = matrix.minCoeff();
  EXPECT_NEAR(min, matrix.minCoeffOfFinites(), 1e-10);

  int i, j;
  matrix.maxCoeff(&i, &j);
  matrix(i, j) = NAN;
  EXPECT_NEAR(min, matrix.minCoeffOfFinites(), 1e-10);

  matrix.setConstant(NAN);
  EXPECT_TRUE(std::isnan(matrix.minCoeffOfFinites()));
  matrix(i, j) = -1.0;
  EXPECT_NEAR(-1.0, matrix.minCoeffOfFinites(), 1e-10);
}

TEST(EigenMatrixBaseAddons, maxCoeffOfFinites)
{
  Matrix<double, 7, 18> matrix;
  matrix.setRandom();
  double max = matrix.maxCoeff();
  EXPECT_NEAR(max, matrix.maxCoeffOfFinites(), 1e-10);

  int i, j;
  matrix.minCoeff(&i, &j);
  matrix(i, j) = NAN;
  EXPECT_NEAR(max, matrix.maxCoeffOfFinites(), 1e-10);

  matrix.setConstant(NAN);
  EXPECT_TRUE(std::isnan(matrix.maxCoeffOfFinites()));
  matrix(i, j) = -1.0;
  EXPECT_NEAR(-1.0, matrix.maxCoeffOfFinites(), 1e-10);
}

TEST(EigenMatrixBaseAddons, clamp)
{
  Eigen::VectorXf vector(Eigen::VectorXf::LinSpaced(9, 1.0, 9.0));
  Eigen::Matrix3f matrix;
  matrix << vector.segment(0, 3), vector.segment(3, 3), vector.segment(6, 3);
  matrix(1, 1) = NAN;
  matrix = matrix.unaryExpr(grid_map::Clamp<float>(2.1, 7.0));
  EXPECT_NEAR(2.1, matrix(0, 0), 1e-7);
  EXPECT_NEAR(2.1, matrix(1, 0), 1e-7);
  EXPECT_NEAR(3.0, matrix(2, 0), 1e-7);
  EXPECT_TRUE(std::isnan(matrix(1, 1)));
  EXPECT_NEAR(7.0, matrix(2, 2), 1e-7);
}


#include "trajopt/belief_constraints.hpp"
#include "trajopt/utils.hpp"
#include <boost/bind.hpp>
#include "sco/expr_ops.hpp"
#include "sco/expr_op_overloads.hpp"
#include "utils/eigen_conversions.hpp"

using namespace std;
using namespace sco;
using namespace Eigen;
using namespace OpenRAVE;
using namespace util;

namespace {
template <typename T>
vector<T> concat(const vector<T>& a, const vector<T>& b) {
	vector<T> out;
	vector<int> x;
	out.insert(out.end(), a.begin(), a.end());
	out.insert(out.end(), b.begin(), b.end());
	return out;
}

}
namespace trajopt {

BeliefDynamicsConstraint::BeliefDynamicsConstraint(const VarVector& theta0_vars,	const VarVector& theta1_vars, const VarVector& u_vars, BeliefRobotAndDOFPtr brad) :
    				Constraint("BeliefDynamics"), brad_(brad), theta0_vars_(theta0_vars), theta1_vars_(theta1_vars), u_vars_(u_vars), type_(EQ)
{}

vector<double> BeliefDynamicsConstraint::value(const vector<double>& xin) {
	VectorXd theta0_hat = getVec(xin, theta0_vars_);
	VectorXd theta1_hat = getVec(xin, theta1_vars_);
	VectorXd u_hat = getVec(xin, u_vars_);
	return toDblVec(brad_->BeliefDynamics(theta0_hat, u_hat) - theta1_hat);
}

/*
bool isMatrixNan(const MatrixXd& m) {
	for (int i=0; i<m.rows(); i++)
		for (int j=0; j<m.cols(); j++)
			if (isnan(m(i,j))) return true;
	return false;
}

bool assertMatrixNotNan(const MatrixXd& m) {
	for (int i=0; i<m.rows(); i++)
		for (int j=0; j<m.cols(); j++)
			assert(!isnan(m(i,j)));
}
*/

ConvexConstraintsPtr BeliefDynamicsConstraint::convex(const vector<double>& xin, Model* model) {
	VectorXd theta0_hat = getVec(xin, theta0_vars_);
	VectorXd theta1_hat = getVec(xin, theta1_vars_);
	VectorXd u_hat = getVec(xin, u_vars_);

	//	cout << "theta0_hat:\n" << theta0_hat.transpose() << endl;
	//	cout << "theta1_hat:\n" << theta1_hat.transpose() << endl;
	//	cout << "u_hat:\n" << u_hat.transpose() << endl;

	// linearize belief dynamics around theta0_hat and u_hat
	MatrixXd A = brad_->dgdb(theta0_hat, u_hat);
	VectorXd c = brad_->BeliefDynamics(theta0_hat, u_hat);
	MatrixXd B = brad_->dgdu(theta0_hat, u_hat);

	//	cout << "theta0_hat " << theta0_hat.transpose() << endl;
	//	cout << "u_hat " << u_hat.transpose() << endl;
	//	if (isMatrixNan(A)) cout << "A" << endl << A << endl;
	//	if (isMatrixNan(B)) cout << "B" << endl << B << endl;
	//	if (isMatrixNan(c)) cout << "c" << endl << c << endl;

	//	cout << "A matrix:\n" << A << endl;
	//	cout << "B matrix:\n" << B << endl;
	//	cout << "c vector:\n" << c.transpose() << endl;

	//	  // test convexification
	//	  VectorXd theta0 = theta0_hat + brad_->VectorXdRand(9)*0.1;
	//	  cout << "theta0" << theta0.transpose() << endl;
	//	  VectorXd u = u_hat +brad_->VectorXdRand(3)*0.1;
	//	  cout << "u "<< u.transpose() << endl;
	//	  VectorXd diff1_approx = A * (theta0 - theta0_hat) + B * (u - u_hat) + c - theta1_hat;
	//	  cout << "diff1_approx" << diff1_approx.transpose() << endl;
	//		VectorXd diff1 = brad_->BeliefDynamics(theta0_hat, u_hat) - theta1_hat;
	//	  cout << "diff1 " << diff1.transpose() << endl;

	// equality constraint
	// theta1_vars_ = A * (theta0_vars_ - theta0_hat) + B * (u_vars_ - u_hat) + c
	// 0 = A * (theta0_vars_ - theta0_hat) + B * (u_vars_ - u_hat) + c - theta1_vars_
	ConvexConstraintsPtr out(new ConvexConstraints(model));
	assert(A.rows() == B.rows());
	for (int i=0; i < A.rows(); ++i) {
		AffExpr aff_theta0;
		aff_theta0.constant = c[i] - A.row(i).dot(theta0_hat);
		aff_theta0.coeffs = toDblVec(A.row(i));
		aff_theta0.vars = theta0_vars_;
		AffExpr aff_u;
		aff_u.constant = - B.row(i).dot(u_hat);
		aff_u.coeffs = toDblVec(B.row(i));
		aff_u.vars = u_vars_;
		AffExpr aff_theta1;
		aff_theta1.constant = 0;
		aff_theta1.coeffs = vector<double>(theta1_vars_.size(),0);
		aff_theta1.coeffs[i] = 1;
		aff_theta1.vars = theta1_vars_;
		AffExpr aff_theta0_u = exprAdd(aff_theta0, aff_u);
		AffExpr aff = exprSub(aff_theta0_u, aff_theta1);
		aff = cleanupAff(aff);
		out->addEqCnt(aff);
	}

	return out;
}

struct BeliefDynamicsErrCalculator : public VectorOfVector {
	BeliefRobotAndDOFPtr brad_;
	BeliefDynamicsErrCalculator(BeliefRobotAndDOFPtr brad) : brad_(brad) {}

	VectorXd operator()(const VectorXd& vals) const {
		int b_dim = brad_->GetBDim();
		int u_dim = brad_->GetUDim();
		VectorXd theta0 = vals.topRows(b_dim);
		VectorXd theta1 = vals.middleRows(b_dim, b_dim);
		VectorXd u0 = vals.bottomRows(u_dim);
		assert(vals.size() == (2*b_dim + u_dim));

		VectorXd err = brad_->BeliefDynamics(theta0, u0) - theta1;

		return err;
	}
};

BeliefDynamicsConstraint2::BeliefDynamicsConstraint2(const VarVector& theta0_vars,	const VarVector& theta1_vars, const VarVector& u_vars,
		BeliefRobotAndDOFPtr brad, const BoolVec& enabled) :
		ConstraintFromFunc(VectorOfVectorPtr(new BeliefDynamicsErrCalculator(brad)),
				concat(concat(theta0_vars, theta1_vars), u_vars), EQ, "BeliefDynamics2")
{
}

CovarianceCost::CovarianceCost(const VarVector& rtSigma_vars, const MatrixXd& Q, BeliefRobotAndDOFPtr brad) :
    		Cost("Covariance"), rtSigma_vars_(rtSigma_vars), Q_(Q), brad_(brad)
{
	int s_dim = brad_->GetSDim();
	int q_dim = brad_->GetQDim();
	assert(rtSigma_vars_.size() == s_dim);
	assert(Q_.rows() == q_dim);

	VarArray rtSigma_matrix_vars = toBasicArray(brad_->toSigmaMatrix(toVectorXd(rtSigma_vars_)));
	QuadExprArray Sigma_vars = rtSigma_matrix_vars * rtSigma_matrix_vars.transpose();
	expr_ =  (Q_ * Sigma_vars).trace();
	expr_ = cleanupQuad(expr_);
}

double CovarianceCost::value(const vector<double>& xin) {
	int x_dim = brad_->GetXDim(), s_dim = brad_->GetSDim(), b_dim = brad_->GetBDim();
	VectorXd rtSigma_vec = getVec(xin, rtSigma_vars_);
	MatrixXd rtSigma(x_dim, x_dim);
	VectorXd x_unused(x_dim);
	VectorXd theta(b_dim);
	theta.bottomRows(s_dim) = rtSigma_vec;
	brad_->decomposeBelief(theta, x_unused, rtSigma);

	return (Q_ * rtSigma * rtSigma.transpose()).trace();
}

ConvexObjectivePtr CovarianceCost::convex(const vector<double>& x, Model* model) {
	ConvexObjectivePtr out(new ConvexObjective(model));
	out->addQuadExpr(expr_);
	return out;
}

ControlCost::ControlCost(const VarArray& vars, const VectorXd& coeffs) :
    		Cost("Control"), vars_(vars), coeffs_(coeffs) {
	for (int i=0; i < vars.rows(); ++i) {
		QuadExpr expr;
		expr.vars1 = vars_.row(i);
		expr.vars2 = vars_.row(i);
		expr.coeffs = toDblVec(coeffs);
		exprInc(expr_, expr);
	}
}
double ControlCost::value(const vector<double>& xvec) {
	MatrixXd traj = getTraj(xvec, vars_);
	return (traj.array().square().matrix() * coeffs_.asDiagonal()).sum();
}
ConvexObjectivePtr ControlCost::convex(const vector<double>& x, Model* model) {
	ConvexObjectivePtr out(new ConvexObjective(model));
	out->addQuadExpr(expr_);

	return out;
}

}


#ifndef ORGANIZATION_MODEL_UTILS_ORGANIZATION_STRUCTURE_GENERATION_HPP
#define ORGANIZATION_MODEL_UTILS_ORGANIZATION_STRUCTURE_GENERATION_HPP

#include <limits>
#include <boost/function.hpp>
#include <boost/thread.hpp>
#include <numeric/IntegerPartitioning.hpp>
#include <base/Time.hpp>
#include <base-logging/Logging.hpp>
#include "../Agent.hpp"

namespace moreorg {
namespace utils {

typedef AtomicAgent::List Coalition;
typedef std::vector< Coalition > CoalitionStructure;

std::ostream& operator<<(std::ostream& os, const Coalition& coalition);
std::ostream& operator<<(std::ostream& os, const CoalitionStructure& list);

struct Bounds
{
    Bounds(double exact);
    Bounds();

    double maximum;
    double average;
    double minimum;

    std::string toString() const;
};

std::ostream& operator<<(std::ostream& os, const Bounds& bounds);

/**
 * This is an adapted implementation of the coalition structure generation as described in:
 * "An Anytime Algorithm for Optimal Coalition Structure Generation", (Rahwan et al., 2009)
 * Please note that the current implementation uses recursion
 *
 * The adaption allows to account for a use of model pools and looking at the
 * identification coalition functions that have a boolean as characteristic value (1.0 or 0.0)
 * and thus represent an activation
 */
class CoalitionStructureGeneration
{
public:
    typedef boost::function1<double, const Coalition&> CoalitionValueFunction;
    typedef boost::function1<double, const CoalitionStructure&> CoalitionStructureValueFunction;

    struct Statistics
    {
        std::vector<numeric::IntegerPartition> allIntegerPartitions;
        std::vector<numeric::IntegerPartition> prunedIntegerPartitions;
        std::vector<numeric::IntegerPartition> searchedIntegerPartitions;

        std::vector<numeric::IntegerPartition> remainingIntegerPartitions() const;

        std::string toString() const;
    };

private:
    AtomicAgent::List mAgents;
    Statistics mStatistics;

    // Agents listed by size of the coalition
    typedef std::map<size_t, std::set< Coalition > > AgentCoalitionMap;
    AgentCoalitionMap mAgentCoalitionMap;

    typedef std::map<size_t, Bounds> CoalitionBoundMap;
    CoalitionBoundMap mCoalitionBoundMap;

    CoalitionValueFunction mCoalitionValueFunction;
    CoalitionStructureValueFunction mCoalitionStructureValueFunction;

    typedef std::map<numeric::IntegerPartition, Bounds> IntegerPartitionBoundsMap;
    IntegerPartitionBoundsMap mIntegerPartitionBoundsMap;

    mutable boost::mutex mSolutionMutex;
    mutable boost::mutex mStatisticsMutex;
    boost::thread mThread;
    base::Time mStartTime;
    base::Time mCompletionTime;
    CoalitionStructure mCurrentBestCoalitionStructure;
    double mCurrentBestCoalitionStructureValue;
    double mCurrentSolutionQuality;
    double mGlobalUpperBound;

    /**
     * Compute the integer partitions and the agent coalition map for coalition size up to
     * the maximum number of agents
     */
    void prepare();

    double bestLowerBound(const CoalitionBoundMap& boundMap);
    double bestLowerBound(const IntegerPartitionBoundsMap& boundMap);
    double bestUpperBound(const IntegerPartitionBoundsMap& boundMap);

    Bounds computeIntegerPartitionBounds(const numeric::IntegerPartition& partition);
    IntegerPartitionBoundsMap prune(const IntegerPartitionBoundsMap& boundsMap);
    CoalitionBoundMap prune(const CoalitionBoundMap& boundMap);

    numeric::IntegerPartition selectIntegerPartition(const IntegerPartitionBoundsMap& boundMap, double maximumBound) const;

    /**
     * Search a subspace
     * \param partition The integer partition to be used
     * \param k index of the entry to start partial coalition with (mainly required for recursive use), e.g., of the integer partition [2,1,1]
     * \param alpha
     * \param agents list of agents that have to be considered for appending to
     * the existing coalition structure
     * \param currentStructure the already constructed coalition structure
     * \param bestStar Quality of the solution, i.e. 1.05 means 95% percent of the optimal solution
     * \return true if this subspace contained a better solution than already existed
     */
    bool searchSubspace(const numeric::IntegerPartition& partition, size_t k, size_t alpha, const AtomicAgent::List& agents, const CoalitionStructure& currentStructure, double betaStar);

    bool updateCurrentBestCoalitionStructure(const CoalitionStructure& coalitionStructure, double value);

public:
    /**
     * Find best coalitionstructure -- will block until coalition structure is found
     */
    CoalitionStructure findBest(double quality = 1.0);

    /**
     * Reset in order to restart a new search
     */
    void reset();

    /**
     * Search for a solution and allow retrieval of intermediate results via currentBestSolution
     */
    void anytimeSearch(double quality = 1.0);

    /**
     * Check if anytimeSearch completed
     * \return true, upon completition, false otherwise -- if search has not been started at all, it will return false
     */
    bool anytimeSearchCompleted() const { return mCompletionTime != base::Time(); }

    /**
     * Compute total elapsed time since last search start
     * \return time object representing the elapsed time, i.e. use toSeconds() or corresponding function to retrieve detailled value
     */
    base::Time elapsed() const { return base::Time::now() - mStartTime; }

    /**
     * Stop the search before completion
     */
    void stopSearch();

    /**
     * Retrieve the current best solution
     * return the current best solution, if none has been found it will return an empty CoalitionStructure
     */
    CoalitionStructure currentBestSolution() const;

    double currentBestSolutionValue() const;
    double currentBestSolutionQuality() const;

    /**
     * \params agents List of agents that are available
     * \param coalitionValueFunction Function that allows to compute the value of an individual coalition
     * \param coalitionStructureValueFunction Function that allows to compute the value of a coalition structure
     */
    CoalitionStructureGeneration(const AtomicAgent::List& agents,
            CoalitionValueFunction coalitionValueFunction,
            CoalitionStructureValueFunction coalitionStructureValueFunction);

    /**
     * Stringify status of this CoalitionStructureGeneration object
     * \return stringified instance
     */
    std::string toString() const;

    /**
     * Stringify a CoalitionStructure
     * \return stringified instance of CoalitionStructure
     */
    static std::string toString(const CoalitionStructure& c);

    Statistics getStatistics() const;
};

} // end namespace utils
} // end namespace moreorg
#endif // ORGANIZATION_MODEL_COALITION_STRUCTURE_GENERATION_HPP


#include "ILP_input.h"
#include <Eigen/Eigen>
#include "cuthill-mckee.h"
#include "bfs_ordering.hxx"
#include "minimum_degree_ordering.hxx"
#include "time_measure_util.h"
#include <iostream>

namespace LPMP {

    bool ILP_input::var_exists(const std::string& var) const
    {
        return var_name_to_index_.count(var) > 0;
    }

    size_t ILP_input::get_var_index(const std::string& var) const
    {
        assert(var_exists(var));
        return var_name_to_index_.find(var)->second;
    }

    std::string ILP_input::get_var_name(const size_t index) const
    {
        assert(index < var_index_to_name_.size());
        return var_index_to_name_[index];
    }

    size_t ILP_input::add_new_variable(const std::string& var)
    {
        assert(!var_exists(var));
        const size_t var_index = var_name_to_index_.size();
        var_name_to_index_.insert({var, var_index});
        assert(var_index_to_name_.size() == var_index);
        var_index_to_name_.push_back(var);
        if(objective_.size() <= var_index) // variables with 0 objective coefficient need not appear in objective line!
            objective_.resize(var_index+1,0.0);
        var_permutation_.push_back(var_permutation_.size()-1);
        return var_index;
    }

    size_t ILP_input::get_or_create_variable_index(const std::string& var)
    {
        if(var_exists(var))
            return get_var_index(var);
        else
            return add_new_variable(var);
    }

    size_t ILP_input::nr_variables() const
    {
        return var_name_to_index_.size();
    }

    void ILP_input::add_to_objective(const double coefficient, const std::string& var)
    {
        add_to_objective(coefficient, get_or_create_variable_index(var));
    }

    void ILP_input::add_to_objective(const double coefficient, const size_t var)
    {
        assert(var < objective_.size());
        objective_[var] += coefficient;
    }

    double ILP_input::objective(const size_t var) const
    {
        if(var >= nr_variables())
        {
            std::cout << var << ", " << nr_variables() << "\n";
            throw std::runtime_error("variable not present");
        }
        if(var >= objective_.size())
            return 0.0;
        return objective_[var];
    }

    double ILP_input::objective(const std::string& var) const
    {
        return objective(get_var_index(var));
    }

    size_t ILP_input::begin_new_inequality()
    {
        linear_constraints_.push_back({});
        // set defaul inequality identifier
        std::string ineq_name = "INEQ_NR_" + std::to_string(linear_constraints_.size()-1);
        set_inequality_identifier(ineq_name);
        return linear_constraints_.size()-1;
    }

    void ILP_input::set_inequality_identifier(const std::string& identifier)
    {
        assert(linear_constraints_.size() > 0);
        if(identifier == "")
            return;
        const std::string& old_identifier = linear_constraints_.back().identifier;
        if(old_identifier != "")
        {
            assert(inequality_identifier_to_index_.count(old_identifier) > 0);
            inequality_identifier_to_index_.erase(old_identifier);
        }
        linear_constraints_.back().identifier = identifier;
        if(inequality_identifier_to_index_.count(identifier) > 0)
            throw std::runtime_error("Duplicate inequality identifier " + identifier);
        inequality_identifier_to_index_.insert({identifier, linear_constraints_.size()-1});
    }

    void ILP_input::set_inequality_type(const inequality_type ineq)
    {
        assert(linear_constraints_.size() > 0);
        linear_constraints_.back().ineq = ineq;
    }

    void ILP_input::add_to_constraint(const int coefficient, const size_t var)
    {
        assert(linear_constraints_.size() > 0);
        auto& constr = linear_constraints_.back();
        constr.variables.push_back({coefficient, var});

        if(constr.variables.size() > 1)
            if(constr.variables.back() < constr.variables[constr.variables.size()-2])
                constr.normalize();
    }
    void ILP_input::add_to_constraint(const int coefficient, const std::string& var)
    {
        add_to_constraint(coefficient, get_or_create_variable_index(var));
    }

    void ILP_input::set_right_hand_side(const int x)
    {
        assert(linear_constraints_.size() > 0);
        linear_constraints_.back().right_hand_side = x;
    }

    size_t ILP_input::nr_constraints() const
    {
        return linear_constraints_.size();
    }

    bool ILP_input::preprocess()
    {
        for (auto it = linear_constraints_.begin(); it != linear_constraints_.end(); it++)
        {
            bool remove = false;

            // empty constraints
            if (it->variables.empty())
            {
                // feasibility check
                switch (it->ineq)
                {
                    case inequality_type::smaller_equal:
                        if(it->right_hand_side < 0)
                            return false;
                        break;
                    case inequality_type::greater_equal:
                        if(it->right_hand_side > 0)
                            return false;
                        break;
                    case inequality_type::equal:
                        if(it->right_hand_side != 0)
                            return false;
                        break;
                    default:
                        throw std::runtime_error("inequality type not supported");
                }
                remove = true;
            }

            // variable bounds and fixations
            if (it->variables.size() == 1)
            {
                auto variable = it->variables.back();
                switch (it->ineq)
                {
                    case inequality_type::smaller_equal:
                        if(std::min(variable.coefficient, 0) > it->right_hand_side)
                            return false;
                        if(std::max(variable.coefficient, 0) <= it->right_hand_side)
                            remove = true;
                        break;
                    case inequality_type::greater_equal:
                        if(std::max(variable.coefficient, 0) < it->right_hand_side)
                            return false;
                        if(std::min(variable.coefficient, 0) >= it->right_hand_side)
                            remove = true;
                        break;
                    case inequality_type::equal:
                        if(it->right_hand_side != 0 && it->right_hand_side != variable.coefficient)
                            return false;
                        break;
                    default:
                        throw std::runtime_error("inequality type not supported");
                }
            }

            // TODO implement preprocessing of variable/group fixations (e.g. x = 1, x + y = 0)

            // remove redundant constraint
            if (remove)
            {
                *it = linear_constraints_.back();
                linear_constraints_.pop_back();
                it--;
            }
        }

        return true;
    }

    inline two_dim_variable_array<size_t> ILP_input::variable_adjacency_matrix() const
    {
        MEASURE_FUNCTION_EXECUTION_TIME
        std::vector<Eigen::Triplet<int>> var_constraint_adjacency_list;

        for(size_t i=0; i<this->linear_constraints_.size(); ++i) {
            const auto& l = this->linear_constraints_[i];
            for(const auto& v : l.variables) {
                var_constraint_adjacency_list.push_back({v.var, i, 1});
            }
        }

        Eigen::SparseMatrix<int> A(this->nr_variables(), this->linear_constraints_.size());
        A.setFromTriplets(var_constraint_adjacency_list.begin(), var_constraint_adjacency_list.end());
        const Eigen::SparseMatrix<int> adj_matrix = A*A.transpose();
        assert(adj_matrix.cols() == nr_variables() && adj_matrix.rows() == nr_variables());

        std::vector<size_t> adjacency_size(nr_variables(), 0);

        for(size_t i=0; i<adj_matrix.outerSize(); ++i) {
            for(typename Eigen::SparseMatrix<int>::InnerIterator it(adj_matrix,i); it; ++it) {
                if(it.value() > 0 && i != it.index()) {
                    adjacency_size[i]++;
                }
            }
        }

        two_dim_variable_array<size_t> adjacency(adjacency_size.begin(), adjacency_size.end());
        std::fill(adjacency_size.begin(), adjacency_size.end(), 0);

        for(size_t i=0; i<adj_matrix.outerSize(); ++i) {
            for(typename Eigen::SparseMatrix<int>::InnerIterator it(adj_matrix,i); it; ++it) {
                if(it.value() > 0 && i != it.index()) {
                    adjacency(i,adjacency_size[i]++) = it.index();
                }
            }
        }

        for(size_t i=0; i<adjacency.size(); ++i) {
            assert(std::is_sorted(adjacency[i].begin(), adjacency[i].end()));
        }

        return adjacency;
    }

    /*
       inline two_dim_variable_array<size_t> ILP_input::variable_adjacency_matrix() const
       {
    //std::unordered_set<std::array<size_t,2>> adjacent_vars;
    tsl::robin_set<std::array<size_t,2>> adjacent_vars;
    for(const auto& l : this->linear_constraints_) {
    for(size_t i=0; i<l.variables.size(); ++i) {
    for(size_t j=i+1; j<l.variables.size(); ++j) {
    const size_t var1 = l.variables[i].var;
    const size_t var2 = l.variables[j].var;
    adjacent_vars.insert({std::min(var1,var2), std::max(var1, var2)});
    }
    }
    }

    std::vector<size_t> adjacency_size(this->nr_variables(),0);
    for(const auto [i,j] : adjacent_vars) {
    ++adjacency_size[i];
    ++adjacency_size[j];
    }

    two_dim_variable_array<size_t> adjacency(adjacency_size.begin(), adjacency_size.end());
    std::fill(adjacency_size.begin(), adjacency_size.end(), 0);
    for(const auto e : adjacent_vars) {
    const auto [i,j] = e;
    assert(i<j);
    assert(adjacency_size[i] < adjacency[i].size());
    assert(adjacency_size[j] < adjacency[j].size());
    adjacency(i, adjacency_size[i]++) = j;
    adjacency(j, adjacency_size[j]++) = i;
    }

    for(size_t i=0; i<adjacency.size(); ++i)
    std::sort(adjacency[i].begin(), adjacency[i].end());

    return adjacency;
    }
    */

    inline two_dim_variable_array<size_t> ILP_input::bipartite_variable_bdd_adjacency_matrix() const
    {
        MEASURE_FUNCTION_EXECUTION_TIME
        std::vector<std::pair<size_t,size_t>> var_bdd_adj_list;
        std::vector<size_t> degrees(this->nr_variables() + this->nr_constraints(), 0);

        // determine list of adjacencies and vertex degrees
        for(size_t i=0; i<this->linear_constraints_.size(); ++i) {
            const auto& l = this->linear_constraints_[i];
            for(const auto& v : l.variables) {
                var_bdd_adj_list.emplace_back(v.var, i);
                degrees[v.var]++;
                degrees[this->nr_variables() + i]++;
            }
        }

        // create adjacency matrix
        two_dim_variable_array<size_t> adjacency(degrees.begin(), degrees.end());
        std::fill(degrees.begin(), degrees.end(), 0);
        for (auto it = var_bdd_adj_list.begin(); it != var_bdd_adj_list.end(); it++)
        {
            size_t var = it->first;
            size_t bdd = this->nr_variables() + it->second;
            adjacency(var, degrees[var]++) = bdd;
            adjacency(bdd, degrees[bdd]++) = var;
        }

        return adjacency;
    }

    permutation ILP_input::reorder(ILP_input::variable_order var_ord)
    {
        if(var_ord == variable_order::input)
            var_permutation_ = permutation(nr_variables());
        else if(var_ord == variable_order::bfs)
            var_permutation_ = this->reorder_bfs();
        else if(var_ord == variable_order::cuthill)
            var_permutation_ = this->reorder_Cuthill_McKee();
        else
        {
            assert(var_ord == variable_order::mindegree);
            var_permutation_ = this->reorder_minimum_degree_ordering();
        }

        return var_permutation_;
    }

    void ILP_input::reorder(const permutation& order)
    {
        MEASURE_FUNCTION_EXECUTION_TIME;
        assert(order.size() == this->nr_variables());
        assert(order.is_permutation());
        std::vector<double> new_objective(this->nr_variables(), 0.0);
        std::vector<size_t> inverse_order(this->nr_variables());
        std::vector<std::string> new_var_index_to_name(this->nr_variables());

        for(size_t i=0; i<this->nr_variables(); ++i)
        {
            assert(var_name_to_index_.count(var_index_to_name_[order[i]]) > 0);
            var_name_to_index_.find(var_index_to_name_[order[i]]).value() = i;
        }

//#pragma omp parallel for schedule(guided)
        for(size_t i=0; i<this->nr_variables(); ++i)
        {
            if(order[i] < this->objective_.size())
                new_objective[i] = this->objective_[order[i]];

            if(order[i] < this->var_index_to_name_.size())
                new_var_index_to_name[i] = std::move(this->var_index_to_name_[order[i]]);

            inverse_order[order[i]] = i;

        }
        std::swap(this->objective_, new_objective);
        std::swap(new_var_index_to_name, this->var_index_to_name_);

//#pragma omp parallel for schedule(guided)
        for(size_t lc_index=0; lc_index<this->linear_constraints_.size(); ++lc_index)
        {
            auto& l = this->linear_constraints_[lc_index];
            for(auto& x : l.variables) {
                x.var = inverse_order[x.var];
            }
            l.normalize();
        }
    }

    inline permutation ILP_input::reorder_bfs()
    {
        const auto adj = bipartite_variable_bdd_adjacency_matrix();
        const auto order = bfs_ordering(adj, this->nr_variables());
        reorder(order);
        return order;
    }

    inline permutation ILP_input::reorder_Cuthill_McKee()
    {
        const auto adj = bipartite_variable_bdd_adjacency_matrix();
        const auto order = Cuthill_McKee(adj, this->nr_variables());
        reorder(order);
        return order;
    }

    inline permutation ILP_input::reorder_minimum_degree_ordering()
    {
        const auto adj = bipartite_variable_bdd_adjacency_matrix();
        const auto order = minimum_degree_ordering(adj, this->nr_variables());
        reorder(order);
        return order;
    }

    size_t ILP_input::nr_constraint_groups() const
    {
        return coalesce_sets_.size();
    }

    std::tuple<const size_t*, const size_t*> ILP_input::constraint_group(const size_t i) const
    {
        assert(i < nr_constraint_groups());
        return std::make_tuple(coalesce_sets_[i].begin(), coalesce_sets_[i].end());
    }

    std::tuple<Eigen::SparseMatrix<int>, Eigen::MatrixXi> ILP_input::export_constraints() const
    {
        using T = Eigen::Triplet<int>;
        std::vector<T> coefficients;

        for(size_t c=0; c<nr_constraints(); ++c)
        {
            for(const auto& l : constraints()[c].variables)
                coefficients.push_back(T(c, l.var, l.coefficient));
        }

        Eigen::SparseMatrix<int> A(nr_constraints(), nr_variables());
        A.setFromTriplets(coefficients.begin(), coefficients.end());

        Eigen::MatrixXi b(nr_constraints(), 2);
        for(size_t c=0; c<nr_constraints(); ++c)
        {
            if(constraints()[c].ineq == inequality_type::equal)
            {
                b(c,0) = constraints()[c].right_hand_side;
                b(c,1) = constraints()[c].right_hand_side;
            }
            else if(constraints()[c].ineq == inequality_type::smaller_equal)
            {
                b(c,0) = std::numeric_limits<int>::min();
                b(c,1) = constraints()[c].right_hand_side;
            }
            else if(constraints()[c].ineq == inequality_type::greater_equal)
            {
                b(c,0) = constraints()[c].right_hand_side;
                b(c,1) = std::numeric_limits<int>::max();
            }
            else
            {
                assert(false);
            }
        }

        return {A,b};
    }

}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/integral_constant.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/logical.hpp>
using namespace boost::hana;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Logical
    //////////////////////////////////////////////////////////////////////////
    {
        auto t = test::ct_eq<3>{};
        auto e = test::ct_eq<4>{};

        // eval_if
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                eval_if(true_, always(t), always(e)),
                t
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                eval_if(false_, always(t), always(e)),
                e
            ));
        }

        // not_
        {
            BOOST_HANA_CONSTANT_CHECK(equal(not_(true_), false_));
            BOOST_HANA_CONSTANT_CHECK(equal(not_(false_), true_));
        }

        // operators
        static_assert(has_operator<IntegralConstant<int>, decltype(not_)>{}, "");
        static_assert(has_operator<IntegralConstant<int>, decltype(and_)>{}, "");
        static_assert(has_operator<IntegralConstant<int>, decltype(or_)>{}, "");

        // laws
        test::TestLogical<IntegralConstant<int>>{tuple_c<int, -2, 0, 1, 2>};
        test::TestLogical<IntegralConstant<bool>>{tuple_c<bool, true, false>};
    }
}


//
// sum の改良版
//
// g++ -std=c++11 -I<BoostPath> safe_printfs.cpp

#include <iostream>
#include <boost/lexical_cast.hpp>

template<typename Ret>
Ret any_sumex()
{
  return Ret();
}

template<typename Ret, typename First, typename... Rest>
Ret any_sumex(const First& first, const Rest&... rest)
{
  return boost::lexical_cast<Ret>(first) + any_sumex<Ret>(rest...);
}

////////////////////////////////////////////////////////////////
//// 使用例
////////////////////////////////////////////////////////////////

using namespace std;

int main()
{
  cout << any_sumex<int>(1.0, 2.2, 3.3, 4.9) << endl;    // 10
  cout << any_sumex<double>(1.0, 2, 3.3, 4.9) << endl;   // 11.2
  cout << any_sumex<string>("Hello", " ", "world", "!") << endl;  // "Hello world!"
  cout << any_sumex<string>("Hello ", "world ", 3, " ", 1.2) << endl;  // "Hello world 3 1.2"

  return 0;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_rect"

#include <boost/test/unit_test.hpp>
#include "nanocv/vision/rect.h"

namespace test
{
        void build_rect(ncv::coord_t x, ncv::coord_t y, ncv::coord_t w, ncv::coord_t h)
        {
                const ncv::rect_t rect(x, y, w, h);

                BOOST_CHECK_EQUAL(rect.left(),          x);
                BOOST_CHECK_EQUAL(rect.top(),           y);
                BOOST_CHECK_EQUAL(rect.right(),         x + w);
                BOOST_CHECK_EQUAL(rect.bottom(),        y + h);

                BOOST_CHECK_EQUAL(rect.area(),          w * h);
                BOOST_CHECK_EQUAL(rect.width(),         w);
                BOOST_CHECK_EQUAL(rect.height(),        h);
                BOOST_CHECK_EQUAL(rect.rows(),          h);
                BOOST_CHECK_EQUAL(rect.cols(),          w);

                BOOST_CHECK_EQUAL(rect.valid(),         rect.area() > 0);
        }
}

BOOST_AUTO_TEST_CASE(test_rect_construction)
{
        test::build_rect(0, 0, 8, 6);
        test::build_rect(1, 2, 3, 4);
        test::build_rect(-1, -1, 7, 5);
}

BOOST_AUTO_TEST_CASE(test_rect_operations)
{
        using namespace ncv;

        // intersecting rectangles
        BOOST_CHECK_EQUAL(ncv::rect_t(1, 1, 3, 3) | ncv::rect_t(2, 2, 5, 4),
                          ncv::rect_t(1, 1, 6, 5));

        BOOST_CHECK_EQUAL(ncv::rect_t(1, 1, 3, 3) & ncv::rect_t(2, 2, 5, 4),
                          ncv::rect_t(2, 2, 2, 2));

        // disjoint rectangles
        BOOST_CHECK_EQUAL(ncv::rect_t(1, 1, 3, 3) & ncv::rect_t(7, 4, 5, 4),
                          ncv::rect_t(0, 0, 0, 0));

        // test center
        BOOST_CHECK_EQUAL(ncv::rect_t(1, 1, 3, 3).center(),
                          ncv::point_t(2, 2));
}


#include "HierarchicalClustering.h"
#include "fastclusterAcdc.h"
#include "MLUtil.h"

#include <Eigen/Dense>

HierarchicalClustering::HierarchicalClustering()
{

}

HierarchicalClustering::~HierarchicalClustering()
{

}

void HierarchicalClustering::clusterNum(const Eigen::MatrixXd & linkage, std::vector<unsigned> & labels, unsigned idx, unsigned clsnum)
{
    unsigned n = labels.size();

    unsigned c1 = linkage(idx, 0);
    unsigned c2 = linkage(idx, 1);

    if (c1 < n) // left leaf, mark label
    {
        labels[c1] = clsnum;
    } else
    {
        HierarchicalClustering::clusterNum(linkage, labels, c1-n, clsnum); // recurse left tree
    }

    if (c2 < n) // right leaf, mark label
    {
        labels[c2] = clsnum;
    } else
    {
        HierarchicalClustering::clusterNum(linkage, labels, c2-n, clsnum); // recurse right tree
    }
}

void HierarchicalClustering::initialize(const Eigen::MatrixXd & data)
{
    linkageMat = HierarchicalClustering::linkage(data);
}

std::vector<unsigned> HierarchicalClustering::cluster(unsigned maxK)
{
    unsigned n = linkageMat.rows()+1;
    std::vector<unsigned> labels(n);

    unsigned i;
    unsigned clsnum = 1;
    for (unsigned k = n-maxK; k < n-1; k++)
    {
        // left tree
        i = linkageMat(k, 0);
        if (i < n) // is leaf, mark label
        {
            labels[i] = clsnum;
            clsnum++;
        } else if (i < 2*n-maxK)
        {
            HierarchicalClustering::clusterNum(linkageMat, labels, i-n, clsnum); // recurse left tree
            clsnum++;
        }

        // right tree
        i = linkageMat(k, 1);
        if (i < n) // is leaf, mark label
        {
            labels[i] = clsnum;
            clsnum++;
        } else if (i < 2*n-maxK)
        {
            HierarchicalClustering::clusterNum(linkageMat, labels, i-n, clsnum); // recurse right tree
            clsnum++;
        }
    }

    return labels;
}

Eigen::MatrixXd HierarchicalClustering::linkage(const Eigen::MatrixXd & data)
{
    // compute pairwise distances
    unsigned n = data.rows();
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> dist = MLUtil::condensedPdist(data);

    // call fastcluster linkage routine
    double * const z = new double[(n-1) * 4];
    fc_linkage(dist.data(), z, n, ClusterMethods::FC_METHOD_METR_WARD);

    // assemble Eigen object from returned linkage
    Eigen::MatrixXd res(n-1, 4);
    for (unsigned i = 0; i < n-1; i++)
    {
        for (unsigned j = 0; j < 4; j++)
        {
            res(i, j) = *(z + i*4+j);
        }
    }

    delete[] z;

    return res;
}

std::string HierarchicalClustering::name()
{
    return "HierarchicalClustering";
}

std::map<std::string, std::string> HierarchicalClustering::parameters()
{
    std::map<std::string, std::string> params = {{"distance", "euclidean"}, {"linkage", "ward"}};
    return params;
}


#include "refill/measurement_models/measurement_model_base.h"

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include "refill/distributions/gaussian_distribution.h"

namespace refill {

class MeasurementModelClass : public MeasurementModelBase {
 public:
  MeasurementModelClass(const size_t& state_dim, const size_t& measurement_dim,
                        const DistributionInterface& measurement_noise)
      : MeasurementModelBase(state_dim, measurement_dim, measurement_noise) {
  }

  void setModelParameters(const size_t& state_dim,
                          const size_t& measurement_dim,
                          const DistributionInterface& measurement_noise) {
    this->setMeasurementModelBaseParameters(state_dim, measurement_dim,
                                            measurement_noise);
  }
  Eigen::VectorXd observe(const Eigen::VectorXd& state,
                          const Eigen::VectorXd& noise) const {
    return state + noise;
  }
};

TEST(MeasurementModelBaseTest, FullTest) {
  GaussianDistribution measurement_noise(Eigen::Vector2d::Zero(),
                                         Eigen::Matrix2d::Identity());

  MeasurementModelClass measurement_model(2, 2, measurement_noise);

  ASSERT_EQ(2, measurement_model.getStateDim());
  ASSERT_EQ(2, measurement_model.getMeasurementDim());
  ASSERT_EQ(2, measurement_model.getNoiseDim());
  ASSERT_EQ(Eigen::Vector2d::Zero(),
            measurement_model.getNoise()->mean());
  ASSERT_EQ(Eigen::Matrix2d::Identity(),
            measurement_model.getNoise()->cov());

  measurement_noise.setDistributionParameters(
      Eigen::Vector3d::Ones(), Eigen::Matrix3d::Identity() * 2.0);

  measurement_model.setModelParameters(3, 3, measurement_noise);

  ASSERT_EQ(3, measurement_model.getStateDim());
  ASSERT_EQ(3, measurement_model.getMeasurementDim());
  ASSERT_EQ(3, measurement_model.getNoiseDim());
  ASSERT_EQ(Eigen::Vector3d::Ones(),
            measurement_model.getNoise()->mean());
  ASSERT_EQ(Eigen::Matrix3d::Identity() * 2.0,
            measurement_model.getNoise()->cov());
}

}  // namespace refill


#pragma once

#include <geogram/mesh/mesh.h>
#include <Eigen/Dense>
#include <vector>

namespace polyfem
{

//
// Compute singular vertices in a polygonal mesh.
//
// @param[in]  M               { Input surface mesh }
// @param[out] V               { List of singular vertices }
// @param[in]  regular_degree  { Degree of regular vertices (3 for triangle
//                             meshes, 4 for quad mesh)
// @param[in]  ignore_border   { Mark vertices on the border as well }
//
void singular_vertices(const GEO::Mesh &M, Eigen::VectorXi &V, int regular_degree = 4, bool ignore_border = true);

//
// Compute singular edges in a polygonal mesh.
//
// @param[in]  M     { Input surface mesh }
// @param[in]  V     { List of singular vertices }
// @param[out] E     { List of edges connecting singular vertices }
//
void singular_edges(const GEO::Mesh &M, const Eigen::VectorXi &V, Eigen::MatrixX2i &E);

//
// Compute singularity graph in a polygonal mesh.
//
// @param[in]  M               { Input surface mesh }
// @param[out] V               { List of singular vertices }
// @param[out] E               { List of edges connecting singular vertices }
// @param[in]  regular_degree  { Degree of regular vertices (3 for triangle
//                             meshes, 4 for quad mesh)
// @param[in]  ignore_border   { Mark vertices on the border as well }
//
void singularity_graph(const GEO::Mesh &M, Eigen::VectorXi &V, Eigen::MatrixX2i &E, int regular_degree = 4, bool ignore_border = true);

//
// Creates polygonal patches around singularities specified in the form of a
// graph.
//
// @param[in,out] M     { Surface mesh to modify }
// @param[in]     V     { List of singular vertices }
// @param[in]     E     { List of singular edges }
// @param[in]     t     { Interpolation parameter to place the subdivided
//                      vertices around the singularities (between 0 and 1) }
//
void create_patch_around_singularities(GEO::Mesh &M, const Eigen::VectorXi &V, const Eigen::MatrixX2i &E, double t = 0.5);

} // namespace polyfem


#pragma once

#include <Eigen/Core>

#include <vector>
#include <array>
#include <cassert>

namespace BVH {

using VectorMax3d =
    Eigen::Matrix<double, Eigen::Dynamic, 1, Eigen::ColMajor, 3, 1>;

class BVH {
private:
    std::vector<std::array<Eigen::Vector3d, 2>> boxlist;
    std::vector<int> new2old;
    size_t n_corners = -1;

    void init_boxes_recursive(
        const std::vector<std::array<Eigen::Vector3d, 2>>& cornerlist,
        int node_index,
        int b,
        int e);

    void box_search_recursive(
        const Eigen::Vector3d& bbd0,
        const Eigen::Vector3d& bbd1,
        std::vector<unsigned int>& list,
        int n,
        int b,
        int e) const;

    static int max_node_index(int node_index, int b, int e);

    bool box_intersects_box(
        const Eigen::Vector3d& bbd0,
        const Eigen::Vector3d& bbd1,
        int index) const;

public:
    void init(const std::vector<std::array<Eigen::Vector3d, 2>>& cornerlist);
    void
    init(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F, const double tol);

    inline void intersect_3D_box(
        const Eigen::Vector3d& bbd0,
        const Eigen::Vector3d& bbd1,
        std::vector<unsigned int>& list) const
    {
        std::vector<unsigned int> tmp;
        assert(n_corners >= 0);
        box_search_recursive(bbd0, bbd1, tmp, 1, 0, n_corners);

        list.resize(tmp.size());
        for (int i = 0; i < tmp.size(); ++i)
            list[i] = new2old[tmp[i]];
    }

    inline void intersect_box(
        const VectorMax3d& bbd0,
        const VectorMax3d& bbd1,
        std::vector<unsigned int>& list) const
    {
        Eigen::Vector3d bbd0_3D = Eigen::Vector3d::Zero();
        bbd0_3D.head(bbd0.size()) = bbd0.head(bbd0.size());

        Eigen::Vector3d bbd1_3D = Eigen::Vector3d::Zero();
        bbd1_3D.head(bbd1.size()) = bbd1.head(bbd1.size());

        intersect_3D_box(bbd0_3D, bbd1_3D, list);
    }

    inline void intersect_2D_box(
        const Eigen::Vector2d& bbd0,
        const Eigen::Vector2d& bbd1,
        std::vector<unsigned int>& list) const
    {
        Eigen::Vector3d bbd0_3D = Eigen::Vector3d::Zero();
        bbd0_3D.head<2>() = bbd0;

        Eigen::Vector3d bbd1_3D = Eigen::Vector3d::Zero();
        bbd1_3D.head<2>() = bbd1;

        intersect_3D_box(bbd0_3D, bbd1_3D, list);
    }
};
} // namespace BVH


// Bring in my package's API, which is what I'm testing
#include <sm/kinematics/EulerAnglesZXY.h>
#include <sm/kinematics/EulerAnglesYawPitchRoll.hpp>
#include <sm/kinematics/EulerAnglesZYX.hpp>
#include <sm/kinematics/EulerRodriguez.hpp>
#include <sm/kinematics/RotationVector.hpp>
#include <sm/kinematics/RotationalKinematics.hpp>

// Bring in gtest
#include <gtest/gtest.h>
#include <boost/cstdint.hpp>

// Helpful functions from libsm
#include <sm/eigen/NumericalDiff.hpp>
#include <sm/eigen/gtest.hpp>

#include <Eigen/LU>

using namespace sm::kinematics;

// A helper class for estimating the S matrix using finite differences.
template <typename ROTATION_TYPE>
struct SMatrixFunctor {
    // Necessary for eigen fixed sized type member variables.
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef ROTATION_TYPE rotation_t;
    typedef Eigen::Vector3d input_t;
    typedef Eigen::Matrix3d jacobian_t;
    typedef Eigen::Vector3d value_t;
    typedef double scalar_t;

    enum { XMinimalDimension = 3 };

    SMatrixFunctor(const Eigen::Vector3d& pbar, const Eigen::Vector3d& v) : pbar_(pbar), v_(v) {
        Cbar_ = rotation.parametersToRotationMatrix(pbar);
    }

    Eigen::Vector3d operator()(const Eigen::Vector3d& dp) {
        Eigen::Matrix3d C = rotation.parametersToRotationMatrix(pbar_ + dp);

        Eigen::Vector3d Cv = C * v_;
        return Cv;
    }

    input_t update(const input_t& x, int c, double delta) {
        input_t xnew = x;
        xnew[c] += delta;
        return xnew;
    }

    /// The nominal parameter value
    Eigen::Vector3d pbar_;
    Eigen::Vector3d v_;
    Eigen::Matrix3d Cbar_;
    rotation_t rotation;
};

// A helper class for estimating the S matrix using finite differences.
template <typename ROTATION_TYPE>
struct SMatrixFunctor2 {
    // Necessary for eigen fixed sized type member variables.
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef ROTATION_TYPE rotation_t;
    typedef Eigen::Vector3d input_t;
    typedef Eigen::Matrix3d jacobian_t;
    typedef Eigen::Vector3d value_t;
    typedef double scalar_t;

    enum { XMinimalDimension = 3 };
    input_t update(const input_t& x, int c, double delta) {
        input_t xnew = x;
        xnew[c] += delta;
        return xnew;
    }

    SMatrixFunctor2(const Eigen::Vector3d& pbar) : pbar_(pbar) { Cbar_ = rotation.parametersToRotationMatrix(pbar); }

    Eigen::Vector3d operator()(const Eigen::Vector3d& dp) {
        Eigen::Matrix3d C = rotation.parametersToRotationMatrix(pbar_ + dp);
        Eigen::Matrix3d wcross = C * Cbar_.transpose();

        Eigen::Vector3d result(wcross(1, 2), -wcross(0, 2), wcross(0, 1));
        return result;
    }

    /// The nominal parameter value
    Eigen::Vector3d pbar_;
    Eigen::Vector3d v_;
    Eigen::Matrix3d Cbar_;
    rotation_t rotation;
};

// A helper class for estimating the S matrix using finite differences.
template <typename ROTATION_TYPE>
struct AngularVelocityFunctor {
    // Necessary for eigen fixed sized type member variables.
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef ROTATION_TYPE rotation_t;
    typedef Eigen::Matrix<double, 6, 1> input_t;
    typedef Eigen::Matrix<double, 3, 6> jacobian_t;
    typedef Eigen::Vector3d value_t;
    typedef double scalar_t;

    enum { XMinimalDimension = 6 };
    input_t update(const input_t& x, int c, double delta) {
        input_t xnew = x;
        xnew[c] += delta;
        return xnew;
    }

    AngularVelocityFunctor() {}

    Eigen::Vector3d operator()(const Eigen::Matrix<double, 6, 1>& p) {
        return rotation.angularVelocityAndJacobian(p.head<3>(), p.tail<3>(), NULL);
    }

    rotation_t rotation;
};

template <typename ROTATION_TYPE>
class RotationalKinematicsTestHarness {
  public:
    typedef ROTATION_TYPE rotation_t;
    rotation_t rotation;

    void testParametersToRotationMatrix() {
        // Make sure that random parameters produce
        // a valid rotation matrix.
        srand(0);
        for (int i = 0; i < 20; i++) {
            SCOPED_TRACE("");
            Eigen::Vector3d p = Eigen::Vector3d::Random();
            Eigen::Matrix3d C = rotation.parametersToRotationMatrix(p);

            // Check that this is a rotation, not a reflection.
            double determinant = C.determinant();
            ASSERT_NEAR(determinant, 1.0, 1e-10);

            // Check that the matrix is orthonormal.
            Eigen::Matrix3d one = C.transpose() * C;
            sm::eigen::assertNear(one, Eigen::Matrix3d::Identity(), 1e-10, SM_SOURCE_FILE_POS);
        }
    }

    void testInvertibleParameterTransformation() {
        // Make sure that the parameter transformations are invertible.
        srand(1);
        for (int i = 0; i < 20; i++) {
            SCOPED_TRACE("");
            // We won't compare against these parameters because
            // often the C --> p transformation returns C to a particular range.
            Eigen::Vector3d p = Eigen::Vector3d::Random();
            if (i == 0) {
                // Test the boundary case
                p = Eigen::Vector3d::Zero();
            }

            Eigen::Matrix3d C1 = rotation.parametersToRotationMatrix(p);
            // Instead, we will use these parameters. Admittedly it makes for a
            // weaker unit test but I can't presently think of a generic way around.
            p = rotation.rotationMatrixToParameters(C1);
            Eigen::Matrix3d C2 = rotation.parametersToRotationMatrix(p);

            sm::eigen::assertNear(C1, C2, 1e-10, SM_SOURCE_FILE_POS);
        }
    }

    void testSMatrix() {
        srand(1);
        for (int i = 0; i < 20; i++) {
            SCOPED_TRACE("");
            // Choose a random parameter vector.
            Eigen::Vector3d pbar = Eigen::Vector3d::Random();
            if (i == 0) {
                // Test the boundary case
                pbar = Eigen::Vector3d::Zero();
            }

            Eigen::Vector3d v = Eigen::Vector3d::Random();
            Eigen::Matrix3d C1 = rotation.parametersToRotationMatrix(pbar);
            Eigen::Vector3d Cv = C1 * v;
            // Make sure the vector is clamped to the valid range
            pbar = rotation.rotationMatrixToParameters(C1);

            // // Create a functor and the numerical diff struct
            SMatrixFunctor<rotation_t> functor(pbar, v);
            sm::eigen::NumericalDiff<SMatrixFunctor<rotation_t> > nd(functor, 1e-10);

            // Check that the estimated S matrix matches the actual S matrix
            Eigen::Matrix3d estCvCrossS = nd.estimateJacobian(Eigen::Vector3d::Zero());
            Eigen::Matrix3d CvCrossS = crossMx(Cv) * rotation.parametersToSMatrix(pbar);
            sm::eigen::assertNear(CvCrossS, estCvCrossS, 1e-5, SM_SOURCE_FILE_POS);

            SMatrixFunctor2<rotation_t> functor2(pbar);
            sm::eigen::NumericalDiff<SMatrixFunctor2<rotation_t> > nd2(functor2);

            Eigen::Matrix3d estS = nd2.estimateJacobian(Eigen::Vector3d::Zero());
            Eigen::Matrix3d S = rotation.parametersToSMatrix(pbar);

            sm::eigen::assertNear(S, estS, 1e-5, SM_SOURCE_FILE_POS);
        }
    }

    void testAngularVelocity() {
        for (int i = 0; i < 10; i++) {
            SCOPED_TRACE("");
            Eigen::Matrix<double, 6, 1> p = Eigen::Matrix<double, 6, 1>::Random();
            AngularVelocityFunctor<rotation_t> avf;
            sm::eigen::NumericalDiff<AngularVelocityFunctor<rotation_t> > nd(avf);
            Eigen::Matrix<double, 3, 6> estJ = nd.estimateJacobian(p);
            Eigen::Matrix<double, 3, 6> J;
            rotation.angularVelocityAndJacobian(p.head<3>(), p.tail<3>(), &J);
            sm::eigen::assertNear(J, estJ, 1e-5, SM_SOURCE_FILE_POS);
        }
    }

    void testAll() {
        testParametersToRotationMatrix();
        testInvertibleParameterTransformation();
        testSMatrix();
        // testAngularVelocity();
    }
};

TEST(RotationalKinematicsTestSuite, testEulerAngles) {
    SCOPED_TRACE("EulerAnglesZYX");
    RotationalKinematicsTestHarness<EulerAnglesZYX> eulerzyx;
    eulerzyx.testAll();
    eulerzyx.testAngularVelocity();
}

TEST(RotationalKinematicsTestSuite, testEulerRodriguez) {
    SCOPED_TRACE("EulerRodriguez");
    RotationalKinematicsTestHarness<EulerRodriguez> rotationvector;
    rotationvector.testAll();
    rotationvector.testAngularVelocity();
    // rotationvector.testParametersToRotationMatrix();
    // rotationvector.testInvertibleParameterTransformation();
    // rotationvector.testParametersToRotationMatrix();
}

TEST(RotationalKinematicsTestSuite, testRotationVector) {
    SCOPED_TRACE("RotationVector");
    RotationalKinematicsTestHarness<RotationVector> rotationvector;
    rotationvector.testAll();
    rotationvector.testAngularVelocity();
}

TEST(RotationalKinematicsTestSuite, testYpr) {
    SCOPED_TRACE("Yaw Pitch Roll");
    RotationalKinematicsTestHarness<EulerAnglesYawPitchRoll> rotationvector;
    rotationvector.testAll();
    rotationvector.testAngularVelocity();
}

TEST(RotationalKinematicsTestSuite, testZXY) {
    SCOPED_TRACE("EulerAnglesZXY");
    RotationalKinematicsTestHarness<EulerAnglesZXY> eulerzxy;
    eulerzxy.testAll();
    eulerzxy.testAngularVelocity();
}