//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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 BOOST_SIMD_ARITHMETIC_FUNCTIONS_MEANOF_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_MEANOF_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
      @brief  meanof generic tag

      Represents the meanof function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct meanof_ : ext::elementwise_<meanof_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<meanof_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_meanof_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::meanof_, Site> dispatching_meanof_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::meanof_, Site>();
   }
   template<class... Args>
   struct impl_meanof_;
  }
  /*!
    Computes the mean of its parameter avoiding overflow.

    @par semantic:
    For any given value @c x and @c y of type @c T:

    @code
    T r = meanof(x, y);
    @endcode

    is similar to:

    @code
    T r = (x+y)/2;
    @endcode

    @par Note:
    Take care that for integers the value returned can differ by one unit
    from \c ceil((a+b)/2.0) or \c floor((a+b)/2.0), but is always one of
    the two values.

    @param  a0
    @param  a1

    @return an value of the same type as the input.

  **/
   BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::meanof_, meanof, 2)
} }

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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_TOOLBOX_LINALG_DETAILS_BLAS_MV_HPP_INCLUDED
#define NT2_TOOLBOX_LINALG_DETAILS_BLAS_MV_HPP_INCLUDED

#include <complex>
#include <boost/preprocessor/cat.hpp>
#include <nt2/toolbox/linalg/details/blas/blas2.hpp>
#include <nt2/toolbox/linalg/details/utility/f77_wrapper.hpp>

namespace nt2 { namespace details
{
#define NT2_MV(T, PX)                                                   \
inline void gemv( const char *ta                                        \
                , const long int *m, const long int *n                  \
                , const T *al                                           \
                , const T *a, const long int *lda                       \
                , const T *x, const long int *incx                      \
                , const T *be                                           \
                , T *y      , const long int *incy                      \
                )                                                       \
{                                                                       \
  NT2_F77NAME(BOOST_PP_CAT(PX,gemv))(ta,m,n,al,a,lda,x,incx,be,y,incy); \
}                                                                       \
/**/

  // INTERNAL ONLY
  // gemv binding for all types
  // _GEMV (TRANS, M, N, ALPHA, A, LDA, X, INCX, BETA,  Y, INCY )
  NT2_MV(double, d)
  NT2_MV(float,  s)
  NT2_MV(std::complex<double>, z)
  NT2_MV(std::complex<float>, c)

#undef NT2_MV

#define NT2_MV(T, PX)                                                         \
inline void gbmv( const char *ta                                              \
                , const long int *m, const long int *n                        \
                , const long int *kl, const long int *ku                      \
                , const T *al                                                 \
                , const T *a, const long int *lda                             \
                , const T *x, const long int *incx                            \
                , const T *be                                                 \
                , T *y      , const long int *incy                            \
                )                                                             \
{                                                                             \
  NT2_F77NAME(BOOST_PP_CAT(PX,gbmv))(ta,m,n,kl,ku,al,a,lda,x,incx,be,y,incy); \
}                                                                             \
/**/

  // INTERNAL ONLY
  // gbmv binding for all types
  // _GBMV(TRANS, M, N, KL, KU, ALPHA, A, LDA, X, INCX, BETA, Y, INCY )
  NT2_MV(double, d)
  NT2_MV(float,  s)
  NT2_MV(std::complex<double>, z)
  NT2_MV(std::complex<float>, c)

#undef NT2_MV

#define NT2_MV(T, P, Q)                                                             \
inline void BOOST_PP_CAT(Q,mv)( const char *uplo, const long int *n                 \
                              , const T *al                                         \
                              , const T *a, const long int *lda                     \
                              , const T *x, const long int *incx                    \
                              , const T *be                                         \
                              , T *y      , const long int *incy                    \
                              )                                                     \
{                                                                                \
  NT2_F77NAME(BOOST_PP_CAT(P,BOOST_PP_CAT(Q,mv)))(ta,n,al,a,lda,x,incx,be,y,incy);  \
}                                                                                \
/**/

  //  INTERNAL ONLY
  // *mv binging for hermittian/symmetric cases
  // _HEMV ( UPLO, N, ALPHA, A, LDA, X, INCX, BETA,  Y, INCY )
  // _SYMV ( UPLO, N, ALPHA, A, LDA, X, INCX, BETA,  Y, INCY )
  NT2_MV(std::complex<double>, z, he)
  NT2_MV(std::complex<float>, c, he)
  NT2_MV(std::complex<double>, z, sy)
  NT2_MV(std::complex<float>, c, sy)
  NT2_MV(double, d, sy)
  NT2_MV(float,  s, sy)

#undef NT2_MV

#define NT2_MV(T, P, Q)                                                               \
inline void BOOST_PP_CAT(Q,mv)( const char *uplo                                      \
                              , const long int *n, const long int *k                  \
                              , const T *al                                           \
                              , const T *a, const long int *lda                       \
                              , const T *x, const long int *incx                      \
                              , const T *be                                           \
                              , T *y      , const long int *incy                      \
                              )                                                       \
{                                                                                \
  NT2_F77NAME(BOOST_PP_CAT(P,BOOST_PP_CAT(Q,mv)))(ta,n,k,al,a,lda,x,incx,be,y,incy);  \
}                                                                                \
/**/

  // _HBMV ( UPLO, N, K, ALPHA, A, LDA, X, INCX, BETA,  Y, INCY )
  // _SBMV ( UPLO, N, K, ALPHA, A, LDA, X, INCX, BETA,  Y, INCY )
  NT2_MV(std::complex<double>, z, hb)
  NT2_MV(std::complex<float>, c, hb)
  NT2_MV(std::complex<double>, z, sb)
  NT2_MV(std::complex<float>, c, sb)
  NT2_MV(double, d, sb)
  NT2_MV(float,  s, sb)

#undef NT2_MV

#define NT2_MV(T, P, Q)                                                         \
inline void BOOST_PP_CAT(Q,mv)( const char *uplo, const long int *n             \
                              , const T *al , const T *ap                       \
                              , const T *x  , const long int *incx              \
                              , const T *be                                     \
                              , T *y        , const long int *incy              \
                              )                                                 \
{                                                                               \
  NT2_F77NAME(BOOST_PP_CAT(P,BOOST_PP_CAT(Q,mv)))(ta,n,al,ap,x,incx,be,y,incy); \
}                                                                               \
/**/

  // _HPMV ( UPLO N, ALPHA, AP, X, INCX, BETA,  Y, INCY )
  // _SPMV ( UPLO N, ALPHA, AP, X, INCX, BETA,  Y, INCY )
  NT2_MV(std::complex<double> , z, hp )
  NT2_MV(std::complex<float>  , c, hp )
  NT2_MV(std::complex<double> , z, sp )
  NT2_MV(std::complex<float>  , c, sp )
  NT2_MV(double, d, sp)
  NT2_MV(float,  s, sp)

#undef NT2_MV

#define NT2_MV(T, P)                                                    \
inline void tpmv( const char *uplo, const char *trans, const char *diag \
                , const long int *n                                     \
                , const T *ap                                           \
                , T *x, const long int *incx                            \
                )                                                       \
{                                                                       \
  NT2_F77NAME(BOOST_PP_CAT(P,tpmv))(uplo,trans,diag,n,ap,x,incx);       \
}                                                                       \
/**/

  // _TPMV ( UPLO, TRANS, DIAG, N, A, LDA, X, INCX )
  NT2_MV(double, d)
  NT2_MV(float,  s)
  NT2_MV(std::complex<double> , z )
  NT2_MV(std::complex<float>  , c )

#undef NT2_MV

#define NT2_MV(T, P)                                                    \
inline void trmv( const char *uplo, const char *trans, const char *diag \
                , const long int *n                                     \
                , const T *a, const long int *lda                       \
                , T *x      , const long int *incx                      \
                )                                                       \
{                                                                       \
  NT2_F77NAME(BOOST_PP_CAT(P,trmv))(uplo,trans,diag,n,a,lda,x,incx);    \
}                                                                       \
/**/

  // _TRMV ( UPLO, TRANS, DIAG, N, AP, X, INCX )
  NT2_MV(double, d)
  NT2_MV(float,  s)
  NT2_MV(std::complex<double> , z )
  NT2_MV(std::complex<float>  , c )
#undef NT2_MV

#define NT2_MV(T, P)                                                    \
inline void tbmv( const char *uplo, const char *trans, const char *diag \
                , const long int *n,  const long int *k                 \
                , const T *a, const long int *lda                       \
                , T *x      , const long int *incx                      \
                )                                                       \
{                                                                       \
  NT2_F77NAME(BOOST_PP_CAT(P,tbmv))(uplo,trans,diag,n,k, a,lda,x,incx); \
}                                                                       \
/**/

  // _TBMV ( UPLO, TRANS, DIAG, N, K, A, LDA, X, INCX )
  NT2_MV(double, d)
  NT2_MV(float,  s)
  NT2_MV(std::complex<double>, z)
  NT2_MV(std::complex<float>, c)

#undef NT2_MV
} }

#endif


#include <boost/test/unit_test.hpp>

#include <ayla/geometry/triangle_mesh.hpp>
#include <ayla/geometry/vector.hpp>

