#ifndef RADIUMENGINE_VECTOR_HPP
#define RADIUMENGINE_VECTOR_HPP

/// This file contains typedefs and basic vector classes and functions

#include <Core/RaCore.hpp>

#include <functional>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Geometry>
#include <unsupported/Eigen/AlignedVector3>

#include <Core/Math/Math.hpp>

// General config
// Use this to force vec3 to be aligned for vectorization (FIXME not working yet)
// #define CORE_USE_ALIGNED_VEC3

namespace Ra
{
    namespace Core
    {
        //
        // Common vector types
        //
        typedef Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorN;
        typedef Eigen::VectorXf                            VectorNf;
        typedef Eigen::VectorXd                            VectorNd;

        typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
        typedef Eigen::Vector4f             Vector4f;
        typedef Eigen::Vector4d             Vector4d;

#ifndef CORE_USE_ALIGNED_VEC3
        typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
        typedef Eigen::Vector3f             Vector3f;
        typedef Eigen::Vector3d             Vector3d;
#else
        typedef Eigen::AlignedVector3<Scalar> Vector3;
        typedef Eigen::AlignedVector3<float> Vector3f;
        typedef Eigen::AlignedVector3<double> Vector3d;
#endif

        typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
        typedef Eigen::Vector2f             Vector2f;
        typedef Eigen::Vector2d             Vector2d;

        typedef Eigen::VectorXi VectorNi;
        typedef Eigen::Vector2i Vector2i;
        typedef Eigen::Vector3i Vector3i;
        typedef Eigen::Vector4i Vector4i;

        typedef Eigen::Matrix<uint, Eigen::Dynamic, 1> VectorNui;
        typedef Eigen::Matrix<uint, 2, 1>              Vector2ui;
        typedef Eigen::Matrix<uint, 3, 1>              Vector3ui;
        typedef Eigen::Matrix<uint, 4, 1>              Vector4ui;

        //
        // Common matrix types
        //

        typedef Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > MatrixN;
        typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
        typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
        typedef Eigen::Matrix<Scalar, 2, 2> Matrix2;

        typedef Eigen::MatrixXf MatrixNf;
        typedef Eigen::Matrix4f Matrix4f;
        typedef Eigen::Matrix3f Matrix3f;
        typedef Eigen::Matrix2f Matrix2f;

        typedef Eigen::MatrixXd MatrixNd;
        typedef Eigen::Matrix4d Matrix4d;
        typedef Eigen::Matrix3d Matrix3d;
        typedef Eigen::Matrix2d Matrix2d;

        typedef Eigen::Matrix< uint, Eigen::Dynamic, Eigen::Dynamic > MatrixNui;

        //typedef Eigen::DiagonalMatrix< Scalar, Eigen::Dynamic > Diagonal;
        typedef Eigen::SparseMatrix< Scalar > Diagonal; // Not optimized for Diagonal matrices, but the operations between Sparse and Diagonal are not defined
        typedef Eigen::SparseMatrix< Scalar > Sparse;

        //
        // Transforms and rotations
        //

        typedef Eigen::Quaternion<Scalar> Quaternion;
        typedef Eigen::Quaternionf        Quaternionf;
        typedef Eigen::Quaterniond        Quaterniond;

        typedef Eigen::Transform<Scalar, 3, Eigen::Affine> Transform;
        typedef Eigen::Affine3f                            Transformf;
        typedef Eigen::Affine3d                            Transformd;

        typedef Eigen::AlignedBox<Scalar, 3> Aabb;
        typedef Eigen::AlignedBox3f          Aabbf;
        typedef Eigen::AlignedBox3d          Aabbd;

        typedef Eigen::AngleAxis<Scalar> AngleAxis;
        typedef Eigen::AngleAxisf        AngleAxisf;
        typedef Eigen::AngleAxisd        AngleAxisd;

        typedef Eigen::Translation<Scalar, 3> Translation;
        typedef Eigen::Translation3f         Translationf;
        typedef Eigen::Translation3d         Translationd;

        inline void print( const MatrixN& matrix );

        //
        // Geometry types
        //

        typedef Eigen::ParametrizedLine< Scalar, 2 > Line2;
        typedef Eigen::ParametrizedLine< Scalar, 3 > Line3;
        typedef Eigen::Hyperplane< Scalar, 3 >       Plane3;

        // Todo : storage transform using quaternions ?

        //
        // Misc types
        //
        typedef Vector4  Color;
        typedef Vector4f Colorf;
        typedef Vector4d Colord;

        //
        // Vector Functions
        //
        namespace Vector
        {

            /// Component-wise floor() function on a floating-point vector.
            template<typename Vector>
            inline Vector floor( const Vector& v );

            /// Component-wise ceil() function on a floating-point vector.
            template<typename Vector>
            inline Vector ceil( const Vector& v );

            /// Component-wise trunc() function on a floating-point vector
            template<typename Vector>
            inline Vector trunc( const Vector& v );

            /// Component-wise clamp() function on a floating-point vector.
            template<typename Vector>
            inline Vector clamp( const Vector& v, const Vector& min, const Vector& max );

            /// Component-wise clamp() function on a floating-point vector.
            template<typename Vector>
            inline Vector clamp( const Vector& v, const Scalar& min, const Scalar& max );

            /// Vector range check, works for any numeric vector.
            template<typename Vector_>
            inline bool checkRange( const Vector_& v, const Scalar& min, const Scalar& max );

            /// Call std::isnormal on vector entries.
            template< typename Vector_ >
            inline bool checkInvalidNumbers(Eigen::Ref<const Vector_> v,
                                             const bool FAIL_ON_ASSERT = false);

            /// Get two vectors orthogonal to a given vector.
            inline void getOrthogonalVectors( const Vector3& fx, Eigen::Ref<Vector3> fy, Eigen::Ref<Vector3> fz );

            /// Get the angle between two vectors. Works for types where the cross product is
            /// defined (i.e. 2D and 3D vectors).
            template<typename Vector_>
            inline Scalar angle( const Vector_& v1, const Vector_& v2 );

            /// Get the spherical linear interpolation between two unit non-colinear vectors.
            /// works for types where the cross-product is defined (i.e. 2D and 3D vectors).
            template<typename Vector_>
            inline Vector_ slerp( const Vector_& v1, const Vector_& v2, Scalar t );

            /// @return the projection of point on the plane define by plane and planeNormal
            inline Vector3 projectOnPlane(const Vector3& planePos, const Vector3& planeNormal, const Vector3& point);

            /// Get the cotangent of the angle between two vectors. Works for vector types where
            /// dot and cross product is defined (2D or 3D vectors).
            template <typename Vector_>
            inline Scalar cotan( const Vector_& v1, const Vector_& v2);

            /// Get the cosine of the angle between two vectors.
            template <typename Vector_>
            inline Scalar cos( const Vector_& v1, const Vector_& v2);

            /// Normalize a vector and returns its norm before normalization.
            /// If the vector's norm is 0, the vector's components will be overwritten by NaNs
            template< typename Vector_ >
            inline Scalar getNormAndNormalize(Vector_& v);

            /// Normalize a vector and returns its norm before normalization.
            /// If the vector's norm is 0, the vector remains null
            template< typename Vector_ >
            inline Scalar getNormAndNormalizeSafe(Vector_& v);

        }

        namespace MatrixUtils
        {
            inline Matrix4 lookAt(const Vector3& position, const Vector3& target, const Vector3& up);
            inline Matrix4 perspective(Scalar fovy, Scalar aspect, Scalar near, Scalar zfar);
            inline Matrix4 orthographic(Scalar left, Scalar right, Scalar bottom, Scalar top, Scalar near, Scalar zfar);

            /// Call std::isnormal on matrix entry.
            /// Dense version
            template< typename Matrix_ >
            inline bool checkInvalidNumbers(Eigen::Ref<const Matrix_> matrix,
                                             const bool FAIL_ON_ASSERT = false);

        }

        //
        // Quaternion functions
        //
        namespace QuaternionUtils
        {
            // Define functions for multiplying a quaternion by a scalar
            // and adding two quaternions. While Quaternion is supposed to
            // represent a unit quaternion (thus a valid rotation), these functions
            // are useful for linear interpolation of quaternions.

            /// Returns the quaternion q multipled by a scalar factor of k.
            inline Quaternion scale(const Quaternion& q, const Scalar k);

            /// Returns the sum of two quaternions.
            inline Quaternion add(const Quaternion& q1, const Quaternion& q2);

            /// Returns the sum of two quaternions, resolving antipodality by flipping
            /// the sign of q2 if q1.q2 is negative. This operation is usually
            /// denoted as a circled + sign, forming the basis of the QLERP algorithm.
            /// See "Spherical Blend Skinning" (Kavan & Zara 2005) for more details.
            inline Quaternion addQlerp(const Quaternion& q1, const Quaternion& q2);

            // Note : the .inl file also define operator+ for quaternions
            // and operator * and / between quaternions and scalar.

            /// Decompose a given rotation Qin into a swing rotation and a twist rotation.
            /// Qswing is a rotation whose axis lies in the XY plane and Qtwist is a rotation about axis Z.
            /// such as Qin = Qswing * Qtwist
            /// If the rotation is already around axis z, Qswing will be set to identity
            /// and Qtwist equal to Qin
            inline void getSwingTwist(const Quaternion& in, Quaternion& swingOut, Quaternion& twistOut);

            /// Call std::isnormal on quaternion entries.
            template< typename Quaternion_ >
            inline bool checkInvalidNumbers(Eigen::Ref<const Quaternion_> q,
                                             const bool FAIL_ON_ASSERT = false) {
                return MatrixUtils::checkInvalidNumbers( q.coeffs(), FAIL_ON_ASSERT );
            }
        }

        // Use this macro in the public: section of a class
        // when declaring objects containing Vector or Matrices.
        // http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html
#define  RA_CORE_ALIGNED_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        /// Use this parameter for aligning structures with Eigen members in it
#define  RA_DEFAULT_ALIGN EIGEN_MAX_ALIGN_BYTES

    }
} // end namespace Ra::Core

#include <Core/Math/LinearAlgebra.inl>

#endif// RADIUMENGINE_VECTOR_HPP


#include <vector>
#include <boost/numeric/ublas/matrix.hpp>
#include <numeric>

#include "operationVector.h" // ceci va chercher le fichier qui contient les operations sur les vecteurs

using namespace std;
using namespace boost::numeric::ublas;

matrix<double> Beta(matrix<double> Obs, matrix<double> M, std::vector<double> pie) {

// 	Obs est la matrice des observations
// 	M est la matrice de transition
// 	pie est le vecteur de probabilite initiale

	const size_t m = Obs.size2(); // Nombre detat, de cases
	const size_t n = Obs.size1(); // Nombre dobservation, duree de la partie

//	matrix Alpha = matrix(NbLigne= n, NbCol= m);  Initialisation de la matric Alpha de tailler n*m
	matrix<double> Beta(n, m);
	matrix<double> bet(1,m);

	fill(bet, [&](int l, int c){
		return 1.0/m;
	});

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

		Beta(n-1, i) = 0; // Si je comprend bien, la derniere ligne est la n-1 ?
	}

	double log1surc = log(sum(bet));
	matrix<double> Bet{1, m};

	matrix<double> Tligne = trans(ligne(Obs,n-1));
	Bet = prod(M,Tligne);
	Bet = trans(Bet);
	for (int i = 0; i < m; ++i) {

		Beta(n-2, i) = log(Bet(0,i))+log1surc;
	}

	log1surc = log1surc + log(sum(Bet));

	 Bet /= sum(Bet);

	//for (int i = 0; i < m; ++i) {

	//	Bet(0, i) = Bet(0,i)/sum(Bet);
	//}

	for (int i = 3; i < n+1 ; ++i) { // J'ai changer la partie "i < n, ++j" pour "i < n ; ++i"

		Tligne = trans(ligne(Obs,n-i+1));

		for (int j = 0; j < m ; ++j) {
			Tligne(j, 0) = Tligne(j, 0) * Bet(0, j);
		}

		Bet = prod(M, Tligne); // prod est le produit matriciel
		Bet = trans(Bet);

		for (int j = 0; j < m ; ++j) {
			Beta(n-i, j) = log(Bet(0, j)) + log1surc;
		}

		log1surc += log(sum(Bet));

		//for (int i = 0; i < m; ++i) {

		//	Bet(0, i) = Bet(0,i)/sum(Bet);
		//}

		 Bet /= sum(Bet);

	}

	return Beta;

}


