/*
 * pose.hpp
 *
 *  Copyright (c) 2013 Kumar Robotics. All rights reserved.
 *
 *  This file is part of kr_math.
 *
 *  Created on: 28/06/2014
 *      Author: gareth
 */

#ifndef KR_MATH_POSE_H_
#define KR_MATH_POSE_H_

/* GTSAM include first since it needs to be included before Eigen as they
 * have a custom Eigen version where they overload some Eigen classes. Having it
 * after the Eigen include prevents their Eigen header being included due to
 * header guards
 */
#ifdef KR_MATH_GTSAM_CONVERSIONS
#include <gtsam/geometry/Pose3.h>
#endif

#include "base_types.hpp"
#include <Eigen/Geometry>

#ifdef KR_MATH_ROS_CONVERSIONS
#include <geometry_msgs/Pose.h>
#endif

namespace kr {

/**
 * @brief Pose, an orientation and position in R3.
 *
 * @note Some important notes on convention:
 *
 * When we write _A to B_ transformation (or transform), it is in reference to
 * the operation effecuated by matrix H, such that:
 *
 * b = H * a, where a is a vector in the A frame and b is that same vector
 * in the B frame.
 *
 * kr::Pose stores `q`, which rotates vectors from body to world frame,
 * and `p`, the center of the body as expressed in the world frame.
 *
 * Transforming a vector v from world to camera (body) is performed with:
 *
 * vb = R(q') * (vw - p), where R(.) maps from S(4) to SO(3).
 *
 * It is important to note that p is equivalent to the body origin in world
 * frame, often denoted wTb.
 *
 */
template <typename Scalar> struct Pose {
private:
  kr::Quat<Scalar> q_; /**< Orientation (body to world transform, or wRb) */
  kr::Vec3<Scalar> p_; /**< Position (origin to body vector) */

public:
  /**
   * @brief Construct with identity pose.
   */
  Pose() : q_(1, 0, 0, 0) { p_[0] = p_[1] = p_[2] = 0; }

  /**
   * @brief Copy-ctor
   */
  Pose(const Pose<Scalar> &pose) : q_(pose.q_), p_(pose.p_) {}

  /**
   * @brief Construct from quaternion and position vector.
   * @param wQb Body to world rotation.
   * @param pInW Origin to body vector.
   */
  Pose(const Quat<Scalar> &wQb, const Vec3<Scalar> &pInW) : q_(wQb), p_(pInW) {}

  /**
   * @brief Construct from a 4x4 homogenous transformation matrix.
   * @param bHw 4x4 matrix representing the body to world transform.
   *
   * @note wHb is expected to perform the operation: vw = wHb * vb.
   */
  Pose(const kr::Mat4<Scalar>& wHb) :
    q_(wHb.template block<3,3>(0,0)), p_(wHb.template block<3,1>(0,3)) {}

#ifdef KR_MATH_ROS_CONVERSIONS
  /**
   * @brief Construct pose from ROS message.
   * @param geo Instance of geometry_msgs::Pose.
   * @note Loss of accuracy will occur if Scalar is float32.
   */
  Pose(const geometry_msgs::Pose &geo) {
    q_ = kr::Quat<Scalar>(geo.orientation.w, geo.orientation.x,
                         geo.orientation.y, geo.orientation.z);
    p_[0] = geo.position.x;
    p_[1] = geo.position.y;
    p_[2] = geo.position.z;
  }

  /**
   * @brief Explicit cast to ROS geometry_msgs::Pose.
   */
  explicit operator geometry_msgs::Pose() const {
    geometry_msgs::Pose geo;
    geo.orientation.w = q_.w();
    geo.orientation.x = q_.x();
    geo.orientation.y = q_.y();
    geo.orientation.z = q_.z();
    geo.position.x = p_[0];
    geo.position.y = p_[1];
    geo.position.z = p_[2];
    return geo;
  }
#endif

#ifdef KR_MATH_GTSAM_CONVERSIONS
  /**
   * @brief Construct pose from GTSAM Pose3.
   * @param gtpose Instance of GTSAM 6DOF pose. GTSAM poses store a body to
   * world transformation.
   */
  Pose(const gtsam::Pose3& gtpose) :
    q_(gtpose.rotation().toQuaternion().cast<Scalar>()),
    p_(gtpose.translation().vector().cast<Scalar>()) {}

  /**
   * @brief Explicit cast to GTSAM Pose3.
   * @note Converts to the GTSAM convention of body to world.
   */
  explicit operator gtsam::Pose3() const {
    const gtsam::Rot3 rot(q_.template cast<double>());
    return gtsam::Pose3(rot, gtsam::Point3(p_.template cast<double>()));
  }
#endif

  /**
   * @brief Position of this pose in the world frame.
   * @return kr::Vec3
   */
  const kr::Vec3<Scalar>& p() const { return p_; }
  kr::Vec3<Scalar>& p() { return p_; }

  /**
   * @brief Quaternion that performs the body to world rotation on vectors.
   * @return kr::Quat
   */
  const kr::Quat<Scalar>& q() const { return q_; }
  kr::Quat<Scalar>& q() { return q_; }

  /**
   * @brief Construct a pose from an rvec/tvec pair.
   *
   * @param rvec Rotation vector. If R = exp(rvec), then R*v will rotate vector
   * v from world to body frame.
   * @param tvec Translation vector, or t = -R*p.
   *
   * @return The corresponding pose.
   *
   * @note The vectors accepted here are in the same form produced by OpenCV.
   */
  static Pose<Scalar> fromVectors(const Vec3<Scalar>& rvec,
                                  const Vec3<Scalar>& tvec) {
    Vec3<Scalar> rnorm(0.0,0.0,0.0);
    const Scalar rn = rvec.norm();
    if (rn > std::numeric_limits<Scalar>::epsilon()*10) {
      rnorm = rvec / rn;
    }
    const Eigen::AngleAxis<Scalar> aa(rn, rnorm);
    Pose<Scalar> pose;
    pose.q_ = kr::Quat<Scalar>(aa).conjugate();
    pose.p_ = -(pose.q_.matrix() * tvec);
    return pose;
  }

  /**
   * @brief Convert this pose so that it is expressed in the body frame of the
   * argument.
   *
   * @param alt Treat as identity reference frame.
   * @return The receiver, as expressed in frame alt.
   */
  Pose<Scalar> expressedIn(const Pose<Scalar> &alt) const {
    const kr::Vec3<Scalar> pn = alt.bRw() * (p_ - alt.p_);
    return Pose(alt.q_.conjugate() * q_, pn);
  }

  /**
   * @brief Generate the inverse transformation.
   * @return Inverse of this pose: [R^T, -R * p]
   *
   * @note If H is the homo. matrix representation of the receiver, then the
   * returned pose will have representation H' where H * H' = eye(4).
   */
  Pose<Scalar> inverse() const {
    return Pose<Scalar>().expressedIn(*this);
  }

  /**
   * @brief Difference between this pose and the argument in world frame.
   * @param alt Pose to subtract from the receiver.
   *
   * @return The difference between the receiver and the argument, as expressed
   * in the world frame.
   */
  Pose<Scalar> difference(const Pose<Scalar> &alt) const {
    return Pose(alt.q_.conjugate() * q_, p_ - alt.p_);
  }

  /**
   * @brief Translation vector of this pose.
   * @return Vector corresponding to: t = -bRw * p.
   */
  kr::Vec3<Scalar> translation() const { return bRw() * -p_; }

  /**
   * @brief Compose a pose onto the receiver.
   * @param rhs Pose to multiply/add onto this one.
   * @return New pose after composition.
   *
   * @note It is assumed that `rhs` expresses a pose in the body frame of the
   * receiver. This operation therefore rotates `rhs` position vector into the
   * world frame prior to addition. Orientation is composed from the right,
   * corresponding to a multiplication in the body frame.
   */
  Pose<Scalar> composeInBody(const Pose<Scalar>& rhs) const {
    return Pose(q_ * rhs.q_, p_ + q_.matrix() * rhs.p_);
  }

  /**
   * @brief Transform a point into the body frame of this pose.
   * @param v Point to transform, expressed in the world frame.
   * @return Point `v` after conversion to the body frame.
   */
  kr::Vec3<Scalar> transformToBody(const kr::Vec3<Scalar>& v) const {
    return bRw() * (v - p_);
  }

  /**
   * @brief Transform a point from the body frame of this pose.
   * @param v Point to transform, expressed in body frame.
   * @return Point `v` after conversion to the world frame.
   */
  kr::Vec3<Scalar> transformFromBody(const kr::Vec3<Scalar>& v) const {
    return wRb()*v + p_;
  }

  /**
   * @brief SE(3) transformation corresponding to this pose.
   * @return 4x4 matrix that performs the body to world transformation: wHb
   */
  kr::Mat4<Scalar> matrix() const {
    kr::Mat4<Scalar> H;
    H.template block<3,3>(0,0) = q_.matrix();
    H.template block<3,1>(0,3) = p_;
    H(3,0) = H(3,1) = H(3,2) = 0;
    H(3,3) = 1;
    return H;
  }

  /**
   * @brief Shorthand for body to world rotation.
   * @note wP = wRb * bP + wTb
   */
  kr::Mat3<Scalar> wRb() const {
    return q_.matrix();
  }

  /**
   * @brief Shorthand for world to body roration.
   * @note bP = bRw * wP + bTw
   */
  kr::Mat3<Scalar> bRw() const {
    return q_.conjugate().matrix();
  }

  /**
   * @brief bTw Translation from body to world in body frame
   * @note bP = bRw * wP + bTw
   * @note Equivalent to the 'translation' vector.
   */
  kr::Vec3<Scalar> bTw() const {
    return translation(); //  -(bRw() * wTb())
  }

  /**
   * @brief Shorthand for translation vector from world origin to body.
   * @note wP = wRb * bP + wTb
   */
  const kr::Vec3<Scalar>& wTb() const {
    return p_;
  }
};

typedef Pose<float> Posef;
typedef Pose<double> Posed;

} // namespace kr

