#include <fstream>
#include <algorithm>
#include <iterator>

#include <boost/functional/value_factory.hpp>
#include <boost/array.hpp>

#include <CGAL/assertions.h>
#include <CGAL/algorithm.h>
#include <CGAL/point_generators_3.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/AABB_face_graph_triangle_primitive.h>
#include <CGAL/Timer.h>

typedef CGAL::Epeck K;
typedef K::FT FT;
typedef K::Point_3 Point;
typedef K::Vector_3 Vector;
typedef K::Segment_3 Segment;
typedef K::Ray_3 Ray;
typedef CGAL::Polyhedron_3<K> Polyhedron;
typedef CGAL::AABB_face_graph_triangle_primitive<Polyhedron> Primitive;
typedef CGAL::AABB_traits<K, Primitive> Traits;
typedef CGAL::AABB_tree<Traits> Tree;
typedef Tree::Primitive_id Primitive_id;
typedef CGAL::Timer Timer;

FT point_on_ray_dist(const Ray& ray, const Point& point) {
  Vector i_ray(point, ray.source());
  return i_ray.squared_length();
}

std::size_t accum = 0;

boost::optional<
  Tree::Intersection_and_primitive_id<Ray>::Type
  >
min_intersection(const Tree& tree, const Ray& ray) {
  typedef std::vector< Tree::Intersection_and_primitive_id<Ray>::Type > IntersectionVector;
  IntersectionVector all_intersections;

  tree.all_intersections(ray, std::back_inserter(all_intersections));
  accum += all_intersections.size();
  Tree::FT min_distance = DBL_MAX;
  boost::optional<
    Tree::Intersection_and_primitive_id<Ray>::Type
    > mini = boost::none;

  for(IntersectionVector::iterator it2 = all_intersections.begin(); it2 != all_intersections.end(); ++it2) {
    if(Point* point = boost::get<Point>(&(it2->first))) {
      Vector i_ray(*point, ray.source());
      Tree::FT new_distance = i_ray.squared_length();
      if(new_distance < min_distance) {
        mini = *it2;
        min_distance = new_distance;
      }
    } else {
      std::cout << "ERROR ignored a segment" << std::endl;
    }
  }
  return mini;
}

int main()
{
  Polyhedron polyhedron;
  {
    // Point p(1.0, 0.0, 0.0);
    // Point q(0.0, 1.0, 0.0);
    // Point r(0.0, 0.0, 1.0);
    // Point s(0.0, 0.0, 0.0);
    // polyhedron.make_tetrahedron(p, q, r, s);
  }

  std::ifstream in("data/bunny00.off");
  if(in)
    in >> polyhedron;
  else{
    std::cout << "error reading bunny" << std::endl;
    return 1;
  }

  Timer t;
  t.start();

  Tree tree(faces(polyhedron).first, faces(polyhedron).second, polyhedron);
  Tree::Bounding_box bbox = tree.bbox();
  Vector bbox_center((bbox.xmin() + bbox.xmax()) / 2,
                     (bbox.ymin() + bbox.ymax()) / 2,
                     (bbox.zmin() + bbox.zmax()) / 2);
  boost::array<double, 3> extents;
  extents[0] = bbox.xmax() - bbox.xmin();
  extents[1] = bbox.ymax() - bbox.ymin();
  extents[2] = bbox.zmax() - bbox.zmin();
  double max_extent = *std::max_element(extents.begin(), extents.end());

  std::cout << bbox << std::endl;
  std::cout << bbox_center << std::endl;
  std::cout << max_extent << std::endl;

  const int NB_RAYS = 1000;
  std::vector<Point> v1, v2;
  v1.reserve(NB_RAYS); v2.reserve(NB_RAYS);

  const double r = max_extent / 2;
  // Generate NB_RAYS*2 points that lie on a sphere of radius r, centered around bbox_center
  CGAL::Random rand = CGAL::Random(23); // fix the seed to yield the same results each run
  std::copy_n(CGAL::Random_points_on_sphere_3<Point>(r, rand), NB_RAYS, std::back_inserter(v1));
  std::copy_n(CGAL::Random_points_on_sphere_3<Point>(r, rand), NB_RAYS, std::back_inserter(v2));

  for(std::vector<Point>::iterator it = v1.begin(); it != v1.end(); ++it) {
    *it = *it + bbox_center;
  }

  for(std::vector<Point>::iterator it = v2.begin(); it != v2.end(); ++it) {
    *it = *it + bbox_center;
  }

  // Generate NB_RAYS using v1 as source and v2 as target.
  std::vector<Ray> rays;
  rays.reserve(NB_RAYS);
  std::transform(v1.begin(), v1.end(), v2.begin(),
                 std::back_inserter(rays), boost::value_factory<Ray>());
  std::vector< boost::optional<Tree::Intersection_and_primitive_id<Ray>::Type > > primitives1, primitives2;
  primitives1.reserve(NB_RAYS); primitives2.reserve(NB_RAYS);

  {
    for(std::vector<Ray>::iterator it = rays.begin(); it != rays.end(); ++it) {
      primitives1.push_back(min_intersection(tree, *it));
    }
  }

  for(std::vector<Ray>::iterator it = rays.begin(); it != rays.end(); ++it) {
    primitives2.push_back(tree.first_intersection(*it));
  }
  assert(primitives1.size() == primitives2.size()); //  Different amount of primitives intersected
  assert(std::equal(primitives1.begin(), primitives1.end(), primitives2.begin())); //  Primitives mismatch
  std::size_t c = primitives1.size() - std::count(primitives1.begin(), primitives1.end(), boost::none);
  std::cout << "Intersected " << c << " primitives with " << NB_RAYS << " rays" << std::endl;
  std::cout << "Primitive method had to sort " << accum/NB_RAYS
            << " intersections on average." << std::endl;
  t.stop();
  std::cout << t.time() << std::endl;
  return 0;
}


#include <iostream>
#include <stdexcept>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/QR>
#include <Eigen/Eigenvalues>
#include <Eigen/IterativeLinearSolvers>
#include <unsupported/Eigen/IterativeSolvers>
#include <chrono>

#include "DavidsonSolver.hpp"

DavidsonSolver::DavidsonSolver(){}

void DavidsonSolver::set_correction(std::string method) {
    if (method == "DPR") this->correction = CORR::DPR;
    else if (method == "JACOBI") this->correction = CORR::JACOBI;
    else if (method == "OLSEN") this->correction = CORR::OLSEN;
    else throw std::runtime_error("Not a valid correction method");
}

void DavidsonSolver::set_jacobi_linsolve(std::string method) {
    if (method == "CG") this->jacobi_linsolve = LSOLVE::CG;
    else if (method == "GMRES") this->jacobi_linsolve = LSOLVE::GMRES;
    else if (method == "LLT") this->jacobi_linsolve = LSOLVE::LLT;
    else throw std::runtime_error("Not a valid linsolve method");
}

Eigen::ArrayXd DavidsonSolver::_sort_index(Eigen::VectorXd& V) const
{
    Eigen::ArrayXd idx = Eigen::ArrayXd::LinSpaced(V.rows(),0,V.rows()-1);
    std::sort(idx.data(),idx.data()+idx.size(),
              [&](int i1, int i2){return V[i1]<V[i2];});
    return idx;
}

Eigen::MatrixXd DavidsonSolver::_get_initial_eigenvectors(Eigen::VectorXd &d, int size_initial_guess) const
{

    Eigen::MatrixXd guess;
    if (this->guess_vectors =="identity")
    {
            guess = Eigen::MatrixXd::Identity(d.size(),size_initial_guess);
    }

    else if (this->guess_vectors == "random")
    {
        guess = Eigen::MatrixXd::Random(d.size(),size_initial_guess);
        guess = DavidsonSolver::_QR(guess);
    }
    else if (this->guess_vectors=="target")
    {
        guess = Eigen::MatrixXd::Zero(d.size(),size_initial_guess);
        Eigen::ArrayXd idx = DavidsonSolver::_sort_index(d);

        for (int j=0; j<size_initial_guess;j++) {
            guess(idx(j),j) = 1.0;
        }
    }
    return guess;
}

Eigen::MatrixXd DavidsonSolver::_solve_linear_system(Eigen::MatrixXd &A, Eigen::VectorXd &r) const
{
    Eigen::MatrixXd w;
    std::chrono::time_point<std::chrono::system_clock> start, end;
    std::chrono::duration<double> elapsed_time;

    start = std::chrono::system_clock::now();
    switch (this->jacobi_linsolve) {

        case LSOLVE::CG :  {
                Eigen::ConjugateGradient<Eigen::MatrixXd, Eigen::Lower|Eigen::Upper> cg;
                cg.setTolerance(this->linsolve_tol);
                cg.compute(A);
                w = cg.solve(r);
            }
            break;
        case LSOLVE::GMRES : {
                Eigen::GMRES<Eigen::MatrixXd, Eigen::IdentityPreconditioner> gmres;
                gmres.setTolerance(this->linsolve_tol);
                gmres.compute(A);
                w = gmres.solve(r);
            }
            break;
        case LSOLVE::LLT :
            w = A.llt().solve(r);
            break;
    }
    end = std::chrono::system_clock::now();
    elapsed_time  = end-start;
    std::cout << "_ solve linear system " << this->jacobi_linsolve << " in " << elapsed_time.count() << " secs" <<  std::endl;
    return w;
}

Eigen::VectorXd DavidsonSolver::_olsen_correction(Eigen::VectorXd &r, Eigen::VectorXd &x, Eigen::VectorXd &D, double lambda) const
{
    /* Compute the olsen correction :

    \delta = (D-\lambda)^{-1} (-r + \epsilon x)

    */

    int size = r.rows();
    Eigen::VectorXd delta = Eigen::VectorXd::Zero(size);

    delta = DavidsonSolver::_dpr_correction(r,D,lambda);

    double _num = - x.transpose() * delta;
    double _denom = - x.transpose() * DavidsonSolver::_dpr_correction(x,D,lambda);
    double eps = _num / _denom;
    delta += eps * x;

    return delta;
}

Eigen::VectorXd DavidsonSolver::_dpr_correction(Eigen::VectorXd &w, Eigen::VectorXd &A0, double lambda) const
{
    int size = w.rows();
    Eigen::VectorXd out = Eigen::VectorXd::Zero(size);
    for (int i=0; i < size; i++) {
        out(i) = w(i) / (lambda - A0(i));
    }

    return out;
}

Eigen::MatrixXd DavidsonSolver::_QR(Eigen::MatrixXd &A) const
{

    int nrows = A.rows();
    int ncols = A.cols();
    ncols = std::min(nrows,ncols);

    Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
    return qr.householderQ() * Eigen::MatrixXd::Identity(nrows,ncols);
}

Eigen::MatrixXd DavidsonSolver::_gramschmidt( Eigen::MatrixXd &A, int nstart ) const
{
    Eigen::MatrixXd Q = A;

    for(unsigned int j = nstart; j < A.cols(); ++j) {
        // Replace inner loop over each previous vector in Q with fast matrix-vector multiplication
        Q.col(j) -= Q.leftCols(j) * (Q.leftCols(j).transpose() * A.col(j));
        // Normalize vector if possible (othw. means colums of A almsost lin. dep.
        if( Q.col(j).norm() <= 10e-14 * A.col(j).norm() ) {
            std::cerr << "Gram-Schmidt failed because A has lin. dep columns. Bye." << std::endl;
            break;
        } else {
            Q.col(j).normalize();
        }
    }
    return Q;
}


// ===========================================================================
// Imagine++ Libraries
// Copyright (C) Imagine
// For detailed information: http://imagine.enpc.fr/software
// ===========================================================================

#include "Imagine/LinAlg/MyEigen.h"
#include <Eigen/Dense>
#include <iostream>
using namespace Eigen;

template <typename T>
void subSymToMatrix (int n, const T *ap, T *t)
{
    for (int i = 0 ; i < n ; i++)
        for (int j = 0 ; j <= i ; j++)
        {
            t[i + n * j] = ap[j + (i * (i + 1) / 2)];
            t[j + n * i] = t[i + n * j];
        }
}

void symToMatrix(int n, const double *ap, double *t) { subSymToMatrix<double>(n, ap, t); }
void symToMatrix(int n, const float *ap, float *t) { subSymToMatrix<float>(n, ap, t); }

// Vectors equalization (replaces xcopy)
template <typename T>
void subEqualize(int n, const T *s, int a, T *t, int b)
{
    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > sMap (s, n, InnerStride<>(a));
    Map<Matrix<T, Dynamic, 1>, 0, InnerStride<> > tMap (t, n, InnerStride<>(b));
    tMap = sMap;
}

void equalize(int n, const double *s, int a, double *t, int b) { subEqualize<double>(n, s, a, t, b); }
void equalize(int n, const float *s, int a, float *t, int b) { subEqualize<float>(n, s, a, t, b); }