BOOST_AUTO_TEST_SUITE(ayla)
BOOST_AUTO_TEST_SUITE(triangle_mesh)

const Float _positions[] = {
	 4.0f, -4.0f, // 0
	-4.0f, -4.0f, // 1
	 4.0f,  4.0f, // 2
	-4.0f,  4.0f, // 3
	 0.0f, -4.0f, // 4
	 0.0f,  4.0f, // 5
	-4.0f,  0.0f, // 6
	 4.0f,  0.0f, // 7
	 0.0f,  0.0f  // 8
};

const TriangularFace::IndexType _f[] = {
	2, 5, 8, // 0
	0, 7, 4, // 1
	8, 6, 1, // 2
	8, 5, 6, // 3
	5, 3, 6, // 4
	4, 8, 1, // 5
	4, 7, 8, // 6
	7, 2, 8  // 7
};

/*
 * Mesh configuration:
 *
 * 3__5__2
 * | /| /|
 * 6/_8/_7
 * | /| /|
 * 1/_4/_0
 */

const std::set<SizeType> _faceNeighbors[] = {
	{ 3u, 7u }, // 0
	{ 6u }, // 1
	{ 3u, 5u }, // 2
	{ 0u, 2u, 4u }, // 3
	{ 3u }, // 4
	{ 2u, 6u }, // 5
	{ 1u, 5u, 7u }, // 6
	{ 0u, 6u } // 7u
};

const std::array<UnorderedPair<SizeType>, 3u> _faceEdges[] = {
	{ UnorderedPair<SizeType>(2, 5), UnorderedPair<SizeType>(5, 8), UnorderedPair<SizeType>(2, 8) }, // 0
	{ UnorderedPair<SizeType>(0, 7), UnorderedPair<SizeType>(7, 4), UnorderedPair<SizeType>(4, 0) }, // 1
	{ UnorderedPair<SizeType>(8, 6), UnorderedPair<SizeType>(6, 1), UnorderedPair<SizeType>(1, 8) }, // 2
	{ UnorderedPair<SizeType>(8, 5), UnorderedPair<SizeType>(5, 6), UnorderedPair<SizeType>(6, 8) }, // 3
	{ UnorderedPair<SizeType>(5, 3), UnorderedPair<SizeType>(3, 6), UnorderedPair<SizeType>(6, 5) }, // 4
	{ UnorderedPair<SizeType>(4, 8), UnorderedPair<SizeType>(8, 1), UnorderedPair<SizeType>(1, 4) }, // 5
	{ UnorderedPair<SizeType>(4, 7), UnorderedPair<SizeType>(7, 8), UnorderedPair<SizeType>(8, 4) }, // 6
	{ UnorderedPair<SizeType>(7, 2), UnorderedPair<SizeType>(2, 8), UnorderedPair<SizeType>(8, 7) }  // 7u
};

const std::array<UnorderedPair<SizeType>, 16u> _meshEdges = {
	UnorderedPair<SizeType>(3, 5), // horizontal
	UnorderedPair<SizeType>(5, 2),
	UnorderedPair<SizeType>(6, 8),
	UnorderedPair<SizeType>(8, 7),
	UnorderedPair<SizeType>(1, 4),
	UnorderedPair<SizeType>(4, 0),
	UnorderedPair<SizeType>(3, 6), // vertical
	UnorderedPair<SizeType>(6, 1),
	UnorderedPair<SizeType>(5, 8),
	UnorderedPair<SizeType>(8, 4),
	UnorderedPair<SizeType>(2, 7),
	UnorderedPair<SizeType>(7, 0),
	UnorderedPair<SizeType>(6, 5), // diagonal
	UnorderedPair<SizeType>(8, 2),
	UnorderedPair<SizeType>(1, 8),
	UnorderedPair<SizeType>(4, 7)
};

TriangleMesh buildTestTriangleMesh( ) {
	std::vector<Vertex> _vertices;
	std::vector<TriangularFace> _faces;

	for (int i = 0; i < 9; i++) {
		Vertex v;
		v.setPosition(glm::vec3(_positions[ 2*i ], _positions[ 2*i + 1 ], 0.0f));
		v.setNormal(glm::vec3(0.0f, 0.0f, 1.0f));
		_vertices.push_back(v);
	}

	for (int i = 0; i < 8; i++) {
		TriangularFace f( _f[ 3*i ], _f[ 3*i + 1 ], _f[ 3*i + 2 ] );
		_faces.push_back( f );
	}

	return TriangleMesh( _vertices, _faces );
}

BOOST_AUTO_TEST_CASE(getters_and_setters) {
	TriangleMesh mesh = buildTestTriangleMesh();
	BOOST_REQUIRE(mesh.checkIntegrity());

	BOOST_CHECK(ayla::epsilonEqual(mesh.getVertex(0).getPosition(), glm::vec3(4.0f, -4.0f, 0.0f)));

	Vertex before = mesh.getVertex(0);
	Vertex other;
	other.setPosition(glm::vec3(10.0f, 5.0f, glm::pi<Float>()));

	mesh.setVertex(0, other);
	BOOST_CHECK(ayla::epsilonEqual(mesh.getVertex(0).getPosition(), other.getPosition()));
	mesh.setVertex(0, before);
	BOOST_CHECK(ayla::epsilonEqual(mesh.getVertex(0).getPosition(), before.getPosition()));
}

BOOST_AUTO_TEST_CASE(connectivity) {
	TriangleMesh mesh = buildTestTriangleMesh();
	BOOST_REQUIRE(mesh.checkIntegrity());

	BOOST_CHECK(mesh.getNumberOfVertices() == 9);
	BOOST_CHECK(mesh.getNumberOfFaces() == 8);

	auto neighbors = mesh.getVerticesConnectedToVertex(0);
	BOOST_CHECK(neighbors.size() == 2);
	BOOST_CHECK(neighbors.find(4) != neighbors.end());
	BOOST_CHECK(neighbors.find(7) != neighbors.end());

	neighbors = mesh.getVerticesConnectedToVertex(5);
	BOOST_CHECK(neighbors.size() == 4);
	BOOST_CHECK(neighbors.find(2) != neighbors.end());
	BOOST_CHECK(neighbors.find(3) != neighbors.end());
	BOOST_CHECK(neighbors.find(6) != neighbors.end());
	BOOST_CHECK(neighbors.find(8) != neighbors.end());

	neighbors = mesh.getVerticesConnectedToVertex(8);
	BOOST_CHECK(neighbors.size() == 6);
	BOOST_CHECK(neighbors.find(1) != neighbors.end());
	BOOST_CHECK(neighbors.find(2) != neighbors.end());
	BOOST_CHECK(neighbors.find(4) != neighbors.end());
	BOOST_CHECK(neighbors.find(5) != neighbors.end());
	BOOST_CHECK(neighbors.find(6) != neighbors.end());
	BOOST_CHECK(neighbors.find(7) != neighbors.end());

	auto neighborFaces = mesh.getFacesConnectedToVertex(0);
	BOOST_CHECK(neighborFaces.size() == 1);
	BOOST_CHECK(neighborFaces.find(1) != neighborFaces.end());

	neighborFaces = mesh.getFacesConnectedToVertex(5);
	BOOST_CHECK(neighborFaces.size() == 3);
	BOOST_CHECK(neighborFaces.find(0) != neighborFaces.end());
	BOOST_CHECK(neighborFaces.find(3) != neighborFaces.end());
	BOOST_CHECK(neighborFaces.find(4) != neighborFaces.end());

	neighborFaces = mesh.getFacesConnectedToVertex(8);
	BOOST_CHECK(neighborFaces.size() == 6);
	BOOST_CHECK(neighborFaces.find(0) != neighborFaces.end());
	BOOST_CHECK(neighborFaces.find(2) != neighborFaces.end());
	BOOST_CHECK(neighborFaces.find(3) != neighborFaces.end());
	BOOST_CHECK(neighborFaces.find(5) != neighborFaces.end());
	BOOST_CHECK(neighborFaces.find(6) != neighborFaces.end());
	BOOST_CHECK(neighborFaces.find(7) != neighborFaces.end());

	BOOST_CHECK(mesh.findFace(2, 5, 8) == 0);
	BOOST_CHECK(mesh.findFace(4, 7, 8) == 6);
	BOOST_CHECK(mesh.findFace(1, 2, 3) == std::numeric_limits<SizeType>::max());
	BOOST_CHECK(mesh.findFace(0, 8, 10) == std::numeric_limits<SizeType>::max());
}

BOOST_AUTO_TEST_CASE(aabb) {
	TriangleMesh mesh = buildTestTriangleMesh();
	BOOST_REQUIRE(mesh.checkIntegrity());

	glm::vec3 minPoint, maxPoint;
	mesh.calculateAABB(minPoint, maxPoint);
	AxisAlignedBox aab = mesh.calculateAABB();

	BOOST_CHECK(ayla::epsilonEqual(minPoint, glm::vec3(-4.0f, -4.0f, 0.0f)));
	BOOST_CHECK(ayla::epsilonEqual(maxPoint, glm::vec3(4.0f, 4.0f, 0.0f)));
	BOOST_CHECK(ayla::epsilonEqual(minPoint, aab.getMinPoint()));
	BOOST_CHECK(ayla::epsilonEqual(maxPoint, aab.getMaxPoint()));
}