#endif // KR_MATH_POSE_H_


#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/filtered_graph.hpp>

namespace boost {
	enum vertex_component_t { vertex_component = 1111111 };
	BOOST_INSTALL_PROPERTY(vertex, component);
}

using namespace boost;

template <typename ComponentMap>
struct vertexComponent {

	vertexComponent() {}

	vertexComponent(ComponentMap component, int f_component) : m_component(component), m_f_component(f_component) {}

	template <typename Vertex>
	bool operator()(const Vertex& v) const {
		return (get(m_component, v) == m_f_component);
	}

	ComponentMap m_component;
	int m_f_component;
};

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

	typedef adjacency_list<vecS, vecS, undirectedS, property<vertex_component_t, int> > Graph;

	typedef property_map<Graph, vertex_component_t>::type ComponentMap;
	typedef filtered_graph<Graph, keep_all,	vertexComponent<ComponentMap> > FilteredGraph;

	graph_traits < Graph >::vertex_iterator vi, vi_end;
	graph_traits < Graph >::out_edge_iterator oei, oei_end;
	graph_traits < FilteredGraph >::vertex_iterator fvi, fvi_end;
	graph_traits < FilteredGraph >::out_edge_iterator foei, foei_end;

	Graph * g;
	FilteredGraph * fg;

	enum { A, B, C, D, E, F, G, H, I, N };
	const char* name = "ABCDEFGHI";
	g = new Graph(N);
	add_edge(A, B, *g);
	add_edge(C, D, *g);
	add_edge(D, E, *g);
	add_edge(E, C, *g);
	add_edge(F, A, *g);
	add_edge(F, B, *g);
	add_edge(G, H, *g);
	add_edge(G, I, *g);
	add_edge(H, I, *g);

	std::cout<<std::endl<<"Graph out-edges:"<<std::endl;
	for (tie(vi, vi_end) = vertices(*g); vi != vi_end; ++vi) {
		std::cout<<name[*vi]<<" outedges - ";
		for(tie(oei, oei_end)=out_edges(*vi, *g); oei != oei_end; ++oei) {
			std::cout<<name[source(*oei, *g)]<<"-->"<<name[target(*oei, *g)]<<"  ";
		}
		std::cout<<std::endl;
	}
	std::cout<<std::endl;

	int numComponents = connected_components(*g, get(vertex_component, *g));

	std::cout<<"Graph has "<<numComponents<<" components."<<std::endl<<std::endl;

	for(int i=0; i<numComponents; i++) {
		keep_all efilter;
		vertexComponent<ComponentMap> vfilter(get(vertex_component, *g), i);
		fg = new FilteredGraph(*g, efilter, vfilter);

		std::cout<<"Filtered graph (component "<<i<<") out-edges:"<<std::endl;
		for (tie(fvi, fvi_end) = vertices(*fg); fvi != fvi_end; ++fvi) {
			std::cout<<name[*fvi]<<" outedges - ";
			for(tie(foei, foei_end)=out_edges(*fvi, *fg); foei != foei_end; ++foei) {
				std::cout<<name[source(*foei, *fg)]<<"-->"<<name[target(*foei, *fg)]<<"  ";
			}
			std::cout<<std::endl;
		}
		std::cout<<std::endl;

		delete fg;
		fg = NULL;
	}

	delete g;
	g = NULL;

	return EXIT_SUCCESS;
}


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

// Helpful functions from libsm
#include <sm/eigen/gtest.hpp>
#include <sm/kinematics/quaternion_algebra.hpp>
#include <sm/kinematics/Transformation.hpp>

TEST(TransformationTestSuite, testConstructor)
{
  using namespace sm::kinematics;

  Transformation T_a_b;
  Eigen::Matrix4d T;
  T.setIdentity();

  sm::eigen::assertNear(T, T_a_b.T(), 1e-10, SM_SOURCE_FILE_POS, "Checking for the default constructor creating identity");

}

TEST(TransformationTestSuite, testTV4Multiplication)
{
  using namespace sm::kinematics;

  for(int i = 0; i < 100; i++)
    {
      Transformation T_a_b(quatRandom(), Eigen::Vector3d::Random() * 100);
      Eigen::Vector4d v_b;
      v_b.setRandom();
      v_b *= 100.0;

      Eigen::Vector4d v_a = T_a_b * v_b;
      Eigen::Vector4d v_a_prime = T_a_b.T() * v_b;
      sm::eigen::assertNear(v_a, v_a_prime, 1e-10, SM_SOURCE_FILE_POS, "Checking for composition equal to matrix multiplication");
    }

}

TEST(TransformationTestSuite, testTVhMultiplication)
{
  using namespace sm::kinematics;

  for(int i = 0; i < 100; i++)
    {
      Transformation T_a_b(quatRandom(), Eigen::Vector3d::Random() * 100);
      Eigen::Vector3d v_b;
      v_b.setRandom();
      v_b *= 100.0;
      HomogeneousPoint V_b(v_b);

      Eigen::Vector3d v_a = T_a_b * v_b;
      HomogeneousPoint V_a = T_a_b * V_b;
      sm::eigen::assertNear(V_a.toEuclidean(), v_a, 1e-10, SM_SOURCE_FILE_POS, "Checking for composition equal to matrix multiplication");
    }

}

TEST(TransformationTestSuite, testTVMultiplication)
{
  using namespace sm::kinematics;

  for(int i = 0; i < 100; i++)
    {
      Transformation T_a_b(quatRandom(), Eigen::Vector3d::Random() * 100);
      Eigen::Vector3d v_b;
      v_b.setRandom();
      v_b *= 100.0;

      Eigen::Vector3d v_a = T_a_b * v_b;
      Eigen::Vector3d v_a_prime = T_a_b.C() * v_b + T_a_b.t();
      sm::eigen::assertNear(v_a, v_a_prime, 1e-10, SM_SOURCE_FILE_POS, "Checking for composition equal to matrix multiplication");
    }

}

TEST(TransformationTestSuite, testTTMultiplication)
{
  using namespace sm::kinematics;

  for(int i = 0; i < 100; i++)
    {
      Transformation T_a_b(quatRandom(), Eigen::Vector3d::Random() * 100);
      Transformation T_b_c(quatRandom(), Eigen::Vector3d::Random() * 100);

      Transformation T_a_c = T_a_b * T_b_c;
      Eigen::Matrix4d T_a_c_prime = T_a_b.T() * T_b_c.T();
      sm::eigen::assertNear(T_a_c.T(), T_a_c_prime, 1e-10, SM_SOURCE_FILE_POS, "Checking for composition equal to matrix multiplication");
    }

}

TEST(TransformationTestSuite, testInvert)
{
  using namespace sm::kinematics;

  for(int i = 0; i < 100; i++)
    {
      Transformation T_a_b(quatRandom(), Eigen::Vector3d::Random());

      Transformation T_b_a = T_a_b.inverse();
      Transformation T_a_b_prime = T_b_a.inverse();

      sm::eigen::assertNear(T_a_b_prime.T(), T_a_b.T(), 1e-10, SM_SOURCE_FILE_POS, "Checking for identity");

    }
}

TEST(TransformationTestSuite, testInvertProducesIdentity)
{
  using namespace sm::kinematics;

  for(int i = 0; i < 100; i++)
    {
      Transformation T_a_b(quatRandom(), Eigen::Vector3d::Random());
      Transformation T_b_a = T_a_b.inverse();
      Transformation Eye1 = T_a_b * T_b_a;
      Transformation Eye2 = T_b_a * T_a_b;

      sm::eigen::assertNear(Eye1.T(), Eigen::Matrix4d::Identity(), 1e-10, SM_SOURCE_FILE_POS, "Checking for identity");
      sm::eigen::assertNear(Eye2.T(), Eigen::Matrix4d::Identity(), 1e-10, SM_SOURCE_FILE_POS, "Checking for identity");
    }

}


#define BOOST_TEST_DYN_LINK

// clang-format off
#include <boost/test/unit_test.hpp>
// clang-format on

#include <sstream>

#include <nuschl/number.hpp>

BOOST_AUTO_TEST_SUITE(Number)
BOOST_AUTO_TEST_CASE(comparison) {
    nuschl::number a(2);
    nuschl::number b(2);
    nuschl::number c(3);
    nuschl::number d(0);

    BOOST_CHECK(a == b);
    BOOST_CHECK(!(a == c));
    BOOST_CHECK(a != c);
    BOOST_CHECK(!(a == c));
    BOOST_CHECK(a < c);
    BOOST_CHECK(d < a);
    BOOST_CHECK(c > a);
    BOOST_CHECK(b > d);
}

BOOST_AUTO_TEST_CASE(cast) {
    nuschl::number a(2);
    BOOST_CHECK_EQUAL(a.get_value(), 2);
}

BOOST_AUTO_TEST_CASE(negate) {
    nuschl::number a(2);
    BOOST_CHECK_EQUAL((-a).get_value(), -2);
}

BOOST_AUTO_TEST_CASE(addition) {
    nuschl::number a(2);
    nuschl::number b(2);
    nuschl::number c(4);
    nuschl::number d(0);

    BOOST_CHECK_EQUAL(a + b, c);
    BOOST_CHECK_EQUAL(a + d, b);
}

BOOST_AUTO_TEST_CASE(subtraction) {
    nuschl::number a(2);
    nuschl::number b(2);
    nuschl::number c(4);
    nuschl::number d(0);

    BOOST_CHECK_EQUAL(a - a, d);
    BOOST_CHECK_EQUAL(a - b, d);
    BOOST_CHECK_EQUAL(c - b, b);
}