// Linear combination alpha * X + Y
template <typename T>
void subCombine(int n, T alpha, const T *X, T *Y)
{
    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > sMap (X, n, InnerStride<>(1));
    Map<Matrix<T, Dynamic, 1>, 0, InnerStride<> > tMap (Y, n, InnerStride<>(1));
    tMap = alpha * sMap + tMap;
}

void combine(int n, double alpha, const double *X, double *Y) { subCombine<double>(n, alpha, X, Y); }
void combine(int n, float alpha, const float *X, float *Y) { subCombine<float>(n, alpha, X, Y); }

// Scalar product
template <typename T>
T subScalarProduct(int n, const T *X, const T *Y)
{
    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > sMap (X, n, InnerStride<>(1));
    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > tMap (Y, n, InnerStride<>(1));
    double s = sMap.dot(tMap);
    return T(s);
}

double scalarProduct(int n, const double *X, const double *Y) { return subScalarProduct<double>(n, X, Y); }
float scalarProduct(int n, const float *X, const float *Y) { return subScalarProduct<float>(n, X, Y); }

// Product by a number
template <typename T>
void subMultiply(int n, T alpha, T *X)
{
    Map<Matrix<T, Dynamic, 1>, 0, InnerStride<> > XMap (X, n, InnerStride<>(1));
    XMap = alpha * XMap;
}

void multiply(int n, double alpha, double *X) { subMultiply<double>(n, alpha, X); }
void multiply(int n, float alpha, float *X) { subMultiply<float>(n, alpha, X); }

// Norm 2 (euclidian)
template <typename T>
T subSquaredNorm(int n, const T *X)
{
    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > XMap (X, n, InnerStride<>(1));
    return ((T) XMap.squaredNorm());
}

double squaredNorm(int n, const double *X) { return subSquaredNorm<double>(n, X); }
float squaredNorm(int n, const float *X) { return subSquaredNorm<float>(n, X); }

// Affine transformation Y <- alpha * [A, A transpose, A adjoint] * X + beta * B (replaces xgemv)
template <typename T>
void subAffinity(int m, int n, T alpha, const T *A, const T *X, T beta, T *Y, char t)
{
    Map<const Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > AMap (A, m, n, OuterStride<>(m));

    if (!(t == 'T' || t == 'C'))
    {
        int tmp = m;
        m = n;
        n = tmp;
    }

    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > XMap (X, m, InnerStride<>(1));
    Map<Matrix<T, Dynamic, 1>, 0, InnerStride<> > YMap (Y, n, InnerStride<>(1));

    if (t == 'T') YMap = alpha * AMap.transpose() * XMap + beta * YMap;
    else if (t == 'C') YMap = alpha * AMap.adjoint() * XMap + beta * YMap;
    else YMap = alpha * AMap * XMap + beta * YMap;
}

void affinity(int m, int n, double alpha, const double *A, const double *X, double beta, double *Y, char t) { subAffinity<double>(m, n, alpha, A, X, beta, Y, t); }
void affinity(int m, int n, float alpha, const float *A, const float *X, float beta, float *Y, char t) { subAffinity<float>(m, n, alpha, A, X, beta, Y, t); }

// Matrix product C <- alpha * [A, A transpose, A adjoint] * [B, B transpose, B adjoint] + beta * C (replaces xgemm)
template <typename T>
void subMatrixProduct(int m, int n, int K, T alpha, const T *A, int lda, const T *B, int ldb, T beta, T *C, char t1, char t2)
{

    Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > CMap (C, m, n, OuterStride<>(m));

    if (!(t1 == 'T' || t1 == 'C'))
    {
        int tmp = m;
        m = K;
        K = tmp;
    }

    Map<const Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > AMap (A, K, m, OuterStride<>(lda));

    if (!(t1 == 'T' || t1 == 'C'))
    {
        int tmp = m;
        m = K;
        K = tmp;
    }

    if (!(t2 == 'T' || t2 == 'C'))
    {
        int tmp = n;
        n = K;
        K = tmp;
    }

    Map<const Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > BMap (B, n, K, OuterStride<>(ldb));

    if (t1 == 'T')
        if (t2 == 'T') CMap = alpha * AMap.transpose() * BMap.transpose() + beta * CMap;
        else if (t2 == 'C') CMap = alpha * AMap.transpose() * BMap.adjoint() + beta * CMap;
        else CMap = alpha * AMap.transpose() * BMap + beta * CMap;
    else if (t1 == 'C')
        if (t2 == 'T') CMap = alpha * AMap.adjoint() * BMap.transpose() + beta * CMap;
        else if (t2 == 'C') CMap = alpha * AMap.adjoint() * BMap.adjoint() + beta * CMap;
        else CMap = alpha * AMap.adjoint() * BMap + beta * CMap;
    else
        if (t2 == 'T') CMap = alpha * AMap * BMap.transpose() + beta * CMap;
        else if (t2 == 'C') CMap = alpha * AMap * BMap.adjoint() + beta * CMap;
        else CMap = alpha * AMap * BMap + beta * CMap;
}

void matrixProduct(int m, int n, int K, double alpha, const double *A, int lda, const double *B, int ldb, double beta, double *C, char t1, char t2) { subMatrixProduct<double>(m, n, K, alpha, A, lda, B, ldb, beta, C, t1, t2); }
void matrixProduct(int m, int n, int K, float alpha, const float *A, int lda, const float *B, int ldb, float beta, float *C, char t1, char t2) { subMatrixProduct<float>(m, n, K, alpha, A, lda, B, ldb, beta, C, t1, t2); }

// Matrix/vector product (replaces xspmv)
template <typename T>
void subVectorProduct(int n,T alpha,const T *ap,const T *x,T beta,T* y)
{
    T* t=new T [n * n];
    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > XMap (x, n, InnerStride<>(1));
    Map<Matrix<T, Dynamic, 1>, 0, InnerStride<> > YMap (y, n, InnerStride<>(1));
    symToMatrix(n, ap, t);
    Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > apMap (t, n, n, OuterStride<>(n));
    YMap = alpha * apMap * XMap + beta * YMap;
    delete[] t;
}

void vectorProduct(int n,double alpha,const double *ap, const double *x,double beta,double* y) { subVectorProduct<double>(n, alpha, ap, x, beta, y); }
void vectorProduct(int n,float alpha,const float *ap,const float *x,float beta,float* y) { subVectorProduct<float>(n, alpha, ap, x, beta, y); }

// TODO:
//info=0 si succes, -i si le ieme parametre a une valeure illegale, i si u(i,i);=0 (transformation LU impossible = mineurs principaux tous nuls)
// LU-inverse
template <typename T>
void subMatrixInverse(int *n, T *a,int *lda,int *ipiv,int *info)
{
    Map<Matrix<T, Dynamic, Dynamic> > AMap (a,*n,*n);
    Map<VectorXi> pMap (ipiv, *n);
    Eigen::FullPivLU<Matrix<T, Dynamic, Dynamic> > lu(AMap);
    MatrixXi P = lu.permutationP();
    MatrixXi Pi = pMap;

    for (int i = 0 ; i < P.rows() - 1 ; i++)
        for (int j = i + 1 ; j < P.cols() ; j++)
            if (P(i, j) == 1) { Pi(i) = j; Pi(j) = i; };

    //pMap = Pi;

    if(lu.isInvertible() && AMap.isApprox(lu.reconstructedMatrix()))
    {
        AMap = lu.inverse();
        *info = 0;
    }
    else *info = 1;
}

void matrixInverse(int *n, double *a,int *lda,int *ipiv,int *info) { subMatrixInverse<double>(n, a, lda, ipiv, info); }
void matrixInverse(int *n, float *a,int *lda,int *ipiv,int *info) { subMatrixInverse<float>(n, a, lda, ipiv, info); }


//
// Created by Hamza El-Kebir on 6/19/21.
//

#include "catchOnce.hpp"
#include "Lodestar/primitives/integrators/RungeKuttaFehlberg78.hpp"
#include <Eigen/Dense>

TEST_CASE("Runge-Kutta-Fehlberg 78", "[primitives][integrators]")
{
    std::function<double(double, double)> f = [](double t, double y) {
        return t * t * t;
    };

    typedef Eigen::Matrix<double, 3, 1> TDStateVector;
    std::function<TDStateVector(double, const TDStateVector&)> F = [](double t, const TDStateVector &x) {
        TDStateVector xdot;
        xdot << t, t*t, t*t*t;

        return xdot;
    };

    SECTION("Higher order") {
        double t = 0, y = 0;
        double h = 0.025;
        int N = 20;
        ls::primitives::RungeKuttaFehlberg78<double>::integrateSimple(f, t, y, h, N);

        REQUIRE(y == Approx(0.015625));
    }

    SECTION("Multivariate") {
        double t = 0;
        TDStateVector x;
        x.setZero();
//        x.array() += 2;

        double h = 0.025;
        int N = 20;
        ls::primitives::RungeKuttaFehlberg78<TDStateVector>::integrateSimple(F, t, x, h, N);

        REQUIRE(x(0) == Approx(0.125));
        REQUIRE(x(1) == Approx(0.0416667));
        REQUIRE(x(2) == Approx( 0.015625));
    }

    SECTION("Truncation error") {
        double t = 0, y = 0;
        double h = 0.5;
        int N = 1;
        auto err = ls::primitives::RungeKuttaFehlberg78<double>::integrateEmbedded(f, t, y, h, N);

        REQUIRE(y == Approx(0.015625));
    }
}


#include <fstream>
#include <boost/config.hpp>
#include <boost/version.hpp>
// CGAL headers
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/Search_traits_2.h>

// Qt headers
#include <QtGui>
#include <QString>
#include <QFileDialog>
#include <QInputDialog>
#include <QGraphicsLineItem>

// GraphicsView items and event filters (input classes)
#include <CGAL/Qt/PointsInKdTreeGraphicsItem.h>
#include <CGAL/Qt/utility.h>
#include <CGAL/IO/WKT.h>

// the two base classes
#include "ui_Spatial_searching_2.h"
#include <CGAL/Qt/DemosMainWindow.h>

#include "NearestNeighbor.h"

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef K::Vector_2 Vector_2;
typedef K::Segment_2 Segment_2;
typedef K::Iso_rectangle_2 Iso_rectangle_2;
typedef CGAL::Search_traits_2<K> TreeTraits;
typedef CGAL::Orthogonal_k_neighbor_search<TreeTraits> Neighbor_search;
typedef Neighbor_search::Tree Tree;

