#include "include/MMVII_all.h"
#include <boost/math/special_functions/fpclassify.hpp>

namespace MMVII
{

// return the variance of  exponential distribution of parameter "a" ( i.e proportiona to  "a^|x|")
double Sigma2FromFactExp(double a)
{
   return (2*a) / Square(1-a);
}

// return the value of "a" such that  exponential distribution of parameter "a" has variance S2
// i.e. this the "inverse" function of Sigma2FromFactExp

double FactExpFromSigma2(double aS2)
{
    return (aS2+1 - sqrt(Square(aS2+1)-Square(aS2))  ) / aS2 ;
}

/* *************************************** */
/*                                         */
/*        cComputeStdDev                   */
/*                                         */
/* *************************************** */

template <class Type> cComputeStdDev<Type>::cComputeStdDev() :
   mSomW   (0.0),
   mSomWV  (0.0),
   mSomWV2 (0.0)
{
}

template <class Type> void cComputeStdDev<Type>::Add(const Type & aW,const Type & aV)
{
    mSomW   += aW;
    mSomWV  += aW *aV;
    mSomWV2 += aW * Square(aV);
}

template <class Type> void cComputeStdDev<Type>::SelfNormalize()
{
    MMVII_ASSERT_INVERTIBLE_VALUE(mSomW);

    mSomWV  /= mSomW;
    mSomWV2  /= mSomW;
    mSomWV2 -= Square(mSomWV);
    mStdDev = std::sqrt(std::max(Type(0.0),mSomWV2));
}

template <class Type> Type cComputeStdDev<Type>::NormalizedVal(const Type & aVal)  const
{
    MMVII_ASSERT_INVERTIBLE_VALUE(mStdDev);
    return (aVal-mSomWV) / mStdDev;
}

template <class Type> cComputeStdDev<Type>  cComputeStdDev<Type>::Normalize() const
{
     cComputeStdDev<Type> aRes = *this;
     aRes.SelfNormalize();
     return aRes;
}

/* ============================================= */
/*      cMatIner2Var<Type>                       */
/* ============================================= */

template <class Type> cMatIner2Var<Type>::cMatIner2Var() :
   mS0  (0.0),
   mS1  (0.0),
   mS11 (0.0),
   mS2  (0.0),
   mS12 (0.0),
   mS22 (0.0)
{
}
template <class Type> void cMatIner2Var<Type>::Add(const double & aPds,const Type & aV1,const Type & aV2)
{
    mS0  += aPds;
    mS1  += aPds * aV1;
    mS11 += aPds * aV1 * aV1 ;
    mS2  += aPds * aV2;
    mS12 += aPds * aV1 * aV2 ;
    mS22 += aPds * aV2 * aV2 ;
}

template <class Type> void cMatIner2Var<Type>::Normalize()
{
     MMVII_ASSERT_INVERTIBLE_VALUE(mS0);

     mS1 /= mS0;
     mS2 /= mS0;
     mS11 /= mS0;
     mS12 /= mS0;
     mS22 /= mS0;
     mS11 -= Square(mS1);
     mS12 -= mS1 * mS2;
     mS22 -= mS2 * mS2;
}

template <class Type> cMatIner2Var<double> StatFromImageDist(const cDataIm2D<Type> & aIm)
{
    cMatIner2Var<double> aRes;
    for (const auto & aP : aIm)
    {
         aRes.Add(aIm.GetV(aP),aP.x(),aP.y());
    }
    aRes.Normalize();
    return aRes;
}

#define INSTANTIATE_MAT_INER(TYPE)\
template class cMatIner2Var<TYPE>;\
template  class cComputeStdDev<TYPE>;\
template  cMatIner2Var<double> StatFromImageDist(const cDataIm2D<TYPE> & aIm);

INSTANTIATE_MAT_INER(tREAL4)
INSTANTIATE_MAT_INER(tREAL8)
INSTANTIATE_MAT_INER(tREAL16)

/* *********************************************** */
/*                                                 */
/*        cUB_ComputeStdDev<Dim>                   */
/*                                                 */
/* *********************************************** */

template  <const int Dim> cUB_ComputeStdDev<Dim>::cUB_ComputeStdDev() :
    mSomW   (0.0),
    mSomWW  (0.0)
{
    for (int aD=0 ; aD<Dim ; aD++)
    {
        mSomWV[aD] = 0.0;
        mSomWVV[aD] = 0.0;
    }
}

template  <const int Dim> void cUB_ComputeStdDev<Dim>::Add(const  double *  aVal,const double & aPds)
{
    mSomW += aPds;
    mSomWW += Square(aPds);
    for (int aD=0 ; aD<Dim ; aD++)
    {
        mSomWV[aD] += aPds * aVal[aD];
        mSomWVV[aD] += aPds * Square(aVal[aD]);
    }
}
template  <const int Dim>  double cUB_ComputeStdDev<Dim>::DeBiasFactor() const
{
    MMVII_INTERNAL_ASSERT_strong(mSomW>0,"No value in DeBiasFactor");
    return   1 - mSomWW/Square(mSomW);
}

template  <const int Dim> bool    cUB_ComputeStdDev<Dim>::OkForUnBiasedVar() const
{
   return (mSomW>0)  && (DeBiasFactor()!=0);
}

template  <const int Dim> const double *   cUB_ComputeStdDev<Dim>::ComputeUnBiasedVar()
{
    /* At least, this formula is correct :
         - when all weight are equal => 1-1/N
         - when all but one weight are 0 => 0
         - and finally if all are equal or 0
    */
    double aDebias = DeBiasFactor();
    MMVII_INTERNAL_ASSERT_strong(aDebias>0,"No var can be computed in ComputeVar");

    for (int aD=0 ; aD<Dim ; aD++)
    {
        double aAver = mSomWV[aD] / mSomW;
        double aVar  = mSomWVV[aD] / mSomW;
        aVar = aVar - Square(aAver);
        mVar[aD] = std::max(0.0,aVar) /aDebias;
    }

   return mVar;
}

template  <const int Dim> const double *   cUB_ComputeStdDev<Dim>::ComputeBiasedVar()
{
    for (int aD=0 ; aD<Dim ; aD++)
    {
        double aAver = mSomWV[aD] / mSomW;
        double aVar  = mSomWVV[aD] / mSomW;
        aVar = aVar - Square(aAver);
        mBVar[aD] = std::max(0.0,aVar);
    }

   return mBVar;
}

template class cUB_ComputeStdDev<1>;

void BenchUnbiasedStdDev()
{
    for (int aNbTest = 0 ; aNbTest<1000 ; aNbTest++)
    {
         int aNbVar = 1 + RandUnif_N(3);
         int aNbTir = aNbVar;
         int aNbComb = pow(aNbVar,aNbTir);
         std::vector<double> aVecVals = VRandUnif_0_1(aNbVar);
         std::vector<double> aVecWeight = VRandUnif_0_1(aNbVar);
// aVecWeight =  std::vector<double> (aNbVar,1.0);

         // compute the average of all empirical variance
         double aMoyVar=0;
         for (int aFlag=0 ; aFlag < aNbComb ; aFlag++) // Explore all combinaison
         {
             cUB_ComputeStdDev<1> aUBS;
             for (int aVar=0 ; aVar < aNbVar ; aVar++) // All Variable of this realization
             {
                 int aNumVar = (aFlag / round_ni(pow(aNbVar,aVar))) % aNbVar; // "Majic" formula to exdtrac p-adic decomp
                 aUBS.Add(&(aVecVals.at(aNumVar)),aVecWeight.at(aNumVar));
             }
             aMoyVar += aUBS.ComputeBiasedVar()[0];
         }
         aMoyVar /= aNbComb;

         cUB_ComputeStdDev<1> aUBS;
StdOut() << "WWW=" ;
         for (int aK=0 ; aK<aNbVar ; aK++)
         {
StdOut() << " " << aVecWeight.at(aK) ;
            aUBS.Add(&(aVecVals.at(aK)),aVecWeight.at(aK));
         }
StdOut() << "\n" ;
         StdOut() << "MOYVAR " << aMoyVar <<  " " <<  aUBS.DeBiasFactor() *  aUBS.ComputeBiasedVar()[0]
                  << " DBF=" <<  aUBS.DeBiasFactor() << "\n";
getchar();
    }
}

/* *********************************************** */
/*                                                 */
/*            Test Exp Filtre                      */
/*                                                 */
/* *********************************************** */

// Test Sigma2FromFactExp
template <class Type> void TestVarFilterExp(cPt2di aP0,cPt2di aP1,const Type & aV0,double aFx,double aFy,int aNbIt)
{
   cPt2di aPMil = (aP0+aP1) / 2;
   cRect2 aRect2(aP0,aP1);

   cIm2D<Type> aIm(aP0,aP1,nullptr,eModeInitImage::eMIA_Null);
   cDataIm2D<Type> & aDIm = aIm.DIm();

   // 1- Make 1 iteration of expon filter on a Direc, check variance and aver
   aDIm.InitDirac(aPMil,aV0);

   ExponentialFilter(true,aDIm,aNbIt,aRect2,aFx,aFy);
   cMatIner2Var<double>  aMat = StatFromImageDist(aDIm);

   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S1() - aPMil.x()) < 1e-5  ,"Average Exp")
   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S2() - aPMil.y()) < 1e-5  ,"Average Exp")

   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S11()-Sigma2FromFactExp(aFx) * aNbIt)<1e-5,"Var Exp");
   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S22()-Sigma2FromFactExp(aFy) * aNbIt)<1e-5,"Var Exp");
}

template <class Type> void TestVarFilterExp(cPt2di aSz,double aStdDev,int aNbIter,double aEps)
{
   cIm2D<Type> aIm(cPt2di(0,0),aSz,nullptr,eModeInitImage::eMIA_Null);
   cDataIm2D<Type> & aDIm = aIm.DIm();
   cPt2di aPMil = aSz / 2;
   double aV0 = 2.0;

   // 1- Make 1 iteration of expon filter on a Direc, check variance and aver
   aDIm.InitDirac(aPMil,aV0);

   ExpFilterOfStdDev(aDIm,aNbIter,aStdDev);
   cMatIner2Var<double>  aMat = StatFromImageDist(aDIm);

   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S11()-Square(aStdDev))<aEps,"Std dev");
   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S22()-Square(aStdDev))<aEps,"Std dev");
}

void BenchStat()
{
   TestVarFilterExp<double>(cPt2di(-2,2),cPt2di(400,375),2.0,0.6,0.67,1);
   TestVarFilterExp<double>(cPt2di(-2,2),cPt2di(400,375),2.0,0.6,0.67,3);

   // Test Sigma2FromFactExp
   for (int aK=1 ; aK<100 ; aK++)
   {
        double aS2 = aK / 5.0;
        double aF = FactExpFromSigma2(aS2);
        {
           // Check both formula are inverse of each others
           double aCheckS2 = Sigma2FromFactExp(aF);
           MMVII_INTERNAL_ASSERT_bench(std::abs( aS2 - aCheckS2 ) < 1e-5  ,"Sigma2FromFactExp")

        }
        {
           // Check formula Sigma2FromFactExp on exponential filters
           TestVarFilterExp<double>(cPt2di(0,0),cPt2di(4000,3),2.0,aF,0,1);
        }
   }

   TestVarFilterExp<double>(cPt2di(300,300),2.0,2,1e-6);
   TestVarFilterExp<float>(cPt2di(300,300),5.0,3,1e-3);
}

};


#include <math_lib/vec_base.h>

#include <math_lib/intersection.h>

#include <boost/qvm/vec_operations.hpp>

#include <gtest/gtest.h>

using namespace pagoda;

TEST(PlaneVsPlaneIntersectionTest, when_the_planes_intersect_should_return_a_line_intersection)
{
	Plane<float> p1(Vec3F{1, 0, 0}, 0);
	Plane<float> p2(Vec3F{0, 1, 0}, 0);

	auto i = intersection(p1, p2);
	EXPECT_EQ(i.m_type, PlaneIntersectionType::Type::Intersection);
	EXPECT_EQ(boost::qvm::dot(i.m_intersection.GetSupportVector(), p1.GetNormal()), 0);
	EXPECT_EQ(boost::qvm::dot(i.m_intersection.GetSupportVector(), p2.GetNormal()), 0);
}

TEST(PlaneVsPlaneIntersectionTest, when_the_planes_dont_intersect_should_return_no_intersection)
{
	Plane<float> p1(Vec3F{1, 0, 0}, 0);
	Plane<float> p2(Vec3F{1, 0, 0}, 1);

	auto i = intersection(p1, p2);
	EXPECT_EQ(i.m_type, PlaneIntersectionType::Type::NoIntersection);
}