BOOST_AUTO_TEST_CASE(planarity) {
	TriangleMesh mesh = buildTestTriangleMesh();
	BOOST_REQUIRE(mesh.checkIntegrity());

	BOOST_CHECK(mesh.isPlanar());

	Vertex before = mesh.getVertex(3);
	Vertex highV;
	highV.setPosition(glm::vec3(before.getPosition().x, before.getPosition().y, 10.0f));
	highV.setNormal(before.getNormal());
	mesh.setVertex(3, highV);

	BOOST_CHECK(!mesh.isPlanar());
	mesh.setVertex(3, before);
	BOOST_CHECK(mesh.isPlanar());
}

BOOST_AUTO_TEST_CASE(face_triangles) {
	TriangleMesh mesh = buildTestTriangleMesh();
	BOOST_REQUIRE(mesh.checkIntegrity());

	Triangle faceA = mesh.getFaceTriangle(0);
	Triangle faceB = mesh.getFaceTriangle(5);

	BOOST_CHECK(ayla::epsilonEqual(faceA.getA(), glm::vec3(4.0f, 4.0f, 0.0f)));
	BOOST_CHECK(ayla::epsilonEqual(faceA.getB(), glm::vec3(0.0f, 4.0f, 0.0f)));
	BOOST_CHECK(ayla::epsilonEqual(faceA.getC(), glm::vec3(0.0f, 0.0f, 0.0f)));

	BOOST_CHECK(ayla::epsilonEqual(faceB.getA(), glm::vec3(0.0f, -4.0f, 0.0f)));
	BOOST_CHECK(ayla::epsilonEqual(faceB.getB(), glm::vec3(0.0f, 0.0f, 0.0f)));
	BOOST_CHECK(ayla::epsilonEqual(faceB.getC(), glm::vec3(-4.0f, -4.0f, 0.0f)));
}

BOOST_AUTO_TEST_CASE(closest_vertex) {
	TriangleMesh mesh = buildTestTriangleMesh();
	BOOST_REQUIRE(mesh.checkIntegrity());

	std::pair< SizeType, Float > a = mesh.getClosestVertex(glm::vec3(-5.0f, 0.0f, 0.0f));

	BOOST_CHECK(ayla::epsilonEqual(mesh.getVertex(a.first).getPosition(), glm::vec3(-4.0f, 0.0f, 0.0f)));
	BOOST_CHECK(a.first == 6);

	std::pair< SizeType, Float > b = mesh.getClosestVertex(glm::vec3(0.0f, 0.0f, 0.4f));

	BOOST_CHECK(ayla::epsilonEqual(mesh.getVertex(b.first).getPosition(), glm::vec3(0.0f, 0.0f, 0.0f)));
	BOOST_CHECK(b.first == 8);

	BOOST_CHECK(mesh.getExtremeVertexIndex(glm::normalize(glm::vec3(1.0f, -1.0f, 0.0f))) == 0);
	BOOST_CHECK(mesh.getExtremeVertexIndex(glm::normalize(glm::vec3(-1.0f, -1.0f, 0.0f))) == 1);
	BOOST_CHECK(mesh.getExtremeVertexIndex(glm::normalize(glm::vec3(1.0f, 1.0f, 0.0f))) == 2);
	BOOST_CHECK(mesh.getExtremeVertexIndex(glm::normalize(glm::vec3(-1.0f, 1.0f, 0.0f))) == 3);
}

BOOST_AUTO_TEST_CASE(invert_normals) {
	TriangleMesh mesh = buildTestTriangleMesh();
	BOOST_REQUIRE(mesh.checkIntegrity());

	mesh.invertFacesOrientation();

	for (SizeType i = 0; i < mesh.getNumberOfFaces(); i++) {
		auto face = mesh.getFace(i);

		BOOST_CHECK(face.getA() == _f[3 * i]);
		BOOST_CHECK(face.getB() == _f[3 * i + 2]);
		BOOST_CHECK(face.getC() == _f[3 * i + 1]);
	}
}

BOOST_AUTO_TEST_CASE(area) {
	TriangleMesh mesh = buildTestTriangleMesh();
	BOOST_REQUIRE(mesh.checkIntegrity());

	BOOST_CHECK(mesh.calculateArea() == 64);
}

TriangleMesh divideAllFaces(const TriangleMesh& mesh) {
	TriangleMesh result = mesh;

	const SizeType originalNumberOfFaces = mesh.getNumberOfFaces();

	for (SizeType i = 0u; i < originalNumberOfFaces; ++i) {
		result.divideFaceInThreeByCentroid(i);
	}

	return result;
}

BOOST_AUTO_TEST_CASE(divide_face) {
	TriangleMesh originalMesh = buildTestTriangleMesh();
	BOOST_REQUIRE(originalMesh.checkIntegrity());

	const SizeType numberOfDivisions = 3u;

	for (SizeType i = 0u; i < numberOfDivisions; ++i) {
		const TriangleMesh dividedMesh = divideAllFaces(originalMesh);

		BOOST_CHECK(dividedMesh.checkIntegrity());
		BOOST_CHECK(dividedMesh.getNumberOfFaces() == (3 * originalMesh.getNumberOfFaces()));
		BOOST_CHECK(dividedMesh.getNumberOfVertices() == (originalMesh.getNumberOfVertices() + originalMesh.getNumberOfFaces()));

		originalMesh = dividedMesh;
	}
}

namespace {

bool _compare(const std::set<SizeType>& a, const std::set<SizeType>& b) {
	std::set<SizeType> difference;
	std::set_difference(a.begin(), a.end(), b.begin(), b.end(), std::inserter(difference, difference.begin()));
	return difference.size() == 0;
}

bool _compare(const std::array<UnorderedPair<SizeType>, 3u>& a, const std::array<UnorderedPair<SizeType>, 3u>& b) {
	for (SizeType i = 0u; i < 3u; ++i) {
		if (a[i] != b[i]) {
			return false;
		}
	}

	return true;
}

}

BOOST_AUTO_TEST_CASE(face_neighbors) {
	const TriangleMesh mesh = buildTestTriangleMesh();

	for (SizeType i = 0u; i < mesh.getNumberOfFaces(); ++i) {
		BOOST_CHECK(_compare(mesh.getFaceNeighbors(i), _faceNeighbors[i]));
	}
}

BOOST_AUTO_TEST_CASE(face_edges) {
	const TriangleMesh mesh = buildTestTriangleMesh();

	for (SizeType i = 0u; i < mesh.getNumberOfFaces(); ++i) {
		BOOST_CHECK(_compare(mesh.getFaceEdges(i), _faceEdges[i]));
	}
}