typedef CGAL::Qt::NearestNeighbor<Neighbor_search> NearestNeighbor;

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Spatial_searching_2
{
  Q_OBJECT

private:
  Tree tree;

  CGAL::Qt::Converter<K> convert;
  QGraphicsScene scene;

  CGAL::Qt::PointsInKdTreeGraphicsItem<Tree> * pgi;
  NearestNeighbor * nearest_neighbor;

public:
  MainWindow();

  template <typename G>
  void
  on_actionGenerate_triggered()
  {

    QRectF rect = CGAL::Qt::viewportsBbox(&scene);
    CGAL::Qt::Converter<K> convert;
    Iso_rectangle_2 isor = convert(rect);
    Point_2 center = CGAL::midpoint(isor[0], isor[2]);
    Vector_2 offset = center - CGAL::ORIGIN;
    double w = isor.xmax() - isor.xmin();
    double h = isor.ymax() - isor.ymin();
    double radius = (w<h) ? w/2 : h/2;

    G pg(radius);
    bool ok = false;

    const int number_of_points =
      QInputDialog::getInt(this,
                               tr("Number of random points"),
                               tr("Enter number of random points"),
                               100,
                               0,
                               (std::numeric_limits<int>::max)(),
                               1,
                               &ok);

    if(!ok) {
      return;
    }

    // wait cursor
    QApplication::setOverrideCursor(Qt::WaitCursor);

    std::vector<Point_2> points;

    points.reserve(number_of_points);
    for(int i = 0; i < number_of_points; ++i){
      points.push_back(*pg + offset);
      ++pg;
    }
    tree.insert(points.begin(), points.end());

    // default cursor
    QApplication::restoreOverrideCursor();
    Q_EMIT( changed());
  }

public Q_SLOTS:

  virtual void open(QString fileName);
  void N_changed(int i);
  void on_actionClear_triggered();
  void on_actionLoadPoints_triggered();
  void on_actionRecenter_triggered();
  void on_actionGeneratePointsOnCircle_triggered();
  void on_actionGeneratePointsInSquare_triggered();
  void on_actionGeneratePointsInDisc_triggered();

  void clear();

Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow()
{
  setupUi(this);

  this->graphicsView->setAcceptDrops(false);
  this->graphicsView->setCursor(Qt::CrossCursor);

  // Add a GraphicItem for the point set
  pgi = new CGAL::Qt::PointsInKdTreeGraphicsItem<Tree>(&tree);

  QObject::connect(this, SIGNAL(changed()),
                   pgi, SLOT(modelChanged()));

  pgi->setVerticesPen(QPen(Qt::black, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(pgi);

  nearest_neighbor = new NearestNeighbor(&scene, &tree, this, this->nn->value());
  nearest_neighbor->setPen(QPen(Qt::red, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.installEventFilter(nearest_neighbor);

  //
  // Manual handling of actions
  //

  QObject::connect(this->nn, SIGNAL(valueChanged(int)),
                   this, SLOT(N_changed(int)));

  QObject::connect(this->actionQuit, SIGNAL(triggered()),
                   this, SLOT(close()));

  //
  // Setup the scene and the view
  //
  scene.setItemIndexMethod(QGraphicsScene::NoIndex);
  scene.setSceneRect(0, 0, 10, 10);
  this->graphicsView->setScene(&scene);
  this->graphicsView->setMouseTracking(true);

  // Uncomment the following line to get antialiasing by default.
//   actionUse_Antialiasing->setChecked(true);

  // Turn the vertical axis upside down
  this->graphicsView->scale(1, -1);

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Spatial_searching_2.html");
  this->addAboutCGAL();

  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)),
          this, SLOT(open(QString)));
}

void MainWindow::N_changed(int i)
{
  nearest_neighbor->setN(i);
  Q_EMIT( changed());
}

/*
 *  Qt Automatic Connections
 *  https://doc.qt.io/qt-5/designer-using-a-ui-file.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */

void
MainWindow::on_actionClear_triggered()
{
  clear();
  Q_EMIT( changed());
}

void
MainWindow::on_actionRecenter_triggered()
{
  if(tree.empty()){
    return;
  }
  this->graphicsView->setSceneRect(pgi->boundingRect());
  this->graphicsView->fitInView(pgi->boundingRect(), Qt::KeepAspectRatio);
}

void
MainWindow::on_actionGeneratePointsOnCircle_triggered()
{
  typedef CGAL::Random_points_on_circle_2<Point_2> Generator;
  on_actionGenerate_triggered<Generator>();
}

void
MainWindow::on_actionGeneratePointsInSquare_triggered()
{
  typedef CGAL::Random_points_in_square_2<Point_2> Generator;
  on_actionGenerate_triggered<Generator>();
}

void
MainWindow::on_actionGeneratePointsInDisc_triggered()
{
  typedef CGAL::Random_points_in_disc_2<Point_2> Generator;
  on_actionGenerate_triggered<Generator>();
}

void
MainWindow::on_actionLoadPoints_triggered()
{
  QString fileName = QFileDialog::getOpenFileName(this,
                                                  tr("Open Points file"),
                                                  ".",
                                                  tr("CGAL files (*.pts.cgal);;"
                                                     "WKT files (*.wkt *.WKT);;"
                                                     "All files (*)"));
  if(! fileName.isEmpty()){
    open(fileName);
  }
}

void
MainWindow::open(QString fileName)
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::ifstream ifs(qPrintable(fileName));

  K::Point_2 p;
  std::vector<K::Point_2> points;
  if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
  {
    CGAL::IO::read_multi_point_WKT(ifs, points);
  }
  else{
    while(ifs >> p) {
      points.push_back(p);
    }
  }
  tree.insert(points.begin(), points.end());

  // default cursor
  QApplication::restoreOverrideCursor();
  this->addToRecentFiles(fileName);
  actionRecenter->trigger();
  Q_EMIT( changed());

}

void
MainWindow::clear()
{
  tree.clear();
}

#include "Spatial_searching_2.moc"
#include <CGAL/Qt/resources.h>

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Spatial_searching_2 demo");

  // Import resources from libCGAL (Qt5).
  // See https://doc.qt.io/qt-5/qdir.html#Q_INIT_RESOURCE
  CGAL_QT_INIT_RESOURCES;
  Q_INIT_RESOURCE(Spatial_searching_2);

  MainWindow mainWindow;
  mainWindow.show();
  mainWindow.on_actionRecenter_triggered();
  return app.exec();
}


/**
 * @file expfittedupwind_test.cc
 * @brief NPDE homework ExpFittedUpwind
 * @author Amélie Loher, Philippe Peter
 * @date 07.01.2021
 * @copyright Developed at ETH Zurich
 */

#include "../expfittedupwind.h"

#include <gtest/gtest.h>
#include <lf/assemble/assemble.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <cmath>
#include <memory>
#include <vector>

namespace ExpFittedUpwind::test {

// The Bernoulli function should be monotonically decreasing
TEST(Bernoulli, Monotinicity) {
  std::vector<double> values = {3.0,     1.5,    0.5,     1.0E-5,
                                1.0E-7,  1.0E-9, -1.0E-9, -1.0E-7,
                                -1.0E-5, -1.0,   -5.0,    -6.0};

  for (int i = 0; i < values.size() - 1; ++i) {
    EXPECT_LE(Bernoulli(values[i]), Bernoulli(values[i + 1]));
  }
}

// Check that B(tau) does not explode for small tau
TEST(Bernoulli, Cancellation) {
  for (double i = 8.0; i >= std::numeric_limits<double>::min(); i /= 2.0) {
    EXPECT_LE(std::abs(Bernoulli(i)), 2.0);
  }
}

// Check that B(log(2)) = log(2)
TEST(Bernoulli, Evaluation) {
  EXPECT_NEAR(Bernoulli(std::log(2)), std::log(2), 1.0E-8);
}

// For Psi = c, all entries of beta are given by beta(e) = std::exp(c)
TEST(CompBeta, ConstantPSI) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto mf_Psi = lf::mesh::utils::MeshFunctionGlobal(
      [](Eigen::Vector2d /*x*/) { return 3.1; });
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  Eigen::VectorXd mu = lf::fe::NodalProjection(*fe_space, mf_Psi);
  double ref = std::exp(3.1);

  auto beta = CompBeta(mesh_p, mu);

  for (auto entity : mesh_p->Entities(1)) {
    EXPECT_DOUBLE_EQ((*beta)(*entity), ref);
  }
}

// Verify the computation of beta for a linear psi
// based on precomputed reference values on a test mesh.
TEST(CompBeta, linearPSI) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);
  auto mf_Psi = lf::mesh::utils::MeshFunctionGlobal(
      [](Eigen::Vector2d x) { return 1.0 + x(0) + 2 * x(1); });
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  Eigen::VectorXd mu = lf::fe::NodalProjection(*fe_space, mf_Psi);

  auto beta = CompBeta(mesh_p, mu);
  auto edges = mesh_p->Entities(1);

  // edges of triangle 0
  EXPECT_DOUBLE_EQ((*beta)(*(edges[0])), std::exp(2.5) * Bernoulli(1.5));
  EXPECT_DOUBLE_EQ((*beta)(*(edges[1])), std::exp(4.0) * Bernoulli(3.0));
  EXPECT_DOUBLE_EQ((*beta)(*(edges[2])), std::exp(4.0) * Bernoulli(3.0));

  // edges of triangle 6:
  EXPECT_DOUBLE_EQ((*beta)(*(edges[12])), std::exp(6.0) * Bernoulli(1.0));
  EXPECT_DOUBLE_EQ((*beta)(*(edges[13])), std::exp(6.0) * Bernoulli(1.0));
  EXPECT_DOUBLE_EQ((*beta)(*(edges[14])), std::exp(6.0) * Bernoulli(0.0));

  // edges of triangle 11
  EXPECT_DOUBLE_EQ((*beta)(*(edges[18])), std::exp(6.0) * Bernoulli(0.0));
  EXPECT_DOUBLE_EQ((*beta)(*(edges[20])), std::exp(6.0) * Bernoulli(-2.5));
  EXPECT_DOUBLE_EQ((*beta)(*(edges[23])), std::exp(8.5) * Bernoulli(2.5));
}

// for Psi = const = c, the exponentially fitted element matrix reduces to
// A^{exp}_K = -A_K where A_K is the element matrix for (u,v) -> \int_K grad u *
// grad v dx
TEST(ExpFittedEMP, Psi_const) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);
  auto mf_Psi = lf::mesh::utils::MeshFunctionGlobal(
      [](Eigen::Vector2d x) { return 3.0; });
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  Eigen::VectorXd mu = lf::fe::NodalProjection(*fe_space, mf_Psi);

  // Imploemented exponentially fitted upwind provider
  ExpFittedEMP upwind_provider(fe_space, mu);

  // Lehrfem++ element matrix provider for the the minus laplcian
  lf::uscalfe::LinearFELaplaceElementMatrix standard_provider;

  // Expect that for Psi=const the ExpFitted Element matrix is -A_k
  for (auto *cell : mesh_p->Entities(0)) {
    Eigen::Matrix3d E_k = upwind_provider.Eval(*cell);
    Eigen::Matrix3d A_k = standard_provider.Eval(*cell).block<3, 3>(0, 0);

    EXPECT_NEAR((E_k + A_k).norm(), 0.0, 1.0E-10);
  }
}

// The last two tests are based on the fact, that
// the expontially Fitted upwind scheme provides a system matrix  A that
// corresponds to the bilinear form (u,v) -> b(u,v) \int_{\Omega} j(u,Psi) *
// grad v dx (j(u,Psi) = const ) In particular this value can be approximated by
// b_v^T * A * b_u, where b_v and b_u are the nodal projections of v and u into
// the underlying FE space

// u = exp(Psi)
// j(u,Psi) = 0
// and b(u,v) = 0 for any v
TEST(ExpFittedEMP, Bilinear_form_1) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};

  Eigen::Vector2d q = Eigen::Vector2d::Ones(2);
  auto Psi = [&q](Eigen::Vector2d x) { return q.dot(x); };

  auto u = [&Psi](Eigen::Vector2d x) { return std::exp(Psi(x)); };

  auto mf_Psi = lf::mesh::utils::MeshFunctionGlobal(Psi);
  auto mf_u = lf::mesh::utils::MeshFunctionGlobal(u);

  Eigen::VectorXd mu = lf::fe::NodalProjection(*fe_space, mf_Psi);
  Eigen::VectorXd u_vec = lf::fe::NodalProjection(*fe_space, mf_u);

  lf::assemble::COOMatrix<double> A(dofh.NumDofs(), dofh.NumDofs());
  ExpFittedEMP elmat_builder(fe_space, mu);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elmat_builder, A);
  Eigen::SparseMatrix<double> A_crs = A.makeSparse();

  EXPECT_NEAR((A_crs * u_vec).norm(), 0.0, 1.0E-10);
}

// Psi = q' * x
// u = c (--> j(u) = c*q)
// v = r' *x
// b(u,v) = |\Omega|*c*q'*r
TEST(ExpFittedEMP, Bilinear_form_2) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);
  double area = 9.0;
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};

  Eigen::Vector2d q = Eigen::Vector2d::Ones(2);
  auto Psi = [&q](Eigen::Vector2d x) { return q.dot(x); };
  auto mf_Psi = lf::mesh::utils::MeshFunctionGlobal(Psi);

  double u_value = 2.0;
  auto mf_u = lf::mesh::utils::MeshFunctionConstant(u_value);

  Eigen::Vector2d r = Eigen::Vector2d::Ones(2);
  auto v = [&r](Eigen::Vector2d x) { return r.dot(x); };
  auto mf_v = lf::mesh::utils::MeshFunctionGlobal(v);

  Eigen::VectorXd mu = lf::fe::NodalProjection(*fe_space, mf_Psi);
  Eigen::VectorXd u_vec = lf::fe::NodalProjection(*fe_space, mf_u);
  Eigen::VectorXd v_vec = lf::fe::NodalProjection(*fe_space, mf_v);

  lf::assemble::COOMatrix<double> A(dofh.NumDofs(), dofh.NumDofs());
  ExpFittedEMP elmat_builder(fe_space, mu);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elmat_builder, A);
  Eigen::SparseMatrix<double> A_crs = A.makeSparse();

  double value = v_vec.transpose() * A_crs * u_vec;
  double reference = area * u_value * r.dot(q);
  EXPECT_NEAR(value, reference, 1.0E-10);
}

}  // namespace ExpFittedUpwind::test


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*!
 * \file
 * \brief Test of explicit_l_curve_test class.
 */
#include "num_collect/regularization/explicit_l_curve.h"

#include <Eigen/Core>
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating.hpp>

#include "eigen_approx.h"
#include "num_collect/regularization/tikhonov.h"
#include "num_prob_collect/regularization/blur_sine.h"