//    Copyright 2017 Rainer Gemulla
//
//    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.
/** \file
 *
 * Illustrates matrix factorization with alternating least squares. We first creates factors and
 * then a data matrix
 * from these factors. THis process ensures that we know the best factorization of the input.
 * We then try to reconstruct the factors.
 */
#include <iostream>

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>
#include <util/io.h>

#include <mf/mf.h>

#include <mf/loss/nzsl.h>
#include <mf/loss/l2.h>

using namespace std;
using namespace mf;
using namespace rg;
using namespace boost::numeric::ublas;

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

int main(int argc, char* argv[]) {
#ifndef NDEBUG
	LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
#endif

	// parameters for the factorization
	mf_size_type size1 = 10000;
	mf_size_type size2 = 10000;
	mf_size_type nnz = 1000000;
	double sigma = 1; // standard deviation
	double lambda = 1/sigma/sigma;
	mf_size_type r = 10;

	// parameters for ALS
	unsigned epochs = 20;
	AlsRegularizer regularizer = ALS_L2;
	typedef SumLoss<NzslLoss, L2Loss> Loss;
	typedef NzRmseLoss TestLoss;
	Loss loss((NzslLoss()), L2Loss(lambda));
	TestLoss testLoss;
	mf_size_type testNnz = nnz/100;

	BalanceType type = BALANCE_L2;;
	BalanceMethod method = BALANCE_SIMPLE;

	// generate original factors by sampling from a normal(0,sigma) distribution
	Random32 random; // note: this takes a default seed (not randomized!)
	DenseMatrix wIn(size1, r);
	DenseMatrixCM hIn(r, size2);
	generateRandom(wIn, random, boost::normal_distribution<>(0, sigma));
	generateRandom(hIn, random, boost::normal_distribution<>(0, sigma));

	// generate a sparse matrix by selecting random entries from the generated factors
	// and add small Gaussian noise
	SparseMatrix v;
	generateRandom(v, nnz, wIn, hIn, random);
	addRandom(v, random, boost::normal_distribution<>(0, 0.1));
	LOG4CXX_INFO(logger, "Data matrix: "
		<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
	v.sort();
	LOG4CXX_INFO(logger, "Loss with original factors: " << loss((FactorizationData<>(v, wIn, hIn))));
	SparseMatrixCM vc;
	copyCm(v, vc);

	// create a test matrix (without noise)
	SparseMatrix vTest;
	generateRandom(vTest, testNnz, wIn, hIn, random);
	LOG4CXX_INFO(logger, "Test matrix: "
		<< v.size1() << " x " << v.size2() << ", " << vTest.nnz() << " nonzeros");

	// generate initial factors by sampling from a uniform[-0.5,0.5] distribution
	DenseMatrix w(size1, r);
	DenseMatrixCM h(r, size2);
	generateRandom(w, random, boost::uniform_real<>(-0.5, 0.5));
	generateRandom(h, random, boost::uniform_real<>(-0.5, 0.5));

	// print the test loss
	LOG4CXX_INFO(logger, "Initial test loss: "
			<< testLoss(FactorizationData<>(vTest,w,h)));

	FactorizationData<> testJob(vTest,w,h);

	// initialize
	FactorizationData<> data(v, w, h, 1, &vc);
	Trace trace;
	// here add fields to Trace
	//trace.addField("balancing-type", type);
	//trace.addField("balancing-method", method);
	Timer t;

	// run ALS to try to reconstruct the original factors
	t.start();
//	alsNzsl(data, epochs, trace, lambda, regularizer, rescale);
	alsNzsl(data, epochs, trace, lambda, regularizer, type, method ,&testJob);
	t.stop();
	LOG4CXX_INFO(logger, "Total time: " << t);

	// print the test loss
	LOG4CXX_INFO(logger, "Final test loss: "
			<< testLoss(FactorizationData<>(vTest,w,h)));

	// write trace to an R file
	string typeString, methodString;
	switch (type) {
	case BALANCE_NONE:
		typeString = "None";
		break;
	case BALANCE_L2:
		typeString = "L2";
		break;
	case BALANCE_NZL2:
		typeString = "Nzl2";
		break;
	}
	switch (method) {
	case BALANCE_SIMPLE:
		methodString = "Simple";
		break;
	case BALANCE_OPTIMAL:
		methodString = "Optimal";
		break;
	}
	string filename = "/tmp/als-trace.R";
	LOG4CXX_INFO(logger, "Writing trace to " << filename);
	trace.toRfile(filename, "als");

	return 0;
}


#include <Eigen/Dense>
#include <thirdparty/HighFive/include/highfive/H5Easy.hpp>

#include <cannon/log/registry.hpp>

using namespace Eigen;

using namespace cannon::log;

int main() {
  H5Easy::File file("test.h5", H5Easy::File::Overwrite);

  Matrix2d m;
  m << 1.0, 2.0,
       3.0, 4.0;

  H5Easy::dump(file, "/tmp/m", m);

  Matrix2d m2 = H5Easy::load<Matrix2d>(file, "/tmp/m");

  log_info("m is", m);
  log_info("m2 is", m2);
}


//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2012
//
//============================================================================

#ifndef __Thea_Algorithms_PcaN_hpp__
#define __Thea_Algorithms_PcaN_hpp__

#include "../Common.hpp"
#include "../Math.hpp"
#include "../MatVec.hpp"
#include "IteratorModifiers.hpp"
#include "PointTraitsN.hpp"
#include "CentroidN.hpp"
#include <Eigen/Eigenvalues>

namespace Thea {
namespace Algorithms {

/** Principal component analysis of N-D data. */
template <typename T, int N, typename ScalarT = Real, typename Enable = void>
class /* THEA_API */ PcaN
{
  public:
    typedef Vector<N, ScalarT> VectorT;  ///< N-D vector used to represent points and directions.

    /**
     * Compute the PCA axes of a set of N-D objects. InputIterator must dereference to type T or pointer-to-T.
     *
     * @param begin The first object in the set.
     * @param end One position beyond the last object in the set.
     * @param eigenvalues The eigenvalues of the covariance matrix, ordered from largest to smallest.
     * @param eigenvectors The unit-length eigenvectors of the covariance matrix, ordered from largest to smallest eigenvalue.
     * @param centroid If non-null, used to return the centroid of the objects, which is computed during PCA.
     */
    template <typename InputIterator> static void compute(InputIterator begin, InputIterator end,
                                                          ScalarT eigenvalues[N], VectorT eigenvectors[N],
                                                          VectorT * centroid = nullptr);

}; // class PcaN

// Principal component analysis of objects that map to single points in 2-space.
template <typename T, typename ScalarT>
class PcaN<T, 2, ScalarT, typename std::enable_if< IsNonReferencedPointN<T, 2>::value >::type>
{
  public:
    typedef Vector<2, ScalarT> VectorT;

    template <typename InputIterator> static void compute(InputIterator begin, InputIterator end,
                                                          ScalarT eigenvalues[2], VectorT eigenvectors[2],
                                                          VectorT * centroid = nullptr)
    {
      typedef Matrix<2, 2, ScalarT> MatrixT;

      // Compute centroid
      VectorT ctr = CentroidN<T, 2>::compute(begin, end);

      // Construct covariance matrix
      MatrixT cov(0, 0, 0, 0);
      intx n = 0;
      for (auto i = makeRefIterator(begin); i != makeRefIterator(end); ++i, ++n)
      {
        VectorT p = PointTraitsN<T, 2, ScalarT>::getPosition(*i) - ctr;

        for (int r = 0; r < 2; ++r)
          for (int c = 0; c < 2; ++c)
            cov(r, c) += (p[r] * p[c]);
      }

      if (n < 2)
      {
        eigenvalues[0]  = eigenvalues[1]  = 0;
        eigenvectors[0] = eigenvectors[1] = VectorT::Zero();
        return;
      }

      cov /= (n - 1);

      // Find eigenvalues of covariance matrix (TODO: Test this)
      ScalarT trc = cov(0, 0) + cov(1, 1);
      ScalarT det = cov(0, 0) * cov(1, 1) - cov(0, 1) * cov(1, 0);
      ScalarT dsc = 0.25f * trc * trc - det;

      // This should never happen: cov is a real symmetric matrix
      alwaysAssertM(dsc < 0, "Pca2: Covariance matrix has complex eigenvalues");

      ScalarT sqd = std::sqrt(dsc);
      eigenvalues[0] = 0.5f * trc + sqd;
      eigenvalues[1] = 0.5f * trc - sqd;

      if (cov(1, 0) > Math::eps<ScalarT>())
      {
        eigenvectors[0] = VectorT(eigenvalues[0] - cov(1, 1), cov(1, 0)).normalized();
        eigenvectors[1] = VectorT(eigenvalues[2] - cov(1, 1), cov(1, 0)).normalized();
      }
      else
      {
        eigenvectors[0] = VectorT(1, 0);
        eigenvectors[1] = VectorT(0, 1);
      }

      if (centroid)
        *centroid = ctr;
    }

}; // class PcaN<Point2>

// Principal component analysis of objects that map to single points in 3-space.
template <typename T, typename ScalarT>
class PcaN<T, 3, ScalarT, typename std::enable_if< IsNonReferencedPointN<T, 3>::value >::type>
{
  public:
    typedef Vector<3, ScalarT> VectorT;

    template <typename InputIterator> static void compute(InputIterator begin, InputIterator end,
                                                          ScalarT eigenvalues[3], VectorT eigenvectors[3],
                                                          VectorT * centroid = nullptr)
    {
      typedef Matrix<3, 3, ScalarT> MatrixT;

      // Compute centroid
      VectorT ctr = CentroidN<T, 3, ScalarT>::compute(begin, end);

      // Construct covariance matrix
      MatrixT cov = MatrixT::Zero();
      intx n = 0;
      for (auto i = makeRefIterator(begin); i != makeRefIterator(end); ++i, ++n)
      {
        VectorT p = PointTraitsN<T, 3, ScalarT>::getPosition(*i) - ctr;

        for (int r = 0; r < 3; ++r)
          for (int c = 0; c < 3; ++c)
            cov(r, c) += (p[r] * p[c]);
      }

      if (n < 2)
      {
        eigenvalues[0]  = eigenvalues[1]  = eigenvalues[2]  = 0;
        eigenvectors[0] = eigenvectors[1] = eigenvectors[2] = VectorT::Zero();
        return;
      }

      cov /= (n - 1);

      // Find eigenvalues of covariance matrix
      Eigen::SelfAdjointEigenSolver<MatrixT> eigensolver;
      if (eigensolver.computeDirect(cov).info() != Eigen::Success)
        throw Error("Pca3: Could not eigensolve covariance matrix");

      for (int i = 0; i < 3; ++i)
      {
        eigenvalues[i]  = eigensolver.eigenvalues()[i];
        eigenvectors[i] = eigensolver.eigenvectors().col(i).normalized();
      }

      if (centroid)
        *centroid = ctr;
    }

}; // class PcaN<Point3>

} // namespace Algorithms
} // namespace Thea

#endif


/*
   bern_modp_util.cpp:  number-theoretic utility functions

   Copyright (C) 2008, 2009, David Harvey

   This file is part of the bernmm package (version 1.1).

   bernmm is released under a BSD-style license. See the README file in
   the source distribution for details.
*/

#include <NTL/ZZ.h>
#include "bern_modp_util.h"

NTL_CLIENT;

namespace bernmm {

long PowerMod(long a, long ee, long n, mulmod_t ninv)
{
   long x, y;

   unsigned long e;

   if (ee < 0)
      e = - ((unsigned long) ee);
   else
      e = ee;

   x = 1;
   y = a;
   while (e) {
      if (e & 1) x = MulMod(x, y, n, ninv);
      y = MulMod(y, y, n, ninv);
      e = e >> 1;
   }

   if (ee < 0) x = InvMod(x, n);

   return x;
}

void Factorisation::helper(long k, long m)
{
   if (m == 1)
      return;

   for (long i = k + 1; i * i <= m; i++)
   {
      if (m % i == 0)
      {
         // found a factor
         factors.push_back(i);
         // remove that factor entirely
         for (m /= i; m % i == 0; m /= i);
         // recurse
         helper(i, m);
         return;
      }
   }

   // no more factors
   factors.push_back(m);
}

Factorisation::Factorisation(long n)
{
   this->n = n;
   helper(1, n);
}

PrimeTable::PrimeTable(long bound)
{
   long size = (bound - 1) / ULONG_BITS + 1;   // = ceil(bound / ULONG_BITS)
   data.resize(size);

   for (long i = 2; i * i < bound; i++)
      if (is_prime(i))
         for (long j = 2*i; j < bound; j += i)
            set(j);
}

long order(long x, long p, mulmod_t pinv, const Factorisation& F)
{
   // in the loop below, m is always some multiple of the order of x
   long m = p - 1;

   // try to remove factors from m until we can't remove any more
   for (size_t i = 0; i < F.factors.size(); i++)
   {
      long q = F.factors[i];

      while (m % q == 0)
      {
         long mm = m / q;
         if (PowerMod(x, mm, p, pinv) != 1)
            break;
         m = mm;
      }
   }

   return m;
}

long primitive_root(long p, mulmod_t pinv, const Factorisation& F)
{
   if (p == 2)
      return 1;

   long g = 2;
   for (; g < p; g++)
      if (order(g, p, pinv, F) == p - 1)
         return g;

   // no generator exists!?
   abort();
}

};    // end namespace

// end of file ================================================================


#pragma once

// C++ standard library
#include <vector>

// Armadillo
#include <armadillo>

namespace mant {
  arma::mat::fixed<2, 2> rotationMatrix2d(
      const double angle);

  arma::mat::fixed<3, 3> rotationMatrix3d(
      const double rollAngle,
      const double pitchAngle,
      const double yawAngle);

  std::vector<arma::vec::fixed<2>> circleCircleIntersections(
      const arma::vec::fixed<2>& firstCenter,
      const double firstRadius,
      const arma::vec::fixed<2>& secondCenter,
      const double secondRadius);

  std::vector<arma::vec::fixed<3>> circleSphereIntersections(
      const arma::vec::fixed<3>& circleCenter,
      const double circleRadius,
      const arma::vec::fixed<3>& circleNormal,
      const arma::vec::fixed<3>& sphereCenter,
      const double sphereRadius);
}


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2021, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include "GeometricBasics.h"

#include <Eigen/Dense>

#include "BaseLib/Logging.h"
#include "Point3d.h"

namespace MathLib
{
double orientation3d(MathLib::Point3d const& p,
                     MathLib::Point3d const& a,
                     MathLib::Point3d const& b,
                     MathLib::Point3d const& c)
{
    auto const pp = Eigen::Map<Eigen::Vector3d const>(p.getCoords());
    auto const pa = Eigen::Map<Eigen::Vector3d const>(a.getCoords());
    auto const pb = Eigen::Map<Eigen::Vector3d const>(b.getCoords());
    auto const pc = Eigen::Map<Eigen::Vector3d const>(c.getCoords());

    Eigen::Vector3d const u = pp - pa;
    Eigen::Vector3d const v = pp - pb;
    Eigen::Vector3d const w = pp - pc;
    return u.cross(v).dot(w);
}

double calcTetrahedronVolume(MathLib::Point3d const& a,
                             MathLib::Point3d const& b,
                             MathLib::Point3d const& c,
                             MathLib::Point3d const& d)
{
    auto const va = Eigen::Map<Eigen::Vector3d const>(a.getCoords());
    auto const vb = Eigen::Map<Eigen::Vector3d const>(b.getCoords());
    auto const vc = Eigen::Map<Eigen::Vector3d const>(c.getCoords());
    auto const vd = Eigen::Map<Eigen::Vector3d const>(d.getCoords());
    Eigen::Vector3d const w = vb - va;
    Eigen::Vector3d const u = vc - va;
    Eigen::Vector3d const v = vd - va;
    return std::abs(u.cross(v).dot(w)) / 6.0;
}

double calcTriangleArea(MathLib::Point3d const& a, MathLib::Point3d const& b,
                        MathLib::Point3d const& c)
{
    auto const va = Eigen::Map<Eigen::Vector3d const>(a.getCoords());
    auto const vb = Eigen::Map<Eigen::Vector3d const>(b.getCoords());
    auto const vc = Eigen::Map<Eigen::Vector3d const>(c.getCoords());
    Eigen::Vector3d const u = vc - va;
    Eigen::Vector3d const v = vb - va;
    Eigen::Vector3d const w = u.cross(v);
    return 0.5 * w.norm();
}

bool isPointInTetrahedron(MathLib::Point3d const& p, MathLib::Point3d const& a,
                          MathLib::Point3d const& b, MathLib::Point3d const& c,
                          MathLib::Point3d const& d, double eps)
{
    double const d0(MathLib::orientation3d(d, a, b, c));
    // if tetrahedron is not coplanar
    if (std::abs(d0) > std::numeric_limits<double>::epsilon())
    {
        bool const d0_sign(d0 > 0);
        // if p is on the same side of bcd as a
        double const d1(MathLib::orientation3d(d, p, b, c));
        if (!(d0_sign == (d1 >= 0) || std::abs(d1) < eps))
        {
            return false;
        }
        // if p is on the same side of acd as b
        double const d2(MathLib::orientation3d(d, a, p, c));
        if (!(d0_sign == (d2 >= 0) || std::abs(d2) < eps))
        {
            return false;
        }
        // if p is on the same side of abd as c
        double const d3(MathLib::orientation3d(d, a, b, p));
        if (!(d0_sign == (d3 >= 0) || std::abs(d3) < eps))
        {
            return false;
        }
        // if p is on the same side of abc as d
        double const d4(MathLib::orientation3d(p, a, b, c));
        return d0_sign == (d4 >= 0) || std::abs(d4) < eps;
    }
    return false;
}

bool isPointInTriangle(MathLib::Point3d const& p,
                       MathLib::Point3d const& a,
                       MathLib::Point3d const& b,
                       MathLib::Point3d const& c,
                       double eps_pnt_out_of_plane,
                       double eps_pnt_out_of_tri,
                       MathLib::TriangleTest algorithm)
{
    switch (algorithm)
    {
        case MathLib::GAUSS:
            return gaussPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
                                        eps_pnt_out_of_tri);
        case MathLib::BARYCENTRIC:
            return barycentricPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
                                              eps_pnt_out_of_tri);
        default:
            ERR("Selected algorithm for point in triangle testing not found, "
                "falling back on default.");
    }
    return gaussPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
                                eps_pnt_out_of_tri);
}