BOOST_AUTO_TEST_CASE(multiplication) {
    nuschl::number a(2);
    nuschl::number b(2);
    nuschl::number c(4);
    nuschl::number d(0);

    BOOST_CHECK_EQUAL(a * b, c);
    BOOST_CHECK_EQUAL(a * d, d);
}

BOOST_AUTO_TEST_CASE(division) {
    nuschl::number a(2);
    nuschl::number b(2);
    nuschl::number c(4);
    nuschl::number d(0);

    BOOST_CHECK_EQUAL(c / b, a);
    BOOST_CHECK_EQUAL(d / c, d);
}

BOOST_AUTO_TEST_CASE(ostream) {
    std::stringstream ss;
    nuschl::number c(4);
    ss << c;
    BOOST_CHECK_EQUAL(ss.str(), "4");
}

BOOST_AUTO_TEST_SUITE_END()


#include "Common/Common.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "Simulation/SimulationModel.h"
#include "Simulation/TimeStepController.h"
#include <iostream>
#include "Demos/Visualization/Visualization.h"
#include "Simulation/DistanceFieldCollisionDetection.h"
#include "Utils/OBJLoader.h"
#include "Utils/Logger.h"
#include "Utils/Timing.h"
#include "Utils/FileSystem.h"
#include "Demos/Common/DemoBase.h"
#include "Demos/Common/TweakBarParameters.h"
#include "Simulation/Simulation.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;
using namespace Utilities;

void timeStep ();
void buildModel ();
void createMesh();
void render ();
void reset();
void TW_CALL setBendingMethod(const void *value, void *clientData);
void TW_CALL getBendingMethod(void *value, void *clientData);
void TW_CALL setSimulationMethod(const void *value, void *clientData);
void TW_CALL getSimulationMethod(void *value, void *clientData);
void TW_CALL setBendingStiffness(const void* value, void* clientData);
void TW_CALL getBendingStiffness(void* value, void* clientData);
void TW_CALL setDistanceStiffness(const void* value, void* clientData);
void TW_CALL getDistanceStiffness(void* value, void* clientData);
void TW_CALL setXXStiffness(const void* value, void* clientData);
void TW_CALL getXXStiffness(void* value, void* clientData);
void TW_CALL setYYStiffness(const void* value, void* clientData);
void TW_CALL getYYStiffness(void* value, void* clientData);
void TW_CALL setXYStiffness(const void* value, void* clientData);
void TW_CALL getXYStiffness(void* value, void* clientData);
void TW_CALL setXYPoissonRatio(const void* value, void* clientData);
void TW_CALL getXYPoissonRatio(void* value, void* clientData);
void TW_CALL setYXPoissonRatio(const void* value, void* clientData);
void TW_CALL getYXPoissonRatio(void* value, void* clientData);
void TW_CALL setNormalizeStretch(const void* value, void* clientData);
void TW_CALL getNormalizeStretch(void* value, void* clientData);
void TW_CALL setNormalizeShear(const void* value, void* clientData);
void TW_CALL getNormalizeShear(void* value, void* clientData);

const int nRows = 50;
const int nCols = 50;
const Real width = 10.0;
const Real height = 10.0;
short simulationMethod = 2;
short bendingMethod = 2;
Real distanceStiffness = 1.0;
Real xxStiffness = 1.0;
Real yyStiffness = 1.0;
Real xyStiffness = 1.0;
Real xyPoissonRatio = 0.3;
Real yxPoissonRatio = 0.3;
bool normalizeStretch = false;
bool normalizeShear = false;
Real bendingStiffness = 0.01;
bool doPause = true;
DemoBase *base;
DistanceFieldCollisionDetection cd;

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

	base = new DemoBase();
	base->init(argc, argv, "Cloth demo");

	SimulationModel *model = new SimulationModel();
	model->init();
	Simulation::getCurrent()->setModel(model);

	buildModel();

	base->createParameterGUI();

	// OpenGL
	MiniGL::setClientIdleFunc (timeStep);
	MiniGL::addKeyFunc('r', reset);
	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3r (0.0, 10.0, 25.0), Vector3r (0.0, 0.0, 0.0));

	TwType enumType2 = TwDefineEnum("SimulationMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "SimulationMethod", enumType2, setSimulationMethod, getSimulationMethod, &simulationMethod,
		" label='Simulation method' enum='0 {None}, 1 {Distance constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics}, 4 {XPBD distance constraints}' group=Simulation");
	TwType enumType3 = TwDefineEnum("BendingMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "BendingMethod", enumType3, setBendingMethod, getBendingMethod, &bendingMethod,
		" label='Bending method' enum='0 {None}, 1 {Dihedral angle}, 2 {Isometric bending}, 3 {XPBD isometric bending}' group=Bending");
	TwAddVarCB(MiniGL::getTweakBar(), "BendingStiffness", TW_TYPE_REAL, setBendingStiffness, getBendingStiffness, model, " label='Bending stiffness'  min=0.0 step=0.1 precision=4 group='Bending' ");
	TwAddVarCB(MiniGL::getTweakBar(), "DistanceStiffness", TW_TYPE_REAL, setDistanceStiffness, getDistanceStiffness, model, " label='Distance constraint stiffness'  min=0.0 step=0.1 precision=4 group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "xxStiffness", TW_TYPE_REAL, setXXStiffness, getXXStiffness, model, " label='xx stiffness'  min=0.0 step=0.1 precision=4 group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "yyStiffness", TW_TYPE_REAL, setYYStiffness, getYYStiffness, model, " label='yy stiffness'  min=0.0 step=0.1 precision=4 group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "xyStiffness", TW_TYPE_REAL, setXYStiffness, getXYStiffness, model, " label='xy stiffness'  min=0.0 step=0.1 precision=4 group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "xyPoissonRatio", TW_TYPE_REAL, setXYPoissonRatio, getXYPoissonRatio, model, " label='xy Poisson ratio'  min=0.0 step=0.1 precision=4 group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "yxPoissonRatio", TW_TYPE_REAL, setYXPoissonRatio, getYXPoissonRatio, model, " label='yx Poisson ratio'  min=0.0 step=0.1 precision=4 group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "normalizeStretch", TW_TYPE_BOOL32, setNormalizeStretch, getNormalizeStretch, model, " label='Normalize stretch' group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "normalizeShear", TW_TYPE_BOOL32, setNormalizeShear, getNormalizeShear, model, " label='Normalize shear' group='Cloth' ");

	MiniGL::mainLoop();

	base->cleanup();

	Utilities::Timing::printAverageTimes();
	Utilities::Timing::printTimeSums();

	delete Simulation::getCurrent();
	delete base;
	delete model;

	return 0;
}

void reset()
{
	Utilities::Timing::printAverageTimes();
	Utilities::Timing::reset();

	Simulation::getCurrent()->reset();
	base->getSelectedParticles().clear();

	Simulation::getCurrent()->getModel()->cleanup();
	cd.cleanup();

	buildModel();
}

void timeStep ()
{
	const Real pauseAt = base->getValue<Real>(DemoBase::PAUSE_AT);
	if ((pauseAt > 0.0) && (pauseAt < TimeManager::getCurrent()->getTime()))
		base->setValue(DemoBase::PAUSE, true);

	if (base->getValue<bool>(DemoBase::PAUSE))
		return;

	// Simulation code
	SimulationModel *model = Simulation::getCurrent()->getModel();
	const unsigned int numSteps = base->getValue<unsigned int>(DemoBase::NUM_STEPS_PER_RENDER);
	for (unsigned int i = 0; i < numSteps; i++)
	{
		START_TIMING("SimStep");
		Simulation::getCurrent()->getTimeStep()->step(*model);
		STOP_TIMING_AVG;
	}

	for (unsigned int i = 0; i < model->getTriangleModels().size(); i++)
		model->getTriangleModels()[i]->updateMeshNormals(model->getParticles());
}

void loadObj(const std::string &filename, VertexData &vd, IndexedFaceMesh &mesh, const Vector3r &scale)
{
	std::vector<OBJLoader::Vec3f> x;
	std::vector<OBJLoader::Vec3f> normals;
	std::vector<OBJLoader::Vec2f> texCoords;
	std::vector<MeshFaceIndices> faces;
	OBJLoader::Vec3f s = { (float)scale[0], (float)scale[1], (float)scale[2] };
	OBJLoader::loadObj(filename, &x, &faces, &normals, &texCoords, s);

	mesh.release();
	const unsigned int nPoints = (unsigned int)x.size();
	const unsigned int nFaces = (unsigned int)faces.size();
	const unsigned int nTexCoords = (unsigned int)texCoords.size();
	mesh.initMesh(nPoints, nFaces * 2, nFaces);
	vd.reserve(nPoints);
	for (unsigned int i = 0; i < nPoints; i++)
	{
		vd.addVertex(Vector3r(x[i][0], x[i][1], x[i][2]));
	}
	for (unsigned int i = 0; i < nTexCoords; i++)
	{
		mesh.addUV(texCoords[i][0], texCoords[i][1]);
	}
	for (unsigned int i = 0; i < nFaces; i++)
	{
		// Reduce the indices by one
		int posIndices[3];
		int texIndices[3];
		for (int j = 0; j < 3; j++)
		{
			posIndices[j] = faces[i].posIndices[j] - 1;
			if (nTexCoords > 0)
			{
				texIndices[j] = faces[i].texIndices[j] - 1;
				mesh.addUVIndex(texIndices[j]);
			}
		}

		mesh.addFace(&posIndices[0]);
	}
	mesh.buildNeighbors();

	mesh.updateNormals(vd, 0);
	mesh.updateVertexNormals(vd);

	LOG_INFO << "Number of triangles: " << nFaces;
	LOG_INFO << "Number of vertices: " << nPoints;
}