TEST_CASE("num_collect::regularization::explicit_l_curve") {
    using coeff_type = Eigen::MatrixXd;
    using data_type = Eigen::VectorXd;
    using solver_type =
        num_collect::regularization::tikhonov<coeff_type, data_type>;
    using param_searcher_type =
        num_collect::regularization::explicit_l_curve<solver_type>;

    SECTION("solve") {
        constexpr num_collect::index_type solution_size = 15;
        constexpr num_collect::index_type data_size = 30;
        const auto prob = num_prob_collect::regularization::blur_sine(
            data_size, solution_size);

        num_collect::regularization::tikhonov<coeff_type, data_type> tikhonov;
        tikhonov.compute(prob.coeff(), prob.data());

        param_searcher_type searcher{tikhonov};
        searcher.search();
        REQUIRE(std::log10(searcher.opt_param()) < 0.0);

        constexpr double tol_sol = 1e-6;

        Eigen::VectorXd solution;
        searcher.solve(solution);
        REQUIRE_THAT(solution, eigen_approx(prob.solution(), tol_sol));
    }
}


#include "goodnessfunction.h"

#include <QImage>
#include <Eigen/Dense>
#include "eigenutility.h"
#include "image.h"

using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;

namespace
{
double computeGaussian(const VectorXd& x, const VectorXd& c, const MatrixXd& S_inv)
{
    const unsigned n = x.rows();
    const VectorXd r = x - c;
    const double   a = 1.0 / sqrt(pow(2.0 * M_PI, static_cast<double>(n)) * (1.0 / S_inv.determinant()));
    const double   b = exp(- 0.5 * r.transpose() * S_inv * r);
    return a * b;
}

template<typename Scalar> Scalar clamp(Scalar x, Scalar m, Scalar M) { return std::max(std::min(x, M), m); }

double computeVar(const MatrixXd &w_mean, const double &o_mean, const vector<MatrixXd> &w, const VectorXd &o, unsigned s, unsigned t, unsigned N)
{
    double t1 = 0.0;
    for (unsigned i = 0; i < N; ++ i)
    {
        t1 += (o[i] * w[i](s, t) - o_mean * w_mean(s, t)) * (o[i] * w[i](s, t) - o_mean * w_mean(s, t));
    }

    double t2 = 0.0;
    for (unsigned i = 0; i < N; ++ i)
    {
        t2 += (o[i] - o_mean) * (o[i] * w[i](s, t) - o_mean * w_mean(s, t));
    }

    double t3 = 0.0;
    for (unsigned i = 0; i < N; ++ i)
    {
        t3 += (o[i] - o_mean) * (o[i] - o_mean);
    }

    return (static_cast<double>(N) / static_cast<double>(N - 1)) * (t1 - 2.0 * w_mean(s, t) * t2 + w_mean(s, t) * w_mean(s, t) * t3);
}
}

GoodnessFunction::GoodnessFunction() :
    alpha(0.0020),
    epsilon(0.020)
{

}

// A pure implementation of the adaptive kernel density estimation algorithms described in [Talton et al. 2009] (Section 5)
void GoodnessFunction::computeCovariance()
{
    const unsigned N = getFeatureList().size();
    const unsigned n = getFeatureList()[0].rows() + getParameterList()[0].rows();

    SList.clear();
    SList.resize(N);

    // Exception
    assert (N != 0);
    if (N == 1)
    {
        SList[0] = MatrixXd::Identity(n, n);
        return;
    }

    // Prepare the data points
    vector<VectorXd> x(N);
    for (unsigned i = 0; i < N; ++ i)
    {
        x[i] = getJointVector(getParameterList()[i], getFeatureList()[i]);
    }

    // Compute the distances from the k-nearest neighborhoods
    const unsigned k = std::min(N - 1, n);
    VectorXd squaredDistance_k = VectorXd::Zero(N);
    for (unsigned i = 0; i < N; ++ i)
    {
        vector<double> d(N);
        for (unsigned j = 0; j < N; ++ j)
        {
            d[j] = (x[i] - x[j]).squaredNorm();
        }
        partial_sort(d.begin(), d.begin() + (k + 1), d.end());  // Note: d[0] is always 0.0
        squaredDistance_k(i) = d[k];
    }

    // For each Gaussian center, ...
    for (unsigned center = 0; center < N; ++ center)
    {
        // Compute weights (omega in the paper) for this data point
        VectorXd o = VectorXd::Zero(N);
        for (unsigned j = 0; j < N; ++ j)
        {
            const double   alpha = 1.0;
            const MatrixXd S_inv = (1.0 / (alpha * squaredDistance_k[center])) * MatrixXd::Identity(n, n);
            o(j) = computeGaussian(x[j], x[center], S_inv);
        }
        o /= o.sum();

        // Compute weights (w in the paper) for this data point
        vector<MatrixXd> w(N);
        for (unsigned j = 0; j < N; ++ j)
        {
            w[j] = MatrixXd::Zero(n, n);
            for (unsigned s = 0; s < n; ++ s) for (unsigned t = s; t < n; ++ t)
            {
                w[j](s, t) = (x[j].row(s) - x[center].row(s)) * (x[j].row(t) - x[center].row(t));
                w[j](t, s) = (x[j].row(s) - x[center].row(s)) * (x[j].row(t) - x[center].row(t));
            }
        }

        // Compute a covariance matrix for this data point
        SList[center] = MatrixXd::Zero(n, n);
        for (unsigned s = 0; s < n; ++ s) for (unsigned t = s; t < n; ++ t)
        {
            double elem = 0.0;
            for (unsigned j = 0; j < N; ++ j)
            {
                elem += o[j] * w[j](s, t);
            }
            SList[center](s, t) = elem;
            SList[center](t, s) = elem;
        }

        // Compute the bandwidth shrinkage algorithm
        const MatrixXd& Sigma = SList[center];

        MatrixXd Phi = MatrixXd::Zero(n, n);
        for (unsigned j = 0; j < n; ++ j) Phi(j, j) = Sigma(j, j);

        const MatrixXd& w_mean = Sigma;
        const double    o_mean = o.sum() / static_cast<double>(N);

        double t1 = 0.0;
        for (unsigned s = 0; s < n; ++ s) for (unsigned t = 0; t < n; ++ t)
        {
            if (s == t) continue;
            t1 += computeVar(w_mean, o_mean, w, o, s, t, N);
        }

        double t2 = 0.0;
        for (unsigned s = 0; s < n; ++ s) for (unsigned t = 0; t < n; ++ t)
        {
            if (s == t) continue;
            t2 += w_mean(s, t) * w_mean(s, t);
        }

        double lambda = t1 / t2;
        lambda = isnan(lambda) ? 0.0 : clamp(lambda, 0.0, 1.0);

        SList[center] = lambda * Phi + (1.0 - lambda) * Sigma;
    }
}

void GoodnessFunction::regularizeCovariance()
{
    for (MatrixXd &Sigma : SList)
    {
        const unsigned n = Sigma.rows();
        for (unsigned i = 0; i < n; ++ i)
        {
            Sigma(i, i) = max(Sigma(i, i), epsilon);
        }
    }
}

VectorXd GoodnessFunction::applyGradientAscent(const VectorXd& x, const VectorXd& f, double scale) const
{
    const VectorXd grad = computeGradient(x, f).block(0, 0, x.rows(), 1);
    const VectorXd xNew = x + scale * alpha * grad;

    return getClippedParameters(xNew);
}

double GoodnessFunction::getValue(const VectorXd& j) const
{
#if 0
    const double   sig = 0.10;
    const unsigned N   = getFeatureList().size();

    double sum = 0.0;
    for (unsigned i = 0; i < N; ++ i) {
        const VectorXd& f_i = getFeatureList()[i];
        const VectorXd& x_i = getParameterList()[i];
        const VectorXd  j_i = getJointVector(x_i, f_i);
        const double    d_i = (j - j_i).squaredNorm();
        sum += exp(- d_i / sig);
    }
    return sum / static_cast<double>(N);
#else
    const unsigned N = getFeatureList().size();
    double sum = 0.0;
    for (unsigned i = 0; i < N; ++ i) {
        const MatrixXd& S     = SList[i];
        const MatrixXd  S_inv = S.inverse();
        const VectorXd& f_i   = getFeatureList()[i];
        const VectorXd& x_i   = getParameterList()[i];
        const VectorXd  j_i   = getJointVector(x_i, f_i);
        sum += computeGaussian(j, j_i, S_inv);
    }
    return sum / static_cast<double>(N);
#endif
}

double GoodnessFunction::getValue(const VectorXd& x, const VectorXd &f) const
{
    const VectorXd j = getJointVector(x, f);
    return getValue(j);
}

VectorXd GoodnessFunction::computeGradient(const VectorXd &j) const
{
    const unsigned n = j.rows();
    const unsigned N = getFeatureList().size();
    VectorXd sum = VectorXd::Zero(n);
    for (unsigned i = 0; i < N; ++ i)
    {
        const MatrixXd& S     = SList[i];
        const MatrixXd  S_inv = S.inverse();
        const VectorXd& f_i   = getFeatureList()[i];
        const VectorXd& x_i   = getParameterList()[i];
        const VectorXd  j_i   = getJointVector(x_i, f_i);
        sum += - computeGaussian(j, j_i, S_inv) * S_inv * (j - j_i);
    }

    // NaN check
    for (unsigned i = 0; i < sum.rows(); ++ i) sum(i) = std::isnan(sum(i)) ? 0.0 : sum(i);

    return sum / static_cast<double>(N);
}

VectorXd GoodnessFunction::computeGradient(const VectorXd &x, const VectorXd &f) const
{
    const VectorXd j = getJointVector(x, f);
    return computeGradient(j);
}

VectorXd GoodnessFunction::getClippedParameters(VectorXd x)
{
    for (int i = 0; i < x.rows(); ++ i)
    {
        if (x(i) < 0.0) x(i) = 0.0;
        if (x(i) > 1.0) x(i) = 1.0;
    }
    return x;
}

VectorXd GoodnessFunction::getJointVector(const VectorXd &x, const Eigen::VectorXd& f)
{
    return EigenUtility::join(x, f);
}

VectorXd GoodnessFunction::getAverageParameterSet() const
{
    assert(!pList.empty());

    VectorXd ave = VectorXd::Zero(pList[0].rows());
    for (const VectorXd& v : pList)
    {
        ave += v;
    }
    return (1.0 / static_cast<double>(pList.size())) * ave;
}


/**
 * @file linear_svm_test.cpp
 * @author Ayush Chamoli
 *
 * Test the Linear SVM class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/linear_svm/linear_svm.hpp>
#include <ensmallen.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::svm;
using namespace mlpack::distribution;

BOOST_AUTO_TEST_SUITE(LinearSVMTest);

/**
 * A simple test for LinearSVMFunction
 */
BOOST_AUTO_TEST_CASE(LinearSVMFunctionEvaluate)
{
  // A very simple fake dataset
  arma::mat dataset = "2 0 0;"
                      "0 0 0;"
                      "0 2 1;"
                      "1 0 2;"
                      "0 1 0";

  //  Corresponding labels
  arma::Row<size_t> labels = "1 0 1";

  LinearSVMFunction<arma::mat> svmf(dataset, labels, 2,
      0.0 /* no regularization */);

  // These were hand-calculated using Python.
  arma::mat parameters = "1 1 1 1 1;"
                         "1 1 1 1 1";
  BOOST_REQUIRE_CLOSE(svmf.Evaluate(parameters.t()), 1.0, 1e-5);

  parameters = "2 0 1 2 2;"
               "1 2 2 2 2";
  BOOST_REQUIRE_CLOSE(svmf.Evaluate(parameters.t()), 2.0, 1e-5);

  parameters = "-0.1425 8.3228 0.1724 -0.3374 0.1548;"
               "0.1435 0.0009 -0.1736 0.3356 -0.1544";
  BOOST_REQUIRE_CLOSE(svmf.Evaluate(parameters.t()), 0.0, 1e-5);

  parameters = "100 3 4 5 23;"
               "43 54 67 32 64";
  BOOST_REQUIRE_CLOSE(svmf.Evaluate(parameters.t()), 85.33333333, 1e-5);

  parameters = "3 71 22 12 6;"
               "100 39 30 57 22";
  BOOST_REQUIRE_CLOSE(svmf.Evaluate(parameters.t()), 11.0, 1e-5);
}

/**
 * A complicated test for the LinearSVMFunction for binary-class
 * classification.
 */
BOOST_AUTO_TEST_CASE(LinearSVMFunctionRandomBinaryEvaluate)
{
  const size_t points = 1000;
  const size_t trials = 10;
  const size_t inputSize = 10;
  const size_t numClasses = 2;
  const double delta = 1.0;

  // Initialize a random dataset.
  arma::mat data;
  data.randu(inputSize, points);

  // Create random class labels.
  arma::Row<size_t> labels(points);
  for (size_t i = 0; i < points; i++)
    labels(i) = math::RandInt(0, numClasses);

  // Create a LinearSVMFunction, Regularization term ignored.
  LinearSVMFunction<arma::mat> svmf(data, labels, numClasses,
      0.0 /* no regularization */);

  // Run a number of trials.
  for (size_t i = 0; i < trials; ++i)
  {
    // Create a random set of parameters.
    arma::mat parameters;
    parameters.randu(inputSize, numClasses);

    // Hand-calculate the loss function
    double hingeLoss = 0;

    // Compute error for each training example.
    for (size_t j = 0; j < points; ++j)
    {
      arma::mat score = parameters.t() * data.col(j);
      double correct = score[labels(j)];
      for (size_t k = 0; k < numClasses; ++k)
      {
        if (k == labels[j])
          continue;
        double margin = score[k] - correct + delta;
        if (margin > 0)
          hingeLoss += margin;
      }
    }
    hingeLoss /= points;

    // Compare with the value returned by the function.
    BOOST_REQUIRE_CLOSE(svmf.Evaluate(parameters), hingeLoss, 1e-5);
  }
}