TEST(PlaneVsPlaneIntersectionTest, when_the_planes_are_coplanar_should_return_coplanar_intersection)
{
	Plane<float> p1(Vec3F{1, 0, 0}, 0);
	Plane<float> p2(Vec3F{1, 0, 0}, 0);

	auto i = intersection(p1, p2);
	EXPECT_EQ(i.m_type, PlaneIntersectionType::Type::Coplanar);
}

TEST(PlaneVsLineIntersectionTest, when_the_plane_and_line_intersect_should_return_the_intersection_point)
{
	Plane<float> p(Vec3F{1, 0, 0}, 0);
	Line3D<float> l(Vec3F{0, 0, 0}, Vec3F{1, 0, 0});

	auto i = intersection(p, l);
	EXPECT_EQ(i.m_type, PlaneLineIntersection::Type::Intersection);
	EXPECT_TRUE(i.m_intersection == (Vec3F{0, 0, 0}));
}

TEST(PlaneVsLineIntersectionTest, when_the_plane_and_line_dont_intersect_should_return_no_intersection)
{
	Plane<float> p(Vec3F{1, 0, 0}, 0);
	Line3D<float> l(Vec3F{1, 0, 0}, Vec3F{0, 1, 0});

	auto i = intersection(p, l);
	EXPECT_EQ(i.m_type, PlaneLineIntersection::Type::NoIntersection);
}

TEST(PlaneVsLineIntersectionTest, when_the_plane_and_line_are_coplanar_should_return_coplanar)
{
	Plane<float> p(Vec3F{1, 0, 0}, 0);
	Line3D<float> l(Vec3F{0, 0, 0}, Vec3F{0, 1, 0});

	auto i = intersection(p, l);
	EXPECT_EQ(i.m_type, PlaneLineIntersection::Type::Coplanar);
}

TEST(PlaneVsLineIntersectionTest, has_intersection_test)
{
	// clang-format off
	std::vector<std::tuple<Plane<float>, Line3D<float>, Vec3F>> tests = {
	    {Plane<float>::FromPointAndNormal({0, 0, 0}, {1, 0, 0}), Line3D<float>::FromTwoPoints({-5, -5, 0}, {5, -5, 0}), Vec3F{0, -5, 0}}
    };
	// clang-format on

	for (const auto &t : tests)
	{
		auto i = intersection(std::get<0>(t), std::get<1>(t));
		EXPECT_TRUE(i.m_intersection == std::get<2>(t));
	}
}


#include <armadillo>
#include <vector>
#include <set>
#include <map>

using namespace std;
using namespace arma;

struct Hormiga {
    vector<int> camino;
    double costoCamino;
};

ostream& operator<<(ostream& os, const Hormiga& hormiga)
{
    os << "Recorrido: ";

    for (int ciudad : hormiga.camino)
        os << ciudad << ' ';

    os << "\nCosto del camino: " << hormiga.costoCamino << endl;

    return os;
}

class ColoniaHormigas {
public:
    ColoniaHormigas(string rutaArchivoDistancias,
                    int nHormigas,
                    int nEpocas,
                    double sigma_cero,
                    double alpha,
                    double beta,
                    double tasaEvaporacion,
                    double Q);

    double calcularCosto(const vector<int>& camino);
    Hormiga buscarCamino();
    int seleccionarVecino(int ciudadActual, set<int> vecinos);
    Hormiga encontrarSolucion();
    void depositarFeromonas();

private:
    vector<Hormiga> m_hormiguero;
    mat m_distancias;
    mat m_feromonas;
    const int m_nHormigas;
    const int m_nEpocas;
    const double m_alpha;
    const double m_beta;
    const double m_Q;
    const double m_tasaEvaporacion;
    unsigned int m_nCiudades;
    int m_nodoOrigen;
};

ColoniaHormigas::ColoniaHormigas(string rutaArchivoDistancias,
                                 int nHormigas,
                                 int nEpocas,
                                 double sigma_cero,
                                 double alpha,
                                 double beta,
                                 double tasaEvaporacion,
                                 double Q)
    : m_nHormigas{nHormigas}
    , m_nEpocas{nEpocas}
    , m_alpha{alpha}
    , m_beta{beta}
    , m_Q{Q}
    , m_tasaEvaporacion{tasaEvaporacion}
{
    m_distancias.load(rutaArchivoDistancias);

    if (!m_distancias.is_square())
        throw runtime_error("La matriz leída no es cuadrada");

    m_nCiudades = m_distancias.n_rows;
    m_nodoOrigen = randi(1, distr_param(1, m_nCiudades))(0);

    m_feromonas = randu(m_distancias.n_rows, m_distancias.n_cols) * sigma_cero;
    //    m_feromonas = mat(m_nCiudades, m_nCiudades);
    //    m_feromonas.fill(sigma_cero);
}

double ColoniaHormigas::calcularCosto(const vector<int>& camino)
{
    double costo = 0;

    for (unsigned int i = 0; i < camino.size() - 1; ++i) {
        const int ciudad1 = camino.at(i);
        const int ciudad2 = camino.at(i + 1);

        if (ciudad1 == ciudad2)
            throw runtime_error("Las ciudades son iguales");

        costo += m_distancias(ciudad1 - 1, ciudad2 - 1);
    }

    return costo;
}

Hormiga ColoniaHormigas::buscarCamino()
{
    Hormiga hormiga;
    hormiga.camino = {m_nodoOrigen};

    set<int> vecinos;
    for (unsigned int i = 1; i <= m_nCiudades; ++i)
        vecinos.insert(i);

    vecinos.erase(m_nodoOrigen);

    while (!vecinos.empty()) {
        const int ciudad = seleccionarVecino(hormiga.camino.back(), vecinos);
        hormiga.camino.push_back(ciudad);
        vecinos.erase(ciudad);
    }

    // Cuando salgo del bucle, pasé por todas las ciudades.
    // Solo falta volver a la ciudad inicial.
    hormiga.camino.push_back(m_nodoOrigen);

    return hormiga;
}

int ColoniaHormigas::seleccionarVecino(int ciudadActual,
                                       set<int> vecinos)
{
    map<int, double> probabilidadVecino;
    double sumaProbabilidades = 0;

    // Calculo el numerador y el denominador de la expresión de la diapositiva
    for (int vecino : vecinos) {
        const double probabilidad = pow(m_feromonas(ciudadActual - 1, vecino - 1), m_alpha)
                                    / pow(m_distancias(ciudadActual - 1, vecino - 1), m_beta);
        //        const double probabilidad = pow(m_feromonas(ciudadActual - 1, vecino - 1), m_alpha);
        probabilidadVecino[vecino] = probabilidad;
        sumaProbabilidades += probabilidad;
    }

    // Normalizo cada numerador por el denominador calculado
    for (auto& p : probabilidadVecino)
        p.second /= sumaProbabilidades;

    // Ahora que tengo las probabilidades normalizadas,
    // selecciono un vecino
    const double moneda = randu(1).eval()(0);
    double probAcumulada = 0;

    for (const auto& p : probabilidadVecino) {
        probAcumulada += p.second;

        if (moneda <= probAcumulada)
            return p.first;
    }

    throw runtime_error("Nunca se debería llegar hasta acá");

    //    pair<int, double> mejor{0, 0};

    //    for (const auto& p : probabilidadVecino) {
    //        if (p.second > mejor.second)
    //            mejor = p;
    //    }

    //    return mejor.first;
}

Hormiga ColoniaHormigas::encontrarSolucion()
{
    for (int epoca = 1; epoca <= m_nEpocas; ++epoca) {
        m_hormiguero.clear();

        // Para cada hormiga, busco un camino que recorra todas
        // las ciudades.
        for (int i = 0; i < m_nHormigas; ++i)
            m_hormiguero.push_back(buscarCamino());

        // Me fijo si todas las hormigas tienen el mismo camino
        bool todosIguales = true;
        for (const Hormiga& hormiga : m_hormiguero) {
            if (hormiga.camino != m_hormiguero.front().camino) {
                todosIguales = false;
                break;
            }
        }

        if (todosIguales) {
            Hormiga solucion = m_hormiguero.front();
            solucion.costoCamino = calcularCosto(solucion.camino);

            return solucion;
        }

        // Evaporar feromonas
        m_feromonas *= (1 - m_tasaEvaporacion);

        // Depositar feromonas
        depositarFeromonas();
    }

    // Si se llega hasta acá las hormigas no convergieron
    // a un único camino.
    throw runtime_error("No se encontró una solución");
}

void ColoniaHormigas::depositarFeromonas()
{
    if (m_hormiguero.empty())
        throw runtime_error("El hormiguero está vacío");

    for (Hormiga& hormiga : m_hormiguero) {
        hormiga.costoCamino = calcularCosto(hormiga.camino);
        const double deltaFeromonas = m_Q / hormiga.costoCamino;

        for (unsigned int i = 0; i < hormiga.camino.size() - 1; ++i) {
            const int ciudad1 = hormiga.camino.at(i);
            const int ciudad2 = hormiga.camino.at(i + 1);

            if (ciudad1 == ciudad2)
                throw runtime_error("Las ciudades son iguales");

            m_feromonas(ciudad1 - 1, ciudad2 - 1) += deltaFeromonas;
            //			m_feromonas(ciudad2 - 1, ciudad1 - 1) += deltaFeromonas;
        }
    }
}


/**
 * @file
 *
 * @copyright
 * SPDX-License-Identifier: Apache-2.0
 *
 * @test @b eigen_sparse
 * @parblock
 * This piece of code aims to stress test the exec (in particular FMA)
 * by repetitively solving the set of linear equations represented by
 * Ax=b where A is a sparse real symmetric matrix uings Cholskey method.
 * The decomposition function is from the 3rd party library Eigen.
 * A random double precision sparse real symmetric matrix (A) and a
 * random vector (b) are generated as inputs and then
 * Eigen::SimplicialCholesky is used to solve the problem Ax=b. The
 * result vector is compared against a golden result that is computed
 * during init.
 *
 * @note Although the test should run fine on a single thread, it is
 * only expected to catch defects if run on at least 2 cores.
 * @endparblock
 */

#include <memory>

#include <sandstone.h>

#include <Eigen/Sparse>

constexpr size_t n=256;
namespace {
struct EigenSparseTestData {
    Eigen::SparseMatrix<double> A{n,n};
    Eigen::VectorXd b{n};
    Eigen::VectorXd golden{n};
};
}

static int initialize_problem(EigenSparseTestData *d)
{
    try {
        std::vector<Eigen::Triplet<double>> trip;
        for(size_t i=0; i<n; ++i) {
            for(size_t j=i+1; j<n; ++j) {
                double x = frandom_scale(1.0);
                if(x < 0.1) {
                    trip.push_back(Eigen::Triplet<double>(i,j,x));
                    if (j>i)
                        trip.push_back(Eigen::Triplet<double>(j,i,x));
                }
            }
        }
        for(size_t i=0; i<n; ++i) {
            double x = fabs(frandom_scale(1.0)) + 0.05;
            trip.push_back(Eigen::Triplet<double>(i,i,x));
        }
        d->A.setFromTriplets(trip.begin(), trip.end());
        d->b = Eigen::VectorXd::Random(n);
    } catch (...) {
        log_error("Exception on Eigen code, most probably OOM");
        return EXIT_SKIP;
    }

    return 0;
}

static int eigen_sparse_init(struct test *test) {
    auto d = std::make_unique<EigenSparseTestData>();
    int ret = initialize_problem(d.get());
    if (ret)
        return ret;
    Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver;
    try {
        d->golden = solver.compute(d->A).solve(d->b);
    } catch (...) {
        log_error("Exception on Eigen code, most probably OOM");
        return EXIT_SKIP;
    }
    if (solver.info() != Eigen::Success) {
        report_fail(test);
        return EXIT_FAILURE;
    }

    test->data = d.release();
    return EXIT_SUCCESS;
}

static int eigen_sparse_cleanup(struct test *test) {
    delete static_cast<EigenSparseTestData *>(test->data);
    return EXIT_SUCCESS;
}

static int eigen_sparse_run(struct test *test, int cpu) {
    auto d = static_cast<EigenSparseTestData *>(test->data);
    do {
        Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver;
        Eigen::VectorXd x;
        try {
            x = solver.compute(d->A).solve(d->b);
        } catch (...) {
            report_fail_msg("Exception on Eigen code, most probably OOM");
        }
        if (solver.info() != Eigen::Success) {
            report_fail(test);
            return EXIT_FAILURE;
        }
        if (x != d->golden) {
            report_fail(test);
            return EXIT_FAILURE;
        }
    } while (test_time_condition(test));
    return EXIT_SUCCESS;
}