BOOST_AUTO_TEST_CASE(edges) {
	const TriangleMesh mesh = buildTestTriangleMesh();
	const auto calculatedMeshEdges = mesh.getEdges();

	for (auto it = _meshEdges.begin(); it != _meshEdges.end(); ++it) {
		BOOST_CHECK(calculatedMeshEdges.find(*it) != calculatedMeshEdges.end());
	}
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()


/*
Copyright 2015 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#define BOOST_TEST_MODULE test_lexicographical_hash
#include "utility/test/boost_unit_test.hpp"

#include "math/lexicographical.hpp"

#include <boost/functional/hash.hpp>

#include "range/tuple.hpp"
#include "range/std/container.hpp"

#include "math/arithmetic_magma.hpp"
#include "math/cost.hpp"
#include "math/sequence.hpp"

#include "math/check/check_hash.hpp"

BOOST_AUTO_TEST_SUITE (test_suite_lexicographical_hash)

BOOST_AUTO_TEST_CASE (test_hash_homogeneous) {
    typedef math::lexicographical <
            math::over <math::cost <double>, math::sequence <char>>>
        lexicographical;
    std::vector <lexicographical> examples;
    examples.push_back (math::zero <lexicographical>());
    examples.push_back (lexicographical (math::cost <double> (0),
        math::sequence_annihilator <char> ()));
    examples.push_back (lexicographical (math::zero <math::cost <double>>(),
        math::sequence <char> ()));
    examples.push_back (lexicographical (math::cost <double> (4),
        math::sequence_annihilator <char>()));
    examples.push_back (lexicographical (math::zero <math::cost <double>>(),
        math::sequence_annihilator <char>()));
    examples.push_back (lexicographical (math::cost <double> (0),
        math::sequence <char> (std::string ("a"))));
    examples.push_back (lexicographical (math::cost <double> (0),
        math::sequence <char> (std::string ("b"))));
    examples.push_back (lexicographical (math::cost <double> (0),
        math::sequence <char> (std::string ("bcd"))));

    math::check_hash (examples);
}

BOOST_AUTO_TEST_CASE (test_hash_heterogeneous) {
    typedef math::lexicographical <math::over <
            math::cost <float>, math::sequence <char>>>
        lexicographical;
    typedef math::lexicographical <math::over <
            math::cost <float>, math::optional_sequence <char>>>
        optional_lexicographical;
    typedef math::lexicographical <math::over <
            math::cost <float>, math::sequence_annihilator <char>>>
        annihilator_lexicographical;

    auto examples = range::make_tuple (
        annihilator_lexicographical (math::cost <float> (0),
            math::sequence_annihilator <char>()),
        annihilator_lexicographical (math::cost <float> (4),
            math::sequence_annihilator <char>()),
        optional_lexicographical (math::cost <float> (0),
            math::optional_sequence <char>()),
        optional_lexicographical (math::cost <float> (1),
            math::optional_sequence <char> ('a')),
        lexicographical (math::cost <float> (5),
            math::sequence <char> (std::string ("abc"))));

    math::check_hash (examples);
    math::check_cast_hash <lexicographical> (examples);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

// own includes -----------------------------------------------------------
#include "aux/timer.hpp"
#include "quadrature/quadrature_handler.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"
#include "spectral/basis/spectral_function.hpp"

// system includes --------------------------------------------------------
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <boost/lexical_cast.hpp>
#include <boost/multi_array.hpp>
#include <complex>
#include <fstream>
#include <iostream>
#include <string>

namespace boltzmann {
/**
 * @brief approximate f(phi, r) in polar basis
 *
 * @param dst          coefficient array
 * @param basis        basis
 * @param f            function without weight
 * @param exponent     e^{-r^2 * exponent}
 * @param weight       inner product weight
 * @param mass_matrix  mass matrix
 */
template <typename BASIS>
void
spectral_project(double* dst, /* output */
                 const BASIS& basis,
                 const std::function<double(double, double)>& f,
                 const double exponent,
                 const double weight,
                 const Eigen::SparseMatrix<double>& mass_matrix)
{
  boltzmann::Timer timer;
  typedef typename BASIS::elem_t elem_t;
  typedef typename std::tuple_element<1, typename BASIS::elem_t::container_t>::type radial_elem_t;
  typedef TensorProductQuadrature<QMidpoint, QMaxwell> quad_t;
  typedef std::shared_ptr<quad_t> qptr_t;
  QuadratureHandler<> quad_handler;
  const int N = basis.n_dofs();
  typedef Eigen::VectorXd vec_t;
  vec_t rhs(N);
  // Assemble rhs
  timer.restart();
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    const auto i = it - basis.begin();
    typename elem_t::Acc::template get<radial_elem_t> getter;
    const auto& phi = getter(*it);
    // const double beta = phi.get_beta();
    const double alpha = exponent + weight + phi.w();
    const auto& quad =
        quad_handler.get_quad(typename QuadratureHandler<>::descriptor(120, 100, alpha));

    double val = 0;
    for (unsigned int q = 0; q < quad.size(); ++q) {
      val += it->evaluate(quad.pts(q)[0], quad.pts(q)[1]) * f(quad.pts(q)[0], quad.pts(q)[1]) *
             quad.wts(q);
    }
    rhs[i] = val;
  }
  typedef Eigen::Map<vec_t> vec_wrapper_t;
  vec_wrapper_t x(dst, N);

  timer.restart();
  Eigen::SparseLU<Eigen::SparseMatrix<double> > lu;
  lu.compute(mass_matrix);
  x = lu.solve(rhs);
}

}  // end namespace boltzmann


/*
 * NProblemInterfaceAD.hpp
 *
 *  Created on: Nov 30, 2012
 *      Author: kremer
 */

#ifndef NPROBLEMINTERFACEAD_HPP_
#define NPROBLEMINTERFACEAD_HPP_

//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#if COMISO_ADOLC_AVAILABLE
#if COMISO_EIGEN3_AVAILABLE

//== INCLUDES =================================================================

#include <vector>

#include <boost/shared_array.hpp>

#include <adolc/adolc.h>
#include <adolc/adouble.h>
#include <adolc/drivers/drivers.h>
#include <adolc/sparse/sparsedrivers.h>
#include <adolc/taping.h>

#include <Eigen/Eigen>
#if !(EIGEN_VERSION_AT_LEAST(3,1,0))
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#endif
#include <Eigen/Sparse>

#include <CoMISo/Config/CoMISoDefines.hh>

#include "TapeIDSingleton.hh"

//== FORWARDDECLARATIONS ======================================================

//== NAMESPACES ===============================================================

namespace COMISO {

//== CLASS DEFINITION =========================================================

/** \class NProblemmInterfaceAD NProblemInterfaceAD.hpp <ACG/.../NProblemInterfaceAD.hh>

 Brief Description.

 Extend the problem interface with auto differentiation using ADOL-C
 */
class COMISODLLEXPORT NProblemInterfaceAD : public NProblemInterface
{
public:

    // Sparse Matrix Type
#if EIGEN_VERSION_AT_LEAST(3,1,0)
    typedef Eigen::SparseMatrix<double,Eigen::ColMajor> SMatrixNP;
#else
    typedef Eigen::DynamicSparseMatrix<double,Eigen::ColMajor> SMatrixNP;
#endif

    /// Default constructor
    NProblemInterfaceAD(int _n_unknowns) :
    n_unknowns_(_n_unknowns),
    dense_hessian_(NULL),
    function_evaluated_(false),
    use_tape_(true),
    tape_(static_cast<short int>(TapeIDSingleton::Instance()->requestId())) {

        for(size_t i = 0; i < 11; ++i) tape_stats_[i] = 0;
    }

    /// Destructor
    virtual ~NProblemInterfaceAD() {

        if(dense_hessian_ != NULL) {
            for(int i = 0; i < n_unknowns_; ++i) {
                delete[] dense_hessian_[i];
            }
            delete[] dense_hessian_;
        }

        TapeIDSingleton::Instance()->releaseId(static_cast<size_t>(tape_));
    }

    // ================================================
    // Override these three methods
    // ================================================

    virtual void initial_x(double* _x) = 0;

    virtual adouble evaluate(const adouble* _x) = 0;

    virtual void store_result(const double* _x) = 0;

    // ================================================
    // Optionally override these methods, too
    // ================================================

    /**
     * \brief Indicate whether the hessian is sparse.
     * If so, the computations (as well as the memory
     * consumption) can be performed more efficiently.
     */
    virtual bool sparse_hessian() {
        return false;
    }

    // ================================================

    virtual int n_unknowns() {

        return n_unknowns_;
    }

    virtual double eval_f(const double* _x) {

        double y = 0.0;

        if(!function_evaluated_ || !use_tape_) {

            adouble y_d = 0.0;

            boost::shared_array<adouble> x_d = x_d_ptr();

            trace_on(tape_); // Start taping

            // Fill data vector
            for(int i = 0; i < n_unknowns_; ++i) {
                x_d[i] <<= _x[i];
            }

            // Call virtual function to compute
            // functional value
            y_d = evaluate(x_d.get());

            y_d >>= y;

            trace_off();

#ifdef ADOLC_STATS
            tapestats(tape_, tape_stats_);
            std::cout << "Status values for tape " << tape_ << std::endl;
            std::cout << "===============================================" << std::endl;
            std::cout << "Number of independent variables:\t" << tape_stats_[0] << std::endl;
            std::cout << "Number of dependent variables:\t\t" << tape_stats_[1] << std::endl;
            std::cout << "Max. number of live active variables:\t" << tape_stats_[2] << std::endl;
            std::cout << "Size of value stack:\t\t\t" << tape_stats_[3] << std::endl;
            std::cout << "Buffer size:\t\t\t\t" << tape_stats_[4] << std::endl;
            std::cout << "Total number of operations recorded:\t" << tape_stats_[5] << std::endl;
            std::cout << "Other stats [6]:\t\t\t" << tape_stats_[6] << std::endl;
            std::cout << "Other stats [7]:\t\t\t" << tape_stats_[7] << std::endl;
            std::cout << "Other stats [8]:\t\t\t" << tape_stats_[8] << std::endl;
            std::cout << "Other stats [9]:\t\t\t" << tape_stats_[9] << std::endl;
            std::cout << "Other stats [10]:\t\t\t" << tape_stats_[10] << std::endl;
            std::cout << "===============================================" << std::endl;
#endif

            function_evaluated_ = true;

        } else {

            double ay[1] = {0.0};

            int ec = function(tape_, 1, n_unknowns_, const_cast<double*>(_x), ay);

#ifdef ADOLC_RET_CODES
            std::cout << "Info: function() returned code " << ec << std::endl;
#endif

            y = ay[0];
        }

        return y;
    }

    virtual void eval_gradient(const double* _x, double* _g) {

        if(!function_evaluated_ || !use_tape_) {
            // Evaluate original functional
            eval_f(_x);
        }

        int ec = gradient(tape_, n_unknowns_, _x, _g);

        if(ec < 0) {
            // Retape function if return code indicates discontinuity
            function_evaluated_ = false;
#ifdef ADOLC_RET_CODES
            std::cout << __FUNCTION__ << " invokes retaping of function due to discontinuity! Return code: " << ec << std::endl;
#endif
            eval_f(_x);
            ec = gradient(tape_, n_unknowns_, _x, _g);
        }

#ifdef ADOLC_RET_CODES
        std::cout << "Info: gradient() returned code " << ec << std::endl;
#endif
    }