/**
 * A complicated test for the LinearSVMFunction for multi-class
 * classification.
 */
BOOST_AUTO_TEST_CASE(LinearSVMFunctionRandomEvaluate)
{
  const size_t points = 1000;
  const size_t trials = 10;
  const size_t inputSize = 10;
  const size_t numClasses = 5;
  const double delta = 1.0;

  // Initialize a random dataset.
  arma::mat data;
  data.randu(inputSize, points);

  // Create random class labels.
  arma::Row<size_t> labels(points);
  for (size_t i = 0; i < points; i++)
    labels(i) = math::RandInt(0, numClasses);

  // Create a LinearSVMFunction, Regularization term ignored.
  LinearSVMFunction<arma::mat> svmf(data, labels, numClasses,
      0.0 /* no regularization */);

  // Run a number of trials.
  for (size_t i = 0; i < trials; ++i)
  {
    // Create a random set of parameters.
    arma::mat parameters;
    parameters.randu(inputSize, numClasses);

    // Hand-calculate the loss function
    double hingeLoss = 0;

    // Compute error for each training example.
    for (size_t j = 0; j < points; ++j)
    {
      arma::mat score = parameters.t() * data.col(j);
      double correct = score[labels(j)];
      for (size_t k = 0; k < numClasses; ++k)
      {
        if (k == labels[j])
          continue;
        double margin = score[k] - correct + delta;
        if (margin > 0)
          hingeLoss += margin;
      }
    }
    hingeLoss /= points;

    // Compare with the value returned by the function.
    BOOST_REQUIRE_CLOSE(svmf.Evaluate(parameters), hingeLoss, 1e-5);
  }
}

/**
 * Test regularization for the LinearSVMFunction Evaluate()
 * function.
 */
BOOST_AUTO_TEST_CASE(LinearSVMFunctionRegularizationEvaluate)
{
  const size_t points = 1000;
  const size_t trials = 10;
  const size_t inputSize = 10;
  const size_t numClasses = 3;

  // Initialize a random dataset.
  arma::mat data;
  data.randu(inputSize, points);

  // Create random class labels.
  arma::Row<size_t> labels(points);
  for (size_t i = 0; i < points; i++)
    labels(i) = math::RandInt(0, numClasses);

  // 3 objects for comparing regularization costs.
  LinearSVMFunction<arma::mat> svmfNoReg(data, labels, numClasses, 0);
  LinearSVMFunction<arma::mat> svmfSmallReg(data, labels, numClasses, 1);
  LinearSVMFunction<arma::mat> svmfBigReg(data, labels, numClasses, 20);

  // Run a number of trials.
  for (size_t i = 0; i < trials; i++)
  {
    // Create a random set of parameters.
    arma::mat parameters;
    parameters.randu(inputSize, numClasses);

    double wL2SquaredNorm;
    wL2SquaredNorm = arma::dot(parameters, parameters);

    // Calculate regularization terms.
    const double smallRegTerm = 0.5 * wL2SquaredNorm;
    const double bigRegTerm = 10 * wL2SquaredNorm;

    BOOST_REQUIRE_CLOSE(svmfNoReg.Evaluate(parameters) + smallRegTerm,
                        svmfSmallReg.Evaluate(parameters), 1e-5);
    BOOST_REQUIRE_CLOSE(svmfNoReg.Evaluate(parameters) + bigRegTerm,
                        svmfBigReg.Evaluate(parameters), 1e-5);
  }
}

/**
 * Test individual Evaluate() functions to be used for
 * optimization.
 */
BOOST_AUTO_TEST_CASE(LinearSVMFunctionSeparableEvaluate)
{
  const size_t points = 1000;
  const size_t trials = 10;
  const size_t inputSize = 10;
  const size_t numClasses = 3;

  // Initialize a random dataset.
  arma::mat data;
  data.randu(inputSize, points);

  // Create random class labels.
  arma::Row<size_t> labels(points);
  for (size_t i = 0; i < points; i++)
    labels(i) = math::RandInt(0, numClasses);

  LinearSVMFunction<> svmf(data, labels, numClasses);

  for (size_t i = 0; i < trials; ++i)
  {
    // Create a random set of parameters.
    arma::mat parameters;
    parameters.randu(inputSize, numClasses);

    double hingeLoss = 0;
    for (size_t j = 0; j < points; ++j)
      hingeLoss += svmf.Evaluate(parameters, j, 1);

    hingeLoss /= points;

    // Compare with the value returned by the function.
    BOOST_REQUIRE_CLOSE(svmf.Evaluate(parameters), hingeLoss, 1e-5);
  }
}

/**
 *
 * Test regularization for the separable Evaluate() function
 * to be used Optimizers.
 */
BOOST_AUTO_TEST_CASE(LinearSVMFunctionRegularizationSeparableEvaluate)
{
  const size_t points = 100;
  const size_t trials = 3;
  const size_t inputSize = 10;
  const size_t numClasses = 3;

  // Initialize a random dataset.
  arma::mat data;
  data.randu(inputSize, points);

  // Create random class labels.
  arma::Row<size_t> labels(points);
  for (size_t i = 0; i < points; i++)
    labels(i) = math::RandInt(0, numClasses);

  LinearSVMFunction<> svmfNoReg(data, labels, numClasses, 0.0);
  LinearSVMFunction<> svmfSmallReg(data, labels, numClasses, 0.5);
  LinearSVMFunction<> svmfBigReg(data, labels, numClasses, 20.0);

  // Check that the number of functions is correct.
  BOOST_REQUIRE_EQUAL(svmfNoReg.NumFunctions(), points);
  BOOST_REQUIRE_EQUAL(svmfSmallReg.NumFunctions(), points);
  BOOST_REQUIRE_EQUAL(svmfBigReg.NumFunctions(), points);

  for (size_t i = 0; i < trials; ++i)
  {
    // Create a random set of parameters.
    arma::mat parameters;
    parameters.randu(inputSize, numClasses);

    double wL2SquaredNorm;
    wL2SquaredNorm = 0.5 * arma::dot(parameters, parameters);

    // Calculate regularization terms.
    const double smallRegTerm = 0.5 * wL2SquaredNorm;
    const double bigRegTerm = 20 * wL2SquaredNorm;

    for (size_t j = 0; j < points; ++j)
    {
      BOOST_REQUIRE_CLOSE(svmfNoReg.Evaluate(parameters, j, 1) + smallRegTerm,
          svmfSmallReg.Evaluate(parameters, j, 1), 1e-5);
      BOOST_REQUIRE_CLOSE(svmfNoReg.Evaluate(parameters, j, 1) + bigRegTerm,
          svmfBigReg.Evaluate(parameters, j, 1), 1e-5);
    }
  }
}

/**
 * Test Gradient() of the LinearSVMFunction.
 */
BOOST_AUTO_TEST_CASE(LinearSVMFunctionGradient)
{
  const size_t points = 1000;
  const size_t trials = 10;
  const size_t inputSize = 10;
  const size_t numClasses = 5;
  const double delta = 1.0;

  // Initialize a random dataset.
  arma::mat data;
  data.randu(inputSize, points);

  // Create random class labels.
  arma::Row<size_t> labels(points);
  for (size_t i = 0; i < points; i++)
    labels(i) = math::RandInt(0, numClasses);

  // Create a LinearSVMFunction, Regularization term ignored.
  LinearSVMFunction<arma::mat> svmf(data, labels, numClasses,
                                    0.0 /* no regularization */,
                                    delta);

  // Run a number of trials.
  for (size_t i = 0; i < trials; ++i)
  {
    // Create a random set of parameters.
    arma::mat parameters;
    parameters.randu(inputSize, numClasses);

    // Hand-calculate the gradient.
    arma::mat difference;
    difference.zeros(numClasses, points);

    // Compute error for each training example.
    for (size_t j = 0; j < points; ++j)
    {
      arma::mat score = parameters.t() * data.col(j);
      double correct = score[labels(j)];
      size_t differenceCount = 0;
      for (size_t k = 0; k < numClasses; ++k)
      {
        if (k == labels[j])
          continue;
        double margin = score[k] - correct + delta;
        if (margin > 0)
        {
          differenceCount += 1;
          difference(k, j) = 1;
        }
      }
      difference(labels(j), j) -= differenceCount;
    }

    arma::mat gradient = (data * difference.t()) / points;
    arma::mat evaluatedGradient;

    svmf.Gradient(parameters, evaluatedGradient);

    // Compare with the values returned by Gradient().
    for (size_t j = 0; j < inputSize ; ++j)
    {
      for (size_t k = 0; k < numClasses ; ++k)
      {
        BOOST_REQUIRE_CLOSE(gradient(j, k), evaluatedGradient(j, k), 1e-5);
      }
    }
  }
}

/**
 * Test separable Gradient() of the LinearSVMFunction when regularization
 * is used.
 */
BOOST_AUTO_TEST_CASE(LinearSVMFunctionSeparableGradient)
{
  const size_t points = 100;
  const size_t trials = 3;
  const size_t inputSize = 5;
  const size_t numClasses = 5;

  // Initialize a random dataset.
  arma::mat data;
  data.randu(inputSize, points);

  // Create random class labels.
  arma::Row<size_t> labels(points);
  for (size_t i = 0; i < points; i++)
    labels(i) = math::RandInt(0, numClasses);

  LinearSVMFunction<> svmfNoReg(data, labels, numClasses, 0.0);
  LinearSVMFunction<> svmfSmallReg(data, labels, numClasses, 0.5);
  LinearSVMFunction<> svmfBigReg(data, labels, numClasses, 20.0);

  for (size_t i = 0; i < trials; ++i)
  {
    // Create a random set of parameters.
    arma::mat parameters;
    parameters.randu(inputSize, numClasses);

    arma::mat gradient;
    arma::mat smallRegGradient;
    arma::mat bigRegGradient;

    // Test separable gradient for each point.  Regularization will be the same.
    for (size_t k = 0; k < points; ++k)
    {
      svmfNoReg.Gradient(parameters, k, gradient, 1);
      svmfSmallReg.Gradient(parameters, k, smallRegGradient, 1);
      svmfBigReg.Gradient(parameters, k, bigRegGradient, 1);

      // Check sizes of gradients.
      BOOST_REQUIRE_EQUAL(gradient.n_elem, parameters.n_elem);
      BOOST_REQUIRE_EQUAL(smallRegGradient.n_elem, parameters.n_elem);
      BOOST_REQUIRE_EQUAL(bigRegGradient.n_elem, parameters.n_elem);

      // Check other terms.
      for (size_t j = 0; j < parameters.n_elem; ++j)
      {
        const double smallRegTerm = 0.5 * parameters[j];
        const double bigRegTerm = 20.0 * parameters[j];

        BOOST_REQUIRE_CLOSE(gradient[j] + smallRegTerm, smallRegGradient[j],
                            1e-5);
        BOOST_REQUIRE_CLOSE(gradient[j] + bigRegTerm, bigRegGradient[j], 1e-5);
      }
    }
  }
}

/**
 * Test training of linear svm on a simple dataset using
 * L-BFGS optimizer
 */
BOOST_AUTO_TEST_CASE(LinearSVMLGFGSSimpleTest)
{
  const size_t numClasses = 2;
  const double lambda = 0.0001;

  // A very simple fake dataset
  arma::mat dataset = "2 0 0;"
                      "0 0 0;"
                      "0 2 1;"
                      "1 0 2;"
                      "0 1 0";

  //  Corresponding labels
  arma::Row<size_t> labels = "1 0 1";

  // Create a linear svm object using L-BFGS optimizer.
  LinearSVM<arma::mat> lsvm(dataset, labels, numClasses, lambda);

  // Compare training accuracy to 1.
  const double acc = lsvm.ComputeAccuracy(dataset, labels);
  BOOST_REQUIRE_CLOSE(acc, 1.0, 0.5);
}

/**
 * Test training of linear svm on a simple dataset using
 * Gradient Descent optimizer
 */