bool gaussPointInTriangle(MathLib::Point3d const& q,
                          MathLib::Point3d const& a,
                          MathLib::Point3d const& b,
                          MathLib::Point3d const& c,
                          double eps_pnt_out_of_plane,
                          double eps_pnt_out_of_tri)
{
    auto const pa = Eigen::Map<Eigen::Vector3d const>(a.getCoords());
    auto const pb = Eigen::Map<Eigen::Vector3d const>(b.getCoords());
    auto const pc = Eigen::Map<Eigen::Vector3d const>(c.getCoords());
    Eigen::Vector3d const v = pb - pa;
    Eigen::Vector3d const w = pc - pa;

    Eigen::Matrix2d mat;
    mat(0, 0) = v.squaredNorm();
    mat(0, 1) = v[0] * w[0] + v[1] * w[1] + v[2] * w[2];
    mat(1, 0) = mat(0, 1);
    mat(1, 1) = w.squaredNorm();
    Eigen::Vector2d y(
        v[0] * (q[0] - a[0]) + v[1] * (q[1] - a[1]) + v[2] * (q[2] - a[2]),
        w[0] * (q[0] - a[0]) + w[1] * (q[1] - a[1]) + w[2] * (q[2] - a[2]));

    y = mat.partialPivLu().solve(y);

    const double lower(eps_pnt_out_of_tri);
    const double upper(1 + lower);

    if (-lower <= y[0] && y[0] <= upper && -lower <= y[1] && y[1] <= upper &&
        y[0] + y[1] <= upper)
    {
        MathLib::Point3d const q_projected(std::array<double, 3>{
            {a[0] + y[0] * v[0] + y[1] * w[0], a[1] + y[0] * v[1] + y[1] * w[1],
             a[2] + y[0] * v[2] + y[1] * w[2]}});
        if (MathLib::sqrDist(q, q_projected) <= eps_pnt_out_of_plane)
        {
            return true;
        }
    }

    return false;
}

bool barycentricPointInTriangle(MathLib::Point3d const& p,
                                MathLib::Point3d const& a,
                                MathLib::Point3d const& b,
                                MathLib::Point3d const& c,
                                double eps_pnt_out_of_plane,
                                double eps_pnt_out_of_tri)
{
    if (std::abs(MathLib::orientation3d(p, a, b, c)) > eps_pnt_out_of_plane)
    {
        return false;
    }

    auto const vp = Eigen::Map<Eigen::Vector3d const>(p.getCoords());
    auto const va = Eigen::Map<Eigen::Vector3d const>(a.getCoords());
    auto const vb = Eigen::Map<Eigen::Vector3d const>(b.getCoords());
    auto const vc = Eigen::Map<Eigen::Vector3d const>(c.getCoords());
    Eigen::Vector3d const pa = va - vp;
    Eigen::Vector3d const pb = vb - vp;
    Eigen::Vector3d const pc = vc - vp;
    double const area_x_2(calcTriangleArea(a, b, c) * 2);

    double const alpha((pb.cross(pc).norm()) / area_x_2);
    if (alpha < -eps_pnt_out_of_tri || alpha > 1 + eps_pnt_out_of_tri)
    {
        return false;
    }
    double const beta((pc.cross(pa).norm()) / area_x_2);
    if (beta < -eps_pnt_out_of_tri || beta > 1 + eps_pnt_out_of_tri)
    {
        return false;
    }
    double const gamma(1 - alpha - beta);
    return !(gamma < -eps_pnt_out_of_tri || gamma > 1 + eps_pnt_out_of_tri);
}

bool isPointInTriangleXY(MathLib::Point3d const& p,
                         MathLib::Point3d const& a,
                         MathLib::Point3d const& b,
                         MathLib::Point3d const& c)
{
    // criterion: p-a = u0 * (b-a) + u1 * (c-a); 0 <= u0, u1 <= 1, u0+u1 <= 1
    Eigen::Matrix2d mat;
    mat(0, 0) = b[0] - a[0];
    mat(0, 1) = c[0] - a[0];
    mat(1, 0) = b[1] - a[1];
    mat(1, 1) = c[1] - a[1];
    Eigen::Vector2d y;
    y << p[0] - a[0], p[1] - a[1];

    y = mat.partialPivLu().solve(y);

    // check if u0 and u1 fulfills the condition
    return 0 <= y[0] && y[0] <= 1 && 0 <= y[1] && y[1] <= 1 && y[0] + y[1] <= 1;
}

bool dividedByPlane(const MathLib::Point3d& a, const MathLib::Point3d& b,
                    const MathLib::Point3d& c, const MathLib::Point3d& d)
{
    for (unsigned x = 0; x < 3; ++x)
    {
        const unsigned y = (x + 1) % 3;
        const double abc =
            (b[x] - a[x]) * (c[y] - a[y]) - (b[y] - a[y]) * (c[x] - a[x]);
        const double abd =
            (b[x] - a[x]) * (d[y] - a[y]) - (b[y] - a[y]) * (d[x] - a[x]);

        if ((abc > 0 && abd < 0) || (abc < 0 && abd > 0))
        {
            return true;
        }
    }
    return false;
}

bool isCoplanar(const MathLib::Point3d& a, const MathLib::Point3d& b,
                const MathLib::Point3d& c, const MathLib::Point3d& d)
{
    auto const pa = Eigen::Map<Eigen::Vector3d const>(a.getCoords());
    auto const pb = Eigen::Map<Eigen::Vector3d const>(b.getCoords());
    auto const pc = Eigen::Map<Eigen::Vector3d const>(c.getCoords());
    auto const pd = Eigen::Map<Eigen::Vector3d const>(d.getCoords());

    Eigen::Vector3d const ab = pb - pa;
    Eigen::Vector3d const ac = pc - pa;
    Eigen::Vector3d const ad = pd - pa;

    auto const eps_squared =
        std::pow(std::numeric_limits<double>::epsilon(), 2);
    if (ab.squaredNorm() < eps_squared || ac.squaredNorm() < eps_squared ||
        ad.squaredNorm() < eps_squared)
    {
        return true;
    }

    // In exact arithmetic <ac*ad^T, ab> should be zero
    // if all four points are coplanar.
    const double sqr_scalar_triple(std::pow(ac.cross(ad).dot(ab), 2));
    // Due to evaluating the above numerically some cancellation or rounding
    // can occur. For this reason a normalisation factor is introduced.
    const double normalisation_factor =
        (ab.squaredNorm() * ac.squaredNorm() * ad.squaredNorm());

    // tolerance 1e-11 is chosen such that
    // a = (0,0,0), b=(1,0,0), c=(0,1,0) and d=(1,1,1e-6) are considered as
    // coplanar
    // a = (0,0,0), b=(1,0,0), c=(0,1,0) and d=(1,1,1e-5) are considered as not
    // coplanar
    return (sqr_scalar_triple / normalisation_factor < 1e-11);
}

}  // end namespace MathLib