    virtual void eval_hessian(const double* _x, SMatrixNP& _H) {

        _H.resize(n_unknowns_, n_unknowns_);
        _H.setZero();

        if(!function_evaluated_ || !use_tape_) {
            // Evaluate original functional
            eval_f(_x);
        }

        if(sparse_hessian()) {

            int nz = 0;
            int opt[2] = {0, 0};

            unsigned int* r_ind_p = NULL;
            unsigned int* c_ind_p = NULL;
            double* val_p = NULL;

            int ec = sparse_hess(tape_, n_unknowns_, 0, _x, &nz, &r_ind_p, &c_ind_p, &val_p, opt);

            if(ec < 0) {
                // Retape function if return code indicates discontinuity
                function_evaluated_ = false;
#ifdef ADOLC_RET_CODES
                std::cout << __FUNCTION__ << " invokes retaping of function due to discontinuity! Return code: " << ec << std::endl;
#endif
                eval_f(_x);
                ec = sparse_hess(tape_, n_unknowns_, 0, _x, &nz, &r_ind_p, &c_ind_p, &val_p, opt);
            }

            assert(*nz >= 0);
            assert(r_ind_p != NULL);
            assert(c_ind_p != NULL);
            assert(val_p != NULL);

#ifdef ADOLC_RET_CODES
            std::cout << "Info: sparse_hessian() returned code " << ec << std::endl;
#endif

            for(int i = 0; i < nz; ++i) {

                _H.coeffRef(r_ind_p[i], c_ind_p[i]) = val_p[i];
            }

            delete[] r_ind_p;
            delete[] c_ind_p;
            delete[] val_p;

        } else {

            // Dense hessian data
            double** h_ptr = dense_hessian_ptr();

            int ec = hessian(tape_, n_unknowns_, const_cast<double*>(_x), h_ptr);

            if(ec < 0) {
                // Retape function if return code indicates discontinuity
                function_evaluated_ = false;
#ifdef ADOLC_RET_CODES
                std::cout << __FUNCTION__ << " invokes retaping of function due to discontinuity! Return code: " << ec << std::endl;
#endif
                eval_f(_x);
                ec = hessian(tape_, n_unknowns_, const_cast<double*>(_x), h_ptr);
            }

#ifdef ADOLC_RET_CODES
            std::cout << "Info: hessian() returned code " << ec << std::endl;
#endif

            for(int i = 0; i < n_unknowns_; ++i) {
                for(int j = 0; j <= i; ++j) {

                    _H.coeffRef(i, j) = h_ptr[i][j];

                    if(i != j) {
                        _H.coeffRef(j, i) = h_ptr[i][j];
                    }
                }
            }
        }
    }

    bool use_tape() const {
        return use_tape_;
    }

    /** \brief Use tape
     * Set this to false if the energy functional
     * is discontinuous (so that the operator tree
     * has to be re-established at each evaluation)
     */
    void use_tape(bool _b) {
        use_tape_ = _b;
    }

    /**
     * \brief Get sample point vector's address
     *
     * The objective function class allocates the
     * memory for this vector at construction.
     * Get the pointer to this vector and inject it
     * into the constraint classes in order to
     * prevent them from allocating their own vectors
     * (which can be inefficient in terms of memory
     * consumption in case there are many constraints)
     */

    boost::shared_array<adouble> x_d_ptr() {
        if(x_d_.get() == NULL) {
            x_d_.reset(new adouble[n_unknowns_]);
        }
        return x_d_;
    }

    boost::shared_array<double> grad_ptr() {
        if(grad_.get() == NULL) {
            grad_.reset(new double[n_unknowns_]);
        }
        return grad_;
    }

    double** dense_hessian_ptr() {
        if(dense_hessian_ == NULL) {
            dense_hessian_ = new double*[n_unknowns_];
            for(int i = 0; i < n_unknowns_; ++i) {
                dense_hessian_[i] = new double[i+1];
            }
        }
        return dense_hessian_;
    }

private:

    int n_unknowns_;

    // Shared data
    boost::shared_array<adouble> x_d_;

    // Gradient vector
    boost::shared_array<double> grad_;

    // Dense hessian
    double** dense_hessian_;

    size_t tape_stats_[11];

    bool function_evaluated_;
    bool use_tape_;

    const short int tape_;
};

//=============================================================================
} // namespace COMISO
//=============================================================================
#endif // COMISO_ADOLC_AVAILABLE
//=============================================================================
#endif // COMISO_EIGEN3_AVAILABLE
//=============================================================================
#endif /* NPROBLEMINTERFACEAD_HPP_ */


// Copyright (C) 2015  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "tester.h"
#include <dlib/control.h>
#include <vector>
#include <sstream>
#include <ctime>

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;
    dlib::logger dlog("test.lspi");

    template <bool have_prior>
    struct chain_model
    {
        typedef int state_type;
        typedef int action_type; // 0 is move left, 1 is move right
        const static bool force_last_weight_to_1 = have_prior;

        const static int num_states = 4; // not required in the model interface

        matrix<double,8,1> offset;
        chain_model()
        {
            offset =
                2.048 ,
                2.56 ,
                2.048 ,
                3.2 ,
                2.56 ,
                4 ,
                3.2,
                5 ;
            if (!have_prior)
                offset = 0;

        }

        unsigned long num_features(
        ) const
        {
            if (have_prior)
                return num_states*2 + 1;
            else
                return num_states*2;
        }

        action_type find_best_action (
            const state_type& state,
            const matrix<double,0,1>& w
        ) const
        {
            if (w(state*2)+offset(state*2) >= w(state*2+1)+offset(state*2+1))
                //if (w(state*2) >= w(state*2+1))
                return 0;
            else
                return 1;
        }

        void get_features (
            const state_type& state,
            const action_type& action,
            matrix<double,0,1>& feats
        ) const
        {
            feats.set_size(num_features());
            feats = 0;
            feats(state*2 + action) = 1;
            if (have_prior)
                feats(num_features()-1) = offset(state*2+action);
        }

    };

    void test_lspi_prior1()
    {
        print_spinner();
        typedef process_sample<chain_model<true> > sample_type;
        std::vector<sample_type> samples;

        samples.push_back(sample_type(0,0,0,0));
        samples.push_back(sample_type(0,1,1,0));

        samples.push_back(sample_type(1,0,0,0));
        samples.push_back(sample_type(1,1,2,0));

        samples.push_back(sample_type(2,0,1,0));
        samples.push_back(sample_type(2,1,3,0));

        samples.push_back(sample_type(3,0,2,0));
        samples.push_back(sample_type(3,1,3,1));

        lspi<chain_model<true> > trainer;
        //trainer.be_verbose();
        trainer.set_lambda(0);
        policy<chain_model<true> > pol = trainer.train(samples);

        dlog << LINFO << pol.get_weights();

        matrix<double,0,1> w = pol.get_weights();
        DLIB_TEST(pol.get_weights().size() == 9);
        DLIB_TEST(w(w.size()-1) == 1);
        w(w.size()-1) = 0;
        DLIB_TEST_MSG(length(w) < 1e-12, length(w));

        dlog << LINFO << "action: " << pol(0);
        dlog << LINFO << "action: " << pol(1);
        dlog << LINFO << "action: " << pol(2);
        dlog << LINFO << "action: " << pol(3);
        DLIB_TEST(pol(0) == 1);
        DLIB_TEST(pol(1) == 1);
        DLIB_TEST(pol(2) == 1);
        DLIB_TEST(pol(3) == 1);
    }

    void test_lspi_prior2()
    {
        print_spinner();
        typedef process_sample<chain_model<true> > sample_type;
        std::vector<sample_type> samples;

        samples.push_back(sample_type(0,0,0,0));
        samples.push_back(sample_type(0,1,1,0));

        samples.push_back(sample_type(1,0,0,0));
        samples.push_back(sample_type(1,1,2,0));

        samples.push_back(sample_type(2,0,1,0));
        samples.push_back(sample_type(2,1,3,1));

        samples.push_back(sample_type(3,0,2,0));
        samples.push_back(sample_type(3,1,3,0));

        lspi<chain_model<true> > trainer;
        //trainer.be_verbose();
        trainer.set_lambda(0);
        policy<chain_model<true> > pol = trainer.train(samples);

        dlog << LINFO << "action: " << pol(0);
        dlog << LINFO << "action: " << pol(1);
        dlog << LINFO << "action: " << pol(2);
        dlog << LINFO << "action: " << pol(3);
        DLIB_TEST(pol(0) == 1);
        DLIB_TEST(pol(1) == 1);
        DLIB_TEST(pol(2) == 1);
        DLIB_TEST(pol(3) == 0);
    }

    void test_lspi_noprior1()
    {
        print_spinner();
        typedef process_sample<chain_model<false> > sample_type;
        std::vector<sample_type> samples;

        samples.push_back(sample_type(0,0,0,0));
        samples.push_back(sample_type(0,1,1,0));

        samples.push_back(sample_type(1,0,0,0));
        samples.push_back(sample_type(1,1,2,0));

        samples.push_back(sample_type(2,0,1,0));
        samples.push_back(sample_type(2,1,3,0));

        samples.push_back(sample_type(3,0,2,0));
        samples.push_back(sample_type(3,1,3,1));

        lspi<chain_model<false> > trainer;
        //trainer.be_verbose();
        trainer.set_lambda(0.01);
        policy<chain_model<false> > pol = trainer.train(samples);

        dlog << LINFO << pol.get_weights();
        DLIB_TEST(pol.get_weights().size() == 8);

        dlog << LINFO << "action: " << pol(0);
        dlog << LINFO << "action: " << pol(1);
        dlog << LINFO << "action: " << pol(2);
        dlog << LINFO << "action: " << pol(3);
        DLIB_TEST(pol(0) == 1);
        DLIB_TEST(pol(1) == 1);
        DLIB_TEST(pol(2) == 1);
        DLIB_TEST(pol(3) == 1);
    }
    void test_lspi_noprior2()
    {
        print_spinner();
        typedef process_sample<chain_model<false> > sample_type;
        std::vector<sample_type> samples;

        samples.push_back(sample_type(0,0,0,0));
        samples.push_back(sample_type(0,1,1,0));

        samples.push_back(sample_type(1,0,0,0));
        samples.push_back(sample_type(1,1,2,1));

        samples.push_back(sample_type(2,0,1,0));
        samples.push_back(sample_type(2,1,3,0));

        samples.push_back(sample_type(3,0,2,0));
        samples.push_back(sample_type(3,1,3,0));

        lspi<chain_model<false> > trainer;
        //trainer.be_verbose();
        trainer.set_lambda(0.01);
        policy<chain_model<false> > pol = trainer.train(samples);

        dlog << LINFO << pol.get_weights();
        DLIB_TEST(pol.get_weights().size() == 8);

        dlog << LINFO << "action: " << pol(0);
        dlog << LINFO << "action: " << pol(1);
        dlog << LINFO << "action: " << pol(2);
        dlog << LINFO << "action: " << pol(3);
        DLIB_TEST(pol(0) == 1);
        DLIB_TEST(pol(1) == 1);
        DLIB_TEST(pol(2) == 0);
        DLIB_TEST(pol(3) == 0);
    }

    class lspi_tester : public tester
    {
    public:
        lspi_tester (
        ) :
            tester (
                "test_lspi",       // the command line argument name for this test
                "Run tests on the lspi object.", // the command line argument description
                0                     // the number of command line arguments for this test
            )
        {
        }

        void perform_test (
        )
        {
            test_lspi_prior1();
            test_lspi_prior2();

            test_lspi_noprior1();
            test_lspi_noprior2();
        }
    };

    lspi_tester a;
}