BOOST_AUTO_TEST_CASE(LinearSVMGradientDescentSimpleTest)
{
  const size_t numClasses = 2;
  const size_t maxIterations = 10000;
  const double stepSize = 0.01;
  const double tolerance = 1e-5;
  const double lambda = 0.0001;
  const double delta = 1.0;

  // A very simple fake dataset
  arma::mat dataset = "2 0 0;"
                      "0 0 0;"
                      "0 2 1;"
                      "1 0 2;"
                      "0 1 0";

  //  Corresponding labels
  arma::Row<size_t> labels = "1 0 1";

  // Create a linear svm object using custom gradient descent optimizer.
  ens::GradientDescent optimizer(stepSize, maxIterations, tolerance);
  LinearSVM<arma::mat> lsvm(dataset, labels, numClasses, lambda,
      delta, false, optimizer);

  // Compare training accuracy to 1.
  const double acc = lsvm.ComputeAccuracy(dataset, labels);
  BOOST_REQUIRE_CLOSE(acc, 1.0, 0.5);
}

/**
 * Test training of linear svm for two classes on a complex gaussian dataset
 * using L-BFGS optimizer.
 */
BOOST_AUTO_TEST_CASE(LinearSVMLBFGSTwoClasses)
{
  const size_t points = 1000;
  const size_t inputSize = 3;
  const size_t numClasses = 2;
  const double lambda = 0.5;

  // Generate two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 9.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("4.0 3.0 4.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(inputSize, points);
  arma::Row<size_t> labels(points);

  for (size_t i = 0; i < points / 2; i++)
  {
    data.col(i) = g1.Random();
    labels(i) = 0;
  }
  for (size_t i = points / 2; i < points; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }

  // Create a linear svm object using L-BFGS optimizer.
  LinearSVM<arma::mat> lsvm(data, labels, numClasses, lambda);

  // Compare training accuracy to 1.
  const double acc = lsvm.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(acc, 1.0, 0.5);

  // Create test dataset.
  for (size_t i = 0; i < points / 2; i++)
  {
    data.col(i) = g1.Random();
    labels(i) =  0;
  }
  for (size_t i = points / 2; i < points; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }

  // Compare test accuracy to 1.
  const double testAcc = lsvm.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(testAcc, 1.0, 0.6);
}

/**
 * Test training of linear svm for two classes on a complex gaussian dataset
 * using L-BFGS optimizer which can't be separated without adding
 * the intercept term.
 */
