/**TODO:  Add copyright*/

#define BOOST_TEST_MODULE ModelInterpreter IG test suite
#include <boost/test/included/unit_test.hpp>
#include <EvoNet/ml/ModelInterpreterDefaultDevice.h>

using namespace EvoNet;
using namespace std;

Model<float> makeModelIG()
{
	/**
	 * Interaction Graph Toy Network Model
	 * Harmonic Oscillator without damping:
	 * F(t) - kx = mx``
	 * for F(t) = 0
	 * x(t) = A*cos(w*t + e)
	 * where undamped angular momentum, w = sqrt(k/m)
	 * with amplitude A, and phase e
	 *
	 * Harmonic Oscillator with damping:
	 * F(t) - kx - cx` = mx``
	 * For F(t) = 0, x`` + 2*l*w*x` + x*w^2 = 0
	 * where damping ratio, l = c/(2*sqrt(m*k)) and undamped angular momentum, w = sqrt(k/m)
	 * x(t) = Ae^(-l*w*t)*sin(sqrt(1-l^2)*w*t + e)
	 * with amplitude A, and phase e
	*/
	Node<float> m1, m2, m3;
	Link l1_to_l2, l2_to_l1, l2_to_l3, l3_to_l2;
	Weight<float> w1_to_w2, w2_to_w1, w2_to_w3, w3_to_w2;
	Model<float> model3;
	// Toy network: 1 hidden layer, fully connected, DCG
	m1 = Node<float>("m1", NodeType::input, NodeStatus::activated, std::make_shared<LinearOp<float>>(LinearOp<float>()), std::make_shared<LinearGradOp<float>>(LinearGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	m2 = Node<float>("m2", NodeType::hidden, NodeStatus::initialized, std::make_shared<LinearOp<float>>(LinearOp<float>()), std::make_shared<LinearGradOp<float>>(LinearGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	m3 = Node<float>("m3", NodeType::output, NodeStatus::initialized, std::make_shared<LinearOp<float>>(LinearOp<float>()), std::make_shared<LinearGradOp<float>>(LinearGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	// weights
	std::shared_ptr<WeightInitOp<float>> weight_init;
	std::shared_ptr<SolverOp<float>> solver;
	// weight_init.reset(new RandWeightInitOp(1.0)); // No random init for testing
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w1_to_w2 = Weight<float>("m1_to_m2", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w2_to_w1 = Weight<float>("m2_to_m1", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w2_to_w3 = Weight<float>("m2_to_m3", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w3_to_w2 = Weight<float>("m3_to_m2", weight_init, solver);
	weight_init.reset();
	solver.reset();
	// links
	l1_to_l2 = Link("l1_to_l2", "m1", "m2", "m1_to_m2");
	l2_to_l1 = Link("l2_to_l1", "m2", "m1", "m2_to_m1");
	l2_to_l3 = Link("l2_to_l3", "m2", "m3", "m2_to_m3");
	l3_to_l2 = Link("l3_to_l2", "m3", "m2", "m3_to_m2");
	model3.setId(3);
	model3.addNodes({ m1, m2, m3 });
	model3.addWeights({ w1_to_w2, w2_to_w1, w2_to_w3, w3_to_w2 });
	model3.addLinks({ l1_to_l2, l2_to_l1, l2_to_l3, l3_to_l2 });
	return model3;
}

BOOST_AUTO_TEST_SUITE(modelInterpreter_IG)

/**
 * Part 2 test suit for the ModelInterpreter class
 *
 * The following test methods that are
 * required of an interaction graph neural network
*/

Model<float> model_getFPOpsGraph = makeModelIG();
BOOST_AUTO_TEST_CASE(getFPOpsGraph_)
{
  // Toy network: 1 hidden layer, fully connected, DAG
	ModelInterpreterDefaultDevice<float> model_interpreter;

	// get the next hidden layer
	int iter;
	std::vector<OperationList<float>> FP_operations_list;
	model_interpreter.getFPOpsGraph_(model_getFPOpsGraph, FP_operations_list, iter);

	BOOST_CHECK_EQUAL(iter, 1);
	BOOST_CHECK_EQUAL(FP_operations_list.size(), 4);

	BOOST_CHECK_EQUAL(FP_operations_list[0].result.time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[0].result.sink_node->getName(), "m2");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].time_step, 1);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].source_node->getName(), "m1");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].weight->getName(), "m1_to_m2");

	BOOST_CHECK_EQUAL(FP_operations_list[1].result.time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[1].result.sink_node->getName(), "m1");
	BOOST_CHECK_EQUAL(FP_operations_list[1].arguments.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_list[1].arguments[0].time_step, 1);
	BOOST_CHECK_EQUAL(FP_operations_list[1].arguments[0].source_node->getName(), "m2");
	BOOST_CHECK_EQUAL(FP_operations_list[1].arguments[0].weight->getName(), "m2_to_m1");

	BOOST_CHECK_EQUAL(FP_operations_list[2].result.time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[2].result.sink_node->getName(), "m3");
	BOOST_CHECK_EQUAL(FP_operations_list[2].arguments.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_list[2].arguments[0].time_step, 1);
	BOOST_CHECK_EQUAL(FP_operations_list[2].arguments[0].source_node->getName(), "m2");
	BOOST_CHECK_EQUAL(FP_operations_list[2].arguments[0].weight->getName(), "m2_to_m3");

	BOOST_CHECK_EQUAL(FP_operations_list[3].result.time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[3].result.sink_node->getName(), "m2");
	BOOST_CHECK_EQUAL(FP_operations_list[3].arguments.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_list[3].arguments[0].time_step, 1);
	BOOST_CHECK_EQUAL(FP_operations_list[3].arguments[0].source_node->getName(), "m3");
	BOOST_CHECK_EQUAL(FP_operations_list[3].arguments[0].weight->getName(), "m3_to_m2");
}

Model<float> model_getTensorOperations = makeModelIG();
BOOST_AUTO_TEST_CASE(getTensorOperations)
{
	ModelInterpreterDefaultDevice<float> model_interpreter;

	int iter;
	std::vector<OperationList<float>> FP_operations_list;
	model_interpreter.getFPOpsGraph_(model_getTensorOperations, FP_operations_list, iter);

	std::set<std::string> identified_sink_nodes;
	std::map<std::string, std::vector<int>> tensor_ops = model_interpreter.getTensorOperations(FP_operations_list, identified_sink_nodes, false);

	BOOST_CHECK_EQUAL(identified_sink_nodes.size(), 4);
	BOOST_CHECK_EQUAL(identified_sink_nodes.count("m1/1"), 1);
	BOOST_CHECK_EQUAL(identified_sink_nodes.count("m2/0"), 1);
	BOOST_CHECK_EQUAL(identified_sink_nodes.count("m2/3"), 1);
	BOOST_CHECK_EQUAL(identified_sink_nodes.count("m3/2"), 1);
	BOOST_CHECK_EQUAL(tensor_ops.size(), 2);
	BOOST_CHECK_EQUAL(tensor_ops.at("m1/1")[0], 1);
	BOOST_CHECK_EQUAL(tensor_ops.at("m1/1")[1], 2);
	BOOST_CHECK_EQUAL(tensor_ops.at("m2/0")[0], 0);
	BOOST_CHECK_EQUAL(tensor_ops.at("m2/0")[1], 3);
}

Model<float> model_getForwardPropogationLayerTensorDimensions = makeModelIG();
BOOST_AUTO_TEST_CASE(getForwardPropogationLayerTensorDimensions)
{
	ModelInterpreterDefaultDevice<float> model_interpreter;

	int iter;
	std::vector<OperationList<float>> FP_operations_list;
	model_interpreter.getFPOpsGraph_(model_getForwardPropogationLayerTensorDimensions, FP_operations_list, iter);

	std::set<std::string> identified_sink_nodes;
	std::map<std::string, std::vector<int>> tensor_ops = model_interpreter.getTensorOperations(FP_operations_list, identified_sink_nodes, false);

  std::map<int, int> max_layer_sizes;
  std::map<std::string, int> layer_name_pos;
  std::vector<int> source_layer_sizes, sink_layer_sizes;
  std::vector<std::vector<std::pair<int, int>>> weight_indices;
  std::vector<std::map<std::string, std::vector<std::pair<int, int>>>> shared_weight_indices;
  std::vector<std::vector<float>> weight_values;
  std::vector<bool> make_source_tensors, make_sink_tensors, make_weight_tensors;
  std::vector<int> source_layer_pos, sink_layer_pos;
  int tensor_layers_cnt = 0;
  int weight_layers_cnt = 0;
  model_interpreter.getForwardPropogationLayerTensorDimensions(FP_operations_list, tensor_ops, source_layer_sizes, sink_layer_sizes, weight_indices, shared_weight_indices, weight_values, make_source_tensors, make_sink_tensors, make_weight_tensors,
    source_layer_pos, sink_layer_pos, max_layer_sizes, layer_name_pos, tensor_layers_cnt, weight_layers_cnt);

	BOOST_CHECK_EQUAL(source_layer_sizes.size(), 2);
	BOOST_CHECK_EQUAL(source_layer_sizes[0], 1);
	BOOST_CHECK_EQUAL(source_layer_sizes[1], 2);
	BOOST_CHECK_EQUAL(sink_layer_sizes.size(), 2);
	BOOST_CHECK_EQUAL(sink_layer_sizes[0], 2);
	BOOST_CHECK_EQUAL(sink_layer_sizes[1], 1);

  BOOST_CHECK_EQUAL(source_layer_pos.size(), 2);
  BOOST_CHECK_EQUAL(source_layer_pos.at(0), 1);
  BOOST_CHECK_EQUAL(source_layer_pos.at(1), 0);
  BOOST_CHECK_EQUAL(sink_layer_pos.size(), 2);
  BOOST_CHECK_EQUAL(sink_layer_pos.at(0), 0);
  BOOST_CHECK_EQUAL(sink_layer_pos.at(1), 1);

  BOOST_CHECK_EQUAL(max_layer_sizes.size(), 2);
  BOOST_CHECK_EQUAL(max_layer_sizes.at(0), 1);
  BOOST_CHECK_EQUAL(max_layer_sizes.at(1), 0);

  BOOST_CHECK_EQUAL(layer_name_pos.size(), 0);

	BOOST_CHECK_EQUAL(weight_indices.size(), 2);
	BOOST_CHECK_EQUAL(weight_indices[0].size(), 2);
	BOOST_CHECK_EQUAL(weight_indices[1].size(), 2);
	std::vector<std::vector<std::pair<int, int>>> weight_indices_test1 = {
		{std::make_pair(0,0),std::make_pair(0,1)},
		{std::make_pair(0,0),std::make_pair(1,0)}
	};
	for (int tensor_iter = 0; tensor_iter < weight_indices_test1.size(); ++tensor_iter) {
		for (int i = 0; i < weight_indices_test1[tensor_iter].size(); ++i) {
			BOOST_CHECK_EQUAL(weight_indices[tensor_iter][i].first, weight_indices_test1[tensor_iter][i].first);
			BOOST_CHECK_EQUAL(weight_indices[tensor_iter][i].second, weight_indices_test1[tensor_iter][i].second);
		}
	}

	BOOST_CHECK_EQUAL(shared_weight_indices.size(), 2);
	BOOST_CHECK_EQUAL(shared_weight_indices[0].size(), 0);
	BOOST_CHECK_EQUAL(shared_weight_indices[1].size(), 0);

	BOOST_CHECK_EQUAL(weight_values.size(), 2);
	BOOST_CHECK_EQUAL(weight_values[0].size(), 2);
	BOOST_CHECK_EQUAL(weight_values[1].size(), 2);
	std::vector<std::vector<float>> weight_values_test1 = { {1, 1}, {1, 1} };
	for (int tensor_iter = 0; tensor_iter < weight_values_test1.size(); ++tensor_iter) {
		for (int i = 0; i < weight_values_test1[tensor_iter].size(); ++i) {
			BOOST_CHECK_EQUAL(weight_values[tensor_iter][i], weight_values_test1[tensor_iter][i]);
		}
	}

	BOOST_CHECK_EQUAL(make_source_tensors.size(), 2);
	BOOST_CHECK(make_source_tensors[0]);
	BOOST_CHECK(!make_source_tensors[1]);
	BOOST_CHECK_EQUAL(make_sink_tensors.size(), 2);
	BOOST_CHECK(make_sink_tensors[0]);
	BOOST_CHECK(!make_sink_tensors[1]);
	BOOST_CHECK_EQUAL(make_weight_tensors.size(), 2);
	BOOST_CHECK(make_weight_tensors[0]);
	BOOST_CHECK(make_weight_tensors[1]);
}

BOOST_AUTO_TEST_SUITE_END()


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

 * 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   testSimilarity3.cpp
 * @brief  Unit tests for Similarity3 class
 * @author Paul Drews
 * @author Zhaoyang Lv
 */

#include <gtsam_unstable/geometry/Similarity3.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/function.hpp>
#include <boost/bind.hpp>

using namespace gtsam;
using namespace std;
using symbol_shorthand::X;

GTSAM_CONCEPT_TESTABLE_INST(Similarity3)

static const Point3 P(0.2, 0.7, -2);
static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
static const double s = 4;
static const Similarity3 id;
static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1);
static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1),
    Point3(3.5, -8.2, 4.2), 1);