void buildModel ()
{
	TimeManager::getCurrent ()->setTimeStepSize (static_cast<Real>(0.005));

	createMesh();

	// create static rigid body
	string fileName = FileSystem::normalizePath(base->getExePath() + "/resources/models/cube.obj");
	IndexedFaceMesh mesh;
	VertexData vd;
	loadObj(fileName, vd, mesh, Vector3r::Ones());
	mesh.setFlatShading(true);

	string fileNameTorus = FileSystem::normalizePath(base->getExePath() + "/resources/models/torus.obj");
	IndexedFaceMesh meshTorus;
	VertexData vdTorus;
	loadObj(fileNameTorus, vdTorus, meshTorus, Vector3r::Ones());

	SimulationModel *model = Simulation::getCurrent()->getModel();
	SimulationModel::RigidBodyVector &rb = model->getRigidBodies();
	rb.resize(2);

	// floor
	rb[0] = new RigidBody();
	rb[0]->initBody(1.0,
		Vector3r(0.0, -2.5, 0.0),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh,
		Vector3r(100.0, 1.0, 100.0));
	rb[0]->setMass(0.0);

	// torus
	rb[1] = new RigidBody();
	rb[1]->initBody(1.0,
		Vector3r(0.0, 1.5, 0.0),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vdTorus, meshTorus,
		Vector3r(2.0, 2.0, 2.0));
	rb[1]->setMass(0.0);
	rb[1]->setFrictionCoeff(static_cast<Real>(0.1));

	Simulation::getCurrent()->getTimeStep()->setCollisionDetection(*model, &cd);
	cd.setTolerance(static_cast<Real>(0.05));

	const std::vector<Vector3r> &vertices1 = rb[0]->getGeometry().getVertexDataLocal().getVertices();
	const unsigned int nVert1 = static_cast<unsigned int>(vertices1.size());
	cd.addCollisionBox(0, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, vertices1.data(), nVert1, Vector3r(100.0, 1.0, 100.0));

	const std::vector<Vector3r> &vertices2 = rb[1]->getGeometry().getVertexDataLocal().getVertices();
	const unsigned int nVert2 = static_cast<unsigned int>(vertices2.size());
	cd.addCollisionTorus(1, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, vertices2.data(), nVert2, Vector2r(2.0, 1.0));

	SimulationModel::TriangleModelVector &tm = model->getTriangleModels();
	ParticleData &pd = model->getParticles();
	for (unsigned int i = 0; i < tm.size(); i++)
	{
		const unsigned int nVert = tm[i]->getParticleMesh().numVertices();
		unsigned int offset = tm[i]->getIndexOffset();
		tm[i]->setFrictionCoeff(static_cast<Real>(0.1));
		cd.addCollisionObjectWithoutGeometry(i, CollisionDetection::CollisionObject::TriangleModelCollisionObjectType, &pd.getPosition(offset), nVert, true);
	}
}

void render ()
{
	base->render();
}

/** Create a particle model mesh
*/
void createMesh()
{
	SimulationModel* model = Simulation::getCurrent()->getModel();
	model->addRegularTriangleModel(nCols, nRows,
		Vector3r(-5, 4, -5), AngleAxisr(M_PI * 0.5, Vector3r(1, 0, 0)).matrix(), Vector2r(width, height));

	// init constraints
	for (unsigned int cm = 0; cm < model->getTriangleModels().size(); cm++)
	{
		distanceStiffness = 1.0;
		if (simulationMethod == 4)
			distanceStiffness = 100000;
		model->addClothConstraints(model->getTriangleModels()[cm], simulationMethod, distanceStiffness, xxStiffness,
			yyStiffness, xyStiffness, xyPoissonRatio, yxPoissonRatio, normalizeStretch, normalizeShear);

		bendingStiffness = 0.01;
		if (bendingMethod == 3)
			bendingStiffness = 100.0;
		model->addBendingConstraints(model->getTriangleModels()[cm], bendingMethod, bendingStiffness);
	}

	LOG_INFO << "Number of triangles: " << model->getTriangleModels()[0]->getParticleMesh().numFaces();
	LOG_INFO << "Number of vertices: " << nRows*nCols;

}

void TW_CALL setBendingMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;
	reset();
}

void TW_CALL getBendingMethod(void *value, void *clientData)
{
	*(short *)(value) = *((short*)clientData);
}

void TW_CALL setSimulationMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;
	reset();
}

void TW_CALL getSimulationMethod(void *value, void *clientData)
{
	*(short *)(value) = *((short*)clientData);
}

void TW_CALL setBendingStiffness(const void* value, void* clientData)
{
	bendingStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<DihedralConstraint, Real, &DihedralConstraint::m_stiffness>(bendingStiffness);
	((SimulationModel*)clientData)->setConstraintValue<IsometricBendingConstraint, Real, &IsometricBendingConstraint::m_stiffness>(bendingStiffness);
	((SimulationModel*)clientData)->setConstraintValue<IsometricBendingConstraint_XPBD, Real, &IsometricBendingConstraint_XPBD::m_stiffness>(bendingStiffness);
}

void TW_CALL getBendingStiffness(void* value, void* clientData)
{
	*(Real*)(value) = bendingStiffness;
}

void TW_CALL setDistanceStiffness(const void* value, void* clientData)
{
	distanceStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<DistanceConstraint, Real, &DistanceConstraint::m_stiffness>(distanceStiffness);
	((SimulationModel*)clientData)->setConstraintValue<DistanceConstraint_XPBD, Real, &DistanceConstraint_XPBD::m_stiffness>(distanceStiffness);
}

void TW_CALL getDistanceStiffness(void* value, void* clientData)
{
	*(Real*)(value) = distanceStiffness;
}

void TW_CALL setXXStiffness(const void* value, void* clientData)
{
	xxStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<FEMTriangleConstraint, Real, &FEMTriangleConstraint::m_xxStiffness>(xxStiffness);
	((SimulationModel*)clientData)->setConstraintValue<StrainTriangleConstraint, Real, &StrainTriangleConstraint::m_xxStiffness>(xxStiffness);
}

void TW_CALL getXXStiffness(void* value, void* clientData)
{
	*(Real*)(value) = xxStiffness;
}

void TW_CALL getYYStiffness(void* value, void* clientData)
{
	*(Real*)(value) = yyStiffness;
}

void TW_CALL setYYStiffness(const void* value, void* clientData)
{
	yyStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<FEMTriangleConstraint, Real, &FEMTriangleConstraint::m_yyStiffness>(yyStiffness);
	((SimulationModel*)clientData)->setConstraintValue<StrainTriangleConstraint, Real, &StrainTriangleConstraint::m_yyStiffness>(yyStiffness);
}

void TW_CALL getXYStiffness(void* value, void* clientData)
{
	*(Real*)(value) = xyStiffness;
}

void TW_CALL setXYStiffness(const void* value, void* clientData)
{
	xyStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<FEMTriangleConstraint, Real, &FEMTriangleConstraint::m_xyStiffness>(xyStiffness);
	((SimulationModel*)clientData)->setConstraintValue<StrainTriangleConstraint, Real, &StrainTriangleConstraint::m_xyStiffness>(xyStiffness);
}

void TW_CALL getXYPoissonRatio(void* value, void* clientData)
{
	*(Real*)(value) = xyPoissonRatio;
}

void TW_CALL setXYPoissonRatio(const void* value, void* clientData)
{
	xyPoissonRatio = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<FEMTriangleConstraint, Real, &FEMTriangleConstraint::m_xyPoissonRatio>(xyPoissonRatio);
}

void TW_CALL getYXPoissonRatio(void* value, void* clientData)
{
	*(Real*)(value) = yxPoissonRatio;
}

void TW_CALL setYXPoissonRatio(const void* value, void* clientData)
{
	yxPoissonRatio = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<FEMTriangleConstraint, Real, &FEMTriangleConstraint::m_yxPoissonRatio>(yxPoissonRatio);
}

void TW_CALL getNormalizeStretch(void* value, void* clientData)
{
	*(bool*)(value) = normalizeStretch;
}

void TW_CALL setNormalizeStretch(const void* value, void* clientData)
{
	normalizeStretch = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<StrainTriangleConstraint, bool, &StrainTriangleConstraint::m_normalizeStretch>(normalizeStretch);
}

void TW_CALL getNormalizeShear(void* value, void* clientData)
{
	*(bool*)(value) = normalizeShear;
}

void TW_CALL setNormalizeShear(const void* value, void* clientData)
{
	normalizeShear= *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<StrainTriangleConstraint, bool, &StrainTriangleConstraint::m_normalizeShear>(normalizeShear);
}


/////////////////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2021 Andreas Milton Maniotis.
//
// Email: andreas.maniotis@gmail.com
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
/////////////////////////////////////////////////////////////////////////////////////////////

#include "aml/find.hpp"
#include "aml/conslist.hpp"

#include <type_traits>

#include <iostream>
#include <boost/core/demangle.hpp>

namespace test::find
{
    using l0 = aml::conslist<>;
    using l1 = aml::conslist<void, int, char*>;
    using l2 = aml::conslist<void, short int, char*, long int>;

    template<typename X>
    struct pred
    {
        static constexpr bool eval() { return std::is_integral<X>::value; };
    };

    using f0 = l0::apply<aml::find<pred>::in>;
    using f1 = l1::apply<aml::find<pred>::in>;

    using f2 = l2::apply<aml::find<pred>::in>;

    using r0 = aml::conslist<>;
    using r1 = aml::conslist<int>;
    using r2 = aml::conslist<short int>;

    void test()
    {
        static_assert(std::is_same<f0, r0>::value, "");
        static_assert(std::is_same<f1, r1>::value, "");
        static_assert(std::is_same<f2, r2>::value, "");
    }
}

#include <iostream>
#include <string>