//////////////////////////////////////////////////////////////////////////////
// multi_array::example::serialize.cpp                        				//
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are 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)        //
//////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <string>
#include <fstream>
#include <boost/mpl/int.hpp>
#include <boost/fusion/include/make_map.hpp>
#include <boost/fusion/include/at_key.hpp>
#include <boost/multi_array.hpp>
#include <boost/statistics/detail/multi_array/serialization/serialize.hpp>
#include <boost/statistics/detail/fusion/serialization/include.hpp>

#include <boost/serialization/serialization.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <libs/statistics/detail/multi_array/example/serialize.h>

void example_multi_array_serialize(std::ostream& os)
{
	// This example serializes an array of boost::fusion::map

	os << "multi_array_serialize ->" << std::endl;

    namespace ma = boost::detail::multi_array;

	typedef std::string str_;
    typedef std::ofstream ofs_;
    typedef std::ifstream ifs_;
	typedef boost::archive::text_oarchive oa_;
	typedef boost::archive::text_iarchive ia_;
    typedef unsigned val_;
    typedef std::vector<val_> vec_;
    typedef boost::mpl::int_<0> tag_;
    typedef boost::fusion::result_of::make_map<tag_,val_>::type map_;
	typedef boost::multi_array<val_,2> array_;
	typedef boost::multi_array<vec_,2> array_vec_;
    typedef boost::multi_array<map_,2> array_map_;
    typedef boost::array<ma::multi_array_base::index,2> extents_;
    const unsigned n0 = 2;
    const unsigned n1 = 2;
	{
    	const str_ path = "./multi_array_serialization";
    	{
    		ofs_ ofs(path.c_str());
	    	oa_ oa(ofs);
    		extents_ extents = {{ n0, n1 }};
			array_ a(extents);
        	// vec_ vec;
			for(unsigned i = 0; i<n0; i++)
    		{
    			for(unsigned j = 0; j<n1; j++)
        		{
					a[i][j] = i * n1 + j;
        		}
    		}
        	oa << a;
    	}
    	{
    		ifs_ ifs(path.c_str());
    		ia_ ia(ifs);
    		array_ a;
        	ia >> a;
			for(unsigned i = 0; i<n0; i++)
    		{
    			for(unsigned j = 0; j<n1; j++)
        		{
					BOOST_ASSERT(a[i][j] == i * n1 + j);
                	os  << a[i][j] << ' ';
        		}
    		}
    	}
    }
	{
    	const str_ path = "./multi_array_serialization2";
        const unsigned v0 = 0;
        const unsigned v1 = 1;
        const unsigned v2 = 2;
    	{

    		ofs_ ofs(path.c_str());
	    	oa_ oa(ofs);
    		extents_ extents = {{ n0, n1 }};
			array_vec_ a(extents);
        	vec_ vec;
            vec.push_back(v0);
            vec.push_back(v1);
			for(unsigned i = 0; i<n0; i++)
    		{
    			for(unsigned j = 0; j<n1; j++)
        		{
					a[i][j] = vec;
                    a[i][j].resize(3);
                    a[i][j][0] = v0;
                    a[i][j][1] = v1;
                    a[i][j][2] = v2;
        		}
    		}
        	oa << a;
    	}
    	{
    		ifs_ ifs(path.c_str());
    		ia_ ia(ifs);
    		array_vec_ a;
        	ia >> a;
			for(unsigned i = 0; i<n0; i++)
    		{
    			for(unsigned j = 0; j<n1; j++)
        		{
					BOOST_ASSERT(a[i][j][0] == v0);
					BOOST_ASSERT(a[i][j][1] == v1);
					BOOST_ASSERT(a[i][j][2] == v2);
        		}
    		}
    	}
	}
	{
    	const str_ path = "./multi_array_serialization3";
    	{

    		ofs_ ofs(path.c_str());
	    	oa_ oa(ofs);
    		extents_ extents = {{ n0, n1 }};
			array_map_ a(extents);
			for(unsigned i = 0; i<n0; i++)
    		{
    			for(unsigned j = 0; j<n1; j++)
        		{
					map_ map = boost::fusion::make_map<tag_>(i * n1 + j);
					a[i][j] = map;
        		}
    		}
        	oa << a;
    	}
    	{
    		ifs_ ifs(path.c_str());
    		ia_ ia(ifs);
    		array_map_ a;
        	ia >> a;
			for(unsigned i = 0; i<n0; i++)
    		{
    			for(unsigned j = 0; j<n1; j++)
        		{
                	map_ map =  a[i][j];
                	val_ val = boost::fusion::at_key<tag_>(map);
					BOOST_ASSERT(
                    	val == i * n1 + j
                    );
                	os  << val << ' ';
        		}
    		}
    	}
	}

	os << "<-" << std::endl;
}


#include <array>
#include <deque>
#include <iostream>
#include <fstream>
#include <chrono>
#include <boost/mpi.hpp>
#include <boost/mpi/collectives.hpp>
#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/serialization/deque.hpp>

using namespace std;

namespace mpi = boost::mpi;

constexpr int root = 0; // root process
constexpr int MAX_SIZE = 50000;

using Board = deque<deque<bool>>;

const int dx[] = {-1, -1, -1, 0, 0, 1, 1, 1};
const int dy[] = {-1, 0, 1, -1, 1, -1, 0, 1};

string to_string(const Board& b) {
   string s = "";
   for (const deque<bool>& line: b) {
      for (const bool& cell: line) {
	  s += cell ? "1 " : "0 ";
      }
      s += "\n";
   }
   return s;
}

Board transpose(const Board& b) {
    if (b.empty()) {
      return deque<deque<bool>> (0, deque<bool>(0));
    }

    Board bt(b[0].size(), deque<bool>(b.size()));

    for (size_t i = 0; i < b.size(); ++i) {
        for (size_t j = 0; j < b[i].size(); ++j) {
            bt[j][i] = b[i][j];
        }
    }

    return bt;
}

Board partition(const Board& b, size_t start, size_t size ) {
    if (size == 0) size = b.size();
    int stop = std::min<size_t>(b.size(), start + size);

    Board partitiondB;
    partitiondB.resize(size);

    for (int i = start; i < stop; ++i) {
        partitiondB[i - start] = (b[i]);
    }

    return partitiondB;
}

Board partition(const Board& b, size_t start) {
    Board partitiondB;

    for (size_t i = start; i < b.size(); ++i) {
        partitiondB.push_back(b[i]);
    }

    return partitiondB;
}