/*
 * Copyright Nick Thompson, 2017
 * 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)
 */

#define BOOST_TEST_MODULE chebyshev_test

#include <boost/type_index.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/chebyshev.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/array.hpp>

using boost::multiprecision::cpp_bin_float_quad;
using boost::multiprecision::cpp_bin_float_50;
using boost::multiprecision::cpp_bin_float_100;
using boost::math::chebyshev_t;
using boost::math::chebyshev_t_prime;
using boost::math::chebyshev_u;

template<class Real>
void test_polynomials()
{
    std::cout << "Testing explicit polynomial representations of the Chebyshev polynomials on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";

    Real x = -2;
    Real tol = 100*std::numeric_limits<Real>::epsilon();
    if (tol > std::numeric_limits<float>::epsilon())
       tol *= 10;   // float results have much larger error rates.
    while (x < 2)
    {
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_t(0, x), Real(1), tol);
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_t(1, x), x, tol);
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_t(2, x), 2*x*x - 1, tol);
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_t(3, x), x*(4*x*x-3), tol);
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_t(4, x), 8*x*x*(x*x - 1) + 1, tol);
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_t(5, x), x*(16*x*x*x*x - 20*x*x + 5), tol);
        x += 1/static_cast<Real>(1<<7);
    }

    x = -2;
    tol = 10*tol;
    while (x < 2)
    {
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_u(0, x), Real(1), tol);
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_u(1, x), 2*x, tol);
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_u(2, x), 4*x*x - 1, tol);
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_u(3, x), 4*x*(2*x*x - 1), tol);
        x += 1/static_cast<Real>(1<<7);
    }
}

template<class Real>
void test_derivatives()
{
    std::cout << "Testing explicit polynomial representations of the Chebyshev polynomial derivatives on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";

    Real x = -2;
    Real tol = 1000*std::numeric_limits<Real>::epsilon();
    while (x < 2)
    {
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_t_prime(0, x), Real(0), tol);
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_t_prime(1, x), Real(1), tol);
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_t_prime(2, x), 4*x, tol);
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_t_prime(3, x), 3*(4*x*x - 1), tol);
        BOOST_CHECK_CLOSE_FRACTION(chebyshev_t_prime(4, x), 16*x*(2*x*x - 1), tol);
        // This one makes the tolerance have to grow too large; the Chebyshev recurrence is more stable than naive polynomial evaluation anyway.
        //BOOST_CHECK_CLOSE_FRACTION(chebyshev_t_prime(5, x), 5*(4*x*x*(4*x*x - 3) + 1), tol);
        x += 1/static_cast<Real>(1<<7);
    }
}

template<class Real>
void test_clenshaw_recurrence()
{
    using boost::math::chebyshev_clenshaw_recurrence;
    boost::array<Real, 5> c0 = { {2, 0, 0, 0, 0} };
    // Check the size = 1 case:
    boost::array<Real, 1> c01 = { {2} };
    // Check the size = 2 case:
    boost::array<Real, 2> c02 = { {2, 0} };
    boost::array<Real, 4> c1 = { {0, 1, 0, 0} };
    boost::array<Real, 4> c2 = { {0, 0, 1, 0} };
    boost::array<Real, 5> c3 = { {0, 0, 0, 1, 0} };
    boost::array<Real, 5> c4 = { {0, 0, 0, 0, 1} };
    boost::array<Real, 6> c5 = { {0, 0, 0, 0, 0, 1} };
    boost::array<Real, 7> c6 = { {0, 0, 0, 0, 0, 0, 1} };

    Real x = -1;
    Real tol = 10*std::numeric_limits<Real>::epsilon();
    if (tol > std::numeric_limits<float>::epsilon())
       tol *= 100;   // float results have much larger error rates.
    while (x <= 1)
    {
        Real y = chebyshev_clenshaw_recurrence(c0.data(), c0.size(), x);
        BOOST_CHECK_CLOSE_FRACTION(y, chebyshev_t(0, x), tol);

        y = chebyshev_clenshaw_recurrence(c01.data(), c01.size(), x);
        BOOST_CHECK_CLOSE_FRACTION(y, chebyshev_t(0, x), tol);

        y = chebyshev_clenshaw_recurrence(c02.data(), c02.size(), x);
        BOOST_CHECK_CLOSE_FRACTION(y, chebyshev_t(0, x), tol);

        y = chebyshev_clenshaw_recurrence(c1.data(), c1.size(), x);
        BOOST_CHECK_CLOSE_FRACTION(y, chebyshev_t(1, x), tol);

        y = chebyshev_clenshaw_recurrence(c2.data(), c2.size(), x);
        BOOST_CHECK_CLOSE_FRACTION(y, chebyshev_t(2, x), tol);

        y = chebyshev_clenshaw_recurrence(c3.data(), c3.size(), x);
        BOOST_CHECK_CLOSE_FRACTION(y, chebyshev_t(3, x), tol);

        y = chebyshev_clenshaw_recurrence(c4.data(), c4.size(), x);
        BOOST_CHECK_CLOSE_FRACTION(y, chebyshev_t(4, x), tol);

        y = chebyshev_clenshaw_recurrence(c5.data(), c5.size(), x);
        BOOST_CHECK_CLOSE_FRACTION(y, chebyshev_t(5, x), tol);

        y = chebyshev_clenshaw_recurrence(c6.data(), c6.size(), x);
        BOOST_CHECK_CLOSE_FRACTION(y, chebyshev_t(6, x), tol);

        x += static_cast<Real>(1)/static_cast<Real>(1 << 7);
    }
}

BOOST_AUTO_TEST_CASE(chebyshev_test)
{
    test_clenshaw_recurrence<float>();
    test_clenshaw_recurrence<double>();
    test_clenshaw_recurrence<long double>();

    test_polynomials<float>();
    test_polynomials<double>();
    test_polynomials<long double>();
    test_polynomials<cpp_bin_float_quad>();

    test_derivatives<float>();
    test_derivatives<double>();
    test_derivatives<long double>();
    test_derivatives<cpp_bin_float_quad>();
}


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2015 Scientific Computing and Imaging Institute,
   University of Utah.

   License for the specific language governing rights and limitations under
   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.
*/

#include <gtest/gtest.h>

#include <Eigen/Dense>

typedef Eigen::MatrixXd DenseMatrix;
typedef std::complex<double> Complex;
typedef Eigen::Matrix2cd ComplexDenseMatrix;

namespace
{
  DenseMatrix matrix1()
  {
    DenseMatrix m(3, 3);
    for (int i = 0; i < m.rows(); ++ i)
      for (int j = 0; j < m.cols(); ++ j)
        m(i, j) = 3.0 * i + j;
    return m;
  }
  DenseMatrix matrixNonSquare()
  {
    DenseMatrix m (3, 4);
    for (int i = 0; i < m.rows(); ++ i)
      for (int j = 0; j < m.cols(); ++ j)
        m(i, j) = 3.0 * i + j;
    return m;
  }
  const DenseMatrix Zero(DenseMatrix::Zero(3,3));
}

#define PRINT_MATRIX(x) //std::cout << #x << " = \n" << (x) << std::endl

TEST(EigenDenseMatrixTest, CanCreateBasicMatrix)
{
  DenseMatrix m(matrix1());
  PRINT_MATRIX(m);
}

TEST(EigenDenseMatrixTest, CanPrintInLegacyFormat)
{
  DenseMatrix m(matrix1());
  std::ostringstream o;
  o << 0.5*m;
  std::string legacy = o.str();
  EXPECT_EQ("  0 0.5   1\n1.5   2 2.5\n  3 3.5   4", legacy);
}

TEST(EigenDenseMatrixTest, CanDetermineSize)
{
  DenseMatrix m(matrixNonSquare());
  EXPECT_EQ(3, m.rows());
  EXPECT_EQ(4, m.cols());
}

TEST(EigenDenseMatrixTest, CanCopyConstruct)
{
  DenseMatrix m(matrixNonSquare());
  DenseMatrix m2(m);
  EXPECT_EQ(m, m2);
  m(1,2) += 1;
  EXPECT_NE(m, m2);
}

TEST(EigenDenseMatrixTest, CanAssign)
{
  DenseMatrix m(matrixNonSquare());

  DenseMatrix m2(3,4);
  m2.setZero();
  EXPECT_NE(m, m2);
  m2 = m;
  EXPECT_EQ(m, m2);
  m(1,2) += 1;
  EXPECT_NE(m, m2);
}

TEST(EigenDenseMatrixUnaryOperationTests, CanNegate)
{
  DenseMatrix m(matrix1());

  PRINT_MATRIX(m);
  PRINT_MATRIX(-m);

  DenseMatrix n = - -m;
  EXPECT_EQ(m, n);
  EXPECT_EQ(m + (-m), Zero);
}

TEST(EigenDenseMatrixUnaryOperationTests, CanScalarMultiply)
{
  DenseMatrix m(matrix1());

  PRINT_MATRIX(m);
  PRINT_MATRIX(2*m);
  PRINT_MATRIX(m*2);
  EXPECT_EQ(2*m, m*2);
}

TEST(EigenDenseMatrixUnaryOperationTests, CanTranspose)
{
  DenseMatrix m(matrix1());

  PRINT_MATRIX(m);
  PRINT_MATRIX(m.transpose());

  EXPECT_EQ(m, m.transpose().transpose());
}

TEST(EigenDenseMatrixBinaryOperationTests, CanMultiply)
{
  DenseMatrix m(matrix1());

  PRINT_MATRIX(m);
  PRINT_MATRIX(m * m);
  EXPECT_EQ(Zero * m, Zero);
}

TEST(EigenDenseMatrixBinaryOperationTests, CanAdd)
{
  DenseMatrix m(matrix1());

  PRINT_MATRIX(m);
  PRINT_MATRIX(m + m);
  EXPECT_EQ(m + m, 2*m);
}

TEST(EigenDenseMatrixBinaryOperationTests, CanSubtract)
{
  DenseMatrix m(matrix1());

  PRINT_MATRIX(m);
  PRINT_MATRIX(m - m);
  EXPECT_EQ(m - m, Zero);
}

TEST(EigenDenseComplexMatrixTests, CanMultiply)
{
  ComplexDenseMatrix A, B, C_expect, C_actual;

  A << Complex(1, 1), Complex(2, 3),
         Complex(3, 2), Complex(4, 4);
  B << Complex(4, 4), Complex(3, 2),
         Complex(2, 3), Complex(1, 1);
  C_expect << Complex(-5, 20), Complex(0, 10),
                Complex(0, 40), Complex(5, 20);

  C_actual = A * B;

  ASSERT_TRUE(C_actual.isApprox(C_expect, 1e-6));
}

namespace std
{
  bool operator<(const Complex& lhs, const Complex& rhs)
  {
    return norm(lhs) < norm(rhs);
  }
}

TEST(EigenDenseComplexMatrixTests, MinMax)
{
  ComplexDenseMatrix A, B, C;

  A << Complex(1, 1), Complex(2, 3),
    Complex(3, 2), Complex(4, 4);
  B << Complex(4, 4), Complex(3, 2),
    Complex(2, 3), Complex(-1, -1);
  C << Complex(0, 0), Complex(1, 0), Complex(-1, 0), Complex(0.5,0.5);

  EXPECT_EQ(Complex(1, 1), A.minCoeff());
  EXPECT_EQ(Complex(4, 4), A.maxCoeff());
  EXPECT_EQ(Complex(-1, -1), B.minCoeff());
  EXPECT_EQ(Complex(4, 4), B.maxCoeff());
  EXPECT_EQ(Complex(0, 0), C.minCoeff());
  EXPECT_EQ(Complex(-1, 0), C.maxCoeff());
}


#ifndef CANNON_MATH_BARYCENTRIC_COORDS_H
#define CANNON_MATH_BARYCENTRIC_COORDS_H

/*!
 * \file cannon/math/barycentric_coords.hpp
 * \brief File containing free functions relating to barycentric coordinate
 * calculation.
 */