int main()
{
    void (*test_set[])() = { test::find::test };

    for ( auto test : test_set )
        test();

    std::cout << __FILE__ << ": " << sizeof(test_set)/sizeof(test_set[0])  << " tests passed." << std::endl;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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)

//[for_each_point_const
//` Sample using for_each_point, using a function to list coordinates

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

template <typename Point>
void list_coordinates(Point const& p)
{
    using boost::geometry::get;
    std::cout << "x = " << get<0>(p) << " y = " << get<1>(p) << std::endl;
}

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point;
    boost::geometry::model::polygon<point> poly;
    boost::geometry::read_wkt("POLYGON((0 0,0 4,4 0,0 0))", poly);
    boost::geometry::for_each_point(poly, list_coordinates<point>);
    return 0;
}

//]

//[for_each_point_const_output
/*`
Output:
[pre
x = 0 y = 0
x = 0 y = 4
x = 4 y = 0
x = 0 y = 0
]
*/
//]


#ifndef EXAMPLES_MPI_DOMAIN_PARTITION_HPP
#define EXAMPLES_MPI_DOMAIN_PARTITION_HPP

#include <vector>
#include <utility>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/adapted/boost_array.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_ARRAY_CS(cs::cartesian)

template <int NDIM>
class domain_partition {
    public:
        typedef boost::array<ptrdiff_t, NDIM>      point;
        typedef boost::geometry::model::box<point> box;
        typedef std::pair<box, int>                process;

        domain_partition(point lo, point hi, int num_processes) {
            split(box(lo, hi), num_processes);

            for(int i = 0; i < num_processes; ++i)
                rtree.insert( std::make_pair(subdomains[i], i) );
        }

        std::pair<int, ptrdiff_t> index(point p) const {
            namespace bgi = boost::geometry::index;

            for(const process &v : rtree | bgi::adaptors::queried(bgi::intersects(p)) )
            {
                return std::make_pair(v.second, local_index(v.first, p));
            }

            // Unreachable:
            return std::make_pair(0, 0l);
        }

        size_t size(size_t process) const {
            if (process >= subdomains.size()) return 0;

            point lo = subdomains[process].min_corner();
            point hi = subdomains[process].max_corner();

            size_t v = 1;

            for(int i = 0; i < NDIM; ++i)
                v *= hi[i] - lo[i] + 1;

            return v;
        }

        box domain(size_t process) const {
            if (process < subdomains.size())
                return subdomains[process];
            else {
                point lo;
                point hi;
                for(int i = 0; i < NDIM; ++i) {
                    lo[i] = 0;
                    hi[i] = -1;
                }
                return box(lo, hi);
            }
        }
    private:
        std::vector<box> subdomains;

        boost::geometry::index::rtree<
            process,
            boost::geometry::index::quadratic<16>
            > rtree;

        static ptrdiff_t local_index(box domain, point p) {
            point lo = domain.min_corner();
            point hi = domain.max_corner();

            ptrdiff_t stride = 1, idx = 0;
            for(int i = 0; i < NDIM; ++i) {
                idx += (p[i] - lo[i]) * stride;
                stride *= hi[i] - lo[i] + 1;
            }

            return idx;
        }

        void split(box domain, int np) {
            if (np == 1) {
                subdomains.push_back(domain);
                return;
            }

            point lo = domain.min_corner();
            point hi = domain.max_corner();

            // Get longest dimension of the domain
            int wd = 0;
            for(int i = 1; i < NDIM; ++i)
                if (hi[i] - lo[i] > hi[wd] - lo[wd]) wd = i;

            ptrdiff_t mid = lo[wd] + (hi[wd] - lo[wd]) * (np / 2) / np;

            box sd1 = domain;
            box sd2 = domain;

            sd1.max_corner()[wd] = mid;
            sd2.min_corner()[wd] = mid + 1;

            split(sd1, np / 2);
            split(sd2, np - np / 2);
        }
};

#endif


#ifndef _MATRIX_OPERATIONS_HPP_
#define _MATRIX_OPERATIONS_HPP_

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

namespace CIS {
/// @brief Manual transform implementation, recommend using the following instead
///
/// @verbatim
///    Eigen::Affine3d transform;
///
///    transform.setIdentity();
///
///    // Define a translation of 2.5 meters on the x axis.
///    transform.translation() << 2.5, 0.0, 0.0;
///
///    double theta = 0;//boost::math::constants::pi<double>();
///    // The same rotation matrix as before; tetha radians arround Z axis
///    transform.rotate (Eigen::AngleAxisd (theta, Eigen::Vector3d::UnitZ()));
///
///    Eigen::Vector3d v3d(1,1,1);
///
///    Eigen::Vector3d v3d2 = (transform*v3d).transpose();
/// @endverbatim
template<typename Scalar>
Eigen::Matrix<Scalar, 3, 1> transform(const Eigen::Transform<Scalar, 3, Eigen::Affine>& hTrans, const Eigen::Matrix<Scalar, 3, 1>& point){
   return Eigen::Matrix<Scalar, 3, 1>
    (
        hTrans (0, 0) * point(0) + hTrans (0, 1) * point(1) + hTrans (0, 2) * point(2) + hTrans (0, 3),
        hTrans (1, 0) * point(0) + hTrans (1, 1) * point(1) + hTrans (1, 2) * point(2) + hTrans (1, 3),
        hTrans (2, 0) * point(0) + hTrans (2, 1) * point(1) + hTrans (2, 2) * point(2) + hTrans (2, 3)
     );

}
}

/// @todo provide instructions to replace this with eigen built in functionality
/// Create transformation matrix from Rotation R and Translation vector P
Eigen::Matrix4d homogeneousmatrix(Eigen::Matrix3d R, Eigen::Vector3d p)
{
    Eigen::Matrix4d F = Eigen::Matrix4d::Identity();
    F.block<3,3>(0,0) = R;
    F.block<3,1>(0,3) = p;
    return F;
}

/// @todo provide instructions to replace this with eigen built in functionality
/// Computes the inverse of the transformation matrix
Eigen::MatrixXd homogeneousInverse(const Eigen::MatrixXd& F)
{
    Eigen::MatrixXd Finv = Eigen::Matrix4d::Identity();
    Eigen::Matrix3d R = F.block<3,3>(0,0);
    Eigen::Matrix3d Rtrans = R.transpose();
    Finv.block<3,3>(0,0) = Rtrans;
    Finv.block<3,1>(0,3) = -Rtrans*F.block<3,1>(0,3);
    return Finv;
}

/// Swap indexing order of vector of vectors, so if it is row major the returned vv will be column major.
/// @todo this is pretty inefficient, but the lengthts can vary. Maybe store the data differently
template<typename T>
const std::vector<std::vector<T> > swapIndexing(const std::vector<std::vector<T> >& uv){
    std::vector<std::vector<T> > vu;
    for(auto uvi : uv){
        int i = 0;
        for(auto vi :uvi){
            if(i == vu.size()) vu.push_back(std::vector<T>());
            vu[i].push_back(vi);
            ++i;
        }
    }
    return vu;
}

/// @brief combine a vector<vector<T> > into a single vector<T>
template<typename T>
const std::vector<T> concat(const std::vector<std::vector<T> >& uv){
    std::vector<T> u;
    for(auto uvi : uv){
        u.insert(u.end(), uvi.begin(), uvi.end());
    }
    return u;
}

/// @brief split a large Eigen::MatrixXd by rows into a vector of smaller MatrixXd
std::vector<Eigen::MatrixXd> splitRows(const Eigen::MatrixXd& mat,std::size_t numRowsPerMat){

    std::vector<Eigen::MatrixXd> vec;
    std::size_t cols = mat.cols();

    for(std::size_t currentRow = 0; currentRow < mat.rows(); currentRow+=numRowsPerMat){
        Eigen::MatrixXd partialMat(mat.block(currentRow,0,numRowsPerMat,cols));
        vec.push_back(partialMat);
    }

    return vec;
}

Eigen::MatrixXd concatToMatrix(const std::vector<Eigen::Vector3d>& points){
    Eigen::MatrixXd mat;
    mat.resize(points.size(),3);
    int i = 0;
    for(Eigen::Vector3d point : points){
        mat.block<1,3>(i,0) = point.transpose();
        ++i;
    }

    return mat;
}

// splits a stack of vectors that is nx3, into a std::vector of n 3x1 vectors
std::vector<Eigen::Vector3d> splitVectors(const Eigen::MatrixXd& mat){
	std::vector<Eigen::Vector3d> vec;

	for(int i = 0; i < mat.rows(); ++i){
		vec.push_back(Eigen::Vector3d(mat.block<1,3>(i,0).transpose()));
	}

	return vec;
}

#endif // _MATRIX_OPERATIONS_HPP_


#include <boost/math/quadrature/tanh_sinh.hpp>


#include "vtkVelodyneHDLPositionReader.h"
#include "vtkVelodyneTransformInterpolator.h"
#include "vtkPointData.h"
#include "vtkTransform.h"

#include <Eigen/Dense>

Eigen::Matrix3d RollPitchYawToMatrix(double roll, double pitch, double yaw)
{
  return Eigen::Matrix3d(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
                  * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
                  * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()));
}

Eigen::Matrix3d RollPitchYawToMatrixDegree(double roll, double pitch, double yaw)
{
  return RollPitchYawToMatrix((vtkMath::Pi() / 180.0) * roll,
                              (vtkMath::Pi() / 180.0) * pitch,
                              (vtkMath::Pi() / 180.0) * yaw);
}