DECLARE_TEST(eigen_sparse, "Eigen sparse linear algebra payload. Solve Ax=b using Cholskey (real symmetric A)")
  .groups = DECLARE_TEST_GROUPS(&group_math),
  .test_init = eigen_sparse_init,
  .test_run = eigen_sparse_run,
  .test_cleanup = eigen_sparse_cleanup,
  .minimum_cpu = cpu_haswell,
  .desired_duration = -1,
  .quality_level = TEST_QUALITY_PROD,
END_DECLARE_TEST


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "AlgorithmUtils.hpp"
#include "FluidEigenMappings.hpp"
#include "../public/WindowFuncs.hpp"
#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>

namespace fluid {
namespace algorithm {

// This implements Foote's novelty curve
class Novelty
{

public:
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXXd = Eigen::ArrayXXd;
  using MatrixXd = Eigen::MatrixXd;
  using VectorXd = Eigen::VectorXd;

  Novelty(index maxSize) : mKernelStorage(maxSize, maxSize) {}

  void init(index kernelSize, index nDims)
  {
    assert(kernelSize % 2);
    mKernelSize = kernelSize;
    mNDims = nDims;
    createKernel();
    mSimilarity = MatrixXd::Zero(mKernelSize, mKernelSize);
    mBufer = MatrixXd::Zero(mKernelSize, nDims);
  }

  double processFrame(const ArrayXd& input)
  {
    using std::vector;
    mBufer.block(0, 0, mKernelSize - 1, mNDims) =
        mBufer.block(1, 0, mKernelSize - 1, mNDims);
    ArrayXXd x = mBufer.block(mKernelSize - 1, 0, 1, mNDims);
    VectorXd in1 = input.matrix();
    mBufer.block(mKernelSize - 1, 0, 1, mNDims) = in1.transpose();
    VectorXd tmp = mBufer * input.matrix();
    VectorXd norm =
        mBufer.rowwise().norm().cwiseMax(epsilon) * input.matrix().norm();
    norm = norm.cwiseMax(epsilon);
    tmp = (tmp.array() / norm.array()).matrix();
    mSimilarity.block(0, 0, mKernelSize - 1, mKernelSize - 1) =
        mSimilarity.block(1, 1, mKernelSize - 1, mKernelSize - 1);
    ArrayXXd x1 = mSimilarity.block(0, mKernelSize - 1, mKernelSize, 1);
    mSimilarity.block(0, mKernelSize - 1, mKernelSize, 1) = tmp;
    mSimilarity.block(mKernelSize - 1, 0, 1, mKernelSize) = tmp.transpose();
    double result = (mSimilarity.array() * mKernel).sum();
    return result / mNorm;
  }

private:
  void createKernel()
  {
    mKernel = mKernelStorage.block(0, 0, mKernelSize, mKernelSize);
    index   h = (mKernelSize - 1) / 2;
    ArrayXd gaussian = ArrayXd::Zero(mKernelSize);
    WindowFuncs::map()[WindowFuncs::WindowTypes::kGaussian](mKernelSize,
                                                            gaussian);
    MatrixXd tmp = gaussian.matrix() * gaussian.matrix().transpose();
    tmp.block(h, 0, h + 1, h) *= -1;
    tmp.block(0, h, h, h + 1) *= -1;
    mKernel = tmp.array();
    mNorm = mKernel.square().sum();
  }

  index    mKernelSize{3};
  index    mNDims{513};
  ArrayXXd mKernel;
  ArrayXXd mKernelStorage;
  MatrixXd mSimilarity;
  MatrixXd mBufer;
  double   mNorm{1.};
};
} // namespace algorithm
} // namespace fluid


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "FullyConnected.hpp"

#include <boost/assert.hpp>

namespace armnn
{

void FullyConnected(const float*      inputData,
                    float*            outputData,
                    const TensorInfo& inputTensorInfo,
                    const TensorInfo& outputTensorInfo,
                    const float*      weightData,
                    const float*      biasData,
                    bool              transposeWeights)
{
    unsigned int N = outputTensorInfo.GetShape()[1]; // Outputs Vector Size.

    BOOST_ASSERT(inputTensorInfo.GetNumDimensions() > 1); // Needs some data.

    unsigned int K = 1; // Total number of activations in the input.
    for (unsigned int i = 1; i < inputTensorInfo.GetNumDimensions(); i++)
    {
        K *= inputTensorInfo.GetShape()[i];
    }

    for (unsigned int n = 0; n < inputTensorInfo.GetShape()[0]; n++)
    {
        for (unsigned int channelOutput = 0; channelOutput < N; channelOutput++)
        {
            float outval = 0.f;

            for (unsigned int channelInput = 0; channelInput < K; channelInput++)
            {
                float weight;
                if (transposeWeights)
                {
                    weight = weightData[channelOutput * K + channelInput];
                }
                else
                {
                    weight = weightData[channelInput * N + channelOutput];
                }

                outval += weight * inputData[n * K + channelInput];
            }

            if (biasData)
            {
                outval += biasData[channelOutput];
            }

            outputData[n * N + channelOutput] = outval;
        }
    }
}

} //namespace armnn


#pragma once

#include <Eigen/Core>
using namespace Eigen;

#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
using namespace autodiff;

#include <samson/so3.hpp>

namespace samson::robot
{
    using namespace samson::se3;

    auto fk(const Matrix4dual &M, const MatrixXdual &S, const VectorXdual &theta)
    {
        Matrix4dual T = M;
        for (int i = 0; i < S.rows(); ++i)
        {
            Vector6dual Vi = S.row(i);
            dual thetai = theta(i);
            Vi *= thetai;
            T *= exp(Vi);
        }
        return T;
    }

} // namespace samson::robot


/*=========================================================================

  Program:   Small Body Geophysical Analysis
  Module:    SBGATPolyhedronGravityModel.hpp

  Class derived from VTK's vtkPolyDataAlgorithm by Benjamin Bercovici

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

/**
@file SBGATPolyhedronGravityModel.hpp
@class  SBGATPolyhedronGravityModel
@author Benjamin Bercovici
@date October 2018

@brief  Evaluation of potential, acceleration caused by a constant-density polyhedron
 @details Computes the potential, acceleration caused by a polyhedron
 of constant density by evaluating the so called Polyhedron Gravity Model as derived by Werner and Scheeres.
The input must be a topologically-closed polyhedron. This class will always use results expressed in `meters` as their distance unit (e.g accelerations in m/s^2, potentials in m^2/s^2,...) . Unit consistency is enforced through the use of the SetScaleMeters()
and SetScaleKiloMeters() method.

See Werner, R. A., & Scheeres, D. J. (1997). Exterior gravitation of a polyhedron derived and compared with harmonic and mascon gravitation representations of asteroid 4769 Castalia. Celestial Mechanics and Dynamical Astronomy, 65(3), 313–344. https://doi.org/10.1007/BF00053511
for further details. Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
@copyright MIT License, Benjamin Bercovici and Jay McMahon
*/

#ifndef SBGATPolyhedronGravityModel_hpp
#define SBGATPolyhedronGravityModel_hpp

#include <vtkFiltersCoreModule.h> // For export macro
#include <vtkPolyDataAlgorithm.h>
#include <armadillo>
#include <SBGATMassProperties.hpp>

class VTKFILTERSCORE_EXPORT SBGATPolyhedronGravityModel : public SBGATMassProperties{
public:

  /**
   * Constructs with initial values of zero.
   */

  static SBGATPolyhedronGravityModel *New();

  vtkTypeMacro(SBGATPolyhedronGravityModel,vtkPolyDataAlgorithm);
  void PrintSelf(std::ostream& os, vtkIndent indent) override;
  void PrintHeader(std::ostream& os, vtkIndent indent) override;
  void PrintTrailer(std::ostream& os, vtkIndent indent) override;

  /**
  Evaluates the Polyhedron Gravity Model potential at the specified point assuming
  a constant density
  @param point pointer to coordinates of queried point, expressed in the same frame as
  the polydata
  @return PGM potential evaluated at the queried point (m ^ 2/ s ^2)
  */
  double GetPotential(double const * point) const;

  /**
  Evaluates the Polyhedron Gravity Model potential at the specified point assuming
  a constant density
  @param point coordinates of queried point, expressed in the same frame as
  the polydata
  @return PGM potential evaluated at the queried point (m ^ 2 / s ^2)
  */
  double GetPotential(const arma::vec::fixed<3> & point) const;

  /**
  Evaluates the Polyhedron Gravity Model potential and acceleration at the specified point assuming
  a constant density
  @param point coordinates of queried point, expressed in the same frame as
  the polydata used to construct the PGM
  @param[out] potential PGM potential evaluated at the queried point (m ^ 2 / s ^2)
  @param[out] acc PGM acceleration evaluated at the queried point (m / s ^2)
  */
  void GetPotentialAcceleration(double const * point,double & potential,
    arma::vec::fixed<3> & acc) const;

  /**
  Evaluates the Polyhedron Gravity Model potential and acceleration at the specified point assuming
  a constant density
  @param point coordinates of queried point, expressed in the same frame as
  the polydata used to construct the PGM
  @param[out] potential PGM potential evaluated at the queried point (m ^ 2 / s ^2)
  @param[out] acc PGM acceleration evaluated at the queried point (m / s ^2)
  */
  void GetPotentialAcceleration(const arma::vec::fixed<3> & point,double & potential,
    arma::vec::fixed<3> & acc) const;

/**
Evaluates the Polyhedron Gravity Model potential, acceleration and gravity gradient matrix at the specified point assuming
a constant density
@param point coordinates of queried point, expressed in the same frame as
the polydata used to construct the PGM
@param[out] potential PGM potential evaluated at the queried point (m ^ 2 / s ^2)
@param[out] acc PGM acceleration evaluated at the queried point (m / s ^2)
@param[out] gravity_gradient_mat PGM gravity gradient matrix evaluated at the queried point (1 / s ^2)
*/
  void GetPotentialAccelerationGravityGradient(double const  * point,double & potential,
    arma::vec::fixed<3> & acc,arma::mat::fixed<3,3> & gravity_gradient_mat) const ;

/**
Get the body-fixed acceleration at the center of the specified facet
@param f facet index
@return body-fixed acceleration (m/s^2)
*/
  arma::vec::fixed<3> GetBodyFixedAccelerationf(const int & f) const;

/**
Evaluates the Polyhedron Gravity Model potential, acceleration and gravity gradient matrix at the specified point assuming
a constant density
@param point coordinates of queried point, expressed in the same frame as
the polydata used to construct the PGM
@param[out] potential PGM potential evaluated at the queried point (m ^ 2 / s ^2)
@param[out] acc PGM acceleration evaluated at the queried point (m / s ^2)
@param[out] gravity_gradient_mat PGM gravity gradient matrix evaluated at the queried point (1 / s ^2)
*/
  void GetPotentialAccelerationGravityGradient(const arma::vec::fixed<3> & point,double & potential,
    arma::vec::fixed<3> & acc,arma::mat::fixed<3,3> & gravity_gradient_mat) const ;

/**
Evaluates the Polyhedron Gravity Model gravity gradient matrix at the specified point assuming
a constant density
@param point coordinates of queried point, expressed in the same frame as
the polydata used to construct the PGM
@param[out] gravity_gradient_mat PGM gravity gradient matrix evaluated at the queried point (1 / s ^2)
*/
  arma::mat::fixed<3,3> GetGravityGradient(const arma::vec::fixed<3> & point) const ;

  /**
  Evaluates the Polyhedron Gravity Model acceleration at the specified point assuming
  a constant density
  @param point coordinates of queried point, expressed in the same frame as
  the polydata used to construct the PGM
  @return PGM acceleration evaluated at the queried point (m / s ^2)
  */
  arma::vec::fixed<3> GetAcceleration(const arma::vec::fixed<3> & point) const;

  /**
  Evaluates the Polyhedron Gravity Model acceleration at the specified point assuming
  a constant density
  @param point pointer to coordinates of queried point, expressed in the same frame as
  the input polydata
  @return PGM acceleration evaluated at the queried point (m / s ^2)
  */
  arma::vec::fixed<3> GetAcceleration(double const * point) const;

  /**
  Determines whether the provided point lies inside or outside the shape
  @param point coordinates of queried point, expressed in the same frame as
  the polydata
  @param tolerance
  @return true if the polydata contains the point, false otherwise
  */
  bool Contains(double const * point, double tol = 1e-8) const;

  /**
  Determines whether the provided point lies inside or outside the shape
  @param point coordinates of queried point, expressed in the same frame as
  the polydata
  @param tolerance
  @return true if the polydata contains the point, false otherwise
  */
  bool Contains(const arma::vec::fixed<3> & point,double tol = 1e-8) const;

  /**
  Computes the slope as the center of the designated facet
  @param f facet index
  @return slope (rad)
  */
  double GetSlope(const int & f ) const;