/*!
 * \namespace cannon::math
 * \brief Namespace containing mathematical utilities.
 */

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace math {

    /*!
     * \brief Function to compute barycentric coordinates for a query point on
     * triangle defined by its three vertices. See
     * https://en.wikipedia.org/wiki/Barycentric_coordinate_system
     *
     * \param v0 First vertex
     * \param v1 Second vertex
     * \param v2 Third vertex
     * \param p Query point
     *
     * \returns Barycentric coordinates for the query point.
     */
    Vector3d compute_barycentric_coords(const Vector3d& v0, const Vector3d& v1,
        const Vector3d& v2, const Vector3d& p);

  } // namespace math
} // namespace cannon
#endif /* ifndef CANNON_MATH_BARYCENTRIC_COORDS_H */


#include "gen_relpose_5p1pt.h"

#include "PoseLib/misc/essential.h"
#include "PoseLib/solvers/relpose_5pt.h"

#include <Eigen/Dense>

namespace poselib {

int gen_relpose_5p1pt(const std::vector<Eigen::Vector3d> &p1, const std::vector<Eigen::Vector3d> &x1,
                      const std::vector<Eigen::Vector3d> &p2, const std::vector<Eigen::Vector3d> &x2,
                      std::vector<CameraPose> *output) {

    output->clear();
    relpose_5pt(x1, x2, output);

    for (size_t k = 0; k < output->size(); ++k) {
        CameraPose &pose = (*output)[k];

        // the translation is given by
        //  t = p2 - R_5pt*p1 + gamma * t_5pt = a + gamma * b

        // we need to solve for gamma using our extra correspondence
        //  R * (p1 + lambda_1 * x1) + a + gamma * b = lambda_2 * x2 + p2

        Eigen::Matrix3d R = pose.R();
        Eigen::Vector3d a = p2[0] - R * p1[0];
        Eigen::Vector3d b = pose.t;

        // vector used to eliminate lambda1 and lambda2
        Eigen::Vector3d w = x2[5].cross(R * x1[5]);

        const double c0 = w.dot(p2[5] - R * p1[5] - a);
        const double c1 = w.dot(b);

        const double gamma = c0 / c1;

        pose.t = a + gamma * b;
        // TODO: Cheirality check for the last point
    }

    return output->size();
}

} // namespace poselib


/*
 * COPYRIGHT AND PERMISSION NOTICE
 * UCSD Software ORCVIO
 * Copyright (C) 2021
 * All rights reserved.
 */

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

#include <Eigen/Dense>

#include "orcvio/utils.h"

using namespace std;
using namespace Eigen;
using namespace orcvio;

TEST(MathUtilsTest, skewSymmetric) {
    Vector3d w(1.0, 2.0, 3.0);
    Matrix3d w_hat = utils::skewSymmetric(w);
    Vector3d zero_vector = w_hat * w;

    FullPivLU<Matrix3d> lu_helper(w_hat);
    EXPECT_EQ(lu_helper.rank(), 2);
    EXPECT_DOUBLE_EQ(zero_vector.norm(), 0.0);
    return;
}

int main(int argc, char** argv) {
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


//! [mandelbrot-all]
#include <chrono>
#include <iostream>
#include <numeric>
#include <vector>

#include <boost/simd/pack.hpp>
#include <boost/simd/memory/allocator.hpp>

#include <boost/simd/function/aligned_load.hpp>
#include <boost/simd/function/aligned_store.hpp>
#include <boost/simd/function/if_inc.hpp>
#include <boost/simd/function/enumerate.hpp>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

typedef float T;
namespace bs = boost::simd;
using pack_t = bs::pack<T>;
using pack_i = bs::pack<int>;
using pack_l = bs::as_logical_t<pack_t>;

struct mandelbrot{
  int size, max_iter;
  T x_min, x_max, y_min, y_max, x_range, y_range;
  std::vector<int, bs::allocator<int>> iterations;

  mandelbrot(int size_, int max_iter_) : size(size_), max_iter(max_iter_){
    x_min = -2.5;
    x_max = 1;
    y_min = -1;
    y_max = 1;
    x_range = x_max - x_min;
    y_range = y_max - y_min;
    iterations.resize(size * size);
  }

  void evaluate_scalar(){
    for(int i = 0; i < size; ++i){
      T x0 = T(i) / T(size) * x_range + x_min;
      for(int j = 0; j < size; ++j){
	int iteration  = 0;
	T y0 = T(j) / T(size) * y_range + y_min;
	T x = 0;
	T y = 0;
	while(x * x + y * y < 4 && iteration < max_iter){
	  T x_temp = x * x - y * y + x0;
	  y = 2 * x * y + y0;
	  x = x_temp;
	  ++iteration ;
	}
	iterations[j + i * size] = iteration;
      }
    }
  }

  //! [mandelbrot-simd]
  void evaluate_simd(){
    pack_t step =  bs::enumerate<pack_t>(0);
    for(int i = 0; i < size; ++i){
      pack_t x0 {T(i) / T(size) * x_range + x_min};
      pack_t fac { y_range / T(size) };
      pack_t y_min_t {y_min};
      for(int j = 0; j < size; j += pack_t::static_size){
	int iteration  = 0;

	pack_t y0 = (step + j )* fac + y_min_t;
	pack_t x {0};
	pack_t y {0};
	pack_i iter {0};
	pack_l mask;
	do{
	  pack_t x2 = x * x;
	  pack_t y2 = y * y;

	  y = 2 * x * y + y0;
	  x = x2 - y2 + x0;
	  mask = x2 + y2 < 4;
	  ++iteration;
          iter = bs::if_inc(mask, iter);
	} while(bs::any(mask) && iteration < max_iter);
	bs::aligned_store(iter, &iterations[j + i * size]);
      }
    }
  }
  //! [mandelbrot-simd]

  void display() {
    cv::Mat display;
    cv::Mat A(size, size, CV_32SC1, iterations.data());
    A.convertTo(display, CV_8UC1, 255.0 / 1000.0);
    cv::applyColorMap(display, display, cv::COLORMAP_JET);
    cv::imshow("Mandelbrot", display);
    cv::waitKey(0);
  }
};

int main(int argc, char** argv)
{
  namespace chr = std::chrono;
  using hrc = chr::high_resolution_clock;

  int size = 1024;
  int max_iteration = 1000;
  mandelbrot image(size, max_iteration);
  auto t0 = hrc::now();
  image.evaluate_scalar();
  auto t1 = hrc::now();
  std::cout << " scalar " << chr::duration_cast<chr::milliseconds>(t1 - t0).count() << std::endl;

  t0 = hrc::now();
  image.evaluate_simd();
  t1 = hrc::now();
  std::cout << " simd " << chr::duration_cast<chr::milliseconds>(t1 - t0).count() << std::endl;
  image.display();
}
//! [mandelbrot-all]


// Compiled on https://wandbox.org/
#include "utils.hpp"

#include <iostream>
#include <algorithm>
#include <boost/math/special_functions.hpp>

#define K 10
#define PI (acos(-1) / 2.0)

void bessels()
{
	double v = rand_between<double>(-K, K);
	double x = rand_between<double>(-K, K);
	unsigned int n = rand_int(0, K);

	double i = boost::math::cyl_bessel_i((x < 0) ? floor(v) : v, x);
	double j = boost::math::cyl_bessel_j((x < 0) ? floor(v) : v, x);
	double k = boost::math::cyl_bessel_k(v, abs(x)); // x > 0
	double neumann = boost::math::cyl_neumann(v, abs(x)); // x > 0

	double sph = boost::math::sph_bessel(n, abs(x)); // unsigned int, x > 0
	double sph_nm = boost::math::sph_neumann(n, abs(x)); // unsigned int, x > 0

	print("cyl_bessel_i", (x < 0) ? floor(v) : v, x, i);
	print("cyl_bessel_j", (x < 0) ? floor(v) : v, x, j);
	print("cyl_bessel_k", v, abs(x), k);
	print("cyl_neumann", v, abs(x), neumann);

	print("sph_bessel", n, abs(x), sph);
	print("sph_neumann", n, abs(x), sph_nm);
}

void betas()
{
	double x = rand_between<double>(0, K);
	double y = rand_between<double>(0, K);

	print("beta", x, y, boost::math::beta(x, y));
}

void ellints()
{
	double k = rand_between<double>(-1, 1);
	double phi = rand_between<double>(-PI, PI);
	double v = rand_between<double>(0, 1 / (pow(sin(phi), 2.0)));

	double first = boost::math::ellint_1(k, phi);
	double second = boost::math::ellint_2(k, phi);
	double third = boost::math::ellint_3(k, v, phi);

	print("ellint_1", k, phi, first);
	print("ellint_2", k, phi, second);
	print("ellint_3", k, v, phi, third);
}

void expints()
{
	double x = rand_between<double>(-K, K);

	print("expint", x, boost::math::expint(x));
}

void hermites()
{
	unsigned int n = rand_int(0, K);
	double x = rand_between<double>(-K, K);

	print("hermite", n, x, boost::math::hermite(n, x));
}

void laguerres()
{
	unsigned int n = rand_int(0, K);
	unsigned int m = rand_int(0, K);
	double x = rand_between<double>(-K, K);

	double ret = boost::math::laguerre(n, m, x);
	print("assoc_laguerre", n, m, x, ret);
}

void legendres()
{
	unsigned int n = rand_int(0, K);
	unsigned int m = rand_int(0, K);
	double x = rand_between<double>(-1, 1);

	double ret = boost::math::legendre_p(n, m, x);
	print("assoc_legendre", n, m, x, ret);
}

void zetas()
{
	double x = rand_between<double>(-K, K);

	print("riemann_zeta", x, boost::math::zeta(x));
}

int main()
{
	std::cout << "[" << std::endl;
	{
		repeat(bessels);
		repeat(betas);
		repeat(ellints);
		repeat(expints);
		repeat(hermites);
		repeat(laguerres);
		repeat(legendres);
		repeat(zetas);
		std::cout << "\t[\"clamp\", 1, 6, 5, 5]" << std::endl;
	};
	std::cout << "]" << std::endl;

	system("pause");
	return 0;
}


// test_regression.cpp
// (c) Tivole

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

namespace numerary
{

    BOOST_AUTO_TEST_SUITE(TestRegression)

    double get_rand(double min_val, double max_val) {
        /*
            Returns random double value from interval [min_val, max_val]
        */
        return (max_val - min_val) * ( (double)rand() / (double)RAND_MAX ) + min_val;
    }

    BOOST_AUTO_TEST_CASE(test_linear_regression)
    {
        const int N = 101;
        const double eps = 1.e-2;
        int i;
        double a, b, k, c, step, x, *X = new double[N], *Y = new double[N], *predicted_kc = new double[2];

        k = -3;
        c = 2;

        // segment [a, b]
        a = 0;
        b = 1;

        // Generating random values for regression
        step = (b - a) / double(N - 1);
        for (i = 0; i < N; i++, x += step) {
            X[i] = rand() % 2 == 0 ? x + eps: x - eps;
            Y[i] = rand() % 2 == 0 ? (k * X[i] + c) + eps * get_rand(1, 3) : (k * X[i] + c) - eps * get_rand(1, 3);
        }

        // Get predicted linear regression line
        predicted_kc = Numerary::linear_regression(X, Y, N);

        BOOST_CHECK(fabs(predicted_kc[0] - k) < 1.e-3);
        BOOST_CHECK(fabs(predicted_kc[1] - c) < 1.e-3);

        delete[] X;
        delete[] Y;
        delete[] predicted_kc;
    }

    BOOST_AUTO_TEST_SUITE_END()
}


//
// ExpansionHunter
// Copyright (c) 2020 Illumina, Inc.
//
// Author: Konrad Scheffler <kscheffler@illumina.com>
//
// 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.
//

// These utility functions were copied from source code of Strelka Small Variant Caller.

#pragma once

#include <boost/math/special_functions/log1p.hpp>

// returns log(1+x), switches to log1p function when abs(x) is small
static double log1p_switch(const double x)
{
    // TODO Justify this switch point. Related discussion:
    // http://cran.r-project.org/web/packages/Rmpfr/vignettes/log1mexp-note.pdf
    static const double smallx_thresh(0.01);

    if (std::abs(x) < smallx_thresh)
    {
        return boost::math::log1p(x);
    }
    else
    {
        return std::log(1 + x);
    }
}

// Returns the equivalent of log(exp(x1)+exp(x2))
static double getLogSum(double x1, double x2)
{
    if (x1 < x2)
        std::swap(x1, x2);
    return x1 + log1p_switch(std::exp(x2 - x1));
}


#include "iostream"
using namespace std;

#include <ctime>
// include Core and Dense compuation parts from Eigen
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

# define MATRIX_SIZE 50

/**************************************************
 * Exercise 5
 * Suppose there is a large Eigen matrix, we want to know the value in the top
 * left 3 x 3 blocks, and then assign it to Identity 3×3.
 * *************************************************/

int main(int argc, char** argv){
    // the large matrix
    Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);

    // Extract the top-left 3*3
    Matrix3d matrix_33;
    for (int i = 0; i < 3; i++){
        for (int j = 0; j < 3; j++){
            matrix_33(i, j) = matrix_NN(i, j);
        }
    }
    // print it out
    cout << "The top left 3*3 matrix is: " << endl << matrix_33 << endl;

    // assign an Identity to it
    for (int i = 0; i < 3; i++){
        for (int j = 0; j < 3; j++){
            if(i == j){
                matrix_NN(i, j) = 1;
            }
            else{
                matrix_NN(i, j) = 0;
            }
        }
    }

}