void doProcessing(Board& b, size_t start, size_t end) {
    Board newB{b};
    for (size_t i = start; i < end; ++i) {
        for (size_t j = 0; j < b[i].size(); ++j) {
	    int numLiveCells = 0;
	    int numDeadCells = 0;

	    for (int k = 0; k < 8; ++k) {
		int newI = i + dx[k];
		int newJ = j + dy[k];

		if (newI >= 0 && newJ >= 0 && newI < b.size() && newJ < b[i].size()) {
		    numLiveCells += b[newI][newJ] ? 1 : 0;
		    numDeadCells += b[newI][newJ] ? 0 : 1;
		}
	    }

	    if (numLiveCells < 2 || numLiveCells > 3) newB[i][j] = false;
	    else if (numLiveCells == 3) newB[i][j] = true;
        }
    }
    b = std::move(newB);
}

deque<bool> todequeOfBool (bool v[], size_t N) {
    deque<bool> b(N);
    for (size_t i = 0; i < N; ++i) {
        b[i] = v[i];
    }
    return b;
}

void doForFirstProcess(mpi::communicator& world, Board& b) {
    deque<bool> lastRow;
    mpi::request reqs;
    reqs = world.isend(1, root + 1, b.back());
    world.recv(1, root, lastRow);

    b.push_back(lastRow);

    doProcessing(b, 0, b.size() - 1);

    b.pop_back();
}

void doForLastProcess(mpi::communicator& world, Board& b, int rank) {
    deque<bool> firstRow;
    mpi::request reqs;
    reqs = world.isend(rank - 1, rank - 1, b[0]);
    world.recv(rank - 1, rank, firstRow);

    b.push_front(std::move(firstRow));

    doProcessing(b, 1, b.size());

    b.pop_front();
}

void doForInBetweenProcess(mpi::communicator& world, Board& b, int rank) {
    deque<bool> firstRow, lastRow;
    mpi::request reqs[2];

    reqs[0] = world.isend(rank - 1, rank - 1, b[0]);
    reqs[1] = world.isend(rank + 1, rank + 1, b.back());
    world.recv(rank - 1, rank, firstRow);
    world.recv(rank + 1, rank, lastRow);

    b.push_front(std::move(firstRow));
    b.push_back(std::move(lastRow));

    doProcessing(b, 1, b.size() - 1);

    b.pop_front();
    b.pop_back();
}

int main() {
   mpi::environment env;
   mpi::communicator world;

    auto start = std::chrono::steady_clock::now();

   Board local;
   size_t M;

   if (root == world.rank()) {
       size_t numRows, numCols, x, y;
       cin >> numRows >> numCols >> M;
       Board b;
       b.resize(numRows);
       for (size_t i = 0; i < numRows; ++i) {
           b[i].resize(numCols);
       }
       for (size_t i = 0; i < M; ++i) {
           cin >> x >> y;
           b[x][y] = true;
       }

       const size_t numProcess = std::min<size_t>(numRows, world.size());
       const size_t sizeOfAPartition = numRows / numProcess;

       local = partition(b, 0, sizeOfAPartition);

       for (size_t i = 1; i < numProcess - 1; ++i) {
           world.send(i, root, partition(b, i * sizeOfAPartition, sizeOfAPartition));
       }
       world.send(numProcess - 1, root, partition (b, (numProcess - 1) * sizeOfAPartition));
   }
   else {
       world.recv(root, root, local);
   }

   broadcast(world, M, root);

   world.barrier();

   for (size_t i = 0; i < M; ++i) {
       if (world.rank() == 0) doForFirstProcess(world, local);
       else if (world.rank() == world.size() - 1) doForLastProcess(world, local, world.rank());
       else doForInBetweenProcess(world, local, world.rank());
   }

   world.barrier();

   string result;
   reduce(world, to_string(local), result, [](const string& s, const string& bAsS) { return s + bAsS; }, root);

   if (world.rank() == root) {
       cout << result ;
   }

   auto duration = std::chrono::duration_cast<std::chrono::milliseconds>
                                   (std::chrono::steady_clock::now() - start);

   result.clear();
   string t = to_string(local.size()) + " " + to_string(local[0].size()) + " " + to_string(M) + " " + to_string(world.rank()) + " " + to_string(duration.count()) + "  milliseconds\n";
   reduce(world, t, result, [] (string s, string t) {return s + t;}, root);

   if (world.rank() == root) {
       cout << result;
   }

   return 0;
}


#pragma once

#include "wavefunction.hpp"

#include <Eigen/Dense>

class SumPooling : public Wavefunction
{
private:
    Wavefunction* const f;

public:
    explicit SumPooling(Wavefunction&);

    Real      operator()(const System&) override;
    RowVector gradient(const System&) override;
    Real      drift_force(const System& system, int k, int dim_index) override;
    Real      laplacian(const System&) override;

    void set_parameters(const RowVector&) override;
};


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

  Copyright 2015 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/constant/minlog2.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check minlog2 behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::minlog2;
  using boost::simd::Minlog2;
  T ref = T(0);
  STF_TYPE_IS(decltype(Minlog2<T>()), T);
  STF_EQUAL(Minlog2<T>(), ref);
  STF_EQUAL(minlog2( as(T{}) ),ref);
}

STF_CASE_TPL( "Check minlog2 behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::minlog2;
  using boost::simd::Minlog2;
  T ref = T(-1022);

  STF_TYPE_IS(decltype(Minlog2<T>()), T);
  STF_IEEE_EQUAL(Minlog2<T>(), ref);
  STF_IEEE_EQUAL(minlog2( as(T{}) ), ref);
}

STF_CASE_TPL( "Check minlog2 behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::minlog2;
  using boost::simd::Minlog2;
  T ref = T(-127);
  STF_TYPE_IS(decltype(Minlog2<T>()), T);
  STF_IEEE_EQUAL(Minlog2<T>(), ref);
  STF_IEEE_EQUAL(minlog2( as(T{}) ), ref);
}


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/LU>
#include <glog/logging.h>
#include <algorithm>

#include "gtest/gtest.h"
#include "theia/math/util.h"
#include "theia/util/random.h"
#include "theia/sfm/bundle_adjustment/optimize_relative_position_with_known_rotation.h"
#include "theia/sfm/camera/camera.h"
#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/pose/test_util.h"