static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1);
static const Similarity3 T4(R, P, s);
static const Similarity3 T5(R, P, 10);
static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform

const double degree = M_PI / 180;

//******************************************************************************
TEST(Similarity3, Concepts) {
  BOOST_CONCEPT_ASSERT((IsGroup<Similarity3 >));
  BOOST_CONCEPT_ASSERT((IsManifold<Similarity3 >));
  BOOST_CONCEPT_ASSERT((IsLieGroup<Similarity3 >));
}

//******************************************************************************
TEST(Similarity3, Constructors) {
  Similarity3 sim3_Construct1;
  Similarity3 sim3_Construct2(s);
  Similarity3 sim3_Construct3(R, P, s);
  Similarity3 sim4_Construct4(R.matrix(), P, s);
}

//******************************************************************************
TEST(Similarity3, Getters) {
  Similarity3 sim3_default;
  EXPECT(assert_equal(Rot3(), sim3_default.rotation()));
  EXPECT(assert_equal(Point3(0,0,0), sim3_default.translation()));
  EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9);

  Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7);
  EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), sim3.rotation()));
  EXPECT(assert_equal(Point3(4, 5, 6), sim3.translation()));
  EXPECT_DOUBLES_EQUAL(7.0, sim3.scale(), 1e-9);
}

//******************************************************************************
TEST(Similarity3, AdjointMap) {
  const Matrix4 T = T2.matrix();
  // Check Ad with actual definition
  Vector7 delta;
  delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
  Matrix4 W = Similarity3::wedge(delta);
  Matrix4 TW = Similarity3::wedge(T2.AdjointMap() * delta);
  EXPECT(assert_equal(TW, Matrix4(T * W * T.inverse()), 1e-9));
}

//******************************************************************************
TEST(Similarity3, inverse) {
  Similarity3 sim3(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7);
  Matrix3 Re; // some values from matlab
  Re << -0.2248, 0.9024, -0.3676, -0.3502, -0.4269, -0.8337, -0.9093, -0.0587, 0.4120;
  Vector3 te(-9.8472, 59.7640, 10.2125);
  Similarity3 expected(Re, te, 1.0 / 7.0);
  EXPECT(assert_equal(expected, sim3.inverse(), 1e-4));
  EXPECT(assert_equal(sim3, sim3.inverse().inverse(), 1e-8));

  // test lie group inverse
  Matrix H1, H2;
  EXPECT(assert_equal(expected, sim3.inverse(H1), 1e-4));
  EXPECT(assert_equal(sim3, sim3.inverse().inverse(H2), 1e-8));
}

//******************************************************************************
TEST(Similarity3, Multiplication) {
  Similarity3 test1(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7);
  Similarity3 test2(Rot3::Ypr(1, 2, 3).inverse(), Point3(8, 9, 10), 11);
  Matrix3 re;
  re << 0.0688, 0.9863, -0.1496, -0.5665, -0.0848, -0.8197, -0.8211, 0.1412, 0.5530;
  Vector3 te(-13.6797, 3.2441, -5.7794);
  Similarity3 expected(re, te, 77);
  EXPECT(assert_equal(expected, test1 * test2, 1e-2));
}

//******************************************************************************
TEST(Similarity3, Manifold) {
  EXPECT_LONGS_EQUAL(7, Similarity3::Dim());
  Vector z = Vector7::Zero();
  Similarity3 sim;
  EXPECT(sim.retract(z) == sim);

  Vector7 v = Vector7::Zero();
  v(6) = 2;
  Similarity3 sim2;
  EXPECT(sim2.retract(z) == sim2);

  EXPECT(assert_equal(z, sim2.localCoordinates(sim)));

  Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1);
  Vector v3(7);
  v3 << 0, 0, 0, 1, 2, 3, 0;
  EXPECT(assert_equal(v3, sim2.localCoordinates(sim3)));

  Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(4, 5, 6), 1);

  Vector vlocal = sim.localCoordinates(other);

  EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2));

  Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0), Point3(4, 5, 6), 1);
  Rot3 R = Rot3::Rodrigues(0.3, 0, 0);

  Vector vlocal2 = sim.localCoordinates(other2);

  EXPECT(assert_equal(sim.retract(vlocal2), other2, 1e-2));

  // TODO add unit tests for retract and localCoordinates
}

//******************************************************************************
TEST( Similarity3, retract_first_order) {
  Similarity3 id;
  Vector v = Z_7x1;
  v(0) = 0.3;
  EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2));
//  v(3) = 0.2;
//  v(4) = 0.7;
//  v(5) = -2;
//  EXPECT(assert_equal(Similarity3(R, P, 1), id.retract(v), 1e-2));
}

//******************************************************************************
TEST(Similarity3, localCoordinates_first_order) {
  Vector7 d12 = Vector7::Constant(0.1);
  d12(6) = 1.0;
  Similarity3 t1 = T1, t2 = t1.retract(d12);
  EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
}

//******************************************************************************
TEST(Similarity3, manifold_first_order) {
  Similarity3 t1 = T1;
  Similarity3 t2 = T3;
  Similarity3 origin;
  Vector d12 = t1.localCoordinates(t2);
  EXPECT(assert_equal(t2, t1.retract(d12)));
  Vector d21 = t2.localCoordinates(t1);
  EXPECT(assert_equal(t1, t2.retract(d21)));
}

//******************************************************************************
// Return as a 4*4 Matrix
TEST(Similarity3, Matrix) {
  Matrix4 expected;
  expected << 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0.5;
  Matrix4 actual = T6.matrix();
  EXPECT(assert_equal(expected, actual));
}

//*****************************************************************************
// Exponential and log maps
TEST(Similarity3, ExpLogMap) {
  Vector7 delta;
  delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
  Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(delta));
  EXPECT(assert_equal(delta, actual));

  Vector7 zeros;
  zeros << 0, 0, 0, 0, 0, 0, 0;
  Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity());
  EXPECT(assert_equal(zeros, logIdentity));

  Similarity3 expZero = Similarity3::Expmap(zeros);
  Similarity3 ident = Similarity3::identity();
  EXPECT(assert_equal(expZero, ident));

  // Compare to matrix exponential, using expm in Lie.h
  EXPECT(
      assert_equal(expm<Similarity3>(delta), Similarity3::Expmap(delta), 1e-3));
}

//******************************************************************************
// Group action on Point3 (with simpler transform)
TEST(Similarity3, GroupAction) {
  EXPECT(assert_equal(Point3(2, 2, 0), T6 * Point3(0, 0, 0)));
  EXPECT(assert_equal(Point3(4, 2, 0), T6 * Point3(1, 0, 0)));

  // Test group action on R^4 via matrix representation
  Vector4 qh;
  qh << 1, 0, 0, 1;
  Vector4 ph;
  ph << 2, 1, 0, 0.5; // equivalent to Point3(4, 2, 0)
  EXPECT(assert_equal((Vector )ph, T6.matrix() * qh));

  // Test some more...
  Point3 pa = Point3(1, 0, 0);
  Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0);
  Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0);
  EXPECT(assert_equal(Point3(2, 2, 3), Ta.transformFrom(pa)));
  EXPECT(assert_equal(Point3(4, 4, 6), Tb.transformFrom(pa)));

  Similarity3 Tc(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 1.0);
  Similarity3 Td(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 2.0);
  EXPECT(assert_equal(Point3(1, 3, 3), Tc.transformFrom(pa)));
  EXPECT(assert_equal(Point3(2, 6, 6), Td.transformFrom(pa)));

  // Test derivative
  boost::function<Point3(Similarity3, Point3)> f = boost::bind(
      &Similarity3::transformFrom, _1, _2, boost::none, boost::none);

  Point3 q(1, 2, 3);
  for (const auto& T : { T1, T2, T3, T4, T5, T6 }) {
    Point3 q(1, 0, 0);
    Matrix H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T, q);
    Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T, q);
    Matrix actualH1, actualH2;
    T.transformFrom(q, actualH1, actualH2);
    EXPECT(assert_equal(H1, actualH1));
    EXPECT(assert_equal(H2, actualH2));
  }
}

//******************************************************************************
// Group action on Pose3
TEST(Similarity3, GroupActionPose3) {
  Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);

  // Create source poses
  Pose3 Ta1(Rot3(), Point3(0, 0, 0));
  Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));

  // Create destination poses
  Pose3 expected_Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
  Pose3 expected_Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));

  EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1)));
  EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2)));
}

// Test left group action compatibility.
// cSa*Ta = cSb*bSa*Ta
TEST(Similarity3, GroupActionPose3_Compatibility) {
  Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
  Similarity3 cSb(Rot3::Ry(90 * degree), Point3(-10, -4, 0), 3.0);
  Similarity3 cSa(Rot3::Ry(270 * degree), Point3(0, 1, -2), 6.0);

  // Create poses
  Pose3 Ta1(Rot3(), Point3(0, 0, 0));
  Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
  Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
  Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
  Pose3 Tc1(Rot3(0, 0, -1, 0, 1, 0, 1, 0, 0), Point3(0, 6, -12));
  Pose3 Tc2(Rot3(0, 0, -1, 0, -1, 0, -1, 0, 0), Point3(0, 6, 12));

  EXPECT(assert_equal(Tc1, cSb.transformFrom(Tb1)));
  EXPECT(assert_equal(Tc2, cSb.transformFrom(Tb2)));

  EXPECT(assert_equal(cSa.transformFrom(Ta1), cSb.transformFrom(Tb1)));
  EXPECT(assert_equal(cSa.transformFrom(Ta2), cSb.transformFrom(Tb2)));
}

//******************************************************************************
// Align with Point3 Pairs
TEST(Similarity3, AlignPoint3_1) {
  Similarity3 expected_aSb(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0);

  Point3 b1(0, 0, 0), b2(3, 0, 0), b3(3, 0, 4);

  Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
  Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
  Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));

  vector<Point3Pair> correspondences{ab1, ab2, ab3};

  Similarity3 actual_aSb = Similarity3::Align(correspondences);
  EXPECT(assert_equal(expected_aSb, actual_aSb));
}

TEST(Similarity3, AlignPoint3_2) {
  Similarity3 expected_aSb(Rot3(), Point3(10, 10, 0), 1.0);

  Point3 b1(0, 0, 0), b2(20, 10, 0), b3(10, 20, 0);

  Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
  Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
  Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));

  vector<Point3Pair> correspondences{ab1, ab2, ab3};

  Similarity3 actual_aSb = Similarity3::Align(correspondences);
  EXPECT(assert_equal(expected_aSb, actual_aSb));
}

TEST(Similarity3, AlignPoint3_3) {
  Similarity3 expected_aSb(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0);

  Point3 b1(0, 0, 1), b2(10, 0, 2), b3(20, -10, 30);

  Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
  Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
  Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));

  vector<Point3Pair> correspondences{ab1, ab2, ab3};

  Similarity3 actual_aSb = Similarity3::Align(correspondences);
  EXPECT(assert_equal(expected_aSb, actual_aSb));
}

//******************************************************************************
// Align with Pose3 Pairs
TEST(Similarity3, AlignPose3) {
  Similarity3 expected_aSb(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);

  // Create source poses
  Pose3 Ta1(Rot3(), Point3(0, 0, 0));
  Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));

  // Create destination poses
  Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
  Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));

  Pose3Pair bTa1(make_pair(Tb1, Ta1));
  Pose3Pair bTa2(make_pair(Tb2, Ta2));

  vector<Pose3Pair> correspondences{bTa1, bTa2};

  Similarity3 actual_aSb = Similarity3::Align(correspondences);
  EXPECT(assert_equal(expected_aSb, actual_aSb));
}

//******************************************************************************
// Test very simple prior optimization example
TEST(Similarity3, Optimization) {
  // Create a PriorFactor with a Sim3 prior
  Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
  Symbol key('x', 1);

  // Create graph
  NonlinearFactorGraph graph;
  graph.addPrior(key, prior, model);

  // Create initial estimate with identity transform
  Values initial;
  initial.insert<Similarity3>(key, Similarity3());

  // Optimize
  Values result;
  LevenbergMarquardtParams params;
  params.setVerbosityLM("TRYCONFIG");
  result = LevenbergMarquardtOptimizer(graph, initial).optimize();

  // After optimization, result should be prior
  EXPECT(assert_equal(prior, result.at<Similarity3>(key), 1e-4));
}