#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/affine.hpp>
#include <kv/autodif.hpp>
#include <kv/interval-vector.hpp>

/*
 * comparison of interval, affine, mean value form
 * in evaluation of a 5 variable function.
 * The function is taken from http://ci.nii.ac.jp/naid/110003291707
 */

namespace ub = boost::numeric::ublas;
typedef kv::interval<double> itv;

struct Func {
	template <class T> T operator()(const ub::vector<T>& t) {
		T f1, f2, y;

		f1 = (-10. - 13.2 * t[0] + 0.5 * t[1] - 0.4 * t[2] + 7.3 * t[3] + 4.4 * t[4]);
		f2 = (0.8 + 10.5 * t[0] + 5.9 * t[1] + 7.5 * t[2] - 14.5 * t[3] + 1.3 * t[4]);

		y = f1 * f2 - ((-113.7) * f2 + (-49.8) * f1) + 5660.;
		return y;
	}
};

int main()
{
	int i;
	ub::vector<itv> I;
	Func f;

	std::cout.precision(17);

	I.resize(5);

	I[0] = itv(8.7, 8.8);
	I[1] = itv(-9.4, -9.3);
	I[2] = itv(-4.6, -4.5);
	I[3] = itv(3.5, 3.6);
	I[4] = itv(-2.9, -2.8);

	// interval arithmetic

	std::cout << "interval: " << f(I) << "\n";

	// affine arithmetic

	// ub::vector< kv::affine<double> > a;
	ub::vector< kv::affine<itv::base_type> > a;

	a.resize(5);
	for (i=0; i<5; i++) a(i) = I(i);
	std::cout << "affine: " << to_interval(f(a)) << "\n";

	// mean value form

	ub::vector<itv> c, fdi;
	itv fc, fi;

	c = mid(I);
	fc = f(c);
	kv::autodif<itv>::split(f(kv::autodif<itv>::init(I)), fi, fdi);

	std::cout << "mvf: " << fc + inner_prod(fdi, I - c) << "\n";
}


/*
 * Copyright (c) 2012 Aldebaran Robotics. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the COPYING file.
 */

#include <almath/tools/almath.h>
#include <almath/tools/altrigonometry.h>

#include <cmath>
#include <boost/math/special_functions/pow.hpp>
#include <boost/algorithm/clamp.hpp>

namespace AL
{
  namespace Math
  {

    void modulo2PIInPlace(float& pAngle)
    {
      pAngle = std::fmod(pAngle, AL::Math::_2_PI_);

      if (pAngle>AL::Math::PI)
      {
        pAngle = pAngle - AL::Math::_2_PI_;
      }
      else if (pAngle<-AL::Math::PI)
      {
        pAngle = pAngle + AL::Math::_2_PI_;
      }
    }

    float modulo2PI(float pAngle)
    {
      float result = pAngle;
      modulo2PIInPlace(result);
      return result;
    }

    float meanAngle(const std::vector<float> &pAngles) {
      const std::vector<float> weights(pAngles.size(), 1.f);
      return weightedMeanAngle(pAngles, weights);
    }

    float weightedMeanAngle(const std::vector<float> &pAngles,
                            const std::vector<float> &pWeights) {
      if (pAngles.size() != pWeights.size()) {
        throw std::runtime_error("Angles and weights must have the same size.");
      }
      Math::Position2D sumPosition;
      std::vector<float>::const_iterator a = pAngles.begin();
      std::vector<float>::const_iterator w = pWeights.begin();
      float weightSum = 0.f;
      for (; a != pAngles.end(); ++a, ++w) {
        if (*w <= 0.f) {
          throw std::runtime_error("All weights must be strictly positive.");
        }
        weightSum += *w;
        sumPosition += *w * Math::Position2D(std::cos(*a), std::sin(*a));
      }
      if (sumPosition.norm() < 1e-3f) {
        throw std::runtime_error("No defined mean.");
      }
      return Math::modulo2PI(std::atan2(sumPosition.y, sumPosition.x));
    }

    bool clipData(
      const float& pMin,
      const float& pMax,
      float& pData)
    {
      const float saved = pData;
      pData = boost::algorithm::clamp(saved, pMin, pMax);
      return saved != pData;
    }

    bool clipData(
      const float& pMin,
      const float& pMax,
      std::vector<float>& pData)
    {
      bool isClipped = false;
      for (unsigned int i=0; i<pData.size(); ++i)
      {
        if (clipData(pMin, pMax, pData[i]))
        {
          isClipped = true;
        }
      }
      return isClipped;
    }

    bool clipData(
      const float& pMin,
      const float& pMax,
      std::vector<std::vector<float> >& pData)
    {
      bool isClipped = false;
      for (unsigned int i=0; i<pData.size(); ++i)
      {
        if (clipData(pMin, pMax, pData[i]))
        {
          isClipped = true;
        }
      }
      return isClipped;
    }

    void changeReferencePose2D(
        const float&  pTheta,
        const Pose2D& pPosIn,
        Pose2D&       pPosOut)
    {
      const float cs = std::cos(pTheta);
      const float sn = std::sin(pTheta);
      pPosOut.x = cs*pPosIn.x - sn*pPosIn.y;
      pPosOut.y = sn*pPosIn.x + cs*pPosIn.y;
      pPosOut.theta = pPosIn.theta;
    }

    void changeReferencePose2DInPlace(
        const float& pTheta,
        Pose2D&      pPosOut)
    {
      const float cs = std::cos(pTheta);
      const float sn = std::sin(pTheta);
      const float xOld = pPosOut.x;
      const float yOld = pPosOut.y;

      pPosOut.x = cs*xOld - sn*yOld;
      pPosOut.y = sn*xOld + cs*yOld;
    }

    Position3D position3DFromPosition6D(const Position6D& pPosition6D)
    {
      return Position3D(pPosition6D.x, pPosition6D.y, pPosition6D.z);
    }

    Position6D position6DFromVelocity6D(const Velocity6D& pVel)
    {
      return Position6D(
            pVel.xd,
            pVel.yd,
            pVel.zd,
            pVel.wxd,
            pVel.wyd,
            pVel.wzd);
    }

    Position3D operator*(
      const Rotation&   pRot,
      const Position3D& pPos)
    {
      return Position3D(
            pRot.r1_c1*pPos.x + pRot.r1_c2*pPos.y + pRot.r1_c3*pPos.z,
            pRot.r2_c1*pPos.x + pRot.r2_c2*pPos.y + pRot.r2_c3*pPos.z,
            pRot.r3_c1*pPos.x + pRot.r3_c2*pPos.y + pRot.r3_c3*pPos.z);
    }

    Position3D operator*(
        const Quaternion& pQuat,
        const Position3D& pPos)
    {
      Position3D result;
      result.x = pQuat.w * pQuat.w * pPos.x + 2.0f *pQuat.y * pQuat.w * pPos.z;
      result.x += - 2.0f * pQuat.z * pQuat.w * pPos.y + pQuat.x * pQuat.x * pPos.x;
      result.x += 2.0f * pQuat.y * pQuat.x * pPos.y + 2.0f * pQuat.z * pQuat.x * pPos.z;
      result.x += - pQuat.z * pQuat.z * pPos.x - pQuat.y * pQuat.y * pPos.x;

      result.y = 2.0f * pQuat.x * pQuat.y * pPos.x + pQuat.y * pQuat.y * pPos.y;
      result.y += 2.0f * pQuat.z * pQuat.y * pPos.z + 2.0f * pQuat.w * pQuat.z * pPos.x;
      result.y += - pQuat.z * pQuat.z * pPos.y + pQuat.w * pQuat.w * pPos.y;
      result.y += - 2.0f * pQuat.x * pQuat.w * pPos.z - pQuat.x * pQuat.x * pPos.y;

      result.z = 2.0f * pQuat.x * pQuat.z * pPos.x + 2.0f * pQuat.y * pQuat.z * pPos.y;
      result.z += pQuat.z * pQuat.z * pPos.z - 2.0f * pQuat.w * pQuat.y * pPos.x;
      result.z += - pQuat.y * pQuat.y * pPos.z + 2.0f * pQuat.w * pQuat.x * pPos.y;
      result.z += - pQuat.x * pQuat.x * pPos.z + pQuat.w * pQuat.w * pPos.z;
      return result;
    }

    Velocity6D operator*(
      const float       pVal,
      const Position6D& pPos)
    {
      return Velocity6D(
            pVal * pPos.x,
            pVal * pPos.y,
            pVal * pPos.z,
            pVal * pPos.wx,
            pVal * pPos.wy,
            pVal * pPos.wz);
    }

    Velocity3D operator*(
      const float       pVal,
      const Position3D& pPos)
    {
      return Velocity3D(
            pVal * pPos.x,
            pVal * pPos.y,
            pVal * pPos.z);
    }

    Velocity3D operator*(
      const Rotation&   pRot,
      const Velocity3D& pVel)
    {
      return Velocity3D(
            pRot.r1_c1*pVel.xd + pRot.r1_c2*pVel.yd + pRot.r1_c3*pVel.zd,
            pRot.r2_c1*pVel.xd + pRot.r2_c2*pVel.yd + pRot.r2_c3*pVel.zd,
            pRot.r3_c1*pVel.xd + pRot.r3_c2*pVel.yd + pRot.r3_c3*pVel.zd);
    }

    AL::Math::Rotation rotationFromAngleDirection(
      const float&                pTheta,
      const AL::Math::Position3D& pPos)
    {
      return AL::Math::rotationFromAngleDirection(
            pTheta,
            pPos.x,
            pPos.y,
            pPos.z);
    }

    void position2DFromPose2DInPlace(
        const Pose2D& pPose2D,
        Position2D&   pPosition2D)
    {
      pPosition2D.x = pPose2D.x;
      pPosition2D.y = pPose2D.y;
    }

    Position2D position2DFromPose2D(const Pose2D& pPose2D)
    {
      return Position2D(pPose2D.x, pPose2D.y);
    }

    void position6DFromPose2DInPlace(
        const Pose2D& pPose2D,
        Position6D&   pPosition6D)
    {
      pPosition6D.x  = pPose2D.x;
      pPosition6D.y  = pPose2D.y;
      pPosition6D.z  = 0.0f;
      pPosition6D.wx = 0.0f;
      pPosition6D.wy = 0.0f;
      pPosition6D.wz = pPose2D.theta;
    }

    Position6D position6DFromPose2D(const Pose2D& pPose2D)
    {
      return Position6D(
            pPose2D.x,
            pPose2D.y,
            0.0f,
            0.0f,
            0.0f,
            pPose2D.theta);
    }

    void position6DFromPosition3DInPlace(
        const Position3D& pPosition3D,
        Position6D&       pPosition6D)
    {
      pPosition6D.x  = pPosition3D.x;
      pPosition6D.y  = pPosition3D.y;
      pPosition6D.z  = pPosition3D.z;
      pPosition6D.wx = 0.0f;
      pPosition6D.wy = 0.0f;
      pPosition6D.wz = 0.0f;
    }

    Position6D position6DFromPosition3D(const Position3D& pPosition3D)
    {
      return Position6D(pPosition3D.x,
                        pPosition3D.y,
                        pPosition3D.z,
                        0.0f,
                        0.0f,
                        0.0f);
    }

    void pose2DFromPosition6DInPlace(
        const Position6D& pPosition6D,
        Pose2D&           pPose2D)
    {
      pPose2D.x     = pPosition6D.x;
      pPose2D.y     = pPosition6D.y;
      pPose2D.theta = pPosition6D.wz;
    }