  /**
  Get polyhedron mass. Must have set the density and call
  @return mass (kg)
  */
  double GetMass() const{
    return this -> density * this -> GetVolume() * std::pow(this -> scaleFactor,3);
  }

  /**
  Return the performance factor of the f-th facet at the specified position
  @param pos position of field point
  @param f facet index
  @return omega_f
  */
  double GetPerformanceFactor(const arma::vec::fixed<3> & pos, const int & f) const;

  /**
  Return the performance factor of the f-th facet at the specified position
  @param pos position of field point
  @param f facet index
  @return omega_f
  */
  double GetPerformanceFactor( const double * pos, const int & f) const;

  /**
  Return the wire potential of the e-th edge at the specified position
  @param pos position of field point
  @param e edge index
  @return L_e
  */
  double GetLe(const arma::vec::fixed<3> & pos, const int & e) const;

  /**
  Return the wire potential of the e-th edge at the specified position
  @param pos position of field point
  @param e edge index
  @return L_e
  */
  double GetLe( const double * pos, const int & e) const;

  /**
  Evaluates the Polyhedron Gravity Model at the surface of the specified surface elements in the provided shape
  @param[in] selected_shape shape for which the surface polyhedron gravity model must be computed
  @param[in] queried_elements vector of elements indices where the polyhedron gravity model should be evaluated
  @param[in] is_in_meters true if the shape coordinates were expressed in meters, false if they were expressed in kilometers
  @param[in] density shape bulk density in kg/m^3
  @param[in] omega fixed angular velocity of shape expressed in rad/s
  @param[out] slopes vector storing the gravitational slopes (degrees) evaluated at the center of each queried element
  @param[out] inertial_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[out] body_fixed_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[out] inertial_acc_magnitudes vector storing the inertial gravitational acceleration magnitudes  (m/s^2) evaluated at the center of each queried element
  @param[out] body_fixed_acc_magnitudes vector storing the body-fixed gravitational acceleration magnitudes (m/s^2) evaluated at the center of each queried element
  */
  static void ComputeSurfacePGM(
    vtkSmartPointer<vtkPolyData> selected_shape,
    const std::vector<unsigned int> & queried_elements,
    bool is_in_meters,
    double density,
    const arma::vec::fixed<3> & omega,
    std::vector<double> & slopes,
    std::vector<double> & inertial_potentials,
    std::vector<double> & body_fixed_potentials,
    std::vector<double> & inertial_acc_magnitudes,
    std::vector<double> & body_fixed_acc_magnitudes);

  /**
  Saves the provided Polyhedron Gravity Model to a file
  @param[in] selected_shape shape for which the surface polyhedron gravity model must be computed
  @param[in] queried_elements shape indices of elements where the polyhedron gravity model should be evaluated
  @param[in] is_in_meters true if the shape coordinates were expressed in meters, false if they were expressed in kilometers
  @param[in] mass mass of shape model (kg)
  @param[in] omega fixed angular velocity of shape (rad/s)
  @param[in] slopes vector storing the gravitational slopes (degrees) evaluated at the center of each queried element
  @param[in] inertial_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[in] body_fixed_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[in] inertial_acc_magnitudes vector storing the inertial gravitational acceleration magnitudes  (m/s^2) evaluated at the center of each queried element
  @param[in] body_fixed_acc_magnitudes vector storing the body-fixed gravitational acceleration magnitudes (m/s^2) evaluated at the center of each queried element
  @param[in] path save path (ex: "pgm_surface.json")
  */
  static void SaveSurfacePGM(vtkSmartPointer<vtkPolyData> selected_shape,
    const std::vector<unsigned int> & queried_elements,
    bool is_in_meters,
    const double & mass,
    const arma::vec::fixed<3> & omega,
    const std::vector<double> & slopes,
    const std::vector<double> & inertial_potentials,
    const std::vector<double> & body_fixed_potentials,
    const std::vector<double> & inertial_acc_magnitudes,
    const std::vector<double> & body_fixed_acc_magnitudes,
    std::string path);

/**
  Saves the provided Polyhedron Gravity Model to a file
  @param[in] selected_shape shape for which the surface polyhedron gravity model must be computed
  @param[in] queried_elements shape indices of elements where the polyhedron gravity model should be evaluated
  @param[in] is_in_meters true if the shape coordinates were expressed in meters, false if they were expressed in kilometers
  @param[in] mass mass of shape model (kg)
  @param[in] omega fixed angular velocity of shape (rad/s)
  @param[in] slopes vector storing the gravitational slopes (degrees) evaluated at the center of each queried element
  @param[in] inertial_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[in] body_fixed_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[in] inertial_acc_magnitudes vector storing the inertial gravitational acceleration magnitudes  (m/s^2) evaluated at the center of each queried element
  @param[in] body_fixed_acc_magnitudes vector storing the body-fixed gravitational acceleration magnitudes (m/s^2) evaluated at the center of each queried element
  @param[in] slope_sds vector storing the standard deviation in the gravitational slopes (degrees) evaluated at the center of each queried element
  @param[in] path save path (ex: "pgm_surface.json")
  */
static void SaveSurfacePGM(vtkSmartPointer<vtkPolyData> selected_shape,
  const std::vector<unsigned int> & queried_elements,
  bool is_in_meters,
  const double & mass,
  const arma::vec::fixed<3> & omega,
  const std::vector<double> & slopes,
  const std::vector<double> & inertial_potentials,
  const std::vector<double> & body_fixed_potentials,
  const std::vector<double> & inertial_acc_magnitudes,
  const std::vector<double> & body_fixed_acc_magnitudes,
  const std::vector<double> & slope_sds,
  std::string path);

  /**
  Loads a previously computed surface Polyhedron Gravity Model from a file
  @param[out] mass mass of shape model (kg)
  @param[out] omega fixed angular velocity of shape (rad/s)
  @param[out] slopes vector storing the gravitational slopes (degrees) evaluated at the center of each queried element
  @param[out] inertial_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[out] body_fixed_potentials vector storing the body-fixed gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[out] inertial_acc_magnitudes vector storing the inertial gravitational acceleration magnitudes  (m/s^2) evaluated at the center of each queried element
  @param[out] body_fixed_acc_magnitudes vector storing the body-fixed gravitational acceleration magnitudes (m/s^2) evaluated at the center of each queried element
  @param[out] slope_sds standard deviation in the surface slopes (deg)
  @param[in] path load path (ex: "pgm_surface.json")
  */
  static void LoadSurfacePGM(double & mass,
    arma::vec::fixed<3> & omega,
    std::vector<double> & slopes,
    std::vector<double> & inertial_potentials,
    std::vector<double> & body_fixed_potentials,
    std::vector<double> & inertial_acc_magnitudes,
    std::vector<double> & body_fixed_acc_magnitudes,
    std::vector<double> & slope_sds,
    std::string path);

  /**
  Return the Xe^E vector holding the e-th edge dyadic factors (Le,r_ie_0^T,Ee^T)^T
  @param pos field point
  @param e edge index
  @return Xe^E vector holding the e-th edge dyadic factors
  */
  arma::vec::fixed<10> GetXe(const arma::vec::fixed<3> & pos,const int & e) const;

  /**
  Return the Xf^F vector holding the f-th facet dyadic factors (omega_f,r_if_0^T,Ff^T)^T
  @param pos field point
  @param f facet index
  @return Xf^F vector holding the f-th facet dyadic factors
  */
  arma::vec::fixed<10> GetXf(const arma::vec::fixed<3> & pos,const int & f) const;

  /**
  Return the parametrization of the designated edge dyad Ee. This dyad
  is stored in a flattened double container holding nine values and ordered like so

  E = {
    {E[0], E[1],  E[2]},
    {E[3], E[4],  E[5]},
    {E[6], E[7],  E[8]}
  };
  Due to the symmetric nature of E, its parametrization is
  {E[0],E[4],E[8],E[1],E[2],E[5]};

  @param e edge index
  @return Ee dyad parametrization
  */
  arma::vec::fixed<6> GetEeParam(const int & e) const;

  /**
  Return the parametrization of the designated facet dyad Ff. This dyad
  is stored in a flattened double container holding nine values and ordered like so

  F = {
    {F[0], F[1],  F[2]},
    {F[3], F[4],  F[5]},
    {F[6], F[7],  F[8]}
  };
  Due to the symmetric nature of E, its parametrization is
  {F[0],F[4],F[8],F[1],F[2],F[5]};

  @param f facet index
  @return Ff dyad parametrization
  */
  arma::vec::fixed<6> GetFfParam(const int & f) const;

  /**
  Return the contribution of a specific edge to the potential at a specified field point
  @param Xe vector of dyadic coefficients for edge e at the prescribed fieldpoint
  @return contribution to the potential of this specific edge at the prescribed fieldpoint
  */
  static double GetUe(const arma::vec::fixed<10> & Xe);

  /**
  Return the contribution of a specific facet to the potential at a specified field point
  @param Xf vector of dyadic coefficients for facet f at the prescribed fieldpoint
  @return contribution to the potential of this specific facet at the prescribed fieldpoint
  */
  static double GetUf(const arma::vec::fixed<10> & Xf);

  /**
  Return the contribution of a specific edge to the acceleration at a specified field point
  @param Xe vector of dyadic coefficients for edge e at the prescribed fieldpoint
  @return contribution to the potential of this specific edge at the prescribed fieldpoint
  */
  static arma::vec::fixed<3> GetAe(const arma::vec::fixed<10> & Xe);

  /**
  Return the contribution of a specific facet to the acceleration at a specified field point
  @param Xf vector of dyadic coefficients for facet f at the prescribed fieldpoint
  @return contribution to the potential of this specific facet at the prescribed fieldpoint
  */
  static arma::vec::fixed<3> GetAf(const arma::vec::fixed<10> & Xf);

  /**
  Set the angular velocity vector
  @param Omega angular velocity expressed in body-frame (rad/s)
  */
  void SetOmega(arma::vec::fixed<3> Omega){this -> Omega = Omega;}

  /**
  Return the angular velocity vector
  @return Omega angular velocity expressed in body-frame (rad/s)
  */
  arma::vec::fixed<3> GetOmega() const {return this -> Omega;}

protected:
  SBGATPolyhedronGravityModel();
  ~SBGATPolyhedronGravityModel() override;

  void Clear();

  int RequestData(vtkInformation* request,
    vtkInformationVector** inputVector,
    vtkInformationVector* outputVector) override;

  double ** facet_dyads;
  double ** edge_dyads;

  arma::vec::fixed<3> Omega;

private:
  SBGATPolyhedronGravityModel(const SBGATPolyhedronGravityModel&) = delete;
  void operator=(const SBGATPolyhedronGravityModel&) = delete;
};

#endif


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2019 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * based on deal.II step-1
 */

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/manifold_lib.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <cmath>
#include <fstream>
#include <iostream>
#include <tuple>

using namespace dealii;

std::tuple<int, int, int> func_tuple(const Triangulation<2> &tri)
// Function returns multivalue: # active cells, # cells, Levels
{
  return std::make_tuple<int, int, int>(tri.n_levels(),
                                        tri.n_cells(),
                                        tri.n_active_cells());
}