// defines an arbitrary order on the columns names
// this order is expected when passing arrays of double to test functions
// this order does not have to be the same as the one that is used inside the
// vtkPolydata, because we do not want to test that this order remain consistent
// (because the order is not documented and currently based on the behavior of // an iterator on a std::map)
const char* arrayNames[17] = { "lat",
                               "lon",
                               "gpstime",
                               "time",
                               "accel1x",
                               "accel1y",
                               "accel2x",
                               "accel2y",
                               "accel3x",
                               "accel3y",
                               "gyro1",
                               "gyro2",
                               "gyro3",
                               "heading",
                               "temp1",
                               "temp2",
                               "temp3" };

bool Equal(double a, double b, double epsilon = 1e-6)
{
  return std::abs(a - b) <= epsilon;
}

bool test_point_count(vtkSmartPointer<vtkVelodyneHDLPositionReader> reader,
                /* expected: */
                int count)
{
  std::cout << "Testing number of points" << std::endl;
  bool isvalid = (reader->GetOutput()->GetNumberOfPoints() == count);
  if (!isvalid)
  {
    std::cerr << "unexpected number of points" << std::endl;
  }
  return isvalid;
}

bool test_interpolator_time_range(
    vtkSmartPointer<vtkVelodyneHDLPositionReader> reader,
    /* expected: */
    double tmin,
    double tmax)
{
  std::cout << "Testing the interpolator time range" << std::endl;
  bool isvalid = (Equal(reader->GetInterpolator()->GetMinimumT(),tmin)
                  && Equal(reader->GetInterpolator()->GetMaximumT(), tmax));
  if (!isvalid)
  {
    std::cerr << "unexpected interpolator time range" << std::endl;
  }
  return isvalid;
}

// roll pitch and yaw angles in degrees
// Producing the matrix Rz(yaw) * Ry(pitch) * Rx(roll)
bool test_interpolator_transform(
    vtkSmartPointer<vtkVelodyneHDLPositionReader> reader,
    double t,
    /* expected: */
    double posRollPitchYaw[6])
{
  vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
  reader->GetInterpolator()->InterpolateTransform(t, transform);

  vtkSmartPointer<vtkMatrix4x4> tmp = vtkSmartPointer<vtkMatrix4x4>::New();
  transform->GetMatrix(tmp);

  Eigen::Matrix3d M = RollPitchYawToMatrixDegree(posRollPitchYaw[3 + 0],
      posRollPitchYaw[3 + 1],
      posRollPitchYaw[3 + 2]);

  bool rotationValid = true;
  for (int i = 0; i < 3; i++)
  {
      for (int j = 0; j < 3; j++)
      {
        rotationValid &= Equal(tmp->GetElement(i,j), M(i,j));
      }
  }
  if (!rotationValid)
  {
    std::cerr << "Unexpected rotation in transform" << std::endl;
  }

  bool translationValid = true;
  for (int i = 0; i < 3; i++)
  {
        if (!Equal(tmp->GetElement(i,3), posRollPitchYaw[i]))
        {
          std::cerr << "For coord " << i
                    << std::setprecision(17)
                    << " got: " << tmp->GetElement(i,3)
                    << " expecting: " << posRollPitchYaw[i] << std::endl;
          translationValid = false;
        }
  }
  if (!translationValid)
  {
    std::cerr << "Unexpected translation in transform" << std::endl;
  }

  return (rotationValid && translationValid);
}

bool test_interpolator_transform_count(
                vtkSmartPointer<vtkVelodyneHDLPositionReader> reader,
                /* expected: */
                int count)
{
  std::cout << "Testing number of transforms in interpolator" << std::endl;
  bool isvalid = (reader->GetInterpolator()->GetNumberOfTransforms() == count);
  if (!isvalid)
  {
    std::cerr << "unexpected number of transforms" << std::endl;
  }
  return isvalid;
}

bool test_point_coords(vtkSmartPointer<vtkVelodyneHDLPositionReader> reader,
                int id,
                /* expected: */
                double coords[3])
{
  std::cout << "Testing coordinates of point: " << id << std::endl;
  bool isvalid = true;
  // check the point value in all 3 coordinates
  double coordsRead[3];
  reader->GetOutput()->GetPoint(id, coordsRead);
  for (int i = 0; i < 3; i++)
  {
    isvalid &= Equal(coordsRead[i], coords[i]);
  }
  if (!isvalid)
  {
    std::cerr << "unexpected value for coordinates of point " << id
              << std::endl;
  }
  return isvalid;
}

bool test_point_arrays(vtkSmartPointer<vtkVelodyneHDLPositionReader> reader,
                int id,
                /* expected: */
                double arrayValues[17])
{
  std::cout << "Testing arrays of point: " << id << std::endl;
  bool isvalid = true;
  // check the point value in all 17 arrays
  for (int i = 0; i < 17; i++)
  {
    if (!Equal(reader->GetOutput()->GetPointData()->GetArray(arrayNames[i])
               ->GetTuple1(id),
               arrayValues[i]))
    {
      std::cerr << "unexpected value for point " << id
                << " in array "
                << reader->GetOutput()->GetPointData()->GetArrayName(i)
                << std::endl;
      isvalid = false;
    }
  }

  if (!isvalid)
  {
    std::cout << "values read for point " << id << " are: " << std::endl;
    std::cout << "(in the order: ";
    for (int i = 0; i < 17; i++)
    {
      std::cout << arrayNames[i] << " ";
    }
    std::cout << ")" << std::endl;
    for (int i = 0; i < 17; i++)
    {
      std::cout << std::setprecision(17)
                << reader->GetOutput()->GetPointData()->GetArray(i)->GetTuple1(id);
      if (i < 16)
      {
        std::cout << ", ";
      }
    }
    std::cout << std::endl;
  }

  return isvalid;
}

int main(int argc, char* argv[])
{
  if (argc != 2)
  {
    std::cerr << "Wrong number of arguments. Usage: "
              << argv[0]
              << "<path to "
              << "\"HDL32-V2_R into Butterfield into Digital Drive.pcap\""
              << "(from data.kitware.com,"
              << "sha1sum: 1edb06c8c4312cb259dd0417a226c235945ff5b6) >"
              << std::endl;
    return 1;
  }

  std::string pathToPcap = std::string(argv[1]);

  vtkSmartPointer<vtkVelodyneHDLPositionReader> reader =
      vtkSmartPointer<vtkVelodyneHDLPositionReader>::New();
  reader->SetFileName(pathToPcap);
  reader->Update();
  // std::cout << *reader->GetOutput() << std::endl; // should you want to inspect the polydata produced

  double point0_arrays[17] =  { 37.139071666666, -121.657165, 78376, 2777073776, 0.9768, 0.108669, 0.970695, -0.272283, -0.068376, -0.272283, 3.12512, -7.42216, -32.52078, 40.6, 36.9146, 38.6582, 42.0001 };
  double point0_coords[3] =  { 0.0, 0.0, 0.0 };
  double point8947_arrays[17] = { 37.1387, -121.65492833333, 78421, 2822076651, 0.990231, 0.023199, 0.98901, -0.002442, 0.001221, -0.003663, -8.7894, -4.10172, -28.3214, 257.7, 36.9146, 38.8035, 42.0001 };
  double point8947_coords[3] =  { 199.2462615966796875,	-38.420330047607421875,	0.0 };
  int numberOfPoints = 8948;
  int numberOfTransforms = 46;
  double minTime = 78376.0;
  double maxTime = 78421.0;
  double transformMinTime[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 40.6 };
  double transformMaxTimeMinus1[6] = { 203.09254455566406, -38.180858612060547, 0.0, 0.0, 0.0, -92.3 };

  vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
  reader->GetInterpolator()->InterpolateTransform(maxTime - 1.0, transform);

  bool isvalid = true;
  isvalid &= test_point_count(reader, numberOfPoints);
  isvalid &= test_point_coords(reader, 0, point0_coords);
  isvalid &= test_point_arrays(reader, 0, point0_arrays);
  isvalid &= test_point_coords(reader, 8947, point8947_coords);
  isvalid &= test_point_arrays(reader, 8947, point8947_arrays);

  isvalid &= test_interpolator_transform_count(reader, numberOfTransforms);
  isvalid &= test_interpolator_time_range(reader, minTime, maxTime);
  isvalid &= test_interpolator_transform(reader, minTime, transformMinTime);
  isvalid &= test_interpolator_transform(reader, maxTime - 1.0, transformMaxTimeMinus1);

  return  isvalid ? 0 : 1;
}


#include "include/ellipse.h"
#include "include/component_labelling.h"
#include "include/point_helpers.h"

#include <cmath>
#include <Eigen/Cholesky>
#include <Eigen/Dense>
using namespace Eigen;

#include <limits>

typedef Matrix<double, 5, 5> Matrix5d;
typedef Matrix<double, 5, 1> Vector5d;