    Pose2D pose2DFromPosition6D(const Position6D& pPosition6D)
    {
      return Pose2D(pPosition6D.x, pPosition6D.y, pPosition6D.wz);
    }

    void pose2DFromPosition2DInPlace(
        const Position2D& pPosition2D,
        const float       pAngle,
        Pose2D&           pPose2D)
    {
      pPose2D.x     = pPosition2D.x;
      pPose2D.y     = pPosition2D.y;
      pPose2D.theta = pAngle;
    }

    Pose2D pose2DFromPosition2D(const Position2D& pPosition2D,
                                const float       pAngle)
    {
      return Pose2D(pPosition2D.x, pPosition2D.y, pAngle);
    }

    Position2D operator*(
      const Pose2D&     pVal,
      const Position2D& pPos)
    {
      return Position2D(
            pVal.x + std::cos(pVal.theta)*pPos.x - std::sin(pVal.theta)*pPos.y,
            pVal.y + std::sin(pVal.theta)*pPos.x + std::cos(pVal.theta)*pPos.y);
    }

    void quaternionFromRotation3D(
        const Rotation3D& pRot3D,
        Quaternion& pQuaternion)
    {
      const float cx = std::cos(0.5f*pRot3D.wx);
      const float cy = std::cos(0.5f*pRot3D.wy);
      const float cz = std::cos(0.5f*pRot3D.wz);
      const float sx = std::sin(0.5f*pRot3D.wx);
      const float sy = std::sin(0.5f*pRot3D.wy);
      const float sz = std::sin(0.5f*pRot3D.wz);

      pQuaternion.w = cz * cy * cx + sz * sy * sx;
      pQuaternion.x = cz * cy * sx - sz * sy * cx;
      pQuaternion.y = cz * sy * cx + cy * sz * sx;
      pQuaternion.z = cy * sz * cx - cz * sy * sx;
    }

    Quaternion quaternionFromRotation3D(
        const Rotation3D& pRot3D)
    {
      Quaternion lQuat;
      quaternionFromRotation3D(pRot3D, lQuat);
      return lQuat;
    }

    void rotationFromQuaternion(
        const Quaternion& pQua,
        Rotation& pRot)
    {
      pRot.r1_c1 = 1.0f - 2.0f*(std::pow(pQua.y, 2) + std::pow(pQua.z, 2));
      pRot.r1_c2 = 2.0f*(pQua.x*pQua.y - pQua.z*pQua.w);
      pRot.r1_c3 = 2.0f*(pQua.x*pQua.z + pQua.y*pQua.w);

      pRot.r2_c1 = 2.0f*(pQua.x*pQua.y + pQua.z*pQua.w);
      pRot.r2_c2 = 1.0f - 2.0f*(std::pow(pQua.x, 2) + std::pow(pQua.z, 2));
      pRot.r2_c3 = 2.0f*(pQua.y*pQua.z - pQua.x*pQua.w);

      pRot.r3_c1 = 2.0f*(pQua.x*pQua.z - pQua.y*pQua.w);
      pRot.r3_c2 = 2.0f*(pQua.y*pQua.z + pQua.x*pQua.w);
      pRot.r3_c3 = 1.0f - 2.0f*(std::pow(pQua.x, 2) + std::pow(pQua.y, 2));
    }

    Rotation rotationFromQuaternion(
        const Quaternion& pQua)
    {
      Rotation lRot;
      rotationFromQuaternion(pQua, lRot);
      return lRot;
    }

    void rotation3DFromQuaternion(
        const AL::Math::Quaternion& pQuaternion,
        AL::Math::Rotation3D& pRot3D)
    {
      //rotationFromQuaternion
      AL::Math::Rotation lRot;
      rotationFromQuaternion(pQuaternion, lRot);

      pRot3D.wz = std::atan2(lRot.r2_c1, lRot.r1_c1);
      const float sy = std::sin(pRot3D.wz);
      const float cy = std::cos(pRot3D.wz);
      pRot3D.wy = std::atan2(-lRot.r3_c1, cy*lRot.r1_c1+sy*lRot.r2_c1);
      pRot3D.wx = std::atan2(sy*lRot.r1_c3-cy*lRot.r2_c3, cy*lRot.r2_c2-sy*lRot.r1_c2);
    }

    Rotation3D rotation3DFromQuaternion(
        const Quaternion& pQuaternion)
    {
      Rotation3D lRotation3D;
      rotation3DFromQuaternion(pQuaternion, lRotation3D);
      return lRotation3D;
    }

    void quaternionPosition3DFromPosition6D(
        const Position6D& pPos6D,
        Quaternion& pQua,
        Position3D& pPos3D)
    {
      quaternionFromRotation3D(
            AL::Math::Rotation3D(pPos6D.wx, pPos6D.wy, pPos6D.wz),
            pQua);

      pPos3D.x = pPos6D.x;
      pPos3D.y = pPos6D.y;
      pPos3D.z = pPos6D.z;
    }

    void pointMassRotationalInertia(
        float pMass,
        const Position3D& pPos,
        std::vector<float>& pInertia)
    {
      const float x2 = boost::math::pow<2>(pPos.x);
      const float y2 = boost::math::pow<2>(pPos.y);
      const float z2 = boost::math::pow<2>(pPos.z);
      pInertia.resize(9);
      pInertia[0] = pMass * (y2 + z2);
      pInertia[4] = pMass * (x2 + z2);
      pInertia[8] = pMass * (x2 + y2);
      pInertia[1] = pInertia[3] = - pMass * pPos.x * pPos.y;
      pInertia[2] = pInertia[6] = - pMass * pPos.x * pPos.z;
      pInertia[5] = pInertia[7] = - pMass * pPos.y * pPos.z;
    }
  } // namespace Math
} // namespace AL


// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007

// 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)

// test_lognormal.cpp

#include <boost/math/concepts/real_concept.hpp> // for real_concept
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp> // Boost.Test
#include <boost/test/tools/floating_point_comparison.hpp>

#include <boost/math/distributions/lognormal.hpp>
    using boost::math::lognormal_distribution;
#include <boost/math/tools/test.hpp>
#include "test_out_of_range.hpp"

#include <iostream>
   using std::cout;
   using std::endl;
   using std::setprecision;
#include <limits>
  using std::numeric_limits;
#include <cassert>

template <class RealType>
void check_lognormal(RealType loc, RealType scale, RealType x, RealType p, RealType q, RealType tol)
{
   BOOST_CHECK_CLOSE(
      ::boost::math::cdf(
         lognormal_distribution<RealType>(loc, scale),       // distribution.
         x),                                            // random variable.
         p,                                             // probability.
         tol);                                          // %tolerance.
   BOOST_CHECK_CLOSE(
      ::boost::math::cdf(
         complement(
            lognormal_distribution<RealType>(loc, scale),    // distribution.
            x)),                                        // random variable.
         q,                                             // probability complement.
         tol);                                          // %tolerance.
   BOOST_CHECK_CLOSE(
      ::boost::math::quantile(
         lognormal_distribution<RealType>(loc, scale),       // distribution.
         p),                                            // probability.
         x,                                             // random variable.
         tol);                                          // %tolerance.
   BOOST_CHECK_CLOSE(
      ::boost::math::quantile(
         complement(
            lognormal_distribution<RealType>(loc, scale),    // distribution.
            q)),                                        // probability complement.
         x,                                             // random variable.
         tol);                                          // %tolerance.
}

template <class RealType>
void test_spots(RealType)
{

   // Basic sanity checks.
   RealType tolerance = 5e-3 * 100;
   // Some tests only pass at 1e-4 because values generated by
   // http://faculty.vassar.edu/lowry/VassarStats.html
   // give only 5 or 6 *fixed* places, so small values have fewer digits.

   cout << "Tolerance for type " << typeid(RealType).name()  << " is " << tolerance << " %" << endl;

   using std::exp;

   //
   // These test values were generated for the normal distribution
   // using the online calculator at http://faculty.vassar.edu/lowry/VassarStats.html
   // and then exponentiating the random variate x.
   //

   check_lognormal(
      static_cast<RealType>(0),     // location
      static_cast<RealType>(5),     // scale
      static_cast<RealType>(1),     // x
      static_cast<RealType>(0.5),   // p
      static_cast<RealType>(0.5),   // q
      tolerance);
   check_lognormal(
      static_cast<RealType>(2),     // location
      static_cast<RealType>(2),     // scale
      static_cast<RealType>(exp(1.8)),     // x
      static_cast<RealType>(0.46017),   // p
      static_cast<RealType>(1-0.46017),   // q
      tolerance);
   check_lognormal(
      static_cast<RealType>(2),     // location
      static_cast<RealType>(2),     // scale
      static_cast<RealType>(exp(2.2)),     // x
      static_cast<RealType>(1-0.46017),   // p
      static_cast<RealType>(0.46017),   // q
      tolerance);
   check_lognormal(
      static_cast<RealType>(2),     // location
      static_cast<RealType>(2),     // scale
      static_cast<RealType>(exp(-1.4)),     // x
      static_cast<RealType>(0.04457),   // p
      static_cast<RealType>(1-0.04457),   // q
      tolerance);
   check_lognormal(
      static_cast<RealType>(2),     // location
      static_cast<RealType>(2),     // scale
      static_cast<RealType>(exp(5.4)),     // x
      static_cast<RealType>(1-0.04457),   // p
      static_cast<RealType>(0.04457),   // q
      tolerance);

   check_lognormal(
      static_cast<RealType>(-3),     // location
      static_cast<RealType>(5),     // scale
      static_cast<RealType>(exp(-5.0)),     // x
      static_cast<RealType>(0.34458),   // p
      static_cast<RealType>(1-0.34458),   // q
      tolerance);
   check_lognormal(
      static_cast<RealType>(-3),     // location
      static_cast<RealType>(5),     // scale
      static_cast<RealType>(exp(-1.0)),     // x
      static_cast<RealType>(1-0.34458),   // p
      static_cast<RealType>(0.34458),   // q
      tolerance);
   check_lognormal(
      static_cast<RealType>(-3),     // location
      static_cast<RealType>(5),     // scale
      static_cast<RealType>(exp(-9.0)),     // x
      static_cast<RealType>(0.11507),   // p
      static_cast<RealType>(1-0.11507),   // q
      tolerance);
   check_lognormal(
      static_cast<RealType>(-3),     // location
      static_cast<RealType>(5),     // scale
      static_cast<RealType>(exp(3.0)),     // x
      static_cast<RealType>(1-0.11507),   // p
      static_cast<RealType>(0.11507),   // q
      tolerance);

   //
   // Tests for PDF
   //
   tolerance = boost::math::tools::epsilon<RealType>() * 5 * 100; // 5 eps as a percentage
   cout << "Tolerance for type " << typeid(RealType).name()  << " is " << tolerance << " %" << endl;
   BOOST_CHECK_CLOSE(
      pdf(lognormal_distribution<RealType>(), static_cast<RealType>(1)),
      static_cast<RealType>(0.3989422804014326779399460599343818684759L), // 1/sqrt(2*pi)
      tolerance);
   BOOST_CHECK_CLOSE(
      pdf(lognormal_distribution<RealType>(3), exp(static_cast<RealType>(3))),
      static_cast<RealType>(0.3989422804014326779399460599343818684759L) / exp(static_cast<RealType>(3)),
      tolerance);
   BOOST_CHECK_CLOSE(
      pdf(lognormal_distribution<RealType>(3, 5), exp(static_cast<RealType>(3))),
      static_cast<RealType>(0.3989422804014326779399460599343818684759L / (5 * exp(static_cast<RealType>(3)))),
      tolerance);
   //
   // Spot checks for location = -5, scale = 6,
   // use relation to normal to test:
   //
   for(RealType x = -15; x < 5; x += 0.125)
   {
      BOOST_CHECK_CLOSE(
         pdf(lognormal_distribution<RealType>(-5, 6), exp(x)),
         pdf(boost::math::normal_distribution<RealType>(-5, 6), x) / exp(x),
         tolerance);
   }

   //
   // These test values were obtained by punching numbers into
   // a calculator, using the formulas at http://mathworld.wolfram.com/LogNormalDistribution.html
   //
   tolerance = (std::max)(
      boost::math::tools::epsilon<RealType>(),
      static_cast<RealType>(boost::math::tools::epsilon<double>())) * 5 * 100; // 5 eps as a percentage
   cout << "Tolerance for type " << typeid(RealType).name()  << " is " << tolerance << " %" << endl;
   lognormal_distribution<RealType> dist(8, 3);
   RealType x = static_cast<RealType>(0.125);

   BOOST_MATH_STD_USING // ADL of std math lib names.

   // mean:
   BOOST_CHECK_CLOSE(
      mean(dist)
      , static_cast<RealType>(268337.28652087445695647967378715L), tolerance);
   // variance:
   BOOST_CHECK_CLOSE(
      variance(dist)
      , static_cast<RealType>(583389737628117.49553037857325892L), tolerance);
   // std deviation:
   BOOST_CHECK_CLOSE(
    standard_deviation(dist)
    , static_cast<RealType>(24153462.228594009489719473727471L), tolerance);
   // hazard:
   BOOST_CHECK_CLOSE(
    hazard(dist, x)
    , pdf(dist, x) / cdf(complement(dist, x)), tolerance);
   // cumulative hazard:
   BOOST_CHECK_CLOSE(
    chf(dist, x)
    , -log(cdf(complement(dist, x))), tolerance);
   // coefficient_of_variation:
   BOOST_CHECK_CLOSE(
    coefficient_of_variation(dist)
    , standard_deviation(dist) / mean(dist), tolerance);
   // mode:
   BOOST_CHECK_CLOSE(
    mode(dist)
    , static_cast<RealType>(0.36787944117144232159552377016146L), tolerance);

   BOOST_CHECK_CLOSE(
    median(dist)
    , static_cast<RealType>(exp(dist.location())), tolerance);

   BOOST_CHECK_CLOSE(
    median(dist),
    quantile(dist, static_cast<RealType>(0.5)), tolerance);

   // skewness:
   BOOST_CHECK_CLOSE(
    skewness(dist)
    , static_cast<RealType>(729551.38304660255658441529235697L), tolerance);
   // kurtosis:
   BOOST_CHECK_CLOSE(
    kurtosis(dist)
    , static_cast<RealType>(4312295840576303.2363383232038251L), tolerance);
   // kurtosis excess:
   BOOST_CHECK_CLOSE(
    kurtosis_excess(dist)
    , static_cast<RealType>(4312295840576300.2363383232038251L), tolerance);

   RealType expected_entropy = 8 + log(boost::math::constants::two_pi<RealType>()*boost::math::constants::e<RealType>()*9)/2;
   BOOST_CHECK_CLOSE(
    entropy(dist)
    , expected_entropy, tolerance);

   BOOST_CHECK_CLOSE(
    range(dist).first
    , static_cast<RealType>(0), tolerance);

   //
   // Special cases:
   //
   BOOST_CHECK(pdf(dist, 0) == 0);
   BOOST_CHECK(cdf(dist, 0) == 0);
   BOOST_CHECK(cdf(complement(dist, 0)) == 1);
   BOOST_CHECK(quantile(dist, 0) == 0);
   BOOST_CHECK(quantile(complement(dist, 1)) == 0);

   //
   // Error checks:
   //
   BOOST_MATH_CHECK_THROW(lognormal_distribution<RealType>(0, 0), std::domain_error);
   BOOST_MATH_CHECK_THROW(lognormal_distribution<RealType>(2, -1), std::domain_error);
   BOOST_MATH_CHECK_THROW(pdf(dist, -1), std::domain_error);
   BOOST_MATH_CHECK_THROW(cdf(dist, -1), std::domain_error);
   BOOST_MATH_CHECK_THROW(cdf(complement(dist, -1)), std::domain_error);
   BOOST_MATH_CHECK_THROW(quantile(dist, 1), std::overflow_error);
   BOOST_MATH_CHECK_THROW(quantile(complement(dist, 0)), std::overflow_error);
   check_out_of_range<lognormal_distribution<RealType> >(1, 2);

} // template <class RealType>void test_spots(RealType)