namespace theia {

namespace {

Camera RandomCamera() {
  Camera camera;
  camera.SetPosition(Eigen::Vector3d::Random());
  camera.SetOrientationFromAngleAxis(0.2 * Eigen::Vector3d::Random());
  camera.SetImageSize(1000, 1000);
  camera.SetFocalLength(800);
  camera.SetAspectRatio(1.0);
  camera.SetSkew(0.0);
  camera.SetPrincipalPoint(500.0, 500.0);
  return camera;
}

void GetRelativeTranslationFromCameras(const Camera& camera1,
                                       const Camera& camera2,
                                       Eigen::Vector3d* relative_position) {
  const Eigen::Vector3d rotated_relative_position =
      camera2.GetPosition() - camera1.GetPosition();
  ceres::AngleAxisRotatePoint(camera1.GetOrientationAsAngleAxis().data(),
                              rotated_relative_position.data(),
                              relative_position->data());
  relative_position->normalize();
}

void TestOptimization(const Camera& camera1,
                      const Camera& camera2,
                      const std::vector<Eigen::Vector3d>& world_points,
                      const double kPixelNoise,
                      const double kTranslationNoise,
                      const double kTolerance) {
  // Project points and create feature correspondences.
  std::vector<FeatureCorrespondence> matches;
  for (int i = 0; i < world_points.size(); i++) {
    const Eigen::Vector4d point = world_points[i].homogeneous();
    FeatureCorrespondence match;
    camera1.ProjectPoint(point, &match.feature1);
    camera2.ProjectPoint(point, &match.feature2);
    AddNoiseToProjection(kPixelNoise, &match.feature1);
    AddNoiseToProjection(kPixelNoise, &match.feature2);

    // Undo the calibration.
    match.feature1 =
        camera1.PixelToNormalizedCoordinates(match.feature1).hnormalized();
    match.feature2 =
        camera2.PixelToNormalizedCoordinates(match.feature2).hnormalized();
    matches.emplace_back(match);
  }

  Eigen::Vector3d relative_position;
  GetRelativeTranslationFromCameras(camera1, camera2, &relative_position);

  const Eigen::Vector3d gt_relative_position = relative_position;

  // Add noise to relative translation.
  const Eigen::AngleAxisd translation_noise(DegToRad(
      RandGaussian(0.0, kTranslationNoise)), Eigen::Vector3d::Random());
  relative_position = translation_noise * relative_position;

  CHECK(OptimizeRelativePositionWithKnownRotation(
      matches,
      camera1.GetOrientationAsAngleAxis(),
      camera2.GetOrientationAsAngleAxis(),
      &relative_position));

  const double translation_error = RadToDeg(
      acos(Clamp(gt_relative_position.dot(relative_position), -1.0, 1.0)));
  EXPECT_LT(translation_error, kTolerance)
      << "GT Position = " << gt_relative_position.transpose()
      << "\nEstimated position = " << relative_position.transpose();
}

}  // namespace

TEST(OptimizeRelativePositionWithKnownRotationTest, NoNoise) {
  static const double kTolerance = 1e-6;
  static const double kPixelNoise = 0.0;
  static const double kTranslationNoise = 0.0;
  static const int kNumPoints = 100;
  std::vector<Eigen::Vector3d> points(kNumPoints);

  // Set up random points.
  InitRandomGenerator();
  for (int i = 0; i < kNumPoints; i++) {
    Eigen::Vector3d point(RandDouble(-2.0, 2.0),
                          RandDouble(-2.0, -2.0),
                          RandDouble(8.0, 10.0));
    points[i] = point;
  }

  // Set up random cameras.
  Camera camera1 = RandomCamera();
  Camera camera2 = RandomCamera();
  camera2.SetPosition(camera2.GetPosition().normalized());
  TestOptimization(camera1, camera2, points, kPixelNoise, kTranslationNoise,
                   kTolerance);
}

TEST(OptimizeRelativePositionWithKnownRotationTest, PixelNoise) {
  static const double kTolerance = 2.0;
  static const double kPixelNoise = 1.0;
  static const double kTranslationNoise = 0.0;
  static const int kNumPoints = 100;
  std::vector<Eigen::Vector3d> points(kNumPoints);

  // Set up random points.
  InitRandomGenerator();
  for (int i = 0; i < kNumPoints; i++) {
    Eigen::Vector3d point(RandDouble(-2.0, 2.0),
                          RandDouble(-2.0, -2.0),
                          RandDouble(8.0, 10.0));
    points[i] = point;
  }

  // Set up random cameras.
  Camera camera1 = RandomCamera();
  Camera camera2 = RandomCamera();
  camera2.SetPosition(camera2.GetPosition().normalized());
  TestOptimization(camera1, camera2, points, kPixelNoise, kTranslationNoise,
                   kTolerance);
}

TEST(OptimizeRelativePositionWithKnownRotationTest, TranslationNoise) {
  static const double kTolerance = 2.0;
  static const double kPixelNoise = 0.0;
  static const double kTranslationNoise = 5.0;
  static const int kNumPoints = 100;
  std::vector<Eigen::Vector3d> points(kNumPoints);

  // Set up random points.
  InitRandomGenerator();
  for (int i = 0; i < kNumPoints; i++) {
    Eigen::Vector3d point(RandDouble(-2.0, 2.0),
                          RandDouble(-2.0, -2.0),
                          RandDouble(8.0, 10.0));
    points[i] = point;
  }

  // Set up random cameras.
  Camera camera1 = RandomCamera();
  Camera camera2 = RandomCamera();
  camera2.SetPosition(camera2.GetPosition().normalized());
  TestOptimization(camera1, camera2, points, kPixelNoise, kTranslationNoise,
                   kTolerance);
}

TEST(OptimizeRelativePositionWithKnownRotationTest, PixelAndTranslationNoise) {
  static const double kTolerance = 5.0;
  static const double kPixelNoise = 1.0;
  static const double kTranslationNoise = 5.0;
  static const int kNumPoints = 100;
  std::vector<Eigen::Vector3d> points(kNumPoints);

  // Set up random points.
  InitRandomGenerator();
  for (int i = 0; i < kNumPoints; i++) {
    Eigen::Vector3d point(RandDouble(-2.0, 2.0),
                          RandDouble(-2.0, -2.0),
                          RandDouble(8.0, 10.0));
    points[i] = point;
  }

  // Set up random cameras.
  Camera camera1 = RandomCamera();
  Camera camera2 = RandomCamera();
  camera2.SetPosition(camera2.GetPosition().normalized());
  TestOptimization(camera1, camera2, points, kPixelNoise, kTranslationNoise,
                   kTolerance);
}

}  // namespace theia


/*!
 * @file
 * Defines `boost::mpl11::detail::count_if::variadic_sum`.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_MPL11_DETAIL_COUNT_IF_VARIADIC_SUM_HPP
#define BOOST_MPL11_DETAIL_COUNT_IF_VARIADIC_SUM_HPP

#include <boost/mpl11/detail/std_size_t.hpp>
#include <boost/mpl11/integer.hpp>

namespace boost { namespace mpl11 { namespace detail { namespace count_if {
    template <std_size_t N>
    constexpr std_size_t homogeneous_sum(const bool (&array)[N]) {
        std_size_t s = 0;
        for (std_size_t i = 0; i < N; ++i)
            s += array[i];
        return s;
    }

    /*!
     * @ingroup details
     *
     * `count_if` algorithm using a constexpr sum.
     */
    template <template <typename ...> class predicate, typename ...xs>
    using variadic_sum = size_t<
        // Avoid empty arrays
        homogeneous_sum<sizeof...(xs)+1>(
            {false, (bool)predicate<xs>::type::value...}
        )
    >;
}}}} // end namespace boost::mpl11::detail::count_if

#endif // !BOOST_MPL11_DETAIL_COUNT_IF_VARIADIC_SUM_HPP


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018-2021 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------
// @author Ignacio Vizzo     [ivizzo@uni-bonn.de]
//
// Copyright (c) 2020 Ignacio Vizzo, Cyrill Stachniss, University of Bonn.
// ----------------------------------------------------------------------------

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <cmath>
#include <memory>
#include <tuple>
#include <vector>

#include "open3d/geometry/KDTreeFlann.h"
#include "open3d/geometry/Keypoint.h"
#include "open3d/geometry/PointCloud.h"
#include "open3d/utility/Eigen.h"
#include "open3d/utility/Logging.h"

namespace open3d {

namespace {

bool IsLocalMaxima(int query_idx,
                   const std::vector<int>& indices,
                   const std::vector<double>& third_eigen_values) {
    for (const auto& idx : indices) {
        if (third_eigen_values[query_idx] < third_eigen_values[idx]) {
            return false;
        }
    }
    return true;
}

double ComputeModelResolution(const std::vector<Eigen::Vector3d>& points,
                              const geometry::KDTreeFlann& kdtree) {
    std::vector<int> indices(2);
    std::vector<double> distances(2);
    double resolution = 0.0;

    for (const auto& point : points) {
        if (kdtree.SearchKNN(point, 2, indices, distances) != 0) {
            resolution += std::sqrt(distances[1]);
        }
    }
    resolution /= points.size();
    return resolution;
}

}  // namespace

namespace geometry {
namespace keypoint {
std::shared_ptr<PointCloud> ComputeISSKeypoints(
        const PointCloud& input,
        double salient_radius /* = 0.0 */,
        double non_max_radius /* = 0.0 */,
        double gamma_21 /* = 0.975 */,
        double gamma_32 /* = 0.975 */,
        int min_neighbors /*= 5 */) {
    if (input.points_.empty()) {
        utility::LogWarning("[ComputeISSKeypoints] Input PointCloud is empty!");
        return std::make_shared<PointCloud>();
    }
    const auto& points = input.points_;
    KDTreeFlann kdtree(input);

    if (salient_radius == 0.0 || non_max_radius == 0.0) {
        const double resolution = ComputeModelResolution(points, kdtree);
        salient_radius = 6 * resolution;
        non_max_radius = 4 * resolution;
        utility::LogDebug(
                "[ComputeISSKeypoints] Computed salient_radius = {}, "
                "non_max_radius = {} from input model",
                salient_radius, non_max_radius);
    }

    std::vector<double> third_eigen_values(points.size());
#pragma omp parallel for schedule(static) shared(third_eigen_values)
    for (int i = 0; i < (int)points.size(); i++) {
        std::vector<int> indices;
        std::vector<double> dist;
        int nb_neighbors =
                kdtree.SearchRadius(points[i], salient_radius, indices, dist);
        if (nb_neighbors < min_neighbors) {
            continue;
        }

        Eigen::Matrix3d cov = utility::ComputeCovariance(points, indices);
        if (cov.isZero()) {
            continue;
        }

        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(cov);
        const double& e1c = solver.eigenvalues()[2];
        const double& e2c = solver.eigenvalues()[1];
        const double& e3c = solver.eigenvalues()[0];

        if ((e2c / e1c) < gamma_21 && e3c / e2c < gamma_32) {
            third_eigen_values[i] = e3c;
        }
    }

    std::vector<size_t> kp_indices;
    kp_indices.reserve(points.size());
#pragma omp parallel for schedule(static) shared(kp_indices)
    for (int i = 0; i < (int)points.size(); i++) {
        if (third_eigen_values[i] > 0.0) {
            std::vector<int> nn_indices;
            std::vector<double> dist;
            int nb_neighbors = kdtree.SearchRadius(points[i], non_max_radius,
                                                   nn_indices, dist);

            if (nb_neighbors >= min_neighbors &&
                IsLocalMaxima(i, nn_indices, third_eigen_values)) {
                kp_indices.emplace_back(i);
            }
        }
    }

    utility::LogDebug("[ComputeISSKeypoints] Extracted {} keypoints",
                      kp_indices.size());
    return input.SelectByIndex(kp_indices);
}

}  // namespace keypoint
}  // namespace geometry
}  // namespace open3d