int Ellipse_detector::fit(const Component_labeller& cl, const Gradient& gradient,
    const Pointlist& raw_points, int tl_x, int tl_y, int dilate) {

    int width  = gradient.width();
    int height = gradient.height();

    const cv::Mat& grad_x = gradient.grad_x();
    const cv::Mat& grad_y = gradient.grad_y();
    const cv::Mat& grad_m = gradient.grad_magnitude();

    set<iPoint> boundary;

    const int border = 1;
    bool edge_touched = false;
    double mx = 0;
    double my = 0;
    for (size_t i=0; i < raw_points.size(); i++) {
        mx += raw_points[i].x;
        my += raw_points[i].y;
        boundary.insert(iPoint(lrint(raw_points[i].x), lrint(raw_points[i].y)) );

        if (raw_points[i].x <= border || raw_points[i].x >= width - 1 - border ||
            raw_points[i].y <= border || raw_points[i].y >= height - 1 - border) {

            edge_touched = true;
        }
    }

    mx /= raw_points.size();
    my /= raw_points.size();

    if (edge_touched) return 0; // skip objects that touch the edge of the image (ellipses not really allowed there)

    if (std::isnan(mx) || std::isnan(my)) {
        return 0; // 0 -> not a circle
    }

    _dilate(boundary, width, height, dilate);
    _dilate_outer_only(boundary, width, height);

    Matrix<double, 5, 5> wK;
    Matrix<double, 5, 1> K;
    K.setZero();
    Vector3d l;
    l.setZero();
    Matrix<double, 5, 1> rhs;
    rhs.setZero();

    double sum_c4 = 0;

    wK.setZero();
    int count = (int)boundary.size();
    Matrix<double, Eigen::Dynamic, 3> L(count, 3);
    L.setZero();

    double mean_dist = 0;
    int counter = 0;
    for (set<iPoint>::const_iterator it=boundary.begin(); it != boundary.end(); it++) {
        const int& x_pos = it->first;
        const int& y_pos = it->second;

        if (grad_m.at<float>(y_pos, x_pos) > 1e-7) {
            Vector3d v;
            v << x_pos,y_pos,1;
            double dist = sqrt( SQR(x_pos-mx) + SQR(y_pos-my) );
            mean_dist += dist;
            counter++;
        }
    }
    mean_dist /= counter;
    double iso_scale = sqrt(2.0) / mean_dist;

    size_t idx = 0;
    for (set<iPoint>::const_iterator it=boundary.begin(); it != boundary.end(); it++) {
        const int& x_pos = it->first;
        const int& y_pos = it->second;

        if (grad_m.at<float>(y_pos, x_pos) > 1e-7) {

            l[0] = grad_x.at<float>(y_pos, x_pos);
            l[1] = grad_y.at<float>(y_pos, x_pos);
            l[2] = -(grad_x.at<float>(y_pos, x_pos) * (x_pos-mx) * iso_scale + grad_y.at<float>(y_pos, x_pos) * (y_pos-my) * iso_scale);

            L.row(idx++) = l;

        }
    }

    for (size_t r=0; r < (size_t)L.rows(); r++) {

        Vector3d l = L.row(r);

        sum_c4 += SQR(SQR(l[2]));

        K[0] = l[0]*l[0];
        K[1] = l[0]*l[1];
        K[2] = l[1]*l[1];
        K[3] = l[0]*l[2];
        K[4] = l[1]*l[2];

        double weight = (l[0]*l[0] + l[1]*l[1]);

        wK += weight * K * K.transpose();

        rhs -= weight * K*(l[2]*l[2]);
    }

    if (rhs.rows() != 5 || rhs.cols() != 1) {
        printf("rhs undefined\n");
        exit(1);
    }

    bool has_nans = false;
    for (size_t ri=0; ri < rhs.rows(); ri++) {
        if (std::isnan(rhs(ri,0))) {
            has_nans = true;
            return 0;
        }
    }

    Vector5d sol;
    sol = wK.fullPivHouseholderQr().solve(rhs);

    Matrix3d Cstar;
    Cstar(0, 0) = sol[0];
    Cstar(0, 1) = sol[1]*0.5;
    Cstar(0, 2) = sol[3]*0.5;
    Cstar(1, 0) = sol[1]*0.5;
    Cstar(1, 1) = sol[2];
    Cstar(1, 2) = sol[4]*0.5;
    Cstar(2, 0) = sol[3]*0.5;
    Cstar(2, 1) = sol[4]*0.5;
    Cstar(2, 2) = 1;      // F* == 1

    Matrix3d C = Cstar.inverse();
    C *= 1.0/C(2,2);
    _C = C;

    Matrix<double, 1, 5> s;
    s.row(0) = sol;
    double sAAs = (s * wK * (s.transpose()))(0,0);
    double R = (sAAs - 2*(sol.dot(rhs)) + sum_c4) / double(count - 5);
    Matrix<double, 5, 5> cov = wK.inverse();
    cov = cov * R;
    Matrix2d cov2;
    cov2(0, 0) = cov(3, 3);
    cov2(0, 1) = cov(3, 4);
    cov2(1, 0) = cov(4, 3);
    cov2(1, 1) = cov(4, 4);
    JacobiSVD<Matrix2d> svd(cov2, ComputeFullU | ComputeFullV);
    Matrix2d Vs = svd.matrixV();
    Vector2d ws = svd.singularValues();

    Matrix2d V;
    V.row(0) = Vs.row(1);
    V.row(1) = Vs.row(0);

    Vector2d w;
    w[0] = ws[1];
    w[1] = ws[0];

    Vector2d centre_uncertainty;
    centre_uncertainty[0] = sqrt(w[0]) * 0.25;
    centre_uncertainty[1] = sqrt(w[1]) * 0.25;

    // shift the ellipse back to the original pixel coordinate system
    Matrix3d S;
    S.setIdentity();
    S(0, 0) = iso_scale;
    S(1, 1) = iso_scale;
    C = _C = S.transpose()*C*S;
    S.setIdentity();
    S(0, 2) = -(tl_x + mx);
    S(1, 2) = -(tl_y + my);
    C = _C = S.transpose()*C*S;

    int result = _matrix_to_ellipse(C);

    if (minor_axis > major_axis) {
        _C = -_C;
        _matrix_to_ellipse(C);
    }

    bool gradient_ok = gradient_check(cl, gradient, raw_points);

    int is_circle = 0;

    if (result == 0 &&
        major_axis >= min_major_axis &&
        minor_axis >= min_minor_axis &&
        gradient_ok) {

        is_circle = 1;
    }

    if (std::isnan(centroid_x) || std::isnan(centroid_y) || std::isnan(major_axis) || std::isnan(minor_axis)) {
        is_circle = 0;
    }

    printf("centre (%.2lf, %.2lf), major = %lf, minor = %lf, angle = %lf, is_circle = %d\n",
        centroid_x, centroid_y, major_axis, minor_axis, angle/M_PI*180.0, is_circle);

    if (is_circle) {
        scanset.clear();
        for (size_t i=0; i < raw_points.size(); i++) {
            int ix = lrint(raw_points[i].x);
            int iy = lrint(raw_points[i].y);

            map<int, scanline>::iterator it = scanset.find(iy);
            if (it == scanset.end()) {
                scanline sl(ix,ix);
                scanset.insert(make_pair(iy, sl));
            }
            if (ix < scanset[iy].start) {
                scanset[iy].start = ix;
            }
            if (ix > scanset[iy].end) {
                scanset[iy].end = ix;
            }
        }
        int clabel = cl(lrint(raw_points[0].x), lrint(raw_points[0].y));
        printf("label used for scanset: %d\n", clabel);
        int total = 0;
        int foreground = 0;
        for (map<int, scanline>::iterator it=scanset.begin(); it != scanset.end(); it++) {
            int y=it->first;
            for (int x=it->second.start; x <= it->second.end; x++) {
                if (cl(x,y) == clabel) {
                    foreground++;
                }
                total++;
            }
        }
        fg_fraction = double(foreground)/double(total);
    }

    return is_circle;
}

void Ellipse_detector::_dilate(set<iPoint>& s, int width, int height, int iters) {
    const int border = 1;

    if (iters > 0) {

        set<iPoint> gen_s;

        for (int k=0; k < iters; k++) {
            for (set<iPoint>::const_iterator it=s.begin(); it != s.end(); it++) {

                gen_s.insert(*it);

                int left = max(border, it->first-1);
                int right = min(width-1-border, it->first+1);
                int top = max(border, it->second-1);
                int bottom = min(height-1-border, it->second+1);

                gen_s.insert(iPoint(right, it->second));
                gen_s.insert(iPoint(right, bottom));
                gen_s.insert(iPoint(it->first, bottom));
                gen_s.insert(iPoint(left, bottom));
                gen_s.insert(iPoint(left, it->second));
                gen_s.insert(iPoint(left, top));
                gen_s.insert(iPoint(it->first, top));
                gen_s.insert(iPoint(right, top));

            }
            s = gen_s; // copy it back
        }
    }
}

void Ellipse_detector::_dilate_outer_only(set<iPoint>& s, int width, int height) {
    const int border = 1;

    double cx = 0;
    double cy = 0;
    for (set<iPoint>::const_iterator it=s.begin(); it != s.end(); it++) {
        cx += it->first;
        cy += it->second;
    }
    cx /= s.size();
    cy /= s.size();

    set<iPoint> gen_s;

    for (set<iPoint>::const_iterator it=s.begin(); it != s.end(); it++) {

        gen_s.insert(*it);

        Point2d dir(it->first - cx, it->second - cy); // current radial direction

        int left = max(border, it->first-1);
        int right = min(width-1-border, it->first+1);
        int top = max(border, it->second-1);
        int bottom = min(height-1-border, it->second+1);

        if ((right - it->first)*dir.x >= 0) gen_s.insert(iPoint(right, it->second));
        if ((right - it->first)*dir.x >= 0 && (bottom - it->second)*dir.y >= 0) gen_s.insert(iPoint(right, bottom));
        if ((bottom - it->second)*dir.y >= 0) gen_s.insert(iPoint(it->first, bottom));
        if ((left - it->first)*dir.x >= 0 && (bottom - it->second)*dir.y >= 0) gen_s.insert(iPoint(left, bottom));
        if ((left - it->first)*dir.x >= 0) gen_s.insert(iPoint(left, it->second));
        if ((left - it->first)*dir.x >= 0 && (top - it->second)*dir.y >= 0) gen_s.insert(iPoint(left, top));
        if ((top - it->second)*dir.y >= 0) gen_s.insert(iPoint(it->first, top));
        if ((right - it->first)*dir.x >= 0 && (top - it->second)*dir.y >= 0) gen_s.insert(iPoint(right, top));

    }
    s = gen_s; // copy it back
}