void
first_grid()
{
  Triangulation<2> triangulation;

  GridGenerator::hyper_cube(triangulation);

  triangulation.refine_global(4);

  std::ofstream out("grid-1.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);
  std::cout << "Grid written to grid-1.svg" << std::endl;

  // Triangualtion tuple construction
  auto vals_tri = func_tuple(triangulation);
  std::cout << "Number of Levels"<< "\t" << std::get<0>(vals_tri) << "\n"
            << "Number of cells"<< "\t" << std::get<1>(vals_tri) << "\n"
            << "Number of active cells" << "\t" << std::get<2>(vals_tri) << "\n";
}

void
second_grid()
{
  Triangulation<2> triangulation;

  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 10);
  for (unsigned int step = 0; step < 5; ++step)
    {
      for (auto &cell : triangulation.active_cell_iterators())
        {
          for (const auto v : cell->vertex_indices())
            {
              const double distance_from_center =
                center.distance(cell->vertex(v));

              if (std::fabs(distance_from_center - inner_radius) <=
                  1e-6 * inner_radius)
                {
                  cell->set_refine_flag();
                  break;
                }
            }
        }
        triangulation.reset_manifold(0); // Reset to default manifold: Flat

      triangulation.execute_coarsening_and_refinement();
    }

  std::ofstream out("grid-2.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);

  std::cout << "Grid written to grid-2.svg" << std::endl;

  // Triangualtion tuple construction
  auto vals_tri = func_tuple(triangulation);
  std::cout << "Number of Levels"<< "\t" << std::get<0>(vals_tri) << "\n"
            << "Number of cells"<< "\t" << std::get<1>(vals_tri) << "\n"
            << "Number of active cells" << "\t" << std::get<2>(vals_tri) << "\n";
}

void
third_grid()
{
  Triangulation<2> triangulation;
  const double LFT = -0.5;
  const double RGT =  0.5;

  GridGenerator::hyper_L(triangulation, LFT, RGT, true);

  std::ofstream out("grid-3.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);
  std::cout << "Grid written to grid-3.svg" << std::endl;

  // Triangualtion tuple construction
  auto vals_tri3 = func_tuple(triangulation);
  std::cout << "Number of Levels"<< "\t" << std::get<0>(vals_tri3) << "\n"
            << "Number of cells"<< "\t" << std::get<1>(vals_tri3) << "\n"
            << "Number of active cells" << "\t" << std::get<2>(vals_tri3) << "\n";

  const unsigned int initial_global_refinement = 1 ;

  triangulation.refine_global(initial_global_refinement);
  std::ofstream out_global_refine("grid-4.svg");

  grid_out.write_svg(triangulation, out_global_refine);
  std :: cout << "Grid written to grid-4.svg" << std::endl;

  // Triangualtion tuple construction
  auto vals_tri4 = func_tuple(triangulation);
  std::cout << "Number of Levels"<< "\t" << std::get<0>(vals_tri4) << "\n"
            << "Number of cells"<< "\t" << std::get<1>(vals_tri4) << "\n"
            << "Number of active cells" << "\t" << std::get<2>(vals_tri4) << "\n";

  const Point<2> corner(0, 0);
  const double corner_dist = 0.3333;
for(unsigned int step = 0; step < 3; step++)
{
    for (auto &cell : triangulation.active_cell_iterators())
    {
      for( const auto v : cell->vertex_indices())
      {
        const double distance_from_corner = corner.distance(cell->center(v));
        //std :: cout << distance_from_corner << "\n";

        if(std :: fabs(distance_from_corner <= corner_dist))
        {
          cell->set_refine_flag();
          break;
        }
        else
        {
          break;
        }

      }

    }

  triangulation.execute_coarsening_and_refinement();
}
  std :: ofstream out_reentrant("grid-5.svg");

  grid_out.write_svg(triangulation, out_reentrant);
  std :: cout << "Grid written to grid-5.svg" <<std :: endl;

  // Triangualtion tuple construction
  auto vals_tri5 = func_tuple(triangulation);
  std::cout << "Number of Levels"<< "\t" << std::get<0>(vals_tri5) << "\n"
            << "Number of cells"<< "\t" << std::get<1>(vals_tri5) << "\n"
            << "Number of active cells" << "\t" << std::get<2>(vals_tri5) << "\n";
}

void
spherical_grid()
{
  const Point<2> center(0, 0);
  const double outer_radius = 1.0;

  Triangulation<2> triangulation;
  GridGenerator :: hyper_ball(triangulation, center, outer_radius);
  triangulation.reset_all_manifolds();
  triangulation.set_all_manifold_ids_on_boundary(0);

  std::ofstream out("grid-6.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);
  std::cout << "Grid written to grid-6.svg" << std::endl;

  // Spherical Manifold on Boundary

  const SphericalManifold<2> manifold(center);
  Triangulation<2> tria_boundary;

  GridGenerator :: hyper_ball(tria_boundary, center, outer_radius, true);
  tria_boundary.reset_all_manifolds();
  tria_boundary.set_all_manifold_ids_on_boundary(0);
  tria_boundary.set_manifold(0, manifold);
  tria_boundary.refine_global(2);

  std::ofstream out_boundary("grid-7.svg");
  grid_out.write_svg(tria_boundary, out_boundary);
  std::cout << "Grid written to grid-7.svg" << std::endl;

  // Spherical Manifold Everywhere

  Triangulation<2> tria_everywhere;

  GridGenerator :: hyper_ball(tria_everywhere, center, outer_radius, true);
  tria_everywhere.reset_all_manifolds();
  tria_everywhere.set_all_manifold_ids(0);
  tria_everywhere.set_manifold(0, manifold);
  tria_everywhere.refine_global(2);

  std::ofstream out_eveywhere("grid-8.svg");
  grid_out.write_svg(tria_everywhere, out_eveywhere);
  std::cout << "Grid written to grid-8.svg" << std::endl;

  // Spherical Manifold Except center

  Triangulation<2> tria_except_center;

  GridGenerator :: hyper_ball(tria_except_center, center, outer_radius, true);
  tria_except_center.reset_all_manifolds();
  for (unsigned int step = 0; step <2; step++)
  {
    for (auto &cell : tria_except_center.active_cell_iterators())
    {
      if (std :: fabs(cell->center().norm() > 0))
      {
        tria_except_center.set_all_manifold_ids(0);
        tria_except_center.set_manifold(0, manifold);
        cell->refine_flag_set();
      }
      tria_except_center.refine_global();
    }
    tria_except_center.execute_coarsening_and_refinement();
  }
  std::ofstream out_except_center("grid-9.svg");
  grid_out.write_svg(tria_except_center, out_except_center);
  std::cout << "Grid written to grid-9.svg" << std::endl;

}

int main()
{
  first_grid();
  second_grid();
  third_grid();
  spherical_grid();
}


#include "xpbd/green_strain_elastic_constraint.h"

#include <Eigen/Dense>
#include <eigen/SVD>

namespace xpbd {

green_strain_elastic_constraint_t::green_strain_elastic_constraint_t(
    std::initializer_list<index_type> indices,
    positions_type const& p,
    scalar_type young_modulus,
    scalar_type poisson_ratio,
    scalar_type const alpha)
    : base_type(indices, alpha), V0_{0.}, DmInv_{}, mu_{}, lambda_{}
{
    assert(indices.size() == 4u);

    auto const v1 = this->indices().at(0);
    auto const v2 = this->indices().at(1);
    auto const v3 = this->indices().at(2);
    auto const v4 = this->indices().at(3);

    auto const p1 = p.row(v1);
    auto const p2 = p.row(v2);
    auto const p3 = p.row(v3);
    auto const p4 = p.row(v4);

    Eigen::Matrix3d Dm;
    Dm.col(0) = (p1 - p4).transpose();
    Dm.col(1) = (p2 - p4).transpose();
    Dm.col(2) = (p3 - p4).transpose();

    V0_     = (1. / 6.) * Dm.determinant();
    DmInv_  = Dm.inverse();
    mu_     = (young_modulus) / (2. * (1 + poisson_ratio));
    lambda_ = (young_modulus * poisson_ratio) / ((1 + poisson_ratio) * (1 - 2 * poisson_ratio));
}

green_strain_elastic_constraint_t::scalar_type
green_strain_elastic_constraint_t::signed_volume(positions_type const& V) const
{
    Eigen::RowVector3d const p1 = V.row(indices()[0]);
    Eigen::RowVector3d const p2 = V.row(indices()[1]);
    Eigen::RowVector3d const p3 = V.row(indices()[2]);
    Eigen::RowVector3d const p4 = V.row(indices()[3]);

    Eigen::Matrix3d Ds;
    Ds.col(0)      = (p1 - p4).transpose();
    Ds.col(1)      = (p2 - p4).transpose();
    Ds.col(2)      = (p3 - p4).transpose();
    auto const vol = (1. / 6.) * Ds.determinant();
    return vol;
}

void green_strain_elastic_constraint_t::project(
    positions_type& p,
    masses_type const& m,
    scalar_type& lagrange,
    scalar_type const dt) const
{
    auto const v1 = this->indices().at(0);
    auto const v2 = this->indices().at(1);
    auto const v3 = this->indices().at(2);
    auto const v4 = this->indices().at(3);

    auto const p1 = p.row(v1);
    auto const p2 = p.row(v2);
    auto const p3 = p.row(v3);
    auto const p4 = p.row(v4);

    auto const w1 = 1. / m(v1);
    auto const w2 = 1. / m(v2);
    auto const w3 = 1. / m(v3);
    auto const w4 = 1. / m(v4);

    auto const Vsigned        = signed_volume(p);
    bool const is_V_positive  = Vsigned >= 0.;
    bool const is_V0_positive = V0_ >= 0.;
    bool const is_tet_inverted =
        (is_V_positive && !is_V0_positive) || (!is_V_positive && is_V0_positive);
    scalar_type constexpr epsilon = 1e-20;

    Eigen::Matrix3d Ds;
    Ds.col(0) = (p1 - p4).transpose();
    Ds.col(1) = (p2 - p4).transpose();
    Ds.col(2) = (p3 - p4).transpose();

    Eigen::Matrix3d const F = Ds * DmInv_;
    Eigen::Matrix3d const I = Eigen::Matrix3d::Identity();

    // TODO: Implement correct inversion handling described in
    // Irving, Geoffrey, Joseph Teran, and Ronald Fedkiw. "Invertible finite elements for robust
    // simulation of large deformation." Proceedings of the 2004 ACM SIGGRAPH/Eurographics symposium
    // on Computer animation. 2004.
    // scalar_type psi{};
    // Eigen::Matrix3d Piola;
    Eigen::JacobiSVD<Eigen::Matrix3d> UFhatV(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Vector3d const Fsigma = UFhatV.singularValues();
    Eigen::Matrix3d Fhat;
    Fhat.setZero();
    Fhat(0, 0) = Fsigma(0);
    Fhat(1, 1) = Fsigma(1);
    Fhat(2, 2) = Fsigma(2);

    Eigen::Matrix3d U       = UFhatV.matrixU();
    Eigen::Matrix3d const V = UFhatV.matrixV();

    if (is_tet_inverted)
    {
        Fhat(2, 2) = -Fhat(2, 2);
        U.col(2)   = -U.col(2);
    }

    // stress reaches maximum at 58% compression
    scalar_type constexpr min_singular_value = 0.577;
    Fhat(0, 0)                               = std::max(Fhat(0, 0), min_singular_value);
    Fhat(1, 1)                               = std::max(Fhat(1, 1), min_singular_value);
    Fhat(2, 2)                               = std::max(Fhat(2, 2), min_singular_value);

    Eigen::Matrix3d const Ehat     = 0.5 * (Fhat.transpose() * Fhat - I);
    scalar_type const EhatTrace    = Ehat.trace();
    Eigen::Matrix3d const Piolahat = Fhat * ((2. * mu_ * Ehat) + (lambda_ * EhatTrace * I));

    Eigen::Matrix3d const E  = U * Ehat * V.transpose();
    scalar_type const Etrace = E.trace();
    scalar_type const psi = mu_ * (E.array() * E.array()).sum() + 0.5 * lambda_ * Etrace * Etrace;

    Eigen::Matrix3d const Piola = U * Piolahat * V.transpose();

    // H is the negative gradient of the elastic potential
    scalar_type const V0     = std::abs(V0_);
    Eigen::Matrix3d const H  = -V0 * Piola * DmInv_.transpose();
    Eigen::Vector3d const f1 = H.col(0);
    Eigen::Vector3d const f2 = H.col(1);
    Eigen::Vector3d const f3 = H.col(2);
    Eigen::Vector3d const f4 = -(f1 + f2 + f3);

    // clang-format off
     auto const weighted_sum_of_gradients =
        w1 * f1.squaredNorm() +
        w2 * f2.squaredNorm() +
        w3 * f3.squaredNorm() +
        w4 * f4.squaredNorm();
    // clang-format on

    if (weighted_sum_of_gradients < epsilon)
        return;

    scalar_type const C           = V0 * psi;
    scalar_type const alpha_tilde = alpha_ / (dt * dt);
    scalar_type const delta_lagrange =
        -(C + alpha_tilde * lagrange) / (weighted_sum_of_gradients + alpha_tilde);

    lagrange += delta_lagrange;
    // because f = - grad(potential), then grad(potential) = -f and thus grad(C) = -f
    p.row(v1) += w1 * -f1 * delta_lagrange;
    p.row(v2) += w2 * -f2 * delta_lagrange;
    p.row(v3) += w3 * -f3 * delta_lagrange;
    p.row(v4) += w4 * -f4 * delta_lagrange;
}

green_strain_elastic_constraint_t::scalar_type
green_strain_elastic_constraint_t::evaluate(positions_type const& p, masses_type const& m) const
{
    auto const v1 = this->indices().at(0);
    auto const v2 = this->indices().at(1);
    auto const v3 = this->indices().at(2);
    auto const v4 = this->indices().at(3);

    auto const p1 = p.row(v1);
    auto const p2 = p.row(v2);
    auto const p3 = p.row(v3);
    auto const p4 = p.row(v4);

    auto const w1 = 1. / m(v1);
    auto const w2 = 1. / m(v2);
    auto const w3 = 1. / m(v3);
    auto const w4 = 1. / m(v4);

    auto const Vsigned        = signed_volume(p);
    bool const is_V_positive  = Vsigned >= 0.;
    bool const is_V0_positive = V0_ >= 0.;
    bool const is_tet_inverted =
        (is_V_positive && !is_V0_positive) || (!is_V_positive && is_V0_positive);
    scalar_type constexpr epsilon = 1e-20;

    Eigen::Matrix3d Ds;
    Ds.col(0) = (p1 - p4).transpose();
    Ds.col(1) = (p2 - p4).transpose();
    Ds.col(2) = (p3 - p4).transpose();

    Eigen::Matrix3d const F = Ds * DmInv_;
    Eigen::Matrix3d const I = Eigen::Matrix3d::Identity();

    // TODO: Implement correct inversion handling described in
    // Irving, Geoffrey, Joseph Teran, and Ronald Fedkiw. "Invertible finite elements for robust
    // simulation of large deformation." Proceedings of the 2004 ACM SIGGRAPH/Eurographics symposium
    // on Computer animation. 2004.
    // scalar_type psi{};
    // Eigen::Matrix3d Piola;
    Eigen::JacobiSVD<Eigen::Matrix3d> UFhatV(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Vector3d const Fsigma = UFhatV.singularValues();
    Eigen::Matrix3d Fhat;
    Fhat.setZero();
    Fhat(0, 0) = Fsigma(0);
    Fhat(1, 1) = Fsigma(1);
    Fhat(2, 2) = Fsigma(2);

    Eigen::Matrix3d U       = UFhatV.matrixU();
    Eigen::Matrix3d const V = UFhatV.matrixV();

    if (is_tet_inverted)
    {
        Fhat(2, 2) = -Fhat(2, 2);
        U.col(2)   = -U.col(2);
    }

    // stress reaches maximum at 58% compression
    scalar_type constexpr min_singular_value = 0.577;
    Fhat(0, 0)                               = std::max(Fhat(0, 0), min_singular_value);
    Fhat(1, 1)                               = std::max(Fhat(1, 1), min_singular_value);
    Fhat(2, 2)                               = std::max(Fhat(2, 2), min_singular_value);

    Eigen::Matrix3d const Ehat     = 0.5 * (Fhat.transpose() * Fhat - I);
    scalar_type const EhatTrace    = Ehat.trace();
    Eigen::Matrix3d const Piolahat = Fhat * ((2. * mu_ * Ehat) + (lambda_ * EhatTrace * I));

    Eigen::Matrix3d const E  = U * Ehat * V.transpose();
    scalar_type const Etrace = E.trace();
    scalar_type const psi = mu_ * (E.array() * E.array()).sum() + 0.5 * lambda_ * Etrace * Etrace;

    scalar_type const V0 = std::abs(V0_);
    scalar_type const C = V0 * psi;
    return C;
}

} // namespace xpbd


#include <iostream>
#include <cstdint>
#include <Eigen/Dense>

#include "hnf.hpp"

template<class Derived>
bool is_rowHNF(const Eigen::MatrixBase<Derived> &mat) noexcept
{
    for(int i = 0, j = 0; j < mat.cols() && i < mat.rows(); ++j) {
        for(int k = i+1; k < mat.rows(); ++k)
            if (mat(k,j) != 0)
                return false;
        if (mat(i,j) != 0) {
            for(int k = 0; k < i; ++k)
                if (mat(k,j) < 0 || mat(i,j) < mat(k,j))
                    return false;
            ++i;
        }
    }

    return true;
}

template<class Derived>
bool is_colHNF(const Eigen::MatrixBase<Derived> &mat) noexcept
{
    return is_rowHNF(mat.transpose().eval());
}

template<class Derived>
bool is_unimodular(const Eigen::MatrixBase<Derived> &mat) noexcept
{
    return std::abs(mat.determinant()) == 1;
}

int main(int argc, char*argv[])
{
    std::cout << "==========" << std::endl;
    std::cout << "Invalid size matrices should be rejected." << std::endl;
    std::cout << "==========" << std::endl;
    {
        using namespace khover;

        Eigen::Matrix<int,6,4> mat, a, b;
        Eigen::Matrix<int,4,6> c, d;
        Eigen::Matrix<int,3,3> e;

        auto fst = hnf_LLL<rowops>(mat,std::tie(b,c,d),std::tie(a,b));
        auto snd = hnf_LLL<rowops>(mat,std::tie(c,d),std::tie(a,b,c));
        auto trd = hnf_LLL<colops>(mat,std::tie(c,d,e),std::tie(a,b));
        auto fth = hnf_LLL<colops>(mat,std::tie(c,d),std::tie(e,a,b));

        if(fst || snd || trd || fth)
        {
            std::cout << "Failed to detect wrong size matrices" << std::endl;
            return -1;
        }
    }

    std::cout << "==========" << std::endl;
    std::cout << "Row HNF" << std::endl;
    std::cout << "==========" << std::endl;
    {
        Eigen::Matrix<std::int64_t, 3,4> mat0;
        mat0 <<
            1, 2, 3, 4,
            5, 6, 7, 8,
            9, 10, 11, 12;
        auto mat = mat0;
        auto u = Eigen::Matrix<std::int64_t,3,3>::Identity().eval();
        auto rk = khover::hnf_LLL<typename khover::rowops>(mat,std::tie(u),std::tuple<>{});

        if (!is_rowHNF(mat) || u*mat != mat0 || !is_unimodular(u) || !rk || *rk!=2)
        {
            std::cout << "mat=" << std::endl;
            std::cout << mat << std::endl;
            std::cout << std::endl;
            std::cout << "u=" << std::endl;
            std::cout << u << std::endl;
            std::cout << "rk=" << (rk ? *rk : -1) << std::endl;
            std::cout << "u*mat=" << std::endl;
            std::cout << u*mat << std::endl;
            return -1;
        }
    }
    {
        Eigen::Matrix<std::int64_t, 3,4> mat0;
        mat0 <<
            0, 1, 2, 3,
            4, 5, 6, 7,
            8, 9,10,11;
        auto mat = mat0;
        auto u = Eigen::Matrix<std::int64_t,3,3>::Identity().eval();
        auto rk = khover::hnf_LLL<typename khover::rowops>(mat,std::tie(u),std::tuple<>{});

        if (!is_rowHNF(mat) || u*mat != mat0 || !is_unimodular(u) || !rk || *rk!=2)
        {
            std::cout << "mat=" << std::endl;
            std::cout << mat << std::endl;
            std::cout << std::endl;
            std::cout << "u=" << std::endl;
            std::cout << u << std::endl;
            std::cout << "rk=" << (rk ? *rk : -1) << std::endl;
            std::cout << "u*mat=" << std::endl;
            std::cout << u*mat << std::endl;
            return -1;
        }
    }

    std::cout << "==========" << std::endl;
    std::cout << "Column HNF" << std::endl;
    std::cout << "==========" << std::endl;
    {
        Eigen::Matrix<std::int64_t, 5,4> mat0;
        mat0 <<
            1, 2, 4, 7,
            2, 3, 4, 5,
            6, 7, 8, 9,
            1, 1, 2, 3,
            5, 8, 13, 21;
        auto mat = mat0;
        Eigen::Matrix<std::int64_t,4,4> u = decltype(u)::Identity();
        Eigen::Matrix<std::int64_t,4,4> v = decltype(v)::Identity();
        auto rk = khover::hnf_LLL<typename khover::colops>(mat,std::tie(u),std::tie(v));

        if (!is_colHNF(mat) || mat*u != mat0
            || v*u != Eigen::Matrix<std::int64_t,4,4>::Identity()
            || !rk || *rk != 4)
        {
            std::cout << "mat=" << std::endl;
            std::cout << mat << std::endl;
            std::cout << std::endl;
            std::cout << "u=" << std::endl;
            std::cout << u << std::endl;
            std::cout << "rk=" << (rk ? *rk : -1) << std::endl;
            return -1;
        }
    }
    {
        Eigen::Matrix<std::int64_t,3,3> mat0;
        mat0 <<
            1, 1, 0,
           -1, 0, 1,
            0,-1,-1;

        Eigen::Matrix<std::int64_t,3,3> u0;
        u0 <<
            1, 0, 0,
           -1, 1, 0,
            1, 0, 1;
        decltype(mat0) mat = mat0;
        decltype(u0) u = u0;
        Eigen::Matrix<std::int64_t,3,3> v = decltype(v)::Identity();
        auto rk = khover::hnf_LLL<typename khover::colops>(
            mat,std::tie(u),std::tie(v));

        if (!is_colHNF(mat) || mat*u != mat0*u0
            || v*u != u0
            || !rk || *rk != 2)
        {
            ERR_MSG(
                "mat=\n" << mat << std::endl
                << "u=\n" << u << std::endl
                << "v=\n" << v << std::endl
                << "rk=" << (rk ? *rk : -1) << std::endl
                << "mat0*u0=\n" << mat0*u0 << std::endl
                << "mat*u=\n" << mat*u << std::endl
                << "v*u=\n" << v*u
                );
            return -1;
        }
    }
    {
        Eigen::Matrix<std::int64_t, 3,4> mat0;
        mat0 <<
            0, 1, 2, 3,
            4, 5, 6, 7,
            8, 9,10,11;
        auto mat = mat0;
        auto u = Eigen::Matrix<std::int64_t,4,4>::Identity().eval();
        auto rk = khover::hnf_LLL<typename khover::colops>(mat,std::tie(u),std::tuple<>{});

        if (!is_colHNF(mat) || mat*u != mat0 || !is_unimodular(u) || !rk || *rk!=2)
        {
            ERR_MSG(
                "mat=\n" << mat << std::endl
                << "u=\n" << u << std::endl
                << "rk=" << (rk ? *rk : -1) << std::endl
                << "mat*u=\n" << mat*u
                );
            return -1;
        }
    }
    {
        Eigen::Matrix<std::int64_t, 9,27> mat0;
        mat0 <<
            1, 0, 0, 0, 0, 1, 0, 1, 3, 0, 0, 1, 0, 0, 0,-1, 0, 0, 0, 1, 3,-1, 0, 0,-3, 0, 0,
            0, 1, 0, 1, 0,-3, 0,-3,-8, 1, 0,-3, 0, 0, 0, 0,-1, 0, 0,-3,-8, 0,-1, 0, 0,-3, 0,
            0, 0, 1, 0, 1, 3, 1, 3, 6, 0, 1, 3, 0, 0, 0, 0, 0,-1, 1, 3, 6, 0, 0,-1, 0, 0,-3,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 3, 1, 3, 0, 0, 0, 3, 1, 3, 8, 0, 0,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 0,-6, 0, 0,-8, 0, 0, 0, 0, 0,-8, 0, 8, 0,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 6, 1, 3, 9, 0, 0, 0, 1, 3, 9, 0, 0, 8,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,-1, 0, 0,-3, 0, 1, 0, 0, 0,-3, 0, 1,-6, 2, 6,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,-1, 0, 1,-3,-3, 0, 0, 0, 1,-3,-3, 0,-12,-16,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,-1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 2, 6, 6;
        auto mat = mat0;
        auto u = Eigen::Matrix<std::int64_t,27,27>::Identity().eval();
        auto v = Eigen::Matrix<std::int64_t,27,27>::Identity().eval();
        auto rk = khover::hnf_LLL<khover::colops>(mat,std::tie(u),std::tie(v));

        if (!is_colHNF(mat)
            || mat*u != mat0
            || !(u*v).isIdentity() ||
            !rk || *rk!=7)
        {
            ERR_MSG(
                "mat=\n" << mat << std::endl
                << "u=\n" << u << std::endl
                << "rk=" << (rk ? *rk : -1) << std::endl
                << "mat*u=\n" << mat*u
                );
            return -1;
        }
        std::cout << mat.leftCols(1+*rk) << std::endl;
    }

    std::cout << "==========" << std::endl;
    std::cout << "HNF of zero matrices" << std::endl;
    std::cout << "==========" << std::endl;
    {
        using matrix_t = Eigen::Matrix<std::int64_t,Eigen::Dynamic,Eigen::Dynamic>;
        matrix_t zmat;

        for(std::size_t r = 0; r < 5; ++r) {
            for(std::size_t c = 0; c < 5; ++c) {
                // test row HNF
                zmat = matrix_t::Zero(r,c);
                auto rowrk = khover::hnf_LLL<khover::rowops>(zmat, {}, {});
                if (!rowrk || *rowrk != 0 || !zmat.isZero()) {
                    std::cout << "row HNF of zero is not zero!" << std::endl;
                    std::cout << "rk=" << (rowrk ? *rowrk : -1) << std::endl;
                    std::cout << zmat << std::endl;
                    return -1;
                }
                // test column HNF
                zmat.setZero();
                auto colrk = khover::hnf_LLL<khover::colops>(zmat, {}, {});
                if (!colrk || *colrk!=0 || !zmat.isZero()) {
                    std::cout << "column HNF of zero is not zero!" << std::endl;
                    std::cout << "rk=" << (colrk ? *colrk : -1) << std::endl;
                    std::cout << zmat << std::endl;
                    return -1;
                }
            }
        }
    }

    return 0;
}


#pragma once
#include <tuple>
#include <vector>
#include <Eigen/Dense>

typedef std::tuple<double, double, double, double, double> quintet;
typedef std::tuple<std::vector<double>, double, double, double, double>
        vquintet;

// user interface

namespace fine {
Eigen::VectorXd heston_formula(std::vector<double> strikes,
                               std::vector<double> maturities,
                               double underlying_price, double volatility,
                               double interest_rate, double kappa, double theta,
                               double nu, double rho);

Eigen::VectorXd heston_delta(std::vector<double> strikes,
                             std::vector<double> maturities,
                             double underlying_price, double volatility,
                             double interest_rate, double kappa, double theta,
                             double nu, double rho);

Eigen::VectorXd heston_vega(std::vector<double> strikes,
                            std::vector<double> maturities,
                            double underlying_price, double volatility,
                            double interest_rate, double kappa, double theta,
                            double nu, double rho);

quintet heston_calibration(std::vector<double> prices,
                           std::vector<double> strikes,
                           std::vector<double> maturities,
                           double underlying_price,
                           double interest_rate);

vquintet heston_calibration_ts(std::vector<double> prices,
                               std::vector<double> strikes,
                               std::vector<double> maturities,
                               std::vector<double> underlying_prices,
                               double interest_rate);

double heston_calibrate_vol(std::vector<double> prices,
                            std::vector<double> strikes,
                            std::vector<double> maturities,
                            double underlying_price, double interest_rate,
                            double kappa, double theta, double nu, double rho);
}


#ifndef SIMPLEMLP_HPP
#define SIMPLEMLP_HPP

#include <string>
#include <vector>
#include <Eigen/Dense>
#include <cstdlib>
#include "iostream"
#include <fstream>
#include <cmath>

namespace rai {

namespace FuncApprox {

enum class ActivationType {
  linear,
  relu,
  tanh,
  softsign
};

template<typename Dtype, ActivationType activationType>
struct Activation {
  inline void nonlinearity(Eigen::Matrix<Dtype, -1, -1> &output) {}
  inline void nonlinearity(Eigen::Matrix<Dtype, -1, 1> &output) {}
};

template<typename Dtype>
struct Activation<Dtype, ActivationType::relu> {
  inline void nonlinearity(Eigen::Matrix<Dtype, -1, -1> &output) {
    output = output.cwiseMax(0.0);
  }

  inline void nonlinearity(Eigen::Matrix<Dtype, -1, 1> &output) {
    output = output.cwiseMax(0.0);
  }
};

template<typename Dtype>
struct Activation<Dtype, ActivationType::tanh> {
  inline void nonlinearity(Eigen::Matrix<Dtype, -1, -1> &output) {
    output = output.array().tanh();
  }
  inline void nonlinearity(Eigen::Matrix<Dtype, -1, 1> &output) {
    output = output.array().tanh();
  }
};

template<typename Dtype>
struct Activation<Dtype, ActivationType::softsign> {
  inline void nonlinearity(Eigen::Matrix<Dtype, -1, -1> &output) {
    for (int i = 0; i < output.size(); i++) {
      output[i] = output[i] / (std::abs(output[i]) + 1.0);
    }
  }

  inline void nonlinearity(Eigen::Matrix<Dtype, -1, 1> &output) {
    for (int i = 0; i < output.size(); i++) {
      output[i] = output[i] / (std::abs(output[i]) + 1.0);
    }
  }
};

template<typename Dtype, int StateDim, int ActionDim, ActivationType activationType>
class MLP_fullyconnected {

 public:
  typedef Eigen::Matrix<Dtype, ActionDim, 1> Action;
  typedef Eigen::Matrix<Dtype, StateDim, 1> State;

  MLP_fullyconnected(std::vector<int> hiddensizes) {
    const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n");

    layersizes.push_back(StateDim);
    layersizes.reserve(layersizes.size() + hiddensizes.size());
    layersizes.insert(layersizes.end(), hiddensizes.begin(), hiddensizes.end());
    layersizes.push_back(ActionDim);
    ///[input hidden output]

    params.resize(2 * (layersizes.size() - 1));
    Ws.resize(layersizes.size() - 1);
    bs.resize(layersizes.size() - 1);
    lo.resize(layersizes.size());
    Stdev.resize(ActionDim);

    for (int i = 0; i < params.size(); i++) {
      int paramSize = 0;

      if (i % 2 == 0) ///W resize
      {
        Ws[i / 2].resize(layersizes[i / 2 + 1], layersizes[i / 2]);
        params[i].resize(layersizes[i / 2] * layersizes[i / 2 + 1]);
      }
      if (i % 2 == 1) ///b resize
      {
        bs[(i - 1) / 2].resize(layersizes[(i + 1) / 2]);
        params[i].resize(layersizes[(i + 1) / 2]);
      }
    }

  }

  bool load_eigen_from_binary(const std::string filename, Eigen::Matrix<Dtype, Eigen::Dynamic, Eigen::Dynamic> &data) {
    std::ifstream in(filename, std::ios::in | std::ios::binary);
    if (!in.is_open()) {
      return false;
    }
    size_t rows = 0;
    size_t cols = 0;
    in.read((char *) (&rows), sizeof(size_t));
    in.read((char *) (&cols), sizeof(size_t));
    data.resize(rows, cols);
    in.read((char *) data.data(), rows * cols * sizeof(Dtype));

    in.close();
    return true;
  }

  void updateParamFromBin(std::string fileName) {
    Eigen::Matrix<Dtype, -1, -1> temp;
    load_eigen_from_binary(fileName, temp);

    size_t pos = 0;

    for (int i = 0; i < params.size(); i++) {
      int paramSize = 0;
      for (size_t j = 0; j < params[i].size(); j++) {
        params[i](j) = temp.data()[pos++];
      }
      if (i % 2 == 0) ///W copy
      {
        memcpy(Ws[i / 2].data(), params[i].data(), sizeof(Dtype) * Ws[i / 2].size());
      }
      if (i % 2 == 1) ///b copy
      {
        memcpy(bs[(i - 1) / 2].data(), params[i].data(), sizeof(Dtype) * bs[(i - 1) / 2].size());
      }
    }
  }

  void updateParamFromBin_noesis(std::string fileName) {
    Eigen::Matrix<Dtype, -1, -1> temp;
    load_eigen_from_binary(fileName, temp);

    for (int i = 0; i < params.size(); i++) {
      int paramSize = 0;

      if (i % 2 == 0) ///W resize
      {
        Ws[i / 2].resize(layersizes[i / 2 + 1], layersizes[i / 2]);
        params[i].resize(layersizes[i / 2] * layersizes[i / 2 + 1]);
      }
      if (i % 2 == 1) ///b resize
      {
        bs[(i - 1) / 2].resize(layersizes[(i + 1) / 2]);
        params[i].resize(layersizes[(i + 1) / 2]);
      }
    }

    /// output layer

    size_t pos = 0;

    // Wo
    for (size_t j = 0; j <params.end()[-2].size(); j++) {
      params.end()[-2](j) = temp.data()[pos++];
    }

    // bo
    for (size_t j = 0; j <params.end()[-1].size(); j++) {
      params.end()[-1](j) = temp.data()[pos++];
    }

    memcpy(Ws.back().data(), params.end()[-2].data(), sizeof(Dtype) * Ws.back().size());
    memcpy(bs.back().data(), params.end()[-1].data(), sizeof(Dtype) * bs.back().size());

    for (int i = 0; i < params.size() - 2; i++) {
      int paramSize = 0;
      for (size_t j = 0; j < params[i].size(); j++) {
        params[i](j) = temp.data()[pos++];
      }
      if (i % 2 == 0) ///W copy
      {
        memcpy(Ws[i / 2].data(), params[i].data(), sizeof(Dtype) * Ws[i / 2].size());
      }
      if (i % 2 == 1) ///b copy
      {
        memcpy(bs[(i - 1) / 2].data(), params[i].data(), sizeof(Dtype) * bs[(i - 1) / 2].size());
      }
    }
  }

  void updateParamFromTxt(std::string fileName) {
    const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n");

    std::ifstream indata;
    indata.open(fileName);
    std::string line;
    getline(indata, line);
    std::stringstream lineStream(line);
    std::string cell;

    int totalN = 0;
    ///assign parameters
    for (int i = 0; i < params.size(); i++) {
      int paramSize = 0;

      while (std::getline(lineStream, cell, ',')) { ///Read param
        params[i](paramSize++) = std::stod(cell);
        if (paramSize == params[i].size()) break;
      }
      totalN += paramSize;
      if (i % 2 == 0) ///W copy
        memcpy(Ws[i / 2].data(), params[i].data(), sizeof(Dtype) * Ws[i / 2].size());
      if (i % 2 == 1) ///b copy
        memcpy(bs[(i - 1) / 2].data(), params[i].data(), sizeof(Dtype) * bs[(i - 1) / 2].size());
    }

  }

  inline Action forward(State &state) {

    lo[0] = state;

    for (int cnt = 0; cnt < Ws.size() - 1; cnt++) {
      lo[cnt + 1] = Ws[cnt] * lo[cnt] + bs[cnt];
      activation_.nonlinearity(lo[cnt + 1]);
    }

    lo[lo.size() - 1] = Ws[Ws.size() - 1] * lo[lo.size() - 2] + bs[bs.size() - 1]; /// output layer
    return lo.back();
  }

  inline Action forwardtemp(State &state) {
    Eigen::Matrix<Dtype, -1, 1> temp;
    temp = state;

    for (int cnt = 0; cnt < Ws.size() - 1; cnt++) {
      temp = Ws[cnt] * temp + bs[cnt];
      activation_.nonlinearity(temp);
    }
    temp = Ws.back() * temp + bs.back(); /// output layer
    return temp;
  }

 private:
  std::vector<Eigen::Matrix<Dtype, -1, 1>> params;
  std::vector<Eigen::Matrix<Dtype, -1, -1>> Ws;
  std::vector<Eigen::Matrix<Dtype, -1, 1>> bs;
  std::vector<Eigen::Matrix<Dtype, -1, 1>> lo;

  Activation<Dtype, activationType> activation_;
  Action Stdev;

  std::vector<int> layersizes;
  bool isTanh = false;
};

}

}

#endif //SIMPLEMLP_HPP


/**
 * expression test
 * @author Tobias Weber <tweber@ill.fr>
 * @date 28-mar-20
 * @license GPLv3, see 'LICENSE' file
 *
 * g++-10 -std=c++20 -I.. -o expr expr.cpp ../libs/log.cpp
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE Expr Test
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include "libs/expr.h"
#include "libs/str.h"
#include "libs/maths.h"

using t_types_real = std::tuple<double, float>;
using t_types_int = std::tuple<int, long>;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_expr_real, t_real, t_types_real)
{
	static constexpr t_real eps = 1e-6;
	tl2::ExprParser<t_real> parser;

	bool ok = parser.parse("1 + 2*3");
	auto result = parser.eval();
	BOOST_TEST(ok);
	BOOST_TEST(tl2::equals<t_real>(result, 7, eps));

	ok = parser.parse("4 + 5*6");
	result = parser.eval();
	BOOST_TEST(ok);
	BOOST_TEST(tl2::equals<t_real>(result, 34, eps));

	ok = parser.parse(" - (sqrt(4)-5)^3 -  5/2 ");
	result = parser.eval();
	BOOST_TEST(ok);
	BOOST_TEST(tl2::equals<t_real>(result, 24.5, eps));

	ok = parser.parse("-cos(sin(1.23*pi))^(-1.2 + 3.2)");
	result = parser.eval();
	BOOST_TEST(ok);
	BOOST_TEST(tl2::equals<t_real>(result, -0.6228, 1e-3));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_expr_func, t_real, t_types_real)
{
	static constexpr t_real eps = 1e-6;
	auto tupres = tl2::eval_expr<std::string, t_real>("\t2 + \t2*3*4\n");

	BOOST_TEST(
		std::get<0>(tupres),
		tl2::equals<t_real>(std::get<1>(tupres), 14., eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_expr_int, t_int, t_types_int)
{
	tl2::ExprParser<t_int> parser;

	bool ok = parser.parse("1 + 2*3");
	t_int result = parser.eval();
	BOOST_TEST(ok);
	BOOST_TEST(result == 7);

	ok = parser.parse("4 + 5*6");
	result = parser.eval();
	BOOST_TEST(ok);
	BOOST_TEST(result == 34);
}


/*
 * concatenate Eigen matrices
 * by R. Falque
 * 03/07/2019
 */

#ifndef EIGEN_FIND_HPP
#define EIGEN_FIND_HPP

#include <iostream>
#include <Eigen/Core>

// https://stackoverflow.com/a/21496281/2562693
//template <typename T>
//inline Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> concatenate(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> in_1,
//                                                                    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> in_2,
//                                                                    int direction);

template <typename T>
inline Eigen::VectorXi find(Eigen::Matrix<T, Eigen::Dynamic, 1> X, T x){
    Eigen::VectorXi elements( (X.array() == x).count() );

    int counter =0;
    for (int i=0; i<X.rows(); i++)
        if (X(i) == x) {
            elements(counter) = i;
            counter ++;
        }
    return elements;
};

template <typename T>
inline Eigen::VectorXi find(Eigen::Matrix<T, 1, Eigen::Dynamic> X, T x){
    Eigen::VectorXi elements( (X.array() == x).count() );

    int counter =0;
    for (int i=0; i<X.rows(); i++)
        if (X(i) == x) {
            elements(counter) = i;
            counter ++;
        }
    return elements;
};

template <typename T>
inline Eigen::Matrix<T, 1, Eigen::Dynamic> boolean_selection(Eigen::Matrix<T, 1, Eigen::Dynamic> X, Eigen::VectorXi selection){

    std::vector <T> vector_handle;

    int counter =0;
    for (int i=0; i<X.rows(); i++)
        if (selection(i) == 1)
            vector_handle.push_back(X(i));

    Eigen::Matrix<T, 1, Eigen::Dynamic> elements = Eigen::Map<Eigen::Matrix<T, 1, Eigen::Dynamic>, Eigen::Unaligned>(vector_handle.data(), vector_handle.size());

    return elements;
};

inline Eigen::VectorXd index_slice(Eigen::VectorXd X, Eigen::VectorXi selection){
    Eigen::VectorXd out;
    out.resize(selection.cols());

    for (int i=0; i<selection.cols(); i++)
        out(i) = X(selection(i));

    return out;
};

//https://stackoverflow.com/a/21068014/2562693
template <typename T>
inline void removeRow(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& matrix, unsigned int rowToRemove)
{
    unsigned int numRows = matrix.rows()-1;
    unsigned int numCols = matrix.cols();

    if( rowToRemove < numRows )
        matrix.block(rowToRemove,0,numRows-rowToRemove,numCols) = matrix.block(rowToRemove+1,0,numRows-rowToRemove,numCols);

    matrix.conservativeResize(numRows,numCols);
}

template <typename T>
inline void removeCol(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& matrix, unsigned int colToRemove)
{
    unsigned int numRows = matrix.rows();
    unsigned int numCols = matrix.cols()-1;

    if( colToRemove < numCols )
        matrix.block(0,colToRemove,numRows,numCols-colToRemove) = matrix.block(0,colToRemove+1,numRows,numCols-colToRemove);

    matrix.conservativeResize(numRows,numCols);
}

#endif


#include <Eigen/Core>
#include <catch2/catch.hpp>
#include <random>
#include <vector>
#include "ear/common/geom.hpp"
#include "ear/common_types.hpp"

using namespace ear;

TEST_CASE("PolarPosition") {
  auto position = GENERATE(std::make_pair(0.0, 0.0), std::make_pair(0.0, 30.0),
                           std::make_pair(30.0, 0.0));
  double az = position.first;
  double el = position.second;
  REQUIRE(toCartesianVector3d(PolarPosition(az, el, 1.0))
              .isApprox(cart(az, el, 1.0)));
  REQUIRE(toCartesianVector3d(PolarPosition(az, el, 2.0))
              .isApprox(cart(az, el, 2.0)));
  REQUIRE(toNormalisedVector3d(PolarPosition(az, el, 2.0))
              .isApprox(cart(az, el, 1.0)));
}

TEST_CASE("cart") {
  REQUIRE(cart(0.0, 0.0, 1.0).isApprox(Eigen::Vector3d(0.0, 1.0, 0.0)));
  REQUIRE(cart(0.0, 0.0, 2.0).isApprox(Eigen::Vector3d(0.0, 2.0, 0.0)));
  REQUIRE(cart(0.0, 45.0, sqrt(2.0)).isApprox(Eigen::Vector3d(0.0, 1.0, 1.0)));
  REQUIRE(cart(45.0, 0.0, sqrt(2.0)).isApprox(Eigen::Vector3d(-1.0, 1.0, 0.0)));
}

TEST_CASE("test_azimuth") {
  REQUIRE(azimuth(Eigen::Vector3d(1.0, 1.0, 0.0)) == Approx(-45.0));
}

TEST_CASE("test_elevation") {
  REQUIRE(elevation(Eigen::Vector3d(0.0, 1.0, 1.0)) == Approx(45.0));
  REQUIRE(elevation(Eigen::Vector3d(std::sqrt(2.0), std::sqrt(2.0), 2.0)) ==
          Approx(45.0));
}

TEST_CASE("test_relative_angle") {
  REQUIRE(relativeAngle(0.0, 10.0) == 10.0);
  REQUIRE(relativeAngle(10.0, 10.0) == 10.0);
  REQUIRE(relativeAngle(11.0, 10.0) == 370.0);
  REQUIRE(relativeAngle(370.0, 10.0) == 370.0);
  REQUIRE(relativeAngle(371.0, 10.0) == 360.0 + 370.0);
}

TEST_CASE("test_inside_angle_range") {
  REQUIRE(insideAngleRange(0, 0, 10));
  REQUIRE(insideAngleRange(5, 0, 10));
  REQUIRE(insideAngleRange(10, 0, 10));

  REQUIRE(insideAngleRange(0, 10, 00));
  REQUIRE(!insideAngleRange(5, 10, 0));
  REQUIRE(insideAngleRange(15, 10, 0));
  REQUIRE(insideAngleRange(10, 10, 0));

  REQUIRE(insideAngleRange(0, -10, 10));
  REQUIRE(!insideAngleRange(180, -10, 10));
  REQUIRE(!insideAngleRange(-180, -10, 10));

  REQUIRE(insideAngleRange(0, -180, 180));
  REQUIRE(!insideAngleRange(0, -181, 181));
  REQUIRE(insideAngleRange(0, -180, 180, 1));

  REQUIRE(insideAngleRange(180, 180, -180));
  REQUIRE(insideAngleRange(180, 180, -180, 1));
  REQUIRE(!insideAngleRange(90, 180, -180));

  REQUIRE(insideAngleRange(0, 0, 0));
  REQUIRE(insideAngleRange(0, 0, 0, 1));
  REQUIRE(!insideAngleRange(90, 0, 0));

  REQUIRE(insideAngleRange(0, 1, 2, 2));
  REQUIRE(insideAngleRange(-1, 1, 2, 2));
  REQUIRE(insideAngleRange(359, 1, 2, 2));
}

/** @brief Are a and b the same, module some reversal or shift? */
bool inSameOrder(Eigen::MatrixXd a, Eigen::MatrixXd b) {
  // just produce all shifted and reversed versions of a, and compare
  // against b. Inefficient but simple.
  Eigen::MatrixXd a_shifted(a.rows(), a.cols());
  Eigen::MatrixXd a_shifted_rev(a.rows(), a.cols());
  Eigen::VectorXi indices(a.rows());
  for (int offset = 0; offset < a.rows(); ++offset) {
    a_shifted << a.bottomRows(a.rows() - offset), a.topRows(offset);
    if (a_shifted.isApprox(b)) {
      return true;
    }
    a_shifted_rev << a_shifted.colwise().reverse();
    if (a_shifted_rev.isApprox(b)) {
      return true;
    }
  }
  return false;
}

TEST_CASE("test_order_vertices_random") {
  Eigen::MatrixXd ngon1(4, 3);
  Eigen::MatrixXd ngon2(4, 3);
  Eigen::MatrixXd ngon3(5, 3);
  Eigen::MatrixXd ngon4(5, 3);
  ngon1 << cartT(30.0, 0.0, 1.0), cartT(-30.0, 0.0, 1.0),
      cartT(-30.0, 30.0, 1.0), cartT(30.0, 30.0, 1.0);
  ngon2 << cartT(30.0, 0.0, 1.0), cartT(-30.0, 0.0, 1.0),
      cartT(-30.0, 30.0, 1.0), cartT(30.0, 30.0, 1.0);
  ngon3 << cartT(30.0, 30.0, 1.0), cartT(0.0, 30.0, 1.0),
      cartT(-30.0, 30.0, 1.0), cartT(-110.0, 30.0, 1.0),
      cartT(110.0, 30.0, 1.0);
  ngon4 << cartT(30.0, 0.0, 1.0), cartT(0.0, 0.0, 1.0), cartT(-30.0, 0.0, 1.0),
      cartT(-110.0, 0.0, 1.0), cartT(110.0, 0.0, 1.0);
  std::vector<Eigen::MatrixXd> ngons = {ngon1, ngon2, ngon3, ngon4};

  std::default_random_engine generator;
  std::normal_distribution<double> distribution(0.0, 1.0);
  auto normal = [&](double) { return distribution(generator); };

  for (const auto& orderedNgon : ngons) {
    int numberOfNgons = static_cast<int>(orderedNgon.rows());
    Eigen::MatrixXd orderedNgonRandomized;
    Eigen::MatrixXd unorderedNgon;
    Eigen::MatrixXd reorderedNgon;
    Eigen::VectorXi indices;

    for (int i = 0; i < 10; ++i) {
      if (i == 0) {
        orderedNgonRandomized = orderedNgon;
      } else {
        Eigen::Matrix3d T = Eigen::Matrix3d::NullaryExpr(3, 3, normal);
        Eigen::Vector3d offset = Eigen::Vector3d::NullaryExpr(3, normal);
        orderedNgonRandomized =
            (orderedNgon * T).rowwise() + offset.transpose();
      }

      Eigen::VectorXi orderVec =
          Eigen::VectorXi::LinSpaced(numberOfNgons, 0, numberOfNgons);
      do {
        unorderedNgon = orderedNgonRandomized(orderVec, Eigen::all);
        indices = ngonVertexOrder(unorderedNgon);
        reorderedNgon = unorderedNgon(indices, Eigen::all);
        REQUIRE(inSameOrder(reorderedNgon, orderedNgonRandomized));
      } while (std::next_permutation(orderVec.begin(), orderVec.end()));
    }
  }
}

TEST_CASE("test_order_vertices_no_random") {
  Eigen::MatrixXd orderedNgon(5, 3);
  Eigen::MatrixXd unorderedNgon;
  Eigen::MatrixXd reorderedNgon;
  Eigen::VectorXi indices;
  orderedNgon << cartT(30.0, 0.0, 1.0),  //
      cartT(0.0, 0.0, 1.0),  //
      cartT(-30.0, 0.0, 1.0),  //
      cartT(-30.0, 30.0, 1.0),  //
      cartT(30.0, 30.0, 1.0);
  Eigen::VectorXi orderVec = Eigen::VectorXi::LinSpaced(
      orderedNgon.rows(), 0, static_cast<int>(orderedNgon.rows()));
  do {
    unorderedNgon = orderedNgon(orderVec, Eigen::all);
    indices = ngonVertexOrder(unorderedNgon);
    reorderedNgon = unorderedNgon(indices, Eigen::all);
    REQUIRE(inSameOrder(reorderedNgon, orderedNgon));
  } while (std::next_permutation(orderVec.begin(), orderVec.end()));
}

TEST_CASE("test_local_coordinate_system") {
  Eigen::RowVector3d x{1.0, 0.0, 0.0};
  Eigen::RowVector3d y{0.0, 1.0, 0.0};
  Eigen::RowVector3d z{0.0, 0.0, 1.0};
  Eigen::Matrix3d expected;

  REQUIRE(localCoordinateSystem(0, 0).isApprox(Eigen::Matrix3d::Identity()));
  expected << -y, x, z;
  REQUIRE(localCoordinateSystem(-90, 0).isApprox(expected));
  expected << y, -x, z;
  REQUIRE(localCoordinateSystem(90, 0).isApprox(expected));
  expected << -x, -y, z;
  REQUIRE(localCoordinateSystem(180, 0).isApprox(expected));
  expected << x, z, -y;
  REQUIRE(localCoordinateSystem(0, 90).isApprox(expected));
  expected << x, -z, y;
  REQUIRE(localCoordinateSystem(0, -90).isApprox(expected));
  expected << -y, z, -x;
  REQUIRE(localCoordinateSystem(-90, 90).isApprox(expected));
}