//******************************************************************************
// Test optimization with both Prior and BetweenFactors
TEST(Similarity3, Optimization2) {
  Similarity3 prior = Similarity3();
  Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI / 4.0, 0, 0), Point3(2.0, 0, 0),
      1.0);
  Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0),
      Point3(sqrt(8) * 0.9, 0, 0), 1.0);
  Similarity3 m3 = Similarity3(Rot3::Ypr(3 * M_PI / 4.0, 0, 0),
      Point3(sqrt(32) * 0.8, 0, 0), 1.0);
  Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0),
      Point3(6 * 0.7, 0, 0), 1.0);
  Similarity3 loop = Similarity3(1.42);

  //prior.print("Goal Transform");
  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7,
      0.01);
  SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(
      (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished());
  SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas(
      (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished());
  BetweenFactor<Similarity3> b1(X(1), X(2), m1, betweenNoise);
  BetweenFactor<Similarity3> b2(X(2), X(3), m2, betweenNoise);
  BetweenFactor<Similarity3> b3(X(3), X(4), m3, betweenNoise);
  BetweenFactor<Similarity3> b4(X(4), X(5), m4, betweenNoise);
  BetweenFactor<Similarity3> lc(X(5), X(1), loop, betweenNoise2);

  // Create graph
  NonlinearFactorGraph graph;
  graph.addPrior(X(1), prior, model); // Prior !
  graph.push_back(b1);
  graph.push_back(b2);
  graph.push_back(b3);
  graph.push_back(b4);
  graph.push_back(lc);

  //graph.print("Full Graph\n");
  Values initial;
  initial.insert<Similarity3>(X(1), Similarity3());
  initial.insert<Similarity3>(X(2),
      Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), Point3(1, 0, 0), 1.1));
  initial.insert<Similarity3>(X(3),
      Similarity3(Rot3::Ypr(2.0 * M_PI / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2));
  initial.insert<Similarity3>(X(4),
      Similarity3(Rot3::Ypr(3.0 * M_PI / 2.0, 0, 0), Point3(0, 1, 0), 1.3));
  initial.insert<Similarity3>(X(5),
      Similarity3(Rot3::Ypr(4.0 * M_PI / 2.0, 0, 0), Point3(0, 0, 0), 1.0));

  //initial.print("Initial Estimate\n");

  Values result;
  result = LevenbergMarquardtOptimizer(graph, initial).optimize();
  //result.print("Optimized Estimate\n");
  Pose3 p1, p2, p3, p4, p5;
  p1 = Pose3(result.at<Similarity3>(X(1)));
  p2 = Pose3(result.at<Similarity3>(X(2)));
  p3 = Pose3(result.at<Similarity3>(X(3)));
  p4 = Pose3(result.at<Similarity3>(X(4)));
  p5 = Pose3(result.at<Similarity3>(X(5)));

  //p1.print("Pose1");
  //p2.print("Pose2");
  //p3.print("Pose3");
  //p4.print("Pose4");
  //p5.print("Pose5");

  Similarity3 expected(0.7);
  EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));
}

//******************************************************************************
// Align points (p,q) assuming that p = T*q + noise
TEST(Similarity3, AlignScaledPointClouds) {
// Create ground truth
  Point3 q1(0, 0, 0), q2(1, 0, 0), q3(0, 1, 0);

  // Create transformed cloud (noiseless)
//  Point3 p1 = T4 * q1, p2 = T4 * q2, p3 = T4 * q3;

  // Create an unknown expression
  Expression<Similarity3> unknownT(0); // use key 0

  // Create constant expressions for the ground truth points
  Expression<Point3> q1_(q1), q2_(q2), q3_(q3);

  // Create prediction expressions
  Expression<Point3> predict1(unknownT, &Similarity3::transformFrom, q1_);
  Expression<Point3> predict2(unknownT, &Similarity3::transformFrom, q2_);
  Expression<Point3> predict3(unknownT, &Similarity3::transformFrom, q3_);

//// Create Expression factor graph
//  ExpressionFactorGraph graph;
//  graph.addExpressionFactor(predict1, p1, R); // |T*q1 - p1|
//  graph.addExpressionFactor(predict2, p2, R); // |T*q2 - p2|
//  graph.addExpressionFactor(predict3, p3, R); // |T*q3 - p3|
}

//******************************************************************************
TEST(Similarity3 , Invariants) {
  Similarity3 id;

  EXPECT(check_group_invariants(id, id));
  EXPECT(check_group_invariants(id, T3));
  EXPECT(check_group_invariants(T2, id));
  EXPECT(check_group_invariants(T2, T3));

  EXPECT(check_manifold_invariants(id, id));
  EXPECT(check_manifold_invariants(id, T3));
  EXPECT(check_manifold_invariants(T2, id));
  EXPECT(check_manifold_invariants(T2, T3));
}

//******************************************************************************
TEST(Similarity3 , LieGroupDerivatives) {
  Similarity3 id;

  CHECK_LIE_GROUP_DERIVATIVES(id, id);
  CHECK_LIE_GROUP_DERIVATIVES(id, T2);
  CHECK_LIE_GROUP_DERIVATIVES(T2, id);
  CHECK_LIE_GROUP_DERIVATIVES(T2, T3);
}

//******************************************************************************
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
//******************************************************************************


#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <Eigen/StdVector>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>

using namespace std;

// path to trajectory file
string trajectory_file = "./compare.txt";

// function for plotting trajectory, don't edit this code
// start point is red and end point is blue