int Ellipse_detector::_matrix_to_ellipse(Matrix3d& C) {

    double a = C(0,0);
    double b = C(0,1)*2;
    double c = C(1,1);
    double d = C(0,2)*2;
    double e = C(1,2)*2;
    double f = C(2,2);

    double thetarad = 0.5*atan2(b, a - c);
    double cost = cos(thetarad);
    double sint = sin(thetarad);
    double sin_squared = sint*sint;
    double cos_squared = cost*cost;
    double cos_sin = sint*cost;

    double Ao = f;
    double Au =   d * cost + e * sint;
    double Av = - d * sint + e * cost;
    double Auu = a * cos_squared + c * sin_squared + b * cos_sin;
    double Avv = a * sin_squared + c * cos_squared - b * cos_sin;

    if(Auu==0 || Avv==0) {
        // invalid ellipse
        return -1;
    }

    double tuCentre = - Au/(2*Auu);
    double tvCentre = - Av/(2*Avv);
    double wCentre = Ao - Auu*tuCentre*tuCentre - Avv*tvCentre*tvCentre;

    double uCentre = tuCentre * cost - tvCentre * sint;
    double vCentre = tuCentre * sint + tvCentre * cost;

    double Ru = -wCentre/Auu;
    double Rv = -wCentre/Avv;

    Ru = sqrt(fabs(Ru))*(Ru < 0 ? -1 : 1);
    Rv = sqrt(fabs(Rv))*(Rv < 0 ? -1 : 1);

    centroid_x = uCentre;
    centroid_y = vCentre;
    major_axis = Ru;
    minor_axis = Rv;
    angle = thetarad;

    return 0;
}

double Ellipse_detector::calculate_curve_length(const Pointlist& points) {

    int n = points.size();

    // calculate the curve length of the  boundary
    double curve_len = 0;
    double prev_x = points[0].x - centroid_x;
    double prev_y = points[0].y - centroid_y;
    for (int i=1; i < n; i++) {
        double x = points[i].x - centroid_x;
        double y = points[i].y - centroid_y;
        curve_len += sqrt(SQR(x - prev_x) + SQR(y - prev_y));
        prev_x = x;
        prev_y = y;
    }
    curve_len += sqrt(SQR(points[0].x - centroid_x - prev_x) +
        SQR(points[0].y - centroid_y - prev_y));

    return curve_len;
}

bool Ellipse_detector::gradient_check(const Component_labeller& cl, const Gradient& gradient, const Pointlist& raw_points) {

    // gradient just outside ellipse must be perpendicular to ellipse tangent
    double cosa = cos(-angle);
    double sina = sin(-angle);
    int neighbours[8][2] = {
        {-1, -1}, {0, -1}, {1, -1},
        {-1, 0}, {1, 0},
        {-1,  1}, {0,  1}, {1, 1}
    };

    // TODO: we need improved handling for objects falling on the edge of the scene?

    int not_fg_count = 0;
    vector<double> phi_diff;
    for (size_t i=0; i < raw_points.size(); i++) {
        // now generate a point just outside the ellipse ....
        int ox = lrint(raw_points[i].x);
        int oy = lrint(raw_points[i].y);
        int px = ox;
        int py = oy;
        double maxdist = sqrt((px - centroid_x)*(px - centroid_x) + (py - centroid_y)*(py - centroid_y));
        for (int n=0; n < 8; n++) {
            int lx = px + neighbours[n][0];
            int ly = py + neighbours[n][1];

            if (lx >= 5 && lx < gradient.width() - 5 &&
                ly >= 5 && ly < gradient.height() - 5) {

                double dist = sqrt((lx - centroid_x)*(lx - centroid_x) + (ly - centroid_y)*(ly - centroid_y));
                if (dist > maxdist) {
                    px = lx;
                    py = ly;
                    maxdist = dist;
                }
            }
        }

        // points just outside ellipse should have labels of 0 or -1 (not foreground)
        not_fg_count += cl(px, py) <= 0 ? 1 : 0;

        Point2d d(raw_points[i].x - centroid_x, raw_points[i].y - centroid_y);
        double rx = cosa*d.x - sina*d.y;
        double ry = sina*d.x + cosa*d.y;
        double theta = atan2(ry, rx); // ellipse curve parameter theta
        Point2d tangent = normalize(Point2d(-major_axis*sin(theta), minor_axis*cos(theta)));
        // rotate the tangent vector back to image coordinates
        rx = cosa*tangent.x + sina*tangent.y;
        ry = (-sina)*tangent.x + cosa*tangent.y;
        tangent.x = rx;
        tangent.y = ry;

        Point2d grad = normalize(Point2d(gradient.grad_x().at<float>(py, px), gradient.grad_y().at<float>(py, px)));

        double dot = tangent.x*grad.x + tangent.y*grad.y;
        double phi = acos(dot);

        phi_diff.push_back(phi/M_PI*180 - 90);
    }
    if ((raw_points.size() - not_fg_count) > 1) {
        return false; // we can leave early here ...
    }

    sort(phi_diff.begin(), phi_diff.end());
    const double phi_percentile = 0.9;

    double phi_delta = phi_diff[phi_percentile*phi_diff.size()];

    return (phi_delta < max_ellipse_gradient_error) && phi_delta >= 0;
}


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/5/problem5.hpp"

BOOST_AUTO_TEST_SUITE( Problem5 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem5::solve(10);
        BOOST_CHECK_EQUAL(res, 2520);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem5::solve();
        BOOST_CHECK_EQUAL(res, 232792560);
    }

BOOST_AUTO_TEST_SUITE_END()


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file Transformation.hpp
/// \brief Header file for a transformation matrix class.
/// \details Light weight transformation class, intended to be fast, and not to provide
///          unnecessary functionality.
///
/// \author Sean Anderson
//////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////
///
/// A note on EIGEN_MAKE_ALIGNED_OPERATOR_NEW (Sean Anderson, as of May 23, 2013)
/// (also see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
///
/// Fortunately, Eigen::Matrix3d and Eigen::Vector3d are NOT 16-byte vectorizable,
/// therefore this class should not require alignment, and can be used normally in STL.
///
/// To inform others of the issue, classes that include *fixed-size vectorizable Eigen types*,
/// see http://eigen.tuxfamily.org/dox-devel/group__TopicFixedSizeVectorizable.html,
/// must include the above macro! Furthermore, special considerations must be taken if
/// you want to use them in STL containers, such as std::vector or std::map.
/// The macro overloads the dynamic "new" operator so that it generates
/// 16-byte-aligned pointers, this MUST be in the public section of the header!
///
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef LGM_TRANSFORMATION_HPP
#define LGM_TRANSFORMATION_HPP

#include <Eigen/Dense>

namespace lgmath {
namespace se3 {

class Transformation
{
 public:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Default constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation();

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy constructor.
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation(const Transformation&) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Move constructor.
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation(Transformation&& T) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy constructor (from Eigen)
  //////////////////////////////////////////////////////////////////////////////////////////////
  explicit Transformation(const Eigen::Matrix4d& T);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor. The transformation will be T_ba = [C_ba, -C_ba*r_ba_ina; 0 0 0 1]
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation(const Eigen::Matrix3d& C_ba, const Eigen::Vector3d& r_ba_ina);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor. The transformation will be T_ba = vec2tran(xi_ab)
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation(const Eigen::Matrix<double,6,1>& xi_ab, unsigned int numTerms = 0);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor. The transformation will be T_ba = vec2tran(xi_ab), xi_ab must be 6x1
  //////////////////////////////////////////////////////////////////////////////////////////////
  // explicit because you want operator*(Eigen::Vector4d) and operator*(this) --- ambiguous
  explicit Transformation(const Eigen::VectorXd& xi_ab);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Destructor. Default implementation.
  //////////////////////////////////////////////////////////////////////////////////////////////
  ~Transformation() = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy assignment operator. Default implementation causes functional failure.
  /// \todo (yuchen) Figure out why default does not work.
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation& operator=(const Transformation&) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Move assignment operator. Manually implemented as Eigen doesn't support moving.
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation& operator=(Transformation&& T) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Gets basic matrix representation of the transformation
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Matrix4d matrix() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Gets the underlying rotation matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  const Eigen::Matrix3d& C_ba() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Gets the "forward" translation r_ba_ina = -C_ba.transpose()*r_ab_inb
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Vector3d r_ba_ina() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Gets the underlying r_ab_inb vector.
  //////////////////////////////////////////////////////////////////////////////////////////////
  const Eigen::Vector3d& r_ab_inb() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get the corresponding Lie algebra using the logarithmic map
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Matrix<double,6,1> vec() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get the inverse matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation inverse() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get the 6x6 adjoint transformation matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Matrix<double,6,6> adjoint() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Reproject the transformation matrix back onto SE(3). Setting force to false
  ///        triggers a conditional reproject that only happens if the determinant is of the
  ///        rotation matrix is poor; this is more efficient than always performing it.
  //////////////////////////////////////////////////////////////////////////////////////////////
  void reproject(bool force = true);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief In-place right-hand side multiply T_rhs
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Transformation& operator*=(const Transformation& T_rhs);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Right-hand side multiply T_rhs
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Transformation operator*(const Transformation& T_rhs) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief In-place right-hand side multiply this matrix by the inverse of T_rhs
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Transformation& operator/=(const Transformation& T_rhs);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Right-hand side multiply this matrix by the inverse of T_rhs
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Transformation operator/(const Transformation& T_rhs) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Right-hand side multiply this matrix by the homogeneous vector p_a
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Vector4d operator*(const Eigen::Ref<const Eigen::Vector4d>& p_a) const;

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// Rotation matrix from a to b
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Matrix3d C_ba_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// Translation vector from b to a, expressed in frame b
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Vector3d r_ab_inb_;
};

} // se3
} // lgmath

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief print transformation
//////////////////////////////////////////////////////////////////////////////////////////////
std::ostream& operator<<(std::ostream& out, const lgmath::se3::Transformation& T);

#endif // LGM_TRANSFORMATION_HPP