BOOST_AUTO_TEST_CASE(LinearSVMFitIntercept)
{
  const size_t points = 1000;
  const size_t inputSize = 3;
  const size_t numClasses = 2;
  const double lambda = 0.5;
  const double delta = 1.0;

  // Generate a two-Gaussian dataset,
  GaussianDistribution g1(arma::vec("1.0 9.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("4.0 3.0 4.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(inputSize, points);
  arma::Row<size_t> labels(points);
  for (size_t i = 0; i < points / 2; ++i)
  {
    data.col(i) = g1.Random();
    labels[i] = 0;
  }
  for (size_t i = points / 2; i < points; ++i)
  {
    data.col(i) = g2.Random();
    labels[i] = 1;
  }

  // Now train a svm object on it.
  LinearSVM<arma::mat> svm(data, labels, numClasses, lambda,
      delta, true, ens::L_BFGS());

  // Ensure that the error is close to zero.
  const double acc = svm.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(acc, 1.0, 2.0);

  // Create a test set.
  for (size_t i = 0; i < 500; ++i)
  {
    data.col(i) = g1.Random();
    labels[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    data.col(i) = g2.Random();
    labels[i] = 1;
  }

  // Ensure that the error is close to zero.
  const double testAcc = svm.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(testAcc, 1.0, 2.0);
}

/**
 * Test training of linear svm on a simple dataset using
 * Gradient Descent optimizer and with another value of delta.
 */
BOOST_AUTO_TEST_CASE(LinearSVMDeltaLBFGSTwoClasses)
{
  const size_t points = 1000;
  const size_t inputSize = 3;
  const size_t numClasses = 2;
  const double lambda = 0.5;
  const double delta = 5.0;

  // Generate two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 9.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("4.0 3.0 4.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(inputSize, points);
  arma::Row<size_t> labels(points);

  for (size_t i = 0; i < points / 2; i++)
  {
    data.col(i) = g1.Random();
    labels(i) = 0;
  }
  for (size_t i = points / 2; i < points; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }

  // Create a linear svm object using L-BFGS optimizer.
  LinearSVM<arma::mat> lsvm(data, labels, numClasses, lambda,
      delta);

  // Compare training accuracy to 1.
  const double acc = lsvm.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(acc, 1.0, 0.5);

  // Create test dataset.
  for (size_t i = 0; i < points / 2; i++)
  {
    data.col(i) = g1.Random();
    labels(i) =  0;
  }
  for (size_t i = points / 2; i < points; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }

  // Compare test accuracy to 1.
  const double testAcc = lsvm.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(testAcc, 1.0, 0.6);
}

/**
 * The test is only compiled if the user has specified OpenMP to be
 * used.
 */
#ifdef HAS_OPENMP

/**
 * Test training of linear svm on a simple dataset using
 * Parallel SGD optimizer.
 */
BOOST_AUTO_TEST_CASE(LinearSVMPSGDSimpleTest)
{
  const size_t numClasses = 2;
  const double lambda = 0.5;
  const double alpha = 0.01;
  const double delta = 1.0;

  // A very simple fake dataset
  arma::mat dataset = "2 0 0;"
                      "0 0 0;"
                      "0 2 1;"
                      "1 0 2;"
                      "0 1 0";

  //  Corresponding labels
  arma::Row<size_t> labels = "1 0 1";

  ens::ConstantStep decayPolicy(alpha);

  // Train linear svm object using Parallel SGD optimizer.
  // The threadShareSize is chosen such that each function gets optimized.
  ens::ParallelSGD<ens::ConstantStep> optimizer(0,
      std::ceil((float) dataset.n_cols / omp_get_max_threads()),
      1e-5, true, decayPolicy);
  LinearSVM<arma::mat> lsvm(dataset, labels, numClasses, lambda,
      delta, false, optimizer);

  // Compare training accuracy to 1.
  const double acc = lsvm.ComputeAccuracy(dataset, labels);
  BOOST_REQUIRE_CLOSE(acc, 1.0, 1.0);
}

/**
 * Test training of linear svm for two classes on a complex gaussian dataset
 * using Parallel SGD optimizer.
 */
BOOST_AUTO_TEST_CASE(LinearSVMParallelSGDTwoClasses)
{
  const size_t points = 500;
  const size_t inputSize = 3;
  const size_t numClasses = 2;
  const double lambda = 0.5;
  const double alpha = 0.01;
  const double delta = 1.0;

  // Generate two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 9.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("4.0 3.0 4.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(inputSize, points);
  arma::Row<size_t> labels(points);

  for (size_t i = 0; i < points / 2; i++)
  {
    data.col(i) = g1.Random();
    labels(i) = 0;
  }
  for (size_t i = points / 2; i < points; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }

  ens::ConstantStep decayPolicy(alpha);

  // Train linear svm object using Parallel SGD optimizer.
  // The threadShareSize is chosen such that each function gets optimized.
  ens::ParallelSGD<ens::ConstantStep> optimizer(0,
      std::ceil((float) data.n_cols / omp_get_max_threads()),
      1e-5, true, decayPolicy);
  LinearSVM<arma::mat> lsvm(data, labels, numClasses, lambda,
      delta, false, optimizer);

  // Compare training accuracy to 1.
  const double acc = lsvm.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(acc, 1.0, 2.0);

  // Create test dataset.
  for (size_t i = 0; i < points / 2; i++)
  {
    data.col(i) = g1.Random();
    labels(i) =  0;
  }
  for (size_t i = points / 2; i < points; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }

  // Compare test accuracy to 1.
  const double testAcc = lsvm.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(testAcc, 1.0, 2.0);
}

#endif

/**
 * Test sparse and dense linear svm and make sure they both work the
 * same using the L-BFGS optimizer.
 */
BOOST_AUTO_TEST_CASE(LinearSVMSparseLBFGSTest)
{
  // Create a random dataset.
  arma::sp_mat dataset;
  dataset.sprandu(10, 800, 0.3);
  arma::mat denseDataset(dataset);
  arma::Row<size_t> labels(800);
  for (size_t i = 0; i < 800; ++i)
    labels[i] = math::RandInt(0, 2);

  LinearSVM<arma::mat> lr(denseDataset, labels, 2, 0.3, 1,
      false, ens::L_BFGS());
  LinearSVM<arma::sp_mat> lrSparse(dataset, labels, 2, 0.3, 1,
      false, ens::L_BFGS());

  BOOST_REQUIRE_EQUAL(lr.Parameters().n_elem, lrSparse.Parameters().n_elem);
  for (size_t i = 0; i < lr.Parameters().n_elem; ++i)
    BOOST_REQUIRE_CLOSE(lr.Parameters()[i], lrSparse.Parameters()[i], 5e-4);
}

/**
 * Test training of linear svm for multiple classes on a complex gaussian
 * dataset using L-BFGS optimizer.
 */
BOOST_AUTO_TEST_CASE(LinearSVMLBFGSMultipleClasses)
{
  const size_t points = 1000;
  const size_t inputSize = 5;
  const size_t numClasses = 5;
  const double lambda = 0.5;

  // Generate five-Gaussian dataset.
  arma::mat identity = arma::eye<arma::mat>(5, 5);
  GaussianDistribution g1(arma::vec("1.0 9.0 1.0 2.0 2.0"), identity);
  GaussianDistribution g2(arma::vec("4.0 3.0 4.0 2.0 2.0"), identity);
  GaussianDistribution g3(arma::vec("3.0 2.0 7.0 0.0 5.0"), identity);
  GaussianDistribution g4(arma::vec("4.0 1.0 1.0 2.0 7.0"), identity);
  GaussianDistribution g5(arma::vec("1.0 0.0 1.0 8.0 3.0"), identity);

  arma::mat data(inputSize, points);
  arma::Row<size_t> labels(points);

  for (size_t i = 0; i < points / 5; i++)
  {
    data.col(i) = g1.Random();
    labels(i) = 0;
  }
  for (size_t i = points / 5; i < (2 * points) / 5; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }
  for (size_t i = (2 * points) / 5; i < (3 * points) / 5; i++)
  {
    data.col(i) = g3.Random();
    labels(i) = 2;
  }
  for (size_t i = (3 * points) / 5; i < (4 * points) / 5; i++)
  {
    data.col(i) = g4.Random();
    labels(i) = 3;
  }
  for (size_t i = (4 * points) / 5; i < points; i++)
  {
    data.col(i) = g5.Random();
    labels(i) = 4;
  }

  // Train linear svm object using L-BFGS optimizer.
  LinearSVM<arma::mat> lsvm(data, labels, numClasses, lambda);

  // Compare training accuracy to 1.
  const double acc = lsvm.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(acc, 1.0, 2.0);

  // Create test dataset.
  for (size_t i = 0; i < points / 5; i++)
  {
    data.col(i) = g1.Random();
    labels(i) = 0;
  }
  for (size_t i = points / 5; i < (2 * points) / 5; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }
  for (size_t i = (2 * points) / 5; i < (3 * points) / 5; i++)
  {
    data.col(i) = g3.Random();
    labels(i) = 2;
  }
  for (size_t i = (3 * points) / 5; i < (4 * points) / 5; i++)
  {
    data.col(i) = g4.Random();
    labels(i) = 3;
  }
  for (size_t i = (4 * points) / 5; i < points; i++)
  {
    data.col(i) = g5.Random();
    labels(i) = 4;
  }

  // Compare test accuracy to 1.
  const double testAcc = lsvm.ComputeAccuracy(data, labels);
  BOOST_REQUIRE_CLOSE(testAcc, 1.0, 2.0);
}

/**
 * Testing single point classification (Classify()).
 */
BOOST_AUTO_TEST_CASE(LinearSVMClassifySinglePointTest)
{
  const size_t points = 500;
  const size_t inputSize = 5;
  const size_t numClasses = 5;
  const double lambda = 0.5;

  // Generate five-Gaussian dataset.
  arma::mat identity = arma::eye<arma::mat>(5, 5);
  GaussianDistribution g1(arma::vec("1.0 9.0 1.0 2.0 2.0"), identity);
  GaussianDistribution g2(arma::vec("4.0 3.0 4.0 2.0 2.0"), identity);
  GaussianDistribution g3(arma::vec("3.0 2.0 7.0 0.0 5.0"), identity);
  GaussianDistribution g4(arma::vec("4.0 1.0 1.0 2.0 7.0"), identity);
  GaussianDistribution g5(arma::vec("1.0 0.0 1.0 8.0 3.0"), identity);

  arma::mat data(inputSize, points);
  arma::Row<size_t> labels(points);

  for (size_t i = 0; i < points / 5; i++)
  {
    data.col(i) = g1.Random();
    labels(i) = 0;
  }
  for (size_t i = points / 5; i < (2 * points) / 5; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }
  for (size_t i = (2 * points) / 5; i < (3 * points) / 5; i++)
  {
    data.col(i) = g3.Random();
    labels(i) = 2;
  }
  for (size_t i = (3 * points) / 5; i < (4 * points) / 5; i++)
  {
    data.col(i) = g4.Random();
    labels(i) = 3;
  }
  for (size_t i = (4 * points) / 5; i < points; i++)
  {
    data.col(i) = g5.Random();
    labels(i) = 4;
  }

  // Train linear svm object.
  LinearSVM<arma::mat> lsvm(data, labels, numClasses, lambda);

  // Create test dataset.
  for (size_t i = 0; i < points / 5; i++)
  {
    data.col(i) = g1.Random();
    labels(i) = 0;
  }
  for (size_t i = points / 5; i < (2 * points) / 5; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }
  for (size_t i = (2 * points) / 5; i < (3 * points) / 5; i++)
  {
    data.col(i) = g3.Random();
    labels(i) = 2;
  }
  for (size_t i = (3 * points) / 5; i < (4 * points) / 5; i++)
  {
    data.col(i) = g4.Random();
    labels(i) = 3;
  }
  for (size_t i = (4 * points) / 5; i < points; i++)
  {
    data.col(i) = g5.Random();
    labels(i) = 4;
  }

  lsvm.Classify(data, labels);

  for (size_t i = 0; i < data.n_cols; ++i)
  {
    BOOST_REQUIRE_EQUAL(lsvm.Classify(data.col(i)), labels(i));
  }
}

/**
 * Test that single-point classification gives the same results as multi-point
 * classification.
 */
BOOST_AUTO_TEST_CASE(SinglePointClassifyTest)
{
  const size_t points = 500;
  const size_t inputSize = 5;
  const size_t numClasses = 5;
  const double lambda = 0.5;

  // Generate five-Gaussian dataset.
  arma::mat identity = arma::eye<arma::mat>(5, 5);
  GaussianDistribution g1(arma::vec("1.0 9.0 1.0 2.0 2.0"), identity);
  GaussianDistribution g2(arma::vec("4.0 3.0 4.0 2.0 2.0"), identity);
  GaussianDistribution g3(arma::vec("3.0 2.0 7.0 0.0 5.0"), identity);
  GaussianDistribution g4(arma::vec("4.0 1.0 1.0 2.0 7.0"), identity);
  GaussianDistribution g5(arma::vec("1.0 0.0 1.0 8.0 3.0"), identity);

  arma::mat data(inputSize, points);
  arma::Row<size_t> labels(points);

  for (size_t i = 0; i < points / 5; i++)
  {
    data.col(i) = g1.Random();
    labels(i) = 0;
  }
  for (size_t i = points / 5; i < (2 * points) / 5; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }
  for (size_t i = (2 * points) / 5; i < (3 * points) / 5; i++)
  {
    data.col(i) = g3.Random();
    labels(i) = 2;
  }
  for (size_t i = (3 * points) / 5; i < (4 * points) / 5; i++)
  {
    data.col(i) = g4.Random();
    labels(i) = 3;
  }
  for (size_t i = (4 * points) / 5; i < points; i++)
  {
    data.col(i) = g5.Random();
    labels(i) = 4;
  }

  // Train linear svm object.
  LinearSVM<arma::mat> lsvm(data, labels, numClasses, lambda);

  // Create test dataset.
  for (size_t i = 0; i < points / 5; i++)
  {
    data.col(i) = g1.Random();
    labels(i) = 0;
  }
  for (size_t i = points / 5; i < (2 * points) / 5; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }
  for (size_t i = (2 * points) / 5; i < (3 * points) / 5; i++)
  {
    data.col(i) = g3.Random();
    labels(i) = 2;
  }
  for (size_t i = (3 * points) / 5; i < (4 * points) / 5; i++)
  {
    data.col(i) = g4.Random();
    labels(i) = 3;
  }
  for (size_t i = (4 * points) / 5; i < points; i++)
  {
    data.col(i) = g5.Random();
    labels(i) = 4;
  }

  arma::Row<size_t> predictions;
  lsvm.Classify(data, predictions);

  for (size_t i = 0; i < data.n_cols; ++i)
  {
    size_t pred = lsvm.Classify(data.col(i));

    BOOST_REQUIRE_EQUAL(pred, predictions[i]);
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include "problemes.h"
#include "utilitaires.h"

#include <boost/math/constants/constants.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

ENREGISTRER_PROBLEME(317, "Firecracker") {
    // A firecracker explodes at a height of 100 m above level ground. It breaks into a large number of very small
    // fragments, which move in every direction; all of them have the same initial velocity of 20 m/s.
    //
    // We assume that the fragments move without air resistance, in a uniform gravitational field with g=9.81 m/s2.
    //
    // Find the volume (in m3) of the region through which the fragments move before reaching the ground. Give your
    // answer rounded to four decimal places.
    const long double v = 20;
    const long double h = 100;
    const long double g = 9.81L;

    long double resultat = M_PIl * (2 * g * v * h + v * v * v) * (2 * g * v * h + v * v * v) / (4 * g * g * g);
    return std::to_fixed(resultat, 4);
}


//
// Copyright (c) 2018 CNRS
//

#include "pinocchio/fwd.hpp"
#include "pinocchio/multibody/joint/joint-generic.hpp"
#include "pinocchio/multibody/liegroup/liegroup.hpp"
#include "pinocchio/multibody/liegroup/liegroup-algo.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_jointRX_motion_space)
{
  using CppAD::AD;
  using CppAD::NearEqual;

  typedef AD<double> AD_double;
  typedef pinocchio::JointCollectionDefaultTpl<AD_double> JointCollectionAD;
  typedef pinocchio::JointCollectionDefaultTpl<double> JointCollection;

  typedef pinocchio::SE3Tpl<AD_double> SE3AD;
  typedef pinocchio::MotionTpl<AD_double> MotionAD;
  typedef pinocchio::SE3Tpl<double> SE3;
  typedef pinocchio::MotionTpl<double> Motion;
  typedef pinocchio::ConstraintTpl<Eigen::Dynamic,double> ConstraintXd;

  typedef Eigen::Matrix<AD_double,Eigen::Dynamic,1> VectorXAD;
  typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> MatrixX;

  typedef JointCollectionAD::JointModelRX JointModelRXAD;
  typedef JointModelRXAD::ConfigVector_t ConfigVectorAD;
//  typedef JointModelRXAD::TangentVector_t TangentVectorAD;
  typedef JointCollectionAD::JointDataRX JointDataRXAD;

  typedef JointCollection::JointModelRX JointModelRX;
  typedef JointModelRX::ConfigVector_t ConfigVector;
  typedef JointModelRX::TangentVector_t TangentVector;
  typedef JointCollection::JointDataRX JointDataRX;

  JointModelRX jmodel; jmodel.setIndexes(0,0,0);
  JointDataRX jdata(jmodel.createData());

  JointModelRXAD jmodel_ad = jmodel.cast<AD_double>();
  JointDataRXAD jdata_ad(jmodel_ad.createData());

  typedef pinocchio::LieGroup<JointModelRX>::type JointOperation;
  ConfigVector q(jmodel.nq()); JointOperation().random(q);
  ConfigVectorAD q_ad(q.cast<AD_double>());

   // Zero order
  jmodel_ad.calc(jdata_ad,q_ad);
  jmodel.calc(jdata,q);

  SE3 M1(jdata.M);
  SE3AD M2(jdata_ad.M);
  BOOST_CHECK(M1.isApprox(M2.cast<double>()));

  // First order
  TangentVector v(TangentVector::Random(jmodel.nv()));
  VectorXAD X(jmodel_ad.nv());

  for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
  {
    X[k] = v[k];
  }
  CppAD::Independent(X);
  jmodel_ad.calc(jdata_ad,q_ad,X);
  jmodel.calc(jdata,q,v);
  VectorXAD Y(6);
  MotionAD m_ad(jdata_ad.v);
  Motion m(jdata.v);
  ConstraintXd Sref(jdata.S.matrix());

  for(Eigen::DenseIndex k = 0; k < 3; ++k)
  {
    Y[k+Motion::LINEAR] = m_ad.linear()[k];
    Y[k+Motion::ANGULAR] = m_ad.angular()[k];
  }

  CppAD::ADFun<double> vjoint(X,Y);

  CPPAD_TESTVECTOR(double) x((size_t)jmodel_ad.nv());
  for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
  {
    x[(size_t)k] = v[k];
  }

  CPPAD_TESTVECTOR(double) jac = vjoint.Jacobian(x);
  MatrixX S(6,jac.size()/6);
  S = Eigen::Map<MatrixX>(jac.data(),S.rows(),S.cols());

  BOOST_CHECK(m.isApprox(m_ad.cast<double>()));

  BOOST_CHECK(Sref.matrix().isApprox(S));
}

struct TestADOnJoints
{
  template<typename JointModel>
  void operator()(const pinocchio::JointModelBase<JointModel> &) const
  {
    JointModel jmodel;
    jmodel.setIndexes(0,0,0);

    test(jmodel);
  }

  template<typename Scalar, int Options>
  void operator()(const pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> & ) const
  {
    typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel;
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());
    jmodel.setIndexes(0,0,0);

    test(jmodel);
  }

  template<typename Scalar, int Options>
  void operator()(const pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> & ) const
  {
    typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel;
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());
    jmodel.setIndexes(0,0,0);

    test(jmodel);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollection>
  void operator()(const pinocchio::JointModelTpl<Scalar,Options,JointCollection> & ) const
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
    typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel;
    JointModel jmodel((JointModelRX()));
    jmodel.setIndexes(0,0,0);

    test(jmodel);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollection>
  void operator()(const pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> & ) const
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
    typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel;
    JointModel jmodel((JointModelRX()));
    jmodel.addJoint(JointModelRY());
    jmodel.setIndexes(0,0,0);

    test(jmodel);
  }

  template<typename JointModel>
  static void test(const pinocchio::JointModelBase<JointModel> & jmodel)
  {
    using CppAD::AD;
    using CppAD::NearEqual;

    typedef typename JointModel::Scalar Scalar;
    typedef typename JointModel::JointDataDerived JointData;

    typedef AD<Scalar> AD_scalar;

    typedef pinocchio::SE3Tpl<AD_scalar> SE3AD;
    typedef pinocchio::MotionTpl<AD_scalar> MotionAD;
    typedef pinocchio::SE3Tpl<Scalar> SE3;
    typedef pinocchio::MotionTpl<Scalar> Motion;
    typedef pinocchio::ConstraintTpl<Eigen::Dynamic,Scalar> ConstraintXd;

    typedef Eigen::Matrix<AD_scalar,Eigen::Dynamic,1> VectorXAD;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> MatrixX;

    typedef typename pinocchio::CastType<AD_scalar,JointModel>::type JointModelAD;
    typedef typename JointModelAD::JointDataDerived JointDataAD;

    typedef typename JointModelAD::ConfigVector_t ConfigVectorAD;

    typedef typename JointModel::ConfigVector_t ConfigVector;
    typedef typename JointModel::TangentVector_t TangentVector;

    JointData jdata(jmodel.createData());
    pinocchio::JointDataBase<JointData> & jdata_base = jdata;

    JointModelAD jmodel_ad = jmodel.template cast<AD_scalar>();
    JointDataAD jdata_ad(jmodel_ad.createData());
    pinocchio::JointDataBase<JointDataAD> & jdata_ad_base = jdata_ad;

    ConfigVector q(jmodel.nq());
    ConfigVector lb(ConfigVector::Constant(jmodel.nq(),-1.));
    ConfigVector ub(ConfigVector::Constant(jmodel.nq(),1.));

    typedef pinocchio::RandomConfigurationStep<pinocchio::LieGroupMap,ConfigVector,ConfigVector,ConfigVector> RandomConfigAlgo;
    RandomConfigAlgo::run(jmodel.derived(),typename RandomConfigAlgo::ArgsType(q,lb,ub));

    ConfigVectorAD q_ad(q.template cast<AD_scalar>());

    // Zero order
    jmodel_ad.calc(jdata_ad,q_ad);
    jmodel.calc(jdata,q);

    SE3 M1(jdata_base.M());
    SE3AD M2(jdata_ad_base.M());
    BOOST_CHECK(M1.isApprox(M2.template cast<Scalar>()));

    // First order
    TangentVector v(TangentVector::Random(jmodel.nv()));
    VectorXAD X(jmodel_ad.nv());

    for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
    {
      X[k] = v[k];
    }
    CppAD::Independent(X);
    jmodel_ad.calc(jdata_ad,q_ad,X);
    jmodel.calc(jdata,q,v);
    VectorXAD Y(6);
    MotionAD m_ad(jdata_ad_base.v());
    Motion m(jdata_base.v());
    ConstraintXd Sref(jdata_base.S().matrix());

    for(Eigen::DenseIndex k = 0; k < 3; ++k)
    {
      Y[k+Motion::LINEAR] = m_ad.linear()[k];
      Y[k+Motion::ANGULAR] = m_ad.angular()[k];
    }

    CppAD::ADFun<Scalar> vjoint(X,Y);

    CPPAD_TESTVECTOR(Scalar) x((size_t)jmodel_ad.nv());
    for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
    {
      x[(size_t)k] = v[k];
    }

    CPPAD_TESTVECTOR(Scalar) jac = vjoint.Jacobian(x);
    MatrixX S(6,jac.size()/6);
    S = Eigen::Map<typename EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixX)>(jac.data(),S.rows(),S.cols());

    BOOST_CHECK(m.isApprox(m_ad.template cast<Scalar>()));

    BOOST_CHECK(Sref.matrix().isApprox(S));
  }
};

BOOST_AUTO_TEST_CASE(test_all_joints)
{
  typedef pinocchio::JointCollectionDefault::JointModelVariant JointModelVariant;
  boost::mpl::for_each<JointModelVariant::types>(TestADOnJoints());

  TestADOnJoints()(pinocchio::JointModel());
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file tests/randomized_svd_test.cpp
 * @author Marcus Edel
 *
 * Test file for the Randomized SVD class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

#include <mlpack/core.hpp>
#include <mlpack/methods/randomized_svd/randomized_svd.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

BOOST_AUTO_TEST_SUITE(RandomizedSVDTest);

using namespace mlpack;

/**
 * The reconstruction and sigular value error of the obtained SVD should be
 * small.
 */
BOOST_AUTO_TEST_CASE(RandomizedSVDReconstructionError)
{
  arma::mat U = arma::randn<arma::mat>(3, 20);
  arma::mat V = arma::randn<arma::mat>(10, 3);

  arma::mat R;
  arma::qr_econ(U, R, U);
  arma::qr_econ(V, R, V);

  arma::mat s = arma::diagmat(arma::vec("1 0.1 0.01"));

  arma::mat data = arma::trans(U * arma::diagmat(s) * V.t());

  // Center the data into a temporary matrix.
  arma::mat centeredData;
  math::Center(data, centeredData);

  arma::mat U1, U2, V1, V2;
  arma::vec s1, s2, s3;

  arma::svd_econ(U1, s1, V1, centeredData);

  svd::RandomizedSVD rSVD(0, 10);
  rSVD.Apply(data, U2, s2, V2, 3);

  // Use the same amount of data for the compariosn (matrix rank).
  s3 = s1.subvec(0, s2.n_elem - 1);

  // The sigular value error should be small.
  double error = arma::norm(s2 - s3, "frob") / arma::norm(s2, "frob");
  BOOST_REQUIRE_SMALL(error, 1e-5);

  arma::mat reconstruct = U2 * arma::diagmat(s2) * V2.t();

  // The relative reconstruction error should be small.
  error = arma::norm(centeredData - reconstruct, "frob") /
      arma::norm(centeredData, "frob");
  BOOST_REQUIRE_SMALL(error, 1e-5);
}

BOOST_AUTO_TEST_SUITE_END();


#include "functions/integral_polynomial.hh"
#include "functions/polynomial.hh"
#include "functions/operators.hh"
#include <boost/test/unit_test.hpp>
#include <complex>
#include "functions/std_functions.hh"
#include "functions/all_simplifications.hh"
#include "data/matrix.hh"

BOOST_AUTO_TEST_CASE(matrix_polynomial_test) {
  using namespace manifolds;

  auto m = GetMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
  auto p = GetPolynomial(m, m * 2);
  BOOST_CHECK_EQUAL(p(2), 5 * m);

  BOOST_CHECK_EQUAL(p(m), 3 * m);

  auto p2 = p * p + 2_c * p;
  auto p2_check = GetPolynomial(3 * m, 8 * m, 4 * m);
  BOOST_CHECK(p2 == p2_check);
}


#include "NeuralNet.hpp"
#include <string>
#include <fstream>
#include <iostream>
#include <boost/range/irange.hpp>
#include <typeinfo>
#include <chrono>
#include <ctime>

int main() {
    // Dataset
    Eigen::MatrixXd X_train;
    Eigen::VectorXi y_train;
    Eigen::MatrixXd X_test;
    Eigen::VectorXi y_test;

    // Load the training and testing data from the file
    X_train = load_matrix_data("./data/X_train_large.csv");
    y_train = load_vector_data("./data/y_train_large.csv");
    X_test = load_matrix_data("./data/X_test_large.csv");
    y_test = load_vector_data("./data/y_test_large.csv");

    std::cout << "The matrix X_train is of size " << X_train.rows() << "x" << X_train.cols() << std::endl;
    std::cout << "The vector y_train is of size " << y_train.rows() << "x" << y_train.cols() << std::endl;

    // Parameters
    int NUM_CLASSES = 4;
    double start_learning_rate = 1.0;

    // Create for neural network objects
    LayerDense dense_layer_1(2, 64);
    ActivationRelu activation_relu;
    LayerDense dense_layer_2(64, NUM_CLASSES);
    ActivationSoftmax activation_softmax;
    CrossEntropyLoss loss_categorical_crossentropy;
    StochasticGradientDescent optimizer_SGD(1.0, 1e-3, 0.9);

    // variables
    double loss;
    double train_accuracy;
    double test_accuracy;
    double pred;
    int index_pred;

    // Train DNN
    auto t_start = std::chrono::high_resolution_clock::now();

    int NUMBER_OF_EPOCHS = 10000;
    for (int epoch : boost::irange(0,NUMBER_OF_EPOCHS)) {
        ////////////////////////////////////////////////////////forward pass//////////////////////////////////////////////////////////////////////////////////
        dense_layer_1.forward(X_train);
        activation_relu.forward(dense_layer_1.output);
        dense_layer_2.forward(activation_relu.output);
        activation_softmax.forward(dense_layer_2.output);
        // calculate loss
        loss = loss_categorical_crossentropy.calculate(activation_softmax.output, y_train);
        // get predictions and accuracy
        Eigen::MatrixXd::Index maxRow, maxCol;
        Eigen::VectorXi predictions(activation_softmax.output.rows());
        Eigen::VectorXd pred_truth_comparison(activation_softmax.output.rows());

        for (int i=0; i < activation_softmax.output.rows(); i++) {
            pred = activation_softmax.output.row(i).maxCoeff(&maxRow, &maxCol);
            index_pred = maxCol;
            predictions(i) = index_pred;
            pred_truth_comparison(i) = predictions(i) == y_train(i);
        }
        train_accuracy = pred_truth_comparison.mean();
        if (epoch % 1000 == 0) {
            std::cout << "epoch: " << epoch << std::endl;
            std::cout << "train_accuracy: " << train_accuracy << std::endl;
            std::cout << "learning_rate: " << optimizer_SGD.learning_rate << std::endl;
            std::cout << "loss: " << loss << std::endl;
        }

        ////////////////////////////////////////////////////////////backward pass/////////////////////////////////////////////////////////////////////////
        loss_categorical_crossentropy.backward(activation_softmax.output, y_train);
        activation_softmax.backward(loss_categorical_crossentropy.dinputs);
        dense_layer_2.backward(activation_softmax.dinputs);
        activation_relu.backward(dense_layer_2.dinputs);
        dense_layer_1.backward(activation_relu.dinputs);

        ////////////////////////////////////////////////////////////debugging/////////////////////////////////////////////////////////////////////////
        // std::cout << "Type of y_train " <<   typeid(y_train).name() << std::endl;
        // std::cout << "Type of X_train " <<   typeid(X_train).name() << std::endl;
        // std::cout << "The matrix X_train is of size " << X_train.rows() << "x" << X_train.cols() << std::endl;
        // std::cout << "The vector y_train is of size " << y_train.rows() << "x" << y_train.cols() << std::endl;
        // std::cout << "X_train " << X_train << std::endl;
        // std::cout << "y_train " << y_train << std::endl;

        // std::cout << "The matrix dense_layer_1.weights is of size " << dense_layer_1.weights.rows() << "x" << dense_layer_1.weights.cols() << std::endl;
        // std::cout << "The matrix dense_layer_1.biases is of size " << dense_layer_1.biases.rows() << "x" << dense_layer_1.biases.cols() << std::endl;
        // std::cout << "The matrix dense_layer_2.weights is of size " << dense_layer_2.weights.rows() << "x" << dense_layer_2.weights.cols() << std::endl;
        // std::cout << "The matrix dense_layer_2.biases is of size " << dense_layer_2.biases.rows() << "x" << dense_layer_2.biases.cols() << std::endl;

        // std::cout << dense_layer_1.weights << std::endl;
        // std::cout << dense_layer_1.biases << std::endl;
        // std::cout << dense_layer_2.weights << std::endl;
        // std::cout << dense_layer_2.biases << std::endl;

        ////////////////////////////////////////////////////////////optimizer - update weights and biases/////////////////////////////////////////////////////////////////////////
        optimizer_SGD.pre_update_params(start_learning_rate);
        optimizer_SGD.update_params(dense_layer_1);
        optimizer_SGD.update_params(dense_layer_2);
        optimizer_SGD.post_update_params();

        ////////////////////////////////////////////////////////////debugging/////////////////////////////////////////////////////////////////////////
        // std::cout << "\nepoch: " << epoch << "\n";

        // std::cout << "\nLayer 1 weights after \n" << dense_layer_1.weights << std::endl;
        // std::cout << "\nLayer 1 biases after \n" << dense_layer_1.biases << std::endl;
        // std::cout << "\nLayer 2 weights after \n" << dense_layer_2.weights << std::endl;
        // std::cout << "\nLayer 2 biases after \n" << dense_layer_2.biases << std::endl;

        // std::cout << "\ndense_layer_1.output\n" << dense_layer_1.output << std::endl;
        // std::cout << "\nactivation_relu.output\n" << activation_relu.output << std::endl;
        // std::cout << "\ndense_layer_2.output\n" << dense_layer_2.output << std::endl;
        // std::cout << "\nactivation_softmax.output\n" << activation_softmax.output << std::endl;

        // std::cout << "loss: " << loss << std::endl;
        // std::cout << "predictions: " << predictions << std::endl;

        // std::cout << "loss_categorical_crossentropy.dinputs: " << loss_categorical_crossentropy.dinputs << std::endl;
        // std::cout << "activation_softmax.dinputs: " << activation_softmax.dinputs << std::endl;
        // std::cout << "dense_layer_2.dinputs: " << dense_layer_2.dinputs << std::endl;
        // std::cout << "activation_relu.dinputs: " << activation_relu.dinputs << std::endl;
    }

    // Time training time
    auto t_end = std::chrono::high_resolution_clock::now();
    std::cout << "\ntraining took " << std::chrono::duration_cast<std::chrono::seconds>(t_end-t_start).count()<< " seconds\n";

    // Test DNN
    dense_layer_1.forward(X_test);
    activation_relu.forward(dense_layer_1.output);
    dense_layer_2.forward(activation_relu.output);
    activation_softmax.forward(dense_layer_2.output);
    // calculate loss
    loss = loss_categorical_crossentropy.calculate(activation_softmax.output, y_test);
    // get predictions and accuracy
    Eigen::MatrixXd::Index maxRow, maxCol;
    Eigen::VectorXi predictions(activation_softmax.output.rows());
    Eigen::VectorXd pred_truth_comparison(activation_softmax.output.rows());

    for (int i=0; i < activation_softmax.output.rows(); i++) {
            pred = activation_softmax.output.row(i).maxCoeff(&maxRow, &maxCol);
            index_pred = maxCol;
            predictions(i) = index_pred;
            pred_truth_comparison(i) = predictions(i) == y_test(i);
    }
    test_accuracy = pred_truth_comparison.mean();
    std::cout << "\ntest_accuracy: " << test_accuracy << std::endl;
}


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

using namespace Eigen;

template <DenseIndex rows, DenseIndex cols>
void resizeLikeTest() {
  MatrixXf A(rows, cols);
  MatrixXf B;
  Matrix<double, rows, cols> C;
  B.resizeLike(A);
  C.resizeLike(B);  // Shouldn't crash.
  EXPECT_EQ(B.rows(), rows);
  EXPECT_EQ(B.cols(), cols);

  VectorXf x(rows);
  RowVectorXf y;
  y.resizeLike(x);
  EXPECT_EQ(y.rows(), 1);
  EXPECT_EQ(y.cols(), rows);

  y.resize(cols);
  x.resizeLike(y);
  EXPECT_EQ(x.rows(), cols);
  EXPECT_EQ(x.cols(), 1);
}

TEST(resizeLikeTest12, EXAMPLE) { resizeLikeTest<1, 2>(); }

TEST(resizeLikeTest1020, EXAMPLE) { resizeLikeTest<10, 20>(); }

TEST(resizeLikeTest31, EXAMPLE) { resizeLikeTest<3, 1>(); }

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