BOOST_AUTO_TEST_CASE( test_main )
{

   // Check that can generate lognormal distribution using the two convenience methods:
   boost::math::lognormal myf1(1., 2); // Using typedef
   lognormal_distribution<> myf2(1., 2); // Using default RealType double.

  // Test range and support using double only,
  // because it supports numeric_limits max for a pseudo-infinity.
  BOOST_CHECK_EQUAL(range(myf2).first, 0); // range 0 to +infinity
  BOOST_CHECK_EQUAL(range(myf2).second, (std::numeric_limits<double>::max)());
  BOOST_CHECK_EQUAL(support(myf2).first, 0); // support 0 to + infinity.
  BOOST_CHECK_EQUAL(support(myf2).second, (std::numeric_limits<double>::max)());

    // Basic sanity-check spot values.
   // (Parameter value, arbitrarily zero, only communicates the floating point type).
  test_spots(0.0F); // Test float. OK at decdigits = 0 tolerance = 0.0001 %
  test_spots(0.0); // Test double. OK at decdigits 7, tolerance = 1e07 %
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  test_spots(0.0L); // Test long double.
#if !BOOST_WORKAROUND(BOOST_BORLANDC, BOOST_TESTED_AT(0x0582))
  test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::endl;
#endif

} // BOOST_AUTO_TEST_CASE( test_main )

/*
Running 1 test case...
Tolerance for type float is 0.5 %
Tolerance for type float is 5.96046e-005 %
Tolerance for type float is 5.96046e-005 %
Tolerance for type double is 0.5 %
Tolerance for type double is 1.11022e-013 %
Tolerance for type double is 1.11022e-013 %
Tolerance for type long double is 0.5 %
Tolerance for type long double is 1.11022e-013 %
Tolerance for type long double is 1.11022e-013 %
Tolerance for type class boost::math::concepts::real_concept is 0.5 %
Tolerance for type class boost::math::concepts::real_concept is 1.11022e-013 %
Tolerance for type class boost::math::concepts::real_concept is 1.11022e-013 %
*** No errors detected
*/


#include "exception.hh"
#include "network.hh"
#include "timer.hh"

#include <Eigen/Dense>
#include <iostream>
#include <random>
#include <utility>

#include <sys/resource.h>
#include <sys/time.h>

using namespace std;
using namespace Eigen;

constexpr size_t batch_size = 1;
constexpr size_t input_size = 1;

/* use squared error as loss function */
float loss_function( const float target, const float actual )
{
  return ( target - actual ) * ( target - actual );
}

/* partial derivative of loss with respect to neural network output */
float compute_pd_loss_wrt_output( const float target, const float actual )
{
  return -2 * ( target - actual );
}

/* actual function we want the neural network to learn */
float true_function( const float input )
{
  return 3 * input + 1;
}

constexpr float learning_rate = 0.001;

void program_body()
{
  /* remove limit on stack size */
  const rlimit limits { RLIM_INFINITY, RLIM_INFINITY };
  CheckSystemCall( "setrlimit", setrlimit( RLIMIT_STACK, &limits ) );

  /* seed C RNG for Eigen random weight initialization */
  srand( Timer::timestamp_ns() );

  /* construct neural network on heap */
  auto nn = make_unique<Network<float, batch_size, input_size, 1>>();
  nn->layer0.weights()( 0 ) = 1;
  nn->layer0.biases()( 0 ) = 1;

  double x_value = rand() % 30;
  // while ( true ) {
  for ( auto i = 0; i < 100; i++ ) {
    cout << "Weight = " << nn->layer0.weights()( 0 ) << ", bias = " << nn->layer0.biases()( 0 ) << "\n";

    /* step 1: construct a unique problem instance */
    Matrix<float, batch_size, input_size> input, ground_truth_output;

    input( 0, 0 ) = x_value;
    ground_truth_output( 0, 0 ) = true_function( x_value );

    // x_value++; /* use a different problem instance next time */
    x_value = rand() % 40;

    cout << "problem instance: " << input( 0, 0 ) << " => " << ground_truth_output( 0, 0 ) << "\n";

    /* step 2: forward propagate and calculate loss function */
    nn->apply( input );

    cout << "NN maps " << input( 0, 0 ) << " => " << nn->output()( 0, 0 ) << "\n";

    cout << "loss when " << ground_truth_output( 0, 0 ) << " desired, " << nn->output()( 0, 0 )
         << " produced = " << loss_function( nn->output()( 0, 0 ), ground_truth_output( 0, 0 ) ) << "\n";

    /* step 3: backpropagate error */
    nn->computeDeltas();
    nn->evaluateGradients( input );

    const float pd_loss_wrt_output
      = compute_pd_loss_wrt_output( ground_truth_output( 0, 0 ), nn->output()( 0, 0 ) );

    /* perturb weight */
    nn->layer0.weights()( 0 ) -= learning_rate * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 0 );
    nn->layer0.biases()( 0 ) -= learning_rate * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 1 );
    // nn -> layer0.weights()(0) -= learning_rate * pd_loss_wrt_output;
    // nn -> layer0.biases()(0) -= learning_rate * pd_loss_wrt_output * nn->layer0.biases()(0);

    cout << "\n";

    cout << "pd_loss_wrt_output = " << pd_loss_wrt_output << "\n";
    cout << "pd_output_wrt_weight = " << nn->getEvaluatedGradient( 0, 0 ) << "\n";
    cout << "pd_output_wrt_bias = " << nn->getEvaluatedGradient( 0, 1 ) << "\n";

    cout << "\n";

    cout << "perturb weight: " << -pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 0 ) << "\n";
    cout << "perturb bias: " << -pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 1 ) << "\n";

    cout << "\n";
  }

#if 0
  /* input and output */
  for ( int i = 0; i < 10; i++ ) {
    Matrix<float, batch_size, input_size> in, out;
    int v = rand() % 100 + 1;
    in( 0 ) = (float)v;
    out( 0 ) = 3 * v + 1 + (float)rand() / ( RAND_MAX );
    input.push_back( in );
    output.push_back( out );
  }
  for ( int round = 0; round < 100; round++ ) {
    for ( int i = 0; i < 10; i++ ) {
      nn->apply( input[i] );
      cout << "right after apply: " << input[i]( 0 ) << "  " << output[i]( 0 ) << endl;

      // cout << "output: " << nn->output() << "\n";

      // const float loss = ( 100 - nn->output()( 0 ) ) * ( 100 - nn->output()( 0 ) );
      const float loss = getLoss( output[i]( 0 ), nn->output()( 0 ) );

      nn->computeDeltas();
      nn->evaluateGradients( input[i] );

      // const float pd_loss_output = -2 * ( 100 - nn->output()( 0 ) );
      const float pd_loss_output = get_pd_loss_output( output[i]( 0 ), nn->output()( 0 ) );

      cout << "partial derivative of output wrt weight = " << nn->getEvaluatedGradient( 0, 0 ) << "\n";
      cout << "partial derivative of output wrt bias = " << nn->getEvaluatedGradient( 0, 1 ) << "\n";

      cout << "loss: " << loss << "\n";

      cout << "partial derivative of loss wrt output = " << pd_loss_output << "\n";

      cout << "partial derivative of loss wrt weight = " << pd_loss_output * nn->getEvaluatedGradient( 0, 0 )
           << "\n";
      cout << "partial derivative of loss wrt bias = " << pd_loss_output * nn->getEvaluatedGradient( 0, 1 ) << "\n";
      cout << "weight = " << nn->layer0.weights()( 0 ) << endl;
      cout << "biase = " << nn->layer0.biases()( 0 ) << endl;

      const float learning_rate = .0001;

      nn->layer0.weights()( 0 ) *= ( 1 - learning_rate * pd_loss_output * nn->getEvaluatedGradient( 0, 0 ) );
      nn->layer0.biases()( 0 ) *= ( 1 - learning_rate * pd_loss_output * nn->getEvaluatedGradient( 0, 1 ) );
    }
  }

  // testing
  cout << "start testing results" << endl;
  Matrix<float, batch_size, input_size> test_input, test_output;
  for ( int i = 0; i < 10; i++ ) {
    int v = rand() % 100 + 1;
    test_input( 0 ) = (float)v;
    test_output( 0 ) = 3 * v + 1;
    cout << "input: " << test_input( 0 ) << ", output: " << test_output( 0 ) << endl;
    nn->apply( test_input );
    auto nnOutput = nn->output();
    cout << "nn output: " << nnOutput << endl;
  }
#endif
}

int main( int argc, char*[] )
{
  try {
    if ( argc <= 0 ) {
      abort();
    }

    program_body();

    return EXIT_SUCCESS;
  } catch ( const exception& e ) {
    cerr << e.what() << "\n";
    return EXIT_FAILURE;
  }
}