/*******************************************************************************************/
void DrawTrajectoryc(vector<Sophus::SE3> poses1, vector<Sophus::SE3> poses2)
{
    if (poses1.empty() || poses2.empty())
    {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));

    pangolin::View &d_cam = pangolin::CreateDisplay()
                                .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
                                .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false)
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);
        for (size_t i = 0; i < poses1.size() - 1; i++)
        {
            glColor3f(1 - (float)i / poses1.size(), 0.0f, (float)i / poses1.size());
            glBegin(GL_LINES);
            auto p1 = poses1[i], p2 = poses1[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        for (size_t i = 0; i < poses2.size() - 1; i++)
        {
            glColor3f(1 - (float)i / poses2.size(), 0.0f, (float)i / poses2.size());
            glBegin(GL_LINES);
            auto p1 = poses2[i], p2 = poses2[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        pangolin::FinishFrame();
        usleep(5000); // sleep 5 ms
    }
}

Sophus::SE3 pose_estimation_3d3d(vector<Sophus::SE3> poses1, vector<Sophus::SE3> poses2)
{

    Eigen::Vector3d center1, center2;
    int Number = poses1.size();
    for (int i = 0; i < N; i++)
    {
        center1 += poses1[i].translation();
        center2 += poses2[i].translation();
    }
    center1 = center1 / Number;
    center2 = center2 / Number;

    vector<Eigen::Vector3d> p1(Number), p2(Number);
    for (int i = 0; i < N; i++)
    {
        p1[i] = poses1[i] - center1;
        p2[i] = poses2[i] - center2;
    }

    Eigen::Matrix3d error = Eigen::Matrix3d::Zero();
    for (int i = 0; i < N; i++)
    {
        error += p1[i] * p2[i].transpose();
    }

    Eigen::JacobiSVD<Eigen::Matrix3d> svd(error, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    cout << "U=" << U << endl;
    cout << "V=" << V << endl;

    Eigen::Matrix3d R = U * (V.transpose());
    Eigen::Vector3d t = p1 - R * p2;
    Sophus::SE3 T(R,t);
    return T;
}

int main(int argc, char **argv)
{

    vector<Sophus::SE3> poses1;
    vector<Sophus::SE3> poses2;

    /// implement pose reading code
    // start your code here (5~10 lines)

    ifstream fin(trajectory_file);
    for (int i = 0; i < 620; i++)
    {
        double data[8] = {0};
        for (auto &d : data)
            fin >> d;
        Eigen::Vector3d track1_t(data[1], data[2], data[3]);
        Eigen::Quaterniond track1_q(data[7], data[4], data[5], data[6]);
        Eigen::Vector3d track2_t(data[9], data[10], data[11]);
        Eigen::Quaterniond track2_q(data[12], data[13], data[14], data[15]);
        Sophus::SE3 SE3_1_qt(track1_q, track1_t);
        Sophus::SE3 SE3_2_qt(track2_q, track2_t);

        poses1.push_back(SE3_1_qt);
        poses2.push_back(SE3_2_qt);
    }
    // end your code here

    Sophus::SE3 T = pose_estimation_3d3d(vector<Sophus::SE3> poses1, vector<Sophus::SE3> poses2);
    for (int k = 0 ;k <poses2.size();k++)
    {
        poses2[k]  = T*poses2[k];
    }

    // draw trajectory in pangolin
    DrawTrajectory(poses1, poses2);

    return 0;
}


#include "apch.h"
#include "Color.h"
	//#include <Eigen/Core>

namespace A {

	static const float HSVToRGBHelper(float n, const Eigen::Vector4f& hsva)
	{
		AP_PROFILE_FN();
		float k = fmod(n + (hsva.x() * 360.0f), 6);
		return hsva.z() - hsva.z() * hsva.y() * std::max(std::min(std::min(k, 4 - k), 1.0f), 0.0f);
	}

	const Eigen::Vector4f HSVToRGB(const Eigen::Vector4f& hsva) noexcept
	{
		AP_PROFILE_FN();
		return Eigen::Vector4f(HSVToRGBHelper(5, hsva), HSVToRGBHelper(3, hsva), HSVToRGBHelper(1, hsva), hsva.w());
	}

	const Eigen::Vector4f RGBToHSV(const Eigen::Vector4f& rgba) noexcept
	{
		float V = std::max(std::max(rgba.x(), rgba.y()), rgba.z());
		float C = V - std::min(std::min(rgba.x(), rgba.y()), rgba.z());

		float h = 0;
		if (V == rgba.x())
			h = (rgba.y() - rgba.z()) / C;
		else if (V == rgba.y())
			h = 2 + (rgba.z() - rgba.x()) / C;
		else if (V == rgba.z())
			h = 4 + (rgba.x() - rgba.y()) / C;

		return Eigen::Vector4f(h, V == 0 ? 0 : C / V, V, rgba.w());
	}

}


#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Header.h>
#include <boost/math/constants/constants.hpp>
#include <tf/tf.h>

using namespace std;

static const double EPSILON = 0.01;
static const double PI = boost::math::constants::pi<double>();

namespace
{
class OnGroundDetector
{

private:
    //! Node handle
    ros::NodeHandle nh;

    //! Private nh
    ros::NodeHandle pnh;

    //! Human IMU subscriber
    auto_ptr<message_filters::Subscriber<sensor_msgs::Imu> > humanIMUSub;

    //! Human fall subscriber
    std::auto_ptr<message_filters::Subscriber<std_msgs::Header> > humanFallSub;

    //! On ground publisher
    ros::Publisher pub;

public:
    OnGroundDetector() : pnh("~")
    {
        ROS_INFO("Initializing the on ground detector");

        // Don't subscribe until the fall starts
        humanIMUSub.reset(new message_filters::Subscriber<sensor_msgs::Imu>(nh, "/in", 1));
        humanIMUSub->registerCallback(boost::bind(&OnGroundDetector::update, this, _1));
        humanIMUSub->unsubscribe();

        humanFallSub.reset(new message_filters::Subscriber<std_msgs::Header>(nh, "/human/fall", 1));
        humanFallSub->registerCallback(boost::bind(&OnGroundDetector::fallDetected, this, _1));

        pub = nh.advertise<std_msgs::Header>("/human/on_ground", 1, true);

        ROS_INFO("On ground detector initialized successfully");
    }

private:

    void fallDetected(const std_msgs::HeaderConstPtr& fallingMsg)
    {
        ROS_INFO("Human fall detected at @ %f. Beginning on ground detection.", fallingMsg->stamp.toSec());
        humanIMUSub->subscribe();
    }

    bool isOnGround(const geometry_msgs::Quaternion& orientation) {

        // Rotate the quaternion to compensate for the initial rotation of the pole
        tf::Quaternion correctedOrientation = orientPose(orientation);

        // Compute a vector from the quaternion
        tf::Vector3 up(0, 0, 1);
        tf::Vector3 poseVector = tf::quatRotate(correctedOrientation, up).normalize();

        // Now compute a vector that is the projection onto the ground plane
        tf::Vector3 ground(poseVector.x(), poseVector.y(), 0.0);
        ground.normalized();

        tfScalar angle = poseVector.angle(ground);

        return fabs(angle) < EPSILON || fabs(angle - PI) < EPSILON;
    }

    // Compensate for the model being initial aligned to the x axis and oriented
    // up
    static tf::Quaternion orientPose(const geometry_msgs::Quaternion& orientationMsg)
    {
        tf::Quaternion rotation = tf::createQuaternionFromRPY(0, PI / 2.0, 0);

        tf::Quaternion orientation;
        tf::quaternionMsgToTF(orientationMsg, orientation);
        orientation *= rotation;
        return orientation;
    }

    void update(const sensor_msgs::ImuConstPtr& imuData)
    {
        if (isOnGround(imuData->orientation))
        {
            std_msgs::Header header;
            header.stamp = ros::Time::now();
            pub.publish(header);
            tf::Quaternion q;
            tf::quaternionMsgToTF(imuData->orientation, q);
            tf::Matrix3x3 m(q);
            double roll, pitch, yaw;
            m.getRPY(roll, pitch, yaw);
            ROS_INFO("On ground detector has detected ground from Roll, Pitch, Yaw [%f] [%f] [%f]", roll, pitch, yaw);

            humanIMUSub->unsubscribe();
        }
    }
};
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "on_ground_detector");
    OnGroundDetector ogd;
    ros::spin();
    ROS_INFO("On ground detector exiting");
}


// Copyright (c) 2021 by Ignacio Alzugaray <alzugaray dot ign at gmail dot com>
// ETH Zurich, Vision for Robotics Lab.

#pragma once
#include <Eigen/Eigen>

namespace haste {
template<typename Value_, typename Location_>
class InterpolatorType {
 public:
  using Value = Value_;
  using Location = Location_;

  // TODO: Consider Rows and Cols to become size_t parameters.
  // TODO: Consider using derived types directly from Eigen to deduct Value / Location / Sizes.
  template<int kRows, int kCols>
  using ValueArray = Eigen::Array<Value, kRows, kCols>;

  template<int kRows, int kCols>
  using LocationArray = Eigen::Array<Location, kRows, kCols>;

  template<int kRows, int kCols>
  static inline auto bilinearIncrementVector(ValueArray<kRows, kCols> &mat, const Location &x, const Location &y,
                                             const Value &w) -> bool;

  template<int kRows, int kCols>
  static inline auto bilinearSample(const ValueArray<kRows, kCols> &mat, const Location &x, const Location &y) -> Value;

  template<int kRows, int kCols, int kSamples>
  static inline auto bilinearSampleVector(const ValueArray<kRows, kCols> &mat, const LocationArray<kSamples, 1> &x_vec,
                                          const LocationArray<kSamples, 1> &y_vec) -> ValueArray<kSamples, 1>;

  static inline auto bilinearKernel(const Location &x, const Location &y) -> ValueArray<2, 2>;

  template<int kRows, int kCols>
  static auto bilinearBlock(const ValueArray<kRows, kCols>& mat, const Location &xp, const Location &yp) -> Eigen::Ref<ValueArray<2, 2>>;

};
}// namespace haste

#include "interpolator_impl.hpp"


// 12.April 2015:  copied from main.cpp (does the same except that relative errors are computed)

// deal.II includes ----------------------------------------------
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_in.h>
#include <deal.II/lac/vector.h>

#include <deal.II/numerics/data_out.h>

// system includes -----------------------------------------------
#include <hdf5.h>
#include <omp.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>
#include <ctime>
#include <fstream>
#include <functional>
#include <iostream>
#include <thread>
// from eigen unsupported
//#include <Eigen/KroneckerProduct>

// own includes --------------------------------------------------
#include "aux/eigen2hdf.hpp"
#include "aux/message.hpp"
#include "aux/timer.hpp"
#include "grid_transfer.hpp"
#include "init/import/load_coefficients.hpp"
#include "post_processing/energy.hpp"
#include "post_processing/mass.hpp"
#include "post_processing/momentum.hpp"

#include "l2errors.hpp"
#include "spectral/basis/indexer.hpp"
// class SimpleGridHandler, Solution
#include "grid/grid_tools.hpp"
#include "outer_product_helper.hpp"
#include "solution_handler.hpp"
#include "spectral_transfer_matrix.hpp"

#include "export/data_out_hdf5.hpp"

using namespace boltzmann;
using namespace std;

const int dim = 2;

namespace bf = boost::filesystem;
namespace po = boost::program_options;

template <typename SPECTRAL_BASIS>
void make_overlap(std::vector<double>& S, const SPECTRAL_BASIS& spectral_basis)
{
  typedef typename SPECTRAL_BASIS::elem_t elem_t;

  // angular basis
  typedef typename std::tuple_element<0, typename SPECTRAL_BASIS::elem_t::container_t>::type
      angular_elem_t;
  typename elem_t::Acc::template get<angular_elem_t> acc_ang;

  // inverse mass matrix
  S.resize(spectral_basis.n_dofs());
  for (unsigned int j = 0; j < spectral_basis.n_dofs(); ++j) {
    auto& elem = spectral_basis.get_elem(j);
    if (acc_ang(elem).get_id().l == 0)
      S[j] = numbers::PI;
    else
      S[j] = numbers::PI / 2;
  }
}

typedef dealii::DoFHandler<dim> dh_t;
typedef dealii::Vector<double> vector_t;

int main(int argc, char* argv[])
{
  boltzmann::Timer<> timer;
  po::options_description options("options");
  options.add_options()
      ("config,c", po::value<string>()->required(), "config file")
      ("help,h", "help")
      ("output,o", "Output directory. Default creates dir `convergence_plots` in input dir")
      ("ignore-restarted", "do not check for symlinks to infer restarted calculations");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  if (vm.count("help")) {
    cout << options << "\n";
    return 0;
  }

  po::notify(vm);
  string config_name = vm["config"].as<string>();

  if (!boost::filesystem::is_regular_file(config_name)) {
    cout << "config file not found\n";
    return 1;
  }

  // load yaml config
  YAML::Node config = YAML::LoadFile(config_name);

  string str_input_grid = config["input"]["grid"].as<string>();
  string str_input_path = config["input"]["path"].as<string>();

  string str_ref_grid = config["reference"]["grid"].as<string>();
  string str_ref_path = config["reference"]["path"].as<string>();

  auto cwd = bf::current_path();

  // initialize working directory
  bf::path working_dir;
  if (vm.count("output")) {
    working_dir = bf::path(vm["output"].as<string>()) / bf::path("convergence_plots");
  } else {
    working_dir = bf::path(str_input_path) / bf::path("convergence_plots");
  }
  bf::create_directory(working_dir);

  // restart.config (load if existent)
  YAML::Node ref_restart_config;
  if (bf::exists(bf::path(str_ref_path) / bf::path("restart.yaml"))) {
    string fname = (bf::path(str_ref_path) / bf::path("restart.yaml")).c_str();
    ref_restart_config = YAML::LoadFile(fname);
  }
  YAML::Node inp_restart_config;
  if (bf::exists(bf::path(str_input_path) / bf::path("restart.yaml"))) {
    string fname = (bf::path(str_input_path) / bf::path("restart.yaml")).c_str();
    inp_restart_config = YAML::LoadFile(fname);
  }

  // initialize logfile
  std::time_t result = std::time(nullptr);
  cout << argv[0] << "at: " << asctime(localtime(&result)) << ", executed in " << cwd.c_str()
       << endl
       << setw(14) << "Solution: " << (working_dir / bf::path(str_input_path)).c_str() << setw(14)
       << "Reference: " << bf::absolute(working_dir, cwd) << endl;

  // ------------------------------------------------------------------------------------------
  // transfer matrix
  dealii::FE_Q<dim> fe(1);
  auto ref_grid_ptr = make_shared<SimpleGridHandler>(str_ref_path, str_ref_grid);
  auto grid_ptr = make_shared<SimpleGridHandler>(str_input_path, str_input_grid);

  const auto& ref_dh = ref_grid_ptr->get_dofhandler();
  const auto& input_dh = grid_ptr->get_dofhandler();

  cout << "Reference mesh: " << ref_dh.get_triangulation().n_used_vertices() << " vertices, "
       << ref_dh.get_triangulation().n_active_cells() << " cells." << endl;

  timer.start();
  GridTransfer<dim> grid_transfer;
  grid_transfer.init(ref_dh, input_dh);
  const auto& Tx = grid_transfer.get_transfer_matrix();
  print_timer(timer.stop(), "init GridTransfer");
  // spectral transfer matrix
  auto Tv =
      spectral_transfer_matrix(ref_grid_ptr->get_spectral_basis(), grid_ptr->get_spectral_basis());

  // ------------------------------------------------------------------------------------------
  // load solution filenames from config
  // S contains (b_i (v), b_i (v))_R^2
  vector<double> S;
  make_overlap(S, ref_grid_ptr->get_spectral_basis());

  // load permutations from `vertex2dofidx.dat`
  std::vector<unsigned int> ref_perm(ref_grid_ptr->get_dofhandler().n_dofs());
  std::vector<unsigned int> input_perm(grid_ptr->get_dofhandler().n_dofs());
  // load permutation from ``
  load_permutation(ref_perm, str_ref_path);
  load_permutation(input_perm, str_input_path);

  auto ref_perm_tmp = v2d_permutation_vector(ref_dh);
  auto input_perm_tmp = v2d_permutation_vector(input_dh);

  Mass mass(ref_grid_ptr->get_spectral_basis());
  Momentum momentum(ref_grid_ptr->get_spectral_basis());
  Energy energy(ref_grid_ptr->get_spectral_basis());
  const unsigned int L = ref_dh.n_dofs();
  dealii::Vector<double> vmass(L);
  dealii::Vector<double> venergy(L);
  dealii::Vector<double> vux(L);  // momentum x
  dealii::Vector<double> vuy(L);  // momnetum y

  dealii::Vector<double> vm_ref(L);       // mass reference
  dealii::Vector<double> venergy_ref(L);  // energy (reference)

  unsigned int nsteps = config["timesteps"].size();
  cout << "__ERRORS__ (relative)\n";
  cout << setw(15) << "# i" << setw(15) << "t" << setw(15) << "l2_squared" << setw(15)
       << "l2_m_squared" << setw(15) << "l2_u_squared (abs)" << setw(15) << "l2_e_squared" << endl;

  bool ref_restarted = false;  // restart toggle  (required to load new vertex2dof ordering)
  bool inp_restarted = false;

  if (vm.count("ignore-restarted")) {
    // set to true => will never enter restarted if(...)
    ref_restarted = true;
    inp_restarted = true;
  }

  auto xdmf_file = working_dir / bf::path("solution.xdmf");
  if (bf::exists(xdmf_file)) bf::remove(xdmf_file);

  for (unsigned int i = 0; i < nsteps; ++i) {
    string str_h5loc_ref = config["timesteps"][i]["reference"]["data"].as<string>();
    string str_h5loc_inp = config["timesteps"][i]["input"]["data"].as<string>();
    double time = config["timesteps"][i]["time"].as<double>();

    Eigen::VectorXd v_inp;  // approximate solution
    Eigen::VectorXd v_ref;  // reference solution

    auto fname_inp = load_solution_vector(v_inp, str_input_path, str_h5loc_inp);
    auto fname_ref = load_solution_vector(v_ref, str_ref_path, str_h5loc_ref);

    // check if coefficient vectors are from restarted computation and require
    // new vertex2dof mapping

    if (!ref_restarted && bf::is_symlink(fname_ref)) {
      string v2d_fname = ref_restart_config["v2d"].as<string>();
      load_permutation(ref_perm, str_ref_path, v2d_fname);
      ref_restarted = true;
      cout << "# restart in REFRENCE detected\n";
    }
    if (!inp_restarted && bf::is_symlink(fname_inp)) {
      string v2d_fname = inp_restart_config["v2d"].as<string>();
      load_permutation(input_perm, str_input_path, v2d_fname);
      inp_restarted = true;
      cout << "# restart in INPUT detected\n";
    }

    // transform to vertex ordering
    to_vertex_ordering(v_inp, input_perm);
    to_vertex_ordering(v_ref, ref_perm);

    // transform to active dofhandler ordering
    to_dof_ordering(v_inp, input_perm_tmp);
    to_dof_ordering(v_ref, ref_perm_tmp);

    // Eigen::VectorXd v_sol = T*v_inp;
    Eigen::VectorXd v_sol(v_ref.size());
    sparse_outer_product_multiply(v_sol, Tx, Tv, v_inp);

    // ----------------------------------------
    // Compute errors
    Errors errors;
    auto errors_result =
        errors.compute2(ref_dh, v_sol.data(), v_ref.data(), S, ref_grid_ptr->get_indexer());
    Eigen::VectorXd vdiff = v_sol - v_ref;
    // relative l2-error: |f-f_ref| / |f|
    const double l2_error_sq = errors_result[0] / errors_result[1];

    auto f1 = [&]() {
      mass.compute(vmass.begin(), vdiff.data(), L);
      std::for_each(vmass.begin(), vmass.end(), [](double v) { return std::abs(v); });
    };

    auto f2 = [&]() {
      energy.compute(venergy.begin(), vdiff.data(), L);
      std::for_each(venergy.begin(), venergy.end(), [](double v) { return std::abs(v); });
    };

    auto f3 = [&]() {
      momentum.compute(vux.begin(), vuy.begin(), vdiff.data(), L);
      std::transform(vux.begin(), vux.end(), vuy.begin(), vux.begin(), [](double x, double y) {
        return std::sqrt(x * x + y * y);
      });
    };

    auto mass_thread = thread(f1);
    auto energy_thread = thread(f2);
    auto momentum_thread = thread(f3);
    //
    mass_thread.join();
    energy_thread.join();
    momentum_thread.join();
    // reference values (ie norms)
    const double* vref_ptr = v_ref.data();
    auto f4 = [&]() {
      mass.compute(vm_ref.begin(), vref_ptr, L);
      std::for_each(vm_ref.begin(), vm_ref.end(), [](double v) { return std::abs(v); });
    };

    auto f5 = [&]() {
      energy.compute(venergy_ref.begin(), vref_ptr, L);
      std::for_each(venergy_ref.begin(), venergy_ref.end(), [](double v) { return std::abs(v); });
    };

    auto f4_thread = thread(f4);
    auto f5_thread = thread(f5);
    f4_thread.join();
    f5_thread.join();
    double l2diff_m = l2norm(ref_dh, vmass) / l2norm(ref_dh, vm_ref);
    double l2diff_u = l2norm(ref_dh, vux);
    double l2diff_e = l2norm(ref_dh, venergy) / l2norm(ref_dh, venergy_ref);
    cout << setw(15) << i << setw(15) << time << setw(15) << scientific << setprecision(5)
         << l2_error_sq << setw(15) << scientific << setprecision(5) << l2diff_m << setw(15)
         << scientific << setprecision(5) << l2diff_u << setw(15) << scientific << setprecision(5)
         << l2diff_e << endl;

    // output
    {
      // debug
      dealii::Vector<double> vmass2(L);
      dealii::Vector<double> vmass3(L);
      mass.compute(vmass2.begin(), v_sol.data(), L);
      mass.compute(vmass3.begin(), v_ref.data(), L);

      // dealii::DataOut<dim> data_out;
      dealii::DataOutHDF<dim> data_out;
      // data_out.attach_triangulation(ref_dh.get_triangulation());
      data_out.attach_dof_handler(ref_dh);
      const auto& cell_wise_error = errors.get_cell_wise_error();
      data_out.add_data_vector(cell_wise_error, "error2");
      data_out.add_data_vector(vmass, "err_mass");
      data_out.add_data_vector(venergy, "err_energy");
      data_out.add_data_vector(vux, "err_abs(u)");
      // debug
      data_out.add_data_vector(vmass2, "mass_input");
      data_out.add_data_vector(vmass3, "mass_ref");
      //  data_out.add_data_vector(tmp_out, "test_direct");
      data_out.build_patches();

      dealii::DataOutBase::VtkFlags flags;
      data_out.set_flags(flags);

      typedef dealii::DataOutBase::DataOutFilterFlags data_out_filter_flags;
      dealii::DataOutBase::DataOutFilter data_out_filter(data_out_filter_flags(true, true));
      data_out.write_filtered_data(data_out_filter);
      string filename =
          (working_dir / bf::path("output" + boost::lexical_cast<string>(i) + ".h5")).c_str();
      string mesh_filename = (working_dir / bf::path("mesh.hdf5")).c_str();
      data_out.write_hdf5(filename, data_out_filter);
      auto xdmf_entry = data_out.create_xdmf_entry(data_out_filter, mesh_filename, filename, time);

      std::ofstream fout(xdmf_file.c_str(), std::ios_base::out | std::ios_base::app);
      fout << xdmf_entry.get_xdmf_content(1) << std::endl;
      fout.close();

      if (i == 0) {
        data_out.write_mesh(mesh_filename, data_out_filter);
      }
    }
  }

  return 0;
}


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file graph_metrics.hpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-02-01
 */
#ifndef PAAL_GRAPH_METRICS_HPP
#define PAAL_GRAPH_METRICS_HPP

#include "basic_metrics.hpp"

#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/adjacency_matrix.hpp>

namespace paal {
namespace data_structures {

namespace graph_type {
class sparse_tag;
class dense_tag;
class large_tag;
}

/**
 * @brief traits for graph metric
 *
 * @tparam Graph
 */
template <typename Graph> struct graph_metric_traits {
    //default graph_type
    using graph_tag_type = graph_type::sparse_tag;
};

/// generic strategies of computing metric
template <typename graph_tag_type> struct graph_metric_filler_impl;

/**
 * @brief specialization for sparse_tag graphs
 */
template <> struct graph_metric_filler_impl<graph_type::sparse_tag> {
    /**
     * @brief fill_matrix function
     *
     * @tparam Graph
     * @tparam ResultMatrix
     * @param g
     * @param rm
     */
    template <typename Graph, typename ResultMatrix>
    void fill_matrix(const Graph &g, ResultMatrix &rm) {
        boost::johnson_all_pairs_shortest_paths(g, rm);
    }
};

/**
 * @brief specialization strategies of computing metric for dense_tag graphs
 */
template <> struct graph_metric_filler_impl<graph_type::dense_tag> {
    template <typename Graph, typename ResultMatrix>
    /**
     * @brief fill_matrixFunction
     *
     * @param g
     * @param rm
     */
        void fill_matrix(const Graph &g, ResultMatrix &rm) {
        boost::floyd_warshall_all_pairs_shortest_paths(g, rm);
    }
};

/**
 * @class graph_metric
 * @brief Adopts boost graph as \ref metric.
 *
 * @tparam Graph
 * @tparam DistanceType
 * @tparam GraphType
 */
// GENERIC
// GraphType could be sparse, dense, large ...
template <
    typename Graph, typename DistanceType,
    typename GraphType = typename graph_metric_traits<Graph>::graph_tag_type>
struct graph_metric : public array_metric<DistanceType>,
                      public graph_metric_filler_impl<
                          typename graph_metric_traits<Graph>::graph_tag_type> {
    typedef array_metric<DistanceType> GMBase;
    typedef graph_metric_filler_impl<
        typename graph_metric_traits<Graph>::graph_tag_type> GMFBase;

    /**
     * @brief constructor
     *
     * @param g
     */
    graph_metric(const Graph &g) : GMBase(num_vertices(g)) {
        GMFBase::fill_matrix(g, GMBase::m_matrix);
    }
};

// TODO implement
/// Specialization for large graphs
template <typename Graph, typename DistanceType>
struct graph_metric<Graph, DistanceType, graph_type::large_tag> {
    /**
     * @brief constructor
     *
     * @param g
     */
    graph_metric(const Graph &g) { assert(false); }
};

/// Specialization for adjacency_list
template <typename OutEdgeList, typename VertexList, typename Directed,
          typename VertexProperties, typename EdgeProperties,
          typename GraphProperties, typename EdgeList>
struct graph_metric_traits<
    boost::adjacency_list<OutEdgeList, VertexList, Directed, VertexProperties,
                          EdgeProperties, GraphProperties, EdgeList>> {
    typedef graph_type::sparse_tag graph_tag_type;
};

/// Specialization for adjacency_matrix
template <typename Directed, typename VertexProperty, typename EdgeProperty,
          typename GraphProperty, typename Allocator>
struct graph_metric_traits<boost::adjacency_matrix<
    Directed, VertexProperty, EdgeProperty, GraphProperty, Allocator>> {
    typedef graph_type::dense_tag graph_tag_type;
};

} //!data_structures
} //!paal

#endif // PAAL_GRAPH_METRICS_HPP


#include <sparse.h>
#include <sparse_fill.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(SPARSE);

BOOST_AUTO_TEST_CASE(product_crm_vector_test)
{
  typedef sparse::Block<1,1,float> block1x1_type;
  typedef sparse::Block<4,1,float> block4x1_type;
  typedef sparse::Block<3,1,float> block3x1_type;
  typedef sparse::Block<3,4,float> block3x4_type;

  // scalar blocks
  sparse::CompressedRowMatrix< block1x1_type > sA(2,4,4);
  sparse::Vector< block1x1_type > su(4);
  sparse::CompressedVector< block1x1_type > csu(4,4);
  block1x1_type result(0);

  sA(0,0)[0] = 1.0f;
  sA(0,3)[0] = 2.0f;
  sA(1,1)[0] = 3.0f;
  sA(1,2)[0] = 4.0f;
  su(0) = 5.0f;
  su(1) = 6.0f;
  su(2) = 7.0f;
  su(3) = 8.0f;
  sparse::row_prod(sA, su, result, 0);
  BOOST_CHECK( result == 21.0f );
  sparse::row_prod(sA, su, result, 1);
  BOOST_CHECK( result == 67.0f ); // 46 + 21
  csu(0) = 5.0f;
  csu(1) = 6.0f;
  csu(2) = 7.0f;
  csu(3) = 8.0f;
  result = 0;
  sparse::row_prod(sA, csu, result, 0);
  BOOST_CHECK( result == 21.0f );
  sparse::row_prod(sA, csu, result, 1);
  BOOST_CHECK( result == 67.0f ); // 46 + 21

  // block case
  sparse::CompressedRowMatrix< block3x4_type > A(1,1,1);
  sparse::Vector< block4x1_type > u(1);
  sparse::CompressedVector< block4x1_type > cu(1,1);
  block3x1_type block_result(0);

  sparse::fill(A(0,0));
  sparse::fill(u(0));
  sparse::row_prod(A,u,block_result,0);
  BOOST_CHECK( block_result[0] == 14 );
  BOOST_CHECK( block_result[1] == 38 );
  BOOST_CHECK( block_result[2] == 62 );
  sparse::fill(cu(0));
  block_result.clear_data();
  sparse::row_prod(A,cu,block_result,0);
  BOOST_CHECK( block_result[0] == 14 );
  BOOST_CHECK( block_result[1] == 38 );
  BOOST_CHECK( block_result[2] == 62 );

  A.clear();
  u.clear();
  A.resize(2,4,4);
  u.resize(4);
  block_result.clear_data();

  sparse::fill(A(0,0),1);
  sparse::fill(A(0,3),2);
  sparse::fill(A(1,1),3);
  sparse::fill(A(1,2),4);
  sparse::fill(u(0),1);
  sparse::fill(u(1),2);
  sparse::fill(u(2),3);
  sparse::fill(u(3),4);
  sparse::row_prod(A, u, block_result, 0);
  BOOST_CHECK( block_result[0] == 112 );
  BOOST_CHECK( block_result[1] == 240 );
  BOOST_CHECK( block_result[2] == 368 );
  cu.clear();
  cu.resize(4,4);
  sparse::fill(cu(0),1);
  sparse::fill(cu(1),2);
  sparse::fill(cu(2),3);
  sparse::fill(cu(3),4);
  block_result.clear_data();
  sparse::row_prod(A, cu, block_result, 0);
  BOOST_CHECK( block_result[0] == 112 );
  BOOST_CHECK( block_result[1] == 240 );
  BOOST_CHECK( block_result[2] == 368 );

  block_result.clear_data();
  sparse::row_prod(A, u, block_result, 1);
  BOOST_CHECK( block_result[0] == 172 );
  BOOST_CHECK( block_result[1] == 300 );
  BOOST_CHECK( block_result[2] == 428 );
  block_result.clear_data();
  sparse::row_prod(A, cu, block_result, 1);
  BOOST_CHECK( block_result[0] == 172 );
  BOOST_CHECK( block_result[1] == 300 );
  BOOST_CHECK( block_result[2] == 428 );

  // test diagonal matrix
  sparse::DiagonalMatrix< block3x4_type > DM(2,2,2);
  sparse::fill(DM(0),1.0f);
  sparse::fill(DM(1),1.0f);
  u.resize(2);
  sparse::fill(u(0),1.0f);
  sparse::fill(u(1),1.0f);
  block_result[0] = 0.0f;
  block_result[1] = 0.0f;
  block_result[2] = 0.0f;
  sparse::row_prod(DM, u, block_result, 0);
  BOOST_CHECK( block_result[0] == 30.0f );
  BOOST_CHECK( block_result[1] == 70.0f );
  BOOST_CHECK( block_result[2] == 110.0f );
  block_result[0] = 0.0f;
  block_result[1] = 0.0f;
  block_result[2] = 0.0f;
  sparse::row_prod(DM, u, block_result, 1);
  BOOST_CHECK( block_result[0] == 30.0f );
  BOOST_CHECK( block_result[1] == 70.0f );
  BOOST_CHECK( block_result[2] == 110.0f );

  // 2 column matrix
  sparse::TwoColumnMatrix< block3x4_type > TCM(2, 4);
  sparse::Vector< block3x1_type > v(2);
  sparse::fill(TCM(0,0),1);
  sparse::fill(TCM(0,1),2);
  sparse::fill(TCM(1,1),3);
  sparse::fill(TCM(1,3),4);
  u.resize(4);
  sparse::fill(u(0),1);
  sparse::fill(u(1),2);
  sparse::fill(u(2),3);
  sparse::fill(u(3),4);
  sparse::prod(TCM,u,v); // compute "correct" data
  block_result[0] = 0.0f;
  block_result[1] = 0.0f;
  block_result[2] = 0.0f;
  sparse::row_prod(TCM,u,block_result,0);
  BOOST_CHECK( block_result == v(0) );
  block_result[0] = 0.0f;
  block_result[1] = 0.0f;
  block_result[2] = 0.0f;
  sparse::row_prod(TCM,u,block_result,1);
  BOOST_CHECK( block_result == v(1) );
}

BOOST_AUTO_TEST_SUITE_END();


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  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/simd/function/scalar/acscd.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/two.hpp>

STF_CASE_TPL (" acscd",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::acscd;

  using r_t = decltype(acscd(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(acscd(bs::Inf<T>()), T(0), 0.5);
  STF_ULP_EQUAL(acscd(bs::Minf<T>()), T(0), 0.5);
  STF_ULP_EQUAL(acscd(bs::Nan<T>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(acscd(bs::Zero<T>()), bs::Nan<r_t>(), 0);
#endif
  STF_ULP_EQUAL(acscd(-bs::Two<T>()), T(-30), 0.5);
  STF_ULP_EQUAL(acscd(bs::Mone<T>()), T(-90), 0.5);
  STF_ULP_EQUAL(acscd(bs::One<T>()),  T(90), 0.5);
  STF_ULP_EQUAL(acscd(bs::Two<T>()),  T(30), 0.5);
}


/*
 *  test_bonds.hpp
 *  bondgeek
 *
 *  Created by BART MOSLEY on 8/16/12.
 *  Copyright 2012 BG Research LLC. All rights reserved.
 *
 */

#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include <bg/bondgeek.hpp>

using namespace QuantLib;
using namespace bondgeek;
using namespace boost::unit_test;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {
    Integer sessionId() { return 0; }
}
#endif

// for arrays
#define LENGTH(x) (sizeof(x)/sizeof(x[0]))

struct BondTests {
	Date todaysDate;

	RateHelperCurve acurve;
    boost::shared_ptr<PricingEngine> discEngine;

	BondTests() :
	todaysDate(TARGET().adjust(Date(20, September, 2004)))
	{
        BOOST_TEST_MESSAGE("\nBond tests");
        string depotenors[] = {"1W", "1M", "3M", "6M", "9M", "1y"};
        double depospots[] = {.055, .055, .055, .055, .055, .055};
        string swaptenors[] = {"2y", "3y", "5y", "10y", "15y", "20y", "30y"};
        double swapspots[] = {.055, .055, .055, .055, .055, .055, .055};

        acurve = RateHelperCurve(USDLiborCurve("3M"));
        acurve.update(depotenors,
                      depospots,
                      6,
                      swaptenors,
                      swapspots,
                      7,
                      todaysDate);

        discEngine = \
        createPriceEngine<DiscountingBondEngine>(
                                                 acurve.discountingTermStructure()
                                                 );

		Settings::instance().evaluationDate() = todaysDate;
	}
};

BOOST_FIXTURE_TEST_SUITE( bondtest, BondTests )

BOOST_AUTO_TEST_CASE( bulletbond_price_yield_consistency )
{
    BOOST_MESSAGE("Testing consistency of bond price-yield calcs...");
    Rate coupon[] = {.06, .05, .04};
    Date maturity[] = {
        Date(15, September,2012),
        Date(1, November,2015),
        Date(30, June,2011),
        Date(28, February, 2017)
    };
    Date dated[] = {
        Date(15, September,2004),
        Date(1, May,2004),
        Date(30, June,2004),
        Date(1, February, 2004)
    };
    Calendar bondCalendar[] = {
        UnitedStates(UnitedStates::GovernmentBond),
        UnitedStates(UnitedStates::Settlement),
        UnitedStates(UnitedStates::NYSE)
    };
    DayCounter bondDayCounter[] = {
        ActualActual(ActualActual::Bond),
        ActualActual(ActualActual::Actual365),
        ActualActual(ActualActual::ISDA),
        ActualActual(ActualActual::Euro),
        Thirty360(Thirty360::BondBasis),
        Thirty360(Thirty360::USA),
        Thirty360(Thirty360::European),
        Thirty360(Thirty360::EurobondBasis)
    };

    Natural settlementDays = 3;
    Frequency frequency = Semiannual;

    Real faceAmount = 100.0; // Notional amount
    Real redemption = 100.0; // Amount paid on redemption

    BusinessDayConvention accrualConvention = Unadjusted;
    BusinessDayConvention paymentConvention = Unadjusted;

    Real tolerance = 1.0e-5;

    double prc;
    double yld;
    for (int i=0; i < LENGTH(coupon); i++) {
        for (int j=0; j < LENGTH(maturity); j++) {
            for (int k=0; k < LENGTH(bondCalendar); k++) {
                for (int l=0; l < LENGTH(bondDayCounter); l++) {
                    BulletBond bnd(
                                   coupon[i],
                                   maturity[j],
                                   dated[j],
                                   bondCalendar[k],
                                   settlementDays,
                                   bondDayCounter[l],
                                   frequency,
                                   redemption,
                                   faceAmount,
                                   accrualConvention,
                                   paymentConvention
                                   );

                    bnd.setEngine(acurve);

                    prc = bnd.toPrice();
                    yld = bnd.toYield(prc);

                    BOOST_CHECK( abs(bnd.toPrice(yld) - prc) < tolerance );
                }
            }
        }
    }

    BOOST_TEST_MESSAGE("OK");
}

BOOST_AUTO_TEST_CASE( callbond_price_yield_consistency )
{
    BOOST_MESSAGE("Testing consistency of bond price-yield calcs...");
    Rate coupon = .05;
    Date maturity(15, September,2016);
    Date dated(15, September, 2004);
    Date calldate[] = {
        Date(15, September,2011),
        Date(15, September,2012),
        Date(15, September,2013)
    };
    Real callprc[] = {100., 101., 102.};
    Date pardate[] = {
        maturity,
        Date(15, September,2013),
        Date(15, September,2015)
    };

    Calendar bondCalendar = UnitedStates(UnitedStates::Settlement);
    DayCounter bondDayCounter = Thirty360(Thirty360::BondBasis);

    Natural settlementDays = 3;
    Frequency frequency = Semiannual;

    Real faceAmount = 100.0; // Notional amount
    Real redemption = 100.0; // Amount paid on redemption

    BusinessDayConvention accrualConvention = Unadjusted;
    BusinessDayConvention paymentConvention = Unadjusted;

    Real tolerance = 1.0e-5;

    double prc;
    double yld;
    for (int i=0; i < LENGTH(calldate); i++) {

        CallBond bnd(
                     coupon,
                     maturity,
                     calldate[i],
                     callprc[i],
                     pardate[i],
                     dated,
                     bondCalendar,
                     settlementDays,
                     bondDayCounter,
                     frequency,
                     Annual,
                     redemption,
                     faceAmount,
                     accrualConvention,
                     paymentConvention
                     );

        bnd.setEngine(acurve);

        prc = bnd.toPrice();
        yld = bnd.toYield(prc);

        BOOST_CHECK( abs(bnd.toPrice(yld) - prc) < tolerance );
    }

    BOOST_TEST_MESSAGE("OK");
}

BOOST_AUTO_TEST_CASE( bond_value_sinker_consistency )
{
    BOOST_MESSAGE("Testing consistency of sinking fund bond calcs...");
    Real coupon = .06;
    Date maturity(15, September,2012);
    Date maturity1(15, September, 2011);
    Date maturity0(15, September, 2010);

    Date dated(16,September,2004);

    Natural settlementDays = 3;
    Frequency frequency = Semiannual;

    Calendar bondCalendar = UnitedStates(UnitedStates::GovernmentBond);
    DayCounter bondDayCounter = ActualActual(ActualActual::Bond);

    Real faceAmount = 100.0; // Notional amount
    Real redemption = 100.0; // Amount paid on redemption

    BusinessDayConvention accrualConvention = Unadjusted;
    BusinessDayConvention paymentConvention = Unadjusted;

    SinkingFundBond bnd(coupon,
                        maturity,
                        std::vector<Real>(1, faceAmount),
                        Annual,
                        dated,
                        bondCalendar,
                        settlementDays,
                        bondDayCounter,
                        frequency,
                        redemption,
                        faceAmount,
                        accrualConvention,
                        paymentConvention
                        );

    SinkingFundBond bnd0(coupon,
                         maturity0,
                         std::vector<Real>(1, faceAmount),
                         Annual,
                         dated,
                         bondCalendar,
                         settlementDays,
                         bondDayCounter,
                         frequency,
                         redemption,
                         faceAmount,
                         accrualConvention,
                         paymentConvention
                         );

    SinkingFundBond bnd1(coupon,
                         maturity1,
                         std::vector<Real>(1, faceAmount),
                         Annual,
                         dated,
                         bondCalendar,
                         settlementDays,
                         bondDayCounter,
                         frequency,
                         redemption,
                         faceAmount,
                         accrualConvention,
                         paymentConvention
                         );

    /* Sinkfing fund amortization:

     notionals start at par on the dated date and end at zero on maturity.
     redemptions are the prices redemption prices (pct of par, e.g. 101.)

     */

    double sf_sch[] = {
        40000, 40000, 40000
    };
    int sf_num = 3;
    Frequency sf_freq = Annual;

    std::vector<double> sf_bal;
    sf_bal.assign(sf_sch, sf_sch+sf_num);

    SinkingFundBond bnd2(coupon,
                         maturity,
                         sf_bal,
                         sf_freq,
                         dated,
                         bondCalendar,
                         settlementDays,
                         bondDayCounter,
                         frequency,
                         redemption,
                         faceAmount,
                         accrualConvention,
                         paymentConvention
                         );

    bnd.setEngine(acurve);

    bnd0.setPricingEngine(discEngine);
    bnd1.setPricingEngine(discEngine);
    bnd2.setPricingEngine(discEngine);

    double prc = bnd.toPrice();

    double prc0 = bnd0.cleanPrice();
    double prc1 = bnd1.cleanPrice();
    double prc2 = bnd2.cleanPrice();

    double avgpx = (prc0+prc1+prc)/3.;

    BOOST_CHECK( abs(prc2-avgpx) < .0001 );
    BOOST_TEST_MESSAGE("OK");
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  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/simd/function/ldexp.hpp>
#include <boost/simd/function/std.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/four.hpp>
#include <boost/simd/constant/halfeps.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/detail/constant/maxexponent.hpp>
#include <boost/simd/detail/constant/limitexponent.hpp>
#include <boost/simd/detail/constant/minexponent.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/function/scalar/dec.hpp>

STF_CASE_TPL("ldexp std", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ldexp;

  using iT = bd::as_integer_t<T>;
  using r_t = decltype(ldexp(T(), iT()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(bs::std_(ldexp)(bs::Inf<T>(),  2), bs::Inf<r_t>());
  STF_EQUAL(bs::std_(ldexp)(bs::Minf<T>(), 2), bs::Minf<r_t>());
  STF_IEEE_EQUAL(bs::std_(ldexp)(bs::Nan<T>(),  2), bs::Nan<r_t>());
#endif
  STF_EQUAL(bs::std_(ldexp)(bs::Mone<T>(), 2), -bs::Four<r_t>());
  STF_EQUAL(bs::std_(ldexp)(bs::One<T>(),  2), bs::Four<r_t>());
  STF_EQUAL(bs::std_(ldexp)(bs::Zero<T>(), 2), bs::Zero<r_t>());
  STF_EQUAL(bs::std_(ldexp)(bs::One <T>(), bs::Minexponent<T>()), bs::Smallestposval<r_t>());
  STF_EQUAL(bs::std_(ldexp)(bs::One<T>()-bs::Halfeps<T>(),  bs::Maxexponent<T>()), bs::Valmax<T>()/2);
  STF_EQUAL(bs::std_(ldexp)(bs::One<T>()-bs::Halfeps<T>(),  bs::Limitexponent<T>()), bs::Valmax<T>());
#ifndef BOOST_SIMD_NO_DENORMALS
  using bs::dec;
  STF_EQUAL(bs::std_(ldexp)(bs::One <T>(), dec(bs::Minexponent<T>())), bs::Smallestposval<T>()/2);
  STF_EQUAL(bs::std_(ldexp)(bs::Two <T>(), dec(bs::Minexponent<T>())), bs::Smallestposval<T>());
  STF_EQUAL(bs::std_(ldexp)(bs::Two <T>(), dec(bs::Minexponent<T>()-1)), bs::Smallestposval<T>()/2);
  STF_EQUAL(bs::std_(ldexp)(bs::One <T>(), bs::Minexponent<T>()-5), bs::Smallestposval<T>()/32);
#endif
}


#pragma once

#include <mtao/types.hpp>
#include <Eigen/Geometry>
#include <mtao/geometry/grid/staggered_grid.hpp>
#include <mandoline/construction/cutdata.hpp>

namespace mandoline::tools {
class SliceGenerator : public mtao::geometry::grid::StaggeredGrid<double, 3> {
  public:
    using Base = mtao::geometry::grid::StaggeredGrid<double, 3>;
    SliceGenerator() {}
    SliceGenerator(const mtao::ColVecs3d &V, const mtao::ColVecs3i &F);
    std::tuple<mtao::ColVecs3d, mtao::ColVecs3i> slice(const mtao::Vec3d &origin, const mtao::Vec3d &direction);
    static Eigen::Affine3d get_transform(const mtao::Vec3d &origin, const mtao::Vec3d &direction);
    std::tuple<mtao::ColVecs3d, mtao::ColVecs3i> slice(const Eigen::Affine3d &t);
    void set_vertices(const mtao::ColVecs3d &V);
    Eigen::SparseMatrix<double> barycentric_map() const;

  private:
    void update_embedding(const mtao::ColVecs3d &V);
    mtao::ColVecs3d V;
    construction::CutData<3> data;
};

template<typename... Args>
std::tuple<mtao::ColVecs3d, mtao::ColVecs3i> slice(const mtao::ColVecs3d &V, const mtao::ColVecs3i &F, Args &&... args) {
    SliceGenerator sg(V, F);
    return sg.slice(std::forward<Args>(args)...);
}

}// namespace mandoline::tools


/*
 *  transfer.hpp
 *
 *
 *  Created by Andrea Bedini on 13/Jul/2009.
 *  Copyright (c) 2009-2014, Andrea Bedini <andrea.bedini@gmail.com>.
 *
 *  Distributed under the terms of the Modified BSD License.
 *  The full license is in the file COPYING, distributed as part of
 *  this software.
 *
 */

#ifndef TRANSFER_HPP
#define TRANSFER_HPP

#include "tree_decomposition/tree_decomposition.hpp"
#include <boost/range/algorithm/set_algorithm.hpp>

namespace transfer {
  using tree_decomposition::vertex_list;
  using tree_decomposition::bag_ptr;

  template<class Operators>
  typename Operators::table_type
  recurse(const Operators& op, bag_ptr b)
  {
    // create a new table containing only the empty state
    auto const n = b->vertices.size();
    auto table = op.empty_state(n);

    // iterates over children
    for (auto b_sib : b->children) {
      // recurse
      auto table_sib = recurse(op, b_sib);

      // diffe contains the vertices in b_sib which are not in b (the parent bag)
      std::vector<unsigned int> diffe;
      boost::set_difference(b_sib->vertices, b->vertices, std::back_inserter(diffe));

      // delete each vertex not present in the parent bag

      // we need to make a copy first because
      // 1) we need to keep the indices consistent while removing vertices
      // 2) we don't want to destroy the tree decomposition
      vertex_list b_sib_left_over(b_sib->vertices);
      for (auto v : diffe) {
        table_sib = op.delete_operator(b_sib_left_over.index(v), table_sib);
        b_sib_left_over.remove(v);
      }

      // create b_sib to b bag mapping
      auto const A_size = b_sib_left_over.size();
      std::vector<unsigned int> A_to_B(A_size);
      for (unsigned int i = 0; i < A_size; ++i)
        A_to_B[i] = b->vertices.index(b_sib_left_over.at(i));

      table = op.table_fusion(A_to_B, table_sib, table);
    }

    // apply the join operator for each edge in the bag
    for (auto e : b->edges) {
      table = op.join_operator(b->vertices.index(e.first),
        b->vertices.index(e.second), table);
    }
    return table;
  }

  template<class Operators>
  typename Operators::weight_type
  transfer(const Operators& op, bag_ptr b)
  {
    auto table = recurse(op, b);

    // we need to make a copy first because
    // 1) we need to keep the indices consistent while removing vertices
    // 2) we don't want to destroy the tree decomposition
    vertex_list v_to_remove(b->vertices);
    for (auto v : b->vertices) {
      table = op.delete_operator(v_to_remove.index(v), table);
      v_to_remove.remove(v);
    }
    assert(table.size() == 1);
    return table.begin()->second;
  }
}

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SCALAR_STIRLING_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SCALAR_STIRLING_HPP_INCLUDED

#include <nt2/euler/functions/stirling.hpp>
#include <nt2/euler/functions/details/stirling_kernel.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/sqrt_2pi.hpp>
#include <nt2/include/constants/stirlinglargelim.hpp>
#include <nt2/include/constants/stirlingsplitlim.hpp>
#include <nt2/include/functions/scalar/exp.hpp>
#include <nt2/include/functions/scalar/fma.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/include/functions/scalar/pow.hpp>
#include <nt2/include/functions/scalar/rec.hpp>

#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <nt2/include/functions/scalar/is_nan.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( stirling_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      #ifndef BOOST_SIMD_NO_INVALIDS
      if (nt2::is_nan(a0)) return a0;
      #endif
      if (a0 > nt2::Stirlinglargelim<A0>()) return nt2::Inf<A0>();
      A0 w = nt2::rec(a0);
      w = fma(w,details::stirling_kernel<A0>::stirling1(w), nt2::One<A0>());
      A0 y = nt2::exp(-a0);
      if(nt2::is_eqz(y)) return nt2::Inf<A0>();
      A0 z =  a0 - nt2::Half<A0>();
      if( a0 >= nt2::Stirlingsplitlim<A0>() )
      { /* Avoid overflow in pow() */
        const A0 v = nt2::pow(a0,z*Half<A0>());
        y *= v;
        y *= v;
      }
      else
      {
        y *= nt2::pow( a0, z );
      }
      y *= nt2::Sqrt_2pi<A0>()*w;
      return y;
    }
  };
} }

#endif


//
//  stochasticTools.cpp
//
//
//  Created by Jose V. Alcala Burgos on 7/30/13.
//  Copyright [2013] Jose V. Alcala Burgos
//

#include "base/stochasticTools.h"

// System
#include <armadillo>

#include <iostream>
#include <random>

stochasticGradient::stochasticGradient(
    const mat& __H,
    const mat& __Sigma,
    const mat& __alpha_optimal): _M_H(__H),
                                 _M_Sigma(__Sigma),
                                 _M_alpha_optimal(__alpha_optimal) {
        /**
         *Check that dimensions are consistent
         */
        _M_par_dim = _M_alpha_optimal.n_cols;
        _GLIBCXX_DEBUG_ASSERT(1 == _M_alpha_optimal.n_rows);
        _GLIBCXX_DEBUG_ASSERT(_M_H.n_cols == _M_alpha_optimal.n_cols);
        _GLIBCXX_DEBUG_ASSERT(_M_H.n_rows == _M_alpha_optimal.n_cols);
        _GLIBCXX_DEBUG_ASSERT(_M_Sigma.n_cols == _M_alpha_optimal.n_cols);
        _GLIBCXX_DEBUG_ASSERT(_M_Sigma.n_rows == _M_alpha_optimal.n_cols);
    }


////////////////////////////////////////////
// File: doosabin_regression.cpp          //
// Copyright Richard Stebbing 2015.       //
// Distributed under the MIT License.     //
// (See accompany file LICENSE or copy at //
//  http://opensource.org/licenses/MIT)   //
////////////////////////////////////////////

// Includes
#include <algorithm>
#include <cstdio>
#include <fstream>
#include <iostream>
#include <memory>
#include <string>
#include <sstream>
#include <utility>

#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"

#include <Eigen/Dense>

#include "rapidjson/document.h"
#include "rapidjson/filewritestream.h"
#include "rapidjson/prettywriter.h"

#include "Ceres/composed_cost_function.h"
#include "Math/linalg.h"

#include "doosabin.h"

#include "ceres_surface.h"
#include "surface.h"

// DooSabinSurface
class DooSabinSurface : public Surface {
 public:
  typedef doosabin::Surface<double> Surface;

  explicit DooSabinSurface(const Surface* surface)
    : surface_(surface)
  {}

  #define EVALUATE(M, SIZE) \
  virtual void M(double* r, const int p, const double* u, \
                 const double* const* X) const { \
    const Eigen::Map<const Eigen::Vector2d> _u(u); \
    const linalg::MatrixOfColumnPointers<double> _X( \
      X, 3, surface_->patch_vertex_indices(p).size()); \
    Eigen::Map<Eigen::VectorXd> _r(r, SIZE); \
    surface_->M(p, _u, _X, &_r); \
  }
  #define SIZE_JACOBIAN_X (3 * 3 * surface_->patch_vertex_indices(p).size())
  EVALUATE(M, 3);
  EVALUATE(Mu, 3);
  EVALUATE(Mv, 3);
  EVALUATE(Muu, 3);
  EVALUATE(Muv, 3);
  EVALUATE(Mvv, 3);
  EVALUATE(Mx, SIZE_JACOBIAN_X);
  EVALUATE(Mux, SIZE_JACOBIAN_X);
  EVALUATE(Mvx, SIZE_JACOBIAN_X);
  #undef EVALUATE
  #undef SIZE_JACOBIAN_X

  virtual int number_of_vertices() const {
    return surface_->number_of_vertices();
  }

  virtual int number_of_faces() const {
    return surface_->number_of_faces();
  }

  virtual int number_of_patches() const {
    return surface_->number_of_patches();
  }

  virtual const std::vector<int>& patch_vertex_indices(const int p) const {
    return surface_->patch_vertex_indices(p);
  }

  virtual const std::vector<int>& adjacent_patch_indices(const int p) const {
    return surface_->adjacent_patch_indices(p);
  }

 private:
  const Surface* surface_;
};

// PositionErrorFunctor
class PositionErrorFunctor {
 public:
  template <typename M>
  PositionErrorFunctor(const M& m0, const double& sqrt_w = 1.0)
    : m0_(m0), sqrt_w_(sqrt_w)
  {}

  template <typename T>
  bool operator()(const T* m, T* e) const {
    e[0] = T(sqrt_w_) * (T(m0_[0]) - m[0]);
    e[1] = T(sqrt_w_) * (T(m0_[1]) - m[1]);
    e[2] = T(sqrt_w_) * (T(m0_[2]) - m[2]);
    return true;
  }

 private:
  Eigen::Vector3d m0_;
  const double sqrt_w_;
};

// PairwiseErrorFunctor
class PairwiseErrorFunctor {
 public:
  PairwiseErrorFunctor(const double& sqrt_w = 1.0)
    : sqrt_w_(sqrt_w)
  {}

  template <typename T>
  bool operator()(const T* x0, const T* x1, T* e) const {
    e[0] = T(sqrt_w_) * (x1[0] - x0[0]);
    e[1] = T(sqrt_w_) * (x1[1] - x0[1]);
    e[2] = T(sqrt_w_) * (x1[2] - x0[2]);
    return true;
  }

 private:
  const double sqrt_w_;
};

// ReadFileIntoDocument
bool ReadFileIntoDocument(const std::string& input_path,
                          rapidjson::Document* document) {
  std::ifstream input(input_path, std::ios::binary);
  if (!input) {
    LOG(ERROR) << "File \"" << input_path << "\" not found.";
    return false;
  }

  std::stringstream input_ss;
  input_ss << input.rdbuf();
  if (document->Parse<0>(input_ss.str().c_str()).HasParseError()) {
    LOG(ERROR) << "Failed to parse input.";
    return false;
  }

  return true;
}

// LoadProblemFromFile
bool LoadProblemFromFile(const std::string& input_path,
                         Eigen::MatrixXd* Y,
                         std::vector<int>* raw_face_array,
                         Eigen::MatrixXd* X,
                         Eigen::VectorXi* p,
                         Eigen::MatrixXd* U) {
  rapidjson::Document document;
  if (!ReadFileIntoDocument(input_path, &document)) {
    return false;
  }

  #define LOAD_MATRIXD(K, DIM) { \
    CHECK(document.HasMember(#K)); \
    auto& v = document[#K]; \
    CHECK(v.IsArray()); \
    K->resize(DIM, v.Size() / DIM); \
    for (rapidjson::SizeType i = 0; i < v.Size(); ++i) { \
      CHECK(v[i].IsDouble()); \
      (*K)(i % DIM, i / DIM) = v[i].GetDouble(); \
    } \
  }
  #define LOAD_VECTORI(K) { \
    CHECK(document.HasMember(#K)); \
    auto& v = document[#K]; \
    CHECK(v.IsArray()); \
    K->resize(v.Size()); \
    for (rapidjson::SizeType i = 0; i < v.Size(); ++i) { \
      CHECK(v[i].IsInt()); \
      (*K)[i] = v[i].GetInt(); \
    } \
  }

  LOAD_MATRIXD(Y, 3);
  LOAD_VECTORI(raw_face_array);
  LOAD_MATRIXD(X, 3);
  LOAD_VECTORI(p);
  LOAD_MATRIXD(U, 2);

  #undef LOAD_MATRIXD
  #undef LOAD_VECTORI

  return true;
}

// UpdateProblemToFile
bool UpdateProblemToFile(const std::string& input_path,
                         const std::string& output_path,
                         const Eigen::MatrixXd& X,
                         const Eigen::VectorXi& p,
                         const Eigen::MatrixXd& U) {

  rapidjson::Document document;
  if (!ReadFileIntoDocument(input_path, &document)) {
    return false;
  }

  #define SAVE_MATRIXD(K) { \
    CHECK(document.HasMember(#K)); \
    auto& v = document[#K]; \
    CHECK(v.IsArray()); \
    CHECK_EQ(v.Size(), K.rows() * K.cols()); \
    for (rapidjson::SizeType i = 0; i < v.Size(); ++i) { \
      CHECK(v[i].IsDouble()); \
      v[i] = K(i % K.rows(), i / K.rows()); \
    } \
  }
  #define SAVE_VECTORI(K) { \
    CHECK(document.HasMember(#K)); \
    auto& v = document[#K]; \
    CHECK(v.IsArray()); \
    CHECK_EQ(v.Size(), K.size()); \
    for (rapidjson::SizeType i = 0; i < v.Size(); ++i) { \
      CHECK(v[i].IsInt()); \
      v[i] = K[i]; \
    } \
  }

  SAVE_MATRIXD(X);
  SAVE_VECTORI(p);
  SAVE_MATRIXD(U);

  #undef SAVE_MATRIXD
  #undef SAVE_VECTORI

  // Use `fopen` instead of streams for `rapidjson::FileWriteStream`.
  FILE* output_handle = fopen(output_path.c_str(), "wb");
  if (output_handle == nullptr) {
    LOG(ERROR) << "Unable to open \"" << output_path << "\"";
  }

  char write_buffer[512];
  rapidjson::FileWriteStream output(output_handle,
                                    write_buffer,
                                    sizeof(write_buffer));
  rapidjson::PrettyWriter<rapidjson::FileWriteStream> writer(output);
  document.Accept(writer);

  fclose(output_handle);

  return true;
}

// main
DEFINE_int32(max_num_iterations, 1000, "Maximum number of iterations.");
DEFINE_double(function_tolerance, 0.0,
  "Minimizer terminates when "
  "(new_cost - old_cost) < function_tolerance * old_cost");
DEFINE_double(gradient_tolerance, 0.0,
  "Minimizer terminates when "
  "max_i |gradient_i| < gradient_tolerance * max_i|initial_gradient_i|");
DEFINE_double(parameter_tolerance, 0.0,
  "Minimizer terminates when "
  "|step|_2 <= parameter_tolerance * ( |x|_2 +  parameter_tolerance)");
DEFINE_double(min_trust_region_radius, 1e-9,
  "Minimizer terminates when the trust region radius becomes smaller than "
  "this value");

DEFINE_int32(num_threads, 1,
  "Number of threads to use for Jacobian evaluation.");
DEFINE_int32(num_linear_solver_threads, 1,
  "Number of threads to use for the linear solver.");

int main(int argc, char** argv) {
  google::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);

  if (argc < 4) {
    LOG(ERROR) << "Usage: " << argv[0] << " input_path lambda output_path";
    return -1;
  }

  // Load the problem data ...
  Eigen::MatrixXd Y;
  std::vector<int> raw_face_array;
  Eigen::MatrixXd X;
  Eigen::VectorXi p;
  Eigen::MatrixXd U;
  if (!LoadProblemFromFile(argv[1], &Y, &raw_face_array, &X, &p, &U)) {
    return -1;
  }

  // ... and check consistency of dimensions.
  typedef Eigen::DenseIndex Index;
  const Index num_data_points = Y.cols();
  CHECK_GT(num_data_points, 0);
  CHECK_EQ(num_data_points, p.size());
  CHECK_EQ(num_data_points, U.cols());

  doosabin::GeneralMesh T(std::move(raw_face_array));
  CHECK_GT(T.number_of_faces(), 0);
  doosabin::Surface<double> surface(T);
  DooSabinSurface doosabin_surface(&surface);
  CHECK_EQ(surface.number_of_vertices(), X.cols());

  // `lambda` is the regularisation weight.
  const double lambda = atof(argv[2]);

  // Setup `problem`.
  ceres::Problem problem;

  // Encode patch indices into the preimage positions.
  for (Index i = 0; i < num_data_points; ++i) {
    EncodePatchIndexInPlace(U.data() + 2 * i, p[i]);
  }

  // Add error residuals.
  std::vector<double*> parameter_blocks;
  parameter_blocks.reserve(1 + surface.number_of_vertices());
  parameter_blocks.push_back(nullptr); // `u`.
  for (Index i = 0; i < X.cols(); ++i) {
    parameter_blocks.push_back(X.data() + 3 * i);
  }

  std::unique_ptr<ceres::CostFunction> surface_position(
    new SurfacePositionCostFunction(&doosabin_surface));

  for (Index i = 0; i < num_data_points; ++i) {
    parameter_blocks[0] = U.data() + 2 * i;

    auto position_error = new ceres::ComposedCostFunction(
      new ceres::AutoDiffCostFunction<PositionErrorFunctor, 3, 3>(
        new PositionErrorFunctor(Y.col(i), 1.0 / sqrt(num_data_points))));
    position_error->AddInputCostFunction(surface_position.get(),
                                         parameter_blocks,
                                         ceres::DO_NOT_TAKE_OWNERSHIP);
    position_error->Finalize();

    problem.AddResidualBlock(position_error, nullptr, parameter_blocks);
  }

  // Add regularisation residuals.
  // Note: `problem` takes ownership of `pairwise_error`.
  auto pairwise_error =
    new ceres::AutoDiffCostFunction<PairwiseErrorFunctor, 3, 3, 3>(
      new PairwiseErrorFunctor(sqrt(lambda)));

  std::set<std::pair<int, int>> full_edges;
  for (std::pair<int, int> e : T.iterate_half_edges()) {
    if (e.first > e.second) {
      std::swap(e.first, e.second);
    }
    if (!full_edges.count(e)) {
      problem.AddResidualBlock(pairwise_error,
                               nullptr,
                               X.data() + 3 * e.first,
                               X.data() + 3 * e.second);
      full_edges.insert(e);
    }
  }

  // Set preimage parameterisations so that they are updated using
  // `doosabin::SurfaceWalker<double>` and NOT Euclidean addition.
  // Note: `problem` takes ownership of `local_parameterisation`.
  typedef doosabin::SurfaceWalker<double> DooSabinWalker;
  DooSabinWalker walker(&surface);
  auto local_parameterisation =
    new PreimageLocalParameterisation<DooSabinWalker, Eigen::MatrixXd>(
      &walker, &X);

  for (Index i = 0; i < num_data_points; ++i) {
    problem.SetParameterization(U.data() + 2 * i, local_parameterisation);
  }

  // Initialise the solver options.
  std::cout << "Solver options:" << std::endl;
  ceres::Solver::Options options;

  options.num_threads = FLAGS_num_threads;
  options.num_linear_solver_threads = FLAGS_num_linear_solver_threads;

  // Disable auto-scaling and set LM to be additive (instead of multiplicative).
  options.min_lm_diagonal = 1.0;
  options.max_lm_diagonal = 1.0;
  options.jacobi_scaling = false;

  options.minimizer_progress_to_stdout = FLAGS_v >= 1;

  // Termination criteria.
  options.max_num_iterations = FLAGS_max_num_iterations;
  std::cout << " max_num_iterations: " << options.max_num_iterations <<
               std::endl;
  options.max_num_consecutive_invalid_steps = FLAGS_max_num_iterations;

  options.function_tolerance = FLAGS_function_tolerance;
  options.gradient_tolerance = FLAGS_gradient_tolerance;
  options.parameter_tolerance = FLAGS_parameter_tolerance;
  options.min_trust_region_radius = FLAGS_min_trust_region_radius;

  // Solver selection.
  options.dynamic_sparsity = true;

  // `update_state_every_iteration` is required by
  // `PreimageLocalParameterisation` instances.
  options.update_state_every_iteration = true;

  // Solve.
  std::cout << "Solving ..." << std::endl;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  std::cout << summary.FullReport() << std::endl;

  // Decode patch indices.
  for (Index i = 0; i < num_data_points; ++i) {
    p[i] = DecodePatchIndexInPlace(U.data() + 2 * i);
  }

  // Save.
  if (!UpdateProblemToFile(argv[1], argv[3], X, p, U)) {
    return -1;
  }
  std::cout << "Output: " << argv[3] << std::endl;

  return 0;
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#define BOOST_CHRONO_HEADER_ONLY

#if !defined(TEST_MPZ) && !defined(TEST_TOMMATH) && !defined(TEST_CPP_INT)
#define TEST_MPZ
#define TEST_TOMMATH
#define TEST_CPP_INT
#endif

#ifdef TEST_MPZ
#include <nil/crypto3/multiprecision/gmp.hpp>
#endif
#ifdef TEST_TOMMATH
#include <nil/crypto3/multiprecision/tommath.hpp>
#endif
#ifdef TEST_CPP_INT
#include <nil/crypto3/multiprecision/cpp_int.hpp>
#endif
#include <nil/crypto3/multiprecision/miller_rabin.hpp>
#include <boost/chrono.hpp>
#include <map>

template <class Clock>
struct stopwatch
{
   typedef typename Clock::duration duration;
   stopwatch()
   {
      m_start = Clock::now();
   }
   duration elapsed()
   {
      return Clock::now() - m_start;
   }
   void reset()
   {
      m_start = Clock::now();
   }

 private:
   typename Clock::time_point m_start;
};

extern unsigned allocation_count;

extern std::map<std::string, double> results;
extern double                        min_time;

template <class IntType>
boost::chrono::duration<double> test_miller_rabin(const char* name)
{
   using namespace boost::random;

   stopwatch<boost::chrono::high_resolution_clock> c;

   independent_bits_engine<mt11213b, 256, IntType> gen;
   //
   // We must use a different generator for the tests and number generation, otherwise
   // we get false positives.
   //
   mt19937  gen2;
   unsigned result_count = 0;

   for (unsigned i = 0; i < 1000; ++i)
   {
      IntType n = gen();
      if (nil::crypto3::multiprecision::miller_rabin_test(n, 25, gen2))
         ++result_count;
   }
   boost::chrono::duration<double> t = c.elapsed();
   double                          d = t.count();
   if (d < min_time)
      min_time = d;
   results[name] = d;
   std::cout << "Time for " << std::setw(30) << std::left << name << " = " << d << std::endl;
   std::cout << "Number of primes found = " << result_count << std::endl;
   return t;
}

boost::chrono::duration<double> test_miller_rabin_gmp();

void test01();
void test02();
void test03();
void test04();
void test05();
void test06();
void test07();
void test08();
void test09();
void test10();
void test11();
void test12();
void test13();
void test14();
void test15();
void test16();
void test17();
void test18();
void test19();