//
// Copyright (c) 2019-2020 INRIA
//

#include <pinocchio/math/rpy.hpp>
#include <pinocchio/math/quaternion.hpp>
#include <pinocchio/spatial/skew.hpp>

#include <boost/variant.hpp> // to avoid C99 warnings

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_rpyToMatrix)
{
  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
  double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;

  Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(r, p, y);

  Eigen::Matrix3d Raa = (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())
                            * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
                            * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())
                            ).toRotationMatrix();

  BOOST_CHECK(R.isApprox(Raa));

  Eigen::Vector3d v;
  v << r, p, y;

  Eigen::Matrix3d Rv = pinocchio::rpy::rpyToMatrix(v);

  BOOST_CHECK(Rv.isApprox(Raa));
  BOOST_CHECK(Rv.isApprox(R));
}

BOOST_AUTO_TEST_CASE(test_matrixToRpy)
{
  #ifdef NDEBUG
    const int n = 1e5;
  #else
    const int n = 1e2;
  #endif
  for(int k = 0; k < n ; ++k)
  {
    Eigen::Quaterniond quat;
    pinocchio::quaternion::uniformRandom(quat);
    const Eigen::Matrix3d R = quat.toRotationMatrix();

    const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
    Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);

    BOOST_CHECK(Rprime.isApprox(R));
    BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
    BOOST_CHECK(-M_PI/2 <= v[1] && v[1] <= M_PI/2);
    BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
  }

#ifdef NDEBUG
  const int n2 = 1e3;
#else
  const int n2 = 1e2;
#endif

  // Test singular case theta = pi/2
  for(int k = 0; k < n2 ; ++k)
  {
    double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
    double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
    Eigen::Matrix3d Rp;
    Rp <<  0.0, 0.0, 1.0,
           0.0, 1.0, 0.0,
          -1.0, 0.0, 0.0;
    const Eigen::Matrix3d R = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix()
                            * Rp
                            * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix();

    const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
    Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);

    BOOST_CHECK(Rprime.isApprox(R));
    BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
    BOOST_CHECK(-M_PI/2 <= v[1] && v[1] <= M_PI/2);
    BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
  }

  // Test singular case theta = -pi/2
  for(int k = 0; k < n2 ; ++k)
  {
    double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
    double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
    Eigen::Matrix3d Rp;
    Rp << 0.0, 0.0, -1.0,
          0.0, 1.0,  0.0,
          1.0, 0.0,  0.0;
    const Eigen::Matrix3d R = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix()
                            * Rp
                            * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix();

    const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
    Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);

    BOOST_CHECK(Rprime.isApprox(R));
    BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
    BOOST_CHECK(-M_PI/2 <= v[1] && v[1] <= M_PI/2);
    BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
  }
}

BOOST_AUTO_TEST_CASE(test_computeRpyJacobian)
{
  // Check identity at zero
  Eigen::Vector3d rpy(Eigen::Vector3d::Zero());
  Eigen::Matrix3d j0 = pinocchio::rpy::computeRpyJacobian(rpy);
  BOOST_CHECK(j0.isIdentity());
  Eigen::Matrix3d jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
  BOOST_CHECK(jL.isIdentity());
  Eigen::Matrix3d jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
  BOOST_CHECK(jW.isIdentity());
  Eigen::Matrix3d jA = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
  BOOST_CHECK(jA.isIdentity());

  // Check correct identities between different versions
  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
  double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
  rpy = Eigen::Vector3d(r, p, y);
  Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(rpy);
  j0 = pinocchio::rpy::computeRpyJacobian(rpy);
  jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
  jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
  jA = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
  BOOST_CHECK(j0 == jL);
  BOOST_CHECK(jW == jA);
  BOOST_CHECK(jW.isApprox(R*jL));

  // Check against analytical formulas
  Eigen::Vector3d jL0Expected = Eigen::Vector3d::UnitX();
  Eigen::Vector3d jL1Expected = Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix().transpose().col(1);
  Eigen::Vector3d jL2Expected = (Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
                               * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())
                                ).toRotationMatrix().transpose().col(2);
  BOOST_CHECK(jL.col(0).isApprox(jL0Expected));
  BOOST_CHECK(jL.col(1).isApprox(jL1Expected));
  BOOST_CHECK(jL.col(2).isApprox(jL2Expected));

  Eigen::Vector3d jW0Expected = (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())
                               * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
                                ).toRotationMatrix().col(0);
  Eigen::Vector3d jW1Expected = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix().col(1);
  Eigen::Vector3d jW2Expected = Eigen::Vector3d::UnitZ();
  BOOST_CHECK(jW.col(0).isApprox(jW0Expected));
  BOOST_CHECK(jW.col(1).isApprox(jW1Expected));
  BOOST_CHECK(jW.col(2).isApprox(jW2Expected));

  // Check against finite differences
  Eigen::Vector3d rpydot = Eigen::Vector3d::Random();
  double const eps = 1e-7;
  double const tol = 1e-5;

  Eigen::Matrix3d dRdr = (pinocchio::rpy::rpyToMatrix(r + eps, p, y) - R) / eps;
  Eigen::Matrix3d dRdp = (pinocchio::rpy::rpyToMatrix(r, p + eps, y) - R) / eps;
  Eigen::Matrix3d dRdy = (pinocchio::rpy::rpyToMatrix(r, p, y + eps) - R) / eps;
  Eigen::Matrix3d Rdot = dRdr * rpydot[0] + dRdp * rpydot[1] + dRdy * rpydot[2];

  Eigen::Vector3d omegaL = jL * rpydot;
  BOOST_CHECK(Rdot.isApprox(R * pinocchio::skew(omegaL), tol));

  Eigen::Vector3d omegaW = jW * rpydot;
  BOOST_CHECK(Rdot.isApprox(pinocchio::skew(omegaW) * R, tol));
}

BOOST_AUTO_TEST_CASE(test_computeRpyJacobianInverse)
{
  // Check correct identities between different versions
  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
  double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
  p *= 0.999; // ensure we are not too close to a singularity
  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
  Eigen::Vector3d rpy(r, p, y);

  Eigen::Matrix3d j0 = pinocchio::rpy::computeRpyJacobian(rpy);
  Eigen::Matrix3d j0inv = pinocchio::rpy::computeRpyJacobianInverse(rpy);
  BOOST_CHECK(j0inv.isApprox(j0.inverse()));

  Eigen::Matrix3d jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
  Eigen::Matrix3d jLinv = pinocchio::rpy::computeRpyJacobianInverse(rpy, pinocchio::LOCAL);
  BOOST_CHECK(jLinv.isApprox(jL.inverse()));

  Eigen::Matrix3d jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
  Eigen::Matrix3d jWinv = pinocchio::rpy::computeRpyJacobianInverse(rpy, pinocchio::WORLD);
  BOOST_CHECK(jWinv.isApprox(jW.inverse()));

  Eigen::Matrix3d jA = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
  Eigen::Matrix3d jAinv = pinocchio::rpy::computeRpyJacobianInverse(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
  BOOST_CHECK(jAinv.isApprox(jA.inverse()));
}

BOOST_AUTO_TEST_CASE(test_computeRpyJacobianTimeDerivative)
{
  // Check zero at zero velocity
  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
  double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
  Eigen::Vector3d rpy(r, p, y);
  Eigen::Vector3d rpydot(Eigen::Vector3d::Zero());
  Eigen::Matrix3d dj0 = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot);
  BOOST_CHECK(dj0.isZero());
  Eigen::Matrix3d djL = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL);
  BOOST_CHECK(djL.isZero());
  Eigen::Matrix3d djW = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::WORLD);
  BOOST_CHECK(djW.isZero());
  Eigen::Matrix3d djA = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL_WORLD_ALIGNED);
  BOOST_CHECK(djA.isZero());

  // Check correct identities between different versions
  rpydot = Eigen::Vector3d::Random();
  dj0 = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot);
  djL = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL);
  djW = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::WORLD);
  djA = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL_WORLD_ALIGNED);
  BOOST_CHECK(dj0 == djL);
  BOOST_CHECK(djW == djA);

  Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(rpy);
  Eigen::Matrix3d jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
  Eigen::Matrix3d jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
  Eigen::Vector3d omegaL = jL * rpydot;
  Eigen::Vector3d omegaW = jW * rpydot;
  BOOST_CHECK(omegaW.isApprox(R*omegaL));
  BOOST_CHECK(djW.isApprox(pinocchio::skew(omegaW)*R*jL + R*djL));
  BOOST_CHECK(djW.isApprox(R*pinocchio::skew(omegaL)*jL + R*djL));

  // Check against finite differences
  double const eps = 1e-7;
  double const tol = 1e-5;
  Eigen::Vector3d rpyEps = rpy;

  rpyEps[0] += eps;
  Eigen::Matrix3d djLdr = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::LOCAL) - jL) / eps;
  rpyEps[0] = rpy[0];
  rpyEps[1] += eps;
  Eigen::Matrix3d djLdp = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::LOCAL) - jL) / eps;
  rpyEps[1] = rpy[1];
  rpyEps[2] += eps;
  Eigen::Matrix3d djLdy = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::LOCAL) - jL) / eps;
  rpyEps[2] = rpy[2];
  Eigen::Matrix3d djLf = djLdr * rpydot[0] + djLdp * rpydot[1] + djLdy * rpydot[2];
  BOOST_CHECK(djL.isApprox(djLf, tol));

  rpyEps[0] += eps;
  Eigen::Matrix3d djWdr = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::WORLD) - jW) / eps;
  rpyEps[0] = rpy[0];
  rpyEps[1] += eps;
  Eigen::Matrix3d djWdp = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::WORLD) - jW) / eps;
  rpyEps[1] = rpy[1];
  rpyEps[2] += eps;
  Eigen::Matrix3d djWdy = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::WORLD) - jW) / eps;
  rpyEps[2] = rpy[2];
  Eigen::Matrix3d djWf = djWdr * rpydot[0] + djWdp * rpydot[1] + djWdy * rpydot[2];
  BOOST_CHECK(djW.isApprox(djWf, tol));
}

BOOST_AUTO_TEST_SUITE_END()


#include "problemes.h"
#include "chiffres.h"
#include "utilitaires.h"
#include "mpz_nombre.h"
#include "mpq_fraction.h"

#include <boost/range/adaptor/reversed.hpp>

ENREGISTRER_PROBLEME(65, "Convergents of e") {
    // The square root of 2 can be written as an infinite continued fraction.
    //
    // The infinite continued fraction can be written, √2 = [1;(2)], (2) indicates that 2 repeats ad infinitum. In a
    // similar way, √23 = [4;(1,3,1,8)].
    //
    // It turns out that the sequence of partial values of continued fractions for square roots provide the best rational
    // approximations. Let us consider the convergents for √2.
    //
    // Hence the sequence of the first ten convergents for √2 are:
    //
    // 1, 3/2, 7/5, 17/12, 41/29, 99/70, 239/169, 577/408, 1393/985, 3363/2378, ...
    // What is most surprising is that the important mathematical constant,
    // e = [2; 1,2,1, 1,4,1, 1,6,1 , ... , 1,2k,1, ...].
    //
    // The first ten terms in the sequence of convergents for e are:
    //
    // 2, 3, 8/3, 11/4, 19/7, 87/32, 106/39, 193/71, 1264/465, 1457/536, ...
    // The sum of digits in the numerator of the 10th convergent is 1+4+5+7=17.
    //
    // Find the sum of digits in the numerator of the 100th convergent of the continued fraction for e.
    std::vector<mpz_nombre> fraction_continue;
    fraction_continue.emplace_back(2);
    for (size_t n = 2; n < 101; n += 2) {
        fraction_continue.emplace_back(1);
        fraction_continue.emplace_back(n);
        fraction_continue.emplace_back(1);
    }
    fraction_continue.resize(99);
    mpq_fraction f(1);
    for (const auto &p: boost::adaptors::reverse(fraction_continue)) f = p + 1 / f;

    mpz_nombre resultat = f.numerateur().somme_chiffres();
    return resultat.to_string();
}


/*
 * Copyright 2012 Karsten Ahnert
 * Copyright 2012 Mario Mulansky
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>
#include <fstream>
#include <utility>
#include "time.h"

#include <boost/numeric/odeint.hpp>
#include <boost/phoenix/phoenix.hpp>
#include <boost/numeric/mtl/mtl.hpp>

#include <boost/numeric/odeint/external/mtl4/implicit_euler_mtl4.hpp>

using namespace std;
using namespace boost::numeric::odeint;

namespace phoenix = boost::phoenix;

typedef mtl::dense_vector< double > vec_mtl4;
typedef mtl::compressed2D< double > mat_mtl4;

typedef boost::numeric::ublas::vector< double > vec_ublas;
typedef boost::numeric::ublas::matrix< double > mat_ublas;

// two systems defined 1 & 2 both are mostly sparse with the number of element variable
struct system1_mtl4
{

    void operator()( const vec_mtl4 &x , vec_mtl4 &dxdt , double t )
    {
        int size = mtl::size(x);

        dxdt[ 0 ] = -0.06*x[0];

        for (int i =1; i< size ; ++i){

            dxdt[ i ] = 4.2*x[i-1]-2.2*x[i]*x[i];
        }

    }
};

struct jacobi1_mtl4
{
    void operator()( const vec_mtl4 &x , mat_mtl4 &J , const double &t )
    {
        int size = mtl::size(x);
        mtl::matrix::inserter<mat_mtl4> ins(J);

        ins[0][0]=-0.06;

        for (int i =1; i< size ; ++i)
        {
            ins[i][i-1] = + 4.2;
            ins[i][i] = -4.2*x[i];
        }
    }
};

struct system1_ublas
{

    void operator()( const vec_ublas &x , vec_ublas &dxdt , double t )
    {
        int size = x.size();

        dxdt[ 0 ] = -0.06*x[0];

        for (int i =1; i< size ; ++i){

            dxdt[ i ] = 4.2*x[i-1]-2.2*x[i]*x[i];
        }

    }
};

struct jacobi1_ublas
{
    void operator()( const vec_ublas &x , mat_ublas &J , const double &t )
    {
        int size = x.size();
// mtl::matrix::inserter<mat_mtl4> ins(J);

        J(0,0)=-0.06;

        for (int i =1; i< size ; ++i){
//ins[i][0]=120.0*x[i];
            J(i,i-1) = + 4.2;
            J(i,i) = -4.2*x[i];

        }
    }
};

struct system2_mtl4
{

    void operator()( const vec_mtl4 &x , vec_mtl4 &dxdt , double t )
    {
        int size = mtl::size(x);

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

            dxdt[ i ] = -0.5*x[i];
            dxdt[i+1]= +25*x[i+1]*x[i+2]-740*x[i+3]*x[i+3]+4.2e-2*x[i];
            dxdt[i+2]= +25*x[i]*x[i]-740*x[i+3]*x[i+3];
            dxdt[i+3]= -25*x[i+1]*x[i+2]+740*x[i+3]*x[i+3];
            dxdt[i+4] = 0.250*x[i]*x[i+1]-44.5*x[i+3];

        }

    }
};

struct jacobi2_mtl4
{
    void operator()( const vec_mtl4 &x , mat_mtl4 &J , const double &t )
    {
        int size = mtl::size(x);
        mtl::matrix::inserter<mat_mtl4> ins(J);

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

            ins[ i ][i] = -0.5;
            ins[i+1][i+1]=25*x[i+2];
            ins[i+1][i+2] = 25*x[i+1];
            ins[i+1][i+3] = -740*2*x[i+3];
            ins[i+1][i] =+4.2e-2;

            ins[i+2][i]= 50*x[i];
            ins[i+2][i+3]= -740*2*x[i+3];
            ins[i+3][i+1] = -25*x[i+2];
            ins[i+3][i+2] = -25*x[i+1];
            ins[i+3][i+3] = +740*2*x[i+3];
            ins[i+4][i] = 0.25*x[i+1];
            ins[i+4][i+1] =0.25*x[i];
            ins[i+4][i+3]=-44.5;

        }
    }
};

struct system2_ublas
{

    void operator()( const vec_ublas &x , vec_ublas &dxdt , double t )
    {
        int size = x.size();
        for (int i =0; i< size/5 ; i+=5){

            dxdt[ i ] = -4.2e-2*x[i];
            dxdt[i+1]= +25*x[i+1]*x[i+2]-740*x[i+3]*x[i+3]+4.2e-2*x[i];
            dxdt[i+2]= +25*x[i]*x[i]-740*x[i+3]*x[i+3];
            dxdt[i+3]= -25*x[i+1]*x[i+2]+740*x[i+3]*x[i+3];
            dxdt[i+4] = 0.250*x[i]*x[i+1]-44.5*x[i+3];

        }

    }
};

struct jacobi2_ublas
{
    void operator()( const vec_ublas &x , mat_ublas &J , const double &t )
    {
        int size = x.size();

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

            J(i ,i) = -4.2e-2;
            J(i+1,i+1)=25*x[i+2];
            J(i+1,i+2) = 25*x[i+1];
            J(i+1,i+3) = -740*2*x[i+3];
            J(i+1,i) =+4.2e-2;

            J(i+2,i)= 50*x[i];
            J(i+2,i+3)= -740*2*x[i+3];
            J(i+3,i+1) = -25*x[i+2];
            J(i+3,i+2) = -25*x[i+1];
            J(i+3,i+3) = +740*2*x[i+3];
            J(i+4,i) = 0.25*x[i+1];
            J(i+4,i+1) =0.25*x[i];
            J(i+4,i+3)=-44.5;

        }

    }
};

void testRidiculouslyMassiveArray( int size )
{
    typedef boost::numeric::odeint::implicit_euler_mtl4 < double > mtl4stepper;
    typedef boost::numeric::odeint::implicit_euler< double > booststepper;

    vec_mtl4 x(size , 0.0);
    x[0]=1;

    double dt = 0.02;
    double endtime = 10.0;

    clock_t tstart_mtl4 = clock();
    size_t num_of_steps_mtl4 = integrate_const(
        mtl4stepper() ,
        make_pair( system1_mtl4() , jacobi1_mtl4() ) ,
        x , 0.0 , endtime , dt  );
    clock_t tend_mtl4 = clock() ;

    clog << x[0] << endl;
    clog << num_of_steps_mtl4 << " time elapsed: " << (double)(tend_mtl4-tstart_mtl4 )/CLOCKS_PER_SEC << endl;

    vec_ublas x_ublas(size , 0.0);
    x_ublas[0]=1;

    clock_t tstart_boost = clock();
    size_t num_of_steps_ublas = integrate_const(
        booststepper() ,
        make_pair( system1_ublas() , jacobi1_ublas() ) ,
        x_ublas , 0.0 , endtime , dt  );
    clock_t tend_boost = clock() ;

    clog << x_ublas[0] << endl;
    clog << num_of_steps_ublas << " time elapsed: " << (double)(tend_boost-tstart_boost)/CLOCKS_PER_SEC<< endl;

    clog << "dt_ublas/dt_mtl4 = " << (double)( tend_boost-tstart_boost )/( tend_mtl4-tstart_mtl4 ) << endl << endl;
    return ;
}

void testRidiculouslyMassiveArray2( int size )
{
    typedef boost::numeric::odeint::implicit_euler_mtl4 < double > mtl4stepper;
    typedef boost::numeric::odeint::implicit_euler< double > booststepper;

    vec_mtl4 x(size , 0.0);
    x[0]=100;

    double dt = 0.01;
    double endtime = 10.0;

    clock_t tstart_mtl4 = clock();
    size_t num_of_steps_mtl4 = integrate_const(
        mtl4stepper() ,
        make_pair( system1_mtl4() , jacobi1_mtl4() ) ,
        x , 0.0 , endtime , dt );

    clock_t tend_mtl4 = clock() ;

    clog << x[0] << endl;
    clog << num_of_steps_mtl4 << " time elapsed: " << (double)(tend_mtl4-tstart_mtl4 )/CLOCKS_PER_SEC << endl;

    vec_ublas x_ublas(size , 0.0);
    x_ublas[0]=100;

    clock_t tstart_boost = clock();
    size_t num_of_steps_ublas = integrate_const(
        booststepper() ,
        make_pair( system1_ublas() , jacobi1_ublas() ) ,
        x_ublas , 0.0 , endtime , dt  );

    clock_t tend_boost = clock() ;

    clog << x_ublas[0] << endl;
    clog << num_of_steps_ublas << " time elapsed: " << (double)(tend_boost-tstart_boost)/CLOCKS_PER_SEC<< endl;

    clog << "dt_ublas/dt_mtl4 = " << (double)( tend_boost-tstart_boost )/( tend_mtl4-tstart_mtl4 ) << endl << endl;
    return ;
}

int main( int argc , char **argv )
{
    std::vector< size_t > length;
    length.push_back( 8 );
    length.push_back( 16 );
    length.push_back( 32 );
    length.push_back( 64 );
    length.push_back( 128 );
    length.push_back( 256 );

    for( size_t i=0 ; i<length.size() ; ++i )
    {
        clog << "Testing with size " << length[i] << endl;
        testRidiculouslyMassiveArray( length[i] );
    }
    clog << endl << endl;

    for( size_t i=0 ; i<length.size() ; ++i )
    {
        clog << "Testing with size " << length[i] << endl;
        testRidiculouslyMassiveArray2( length[i] );
    }
}


#include "problemes.h"
#include "arithmetique.h"

#include <fstream>
#include <boost/algorithm/string.hpp>
#include <numeric>

typedef unsigned long long nombre;

ENREGISTRER_PROBLEME(42, "Coded triangle numbers") {
    // The nth term of the sequence of triangle numbers is given by, tn = ½n(n+1); so the first ten triangle numbers are:
    //
    //                                      1, 3, 6, 10, 15, 21, 28, 36, 45, 55, ...
    //
    // By converting each letter in a word to a number corresponding to its alphabetical position and adding these values
    // we form a word value. For example, the word value for SKY is 19 + 11 + 25 = 55 = t10. If the word value is a triangle
    // number then we shall call the word a triangle word.
    //
    // Using words.txt (right click and 'Save Link/Target As...'), a 16K text file containing nearly two-thousand common English words, how many are triangle words?
    std::ifstream ifs("data/p042_words.txt");
    std::string entree;
    ifs >> entree;
    std::vector<std::string> names;
    boost::split(names, entree, boost::is_any_of(","));

    std::set<nombre> triangle;
    for (nombre n = 1; n < 50; ++n)
        triangle.insert(n * (n + 1) / 2);

    nombre resultat = 0;
    for (const auto &name: names) {
        nombre score = std::accumulate(name.begin(), name.end(), 0ULL, [](const nombre &n, char c) {
            if (c != '"')
                return n + 1 + static_cast<nombre>(c - 'A');
            return n;
        });
        if (triangle.find(score) != triangle.end())
            ++resultat;
    }
    return std::to_string(resultat);
}


//
//  gmatrix9float.cpp
//  GCommon
//
//  Created by David Coen on 2011 06 01
//  Copyright Pleasure seeking morons 2011. All rights reserved.
//

#include "gmatrix9float.h"

#include "gvector3float.h"
#include "gmathmatrix.h"
#include "gmath.h"

#include <boost/swap.hpp>

#define DSC_INLINE_MATRIX_MUL

/*static*/ const GMatrix9Float GMatrix9Float::sIdentity(1.0F, 0.0F, 0.0F,
														  0.0F, 1.0F, 0.0F,
														  0.0F, 0.0F, 1.0F);

//constructors
GMatrix9Float::GMatrix9Float(const GR32 in_data_0_0, const GR32 in_data_0_1, const GR32 in_data_0_2,
	const GR32 in_data_1_0, const GR32 in_data_1_1, const GR32 in_data_1_2,
	const GR32 in_data_2_0, const GR32 in_data_2_1, const GR32 in_data_2_2
	)
{
	SetData(in_data_0_0, in_data_0_1, in_data_0_2,
		in_data_1_0, in_data_1_1, in_data_1_2,
		in_data_2_0, in_data_2_1, in_data_2_2
		);
	return;
}
GMatrix9Float::GMatrix9Float(const GR32* const in_data)
{
	for (GS32 index = 0; index < 9; ++index)
	{
		m_data[index] = in_data[index];
	}
	return;

}

GMatrix9Float::GMatrix9Float(const GMatrix9Float& in_src)
{
	(*this) = in_src;
	return;
}

GMatrix9Float::~GMatrix9Float()
{
	return;
}

//operators
const GMatrix9Float& GMatrix9Float::operator=(const GMatrix9Float& in_rhs)
{
	for (GS32 index = 0; index < 9; ++index)
	{
		m_data[index] = in_rhs.m_data[index];
	}
	return (*this);
}

//public methods
GMatrix9Float& GMatrix9Float::TransposeSelf()
{
	std::swap(m_0_1, m_1_0);
	std::swap(m_0_2, m_2_0);
	std::swap(m_1_2, m_2_1);

	return (*this);
}

const GMatrix9Float GMatrix9Float::ReturnInverse()const
{
	GMatrix9Float result;
	GMathMatrix<GR32>::Inverse4(&result.m_data[0], &m_data[0]);

	return result;
}

//public accessors
void GMatrix9Float::SetData(const GR32 in_data_0_0, const GR32 in_data_0_1, const GR32 in_data_0_2,
	const GR32 in_data_1_0, const GR32 in_data_1_1, const GR32 in_data_1_2,
	const GR32 in_data_2_0, const GR32 in_data_2_1, const GR32 in_data_2_2
	)
{
	m_0_0 = in_data_0_0;
	m_1_0 = in_data_1_0;
	m_2_0 = in_data_2_0;

	m_0_1 = in_data_0_1;
	m_1_1 = in_data_1_1;
	m_2_1 = in_data_2_1;

	m_0_2 = in_data_0_2;
	m_1_2 = in_data_1_2;
	m_2_2 = in_data_2_2;

	return;
}

const GVector3Float GMatrix9Float::GetAt()const
{
	const GVector3Float result(
		m_0_2,
		m_1_2,
		m_2_2
		);
	return result;
}

const GVector3Float GMatrix9Float::GetUp()const
{
	const GVector3Float result(
		m_0_1,
		m_1_1,
		m_2_1
		);
	return result;
}

//global operators
const GMatrix9Float operator*(const GMatrix9Float& in_lhs, const GMatrix9Float& in_rhs)
{
	GR32 value[9];
#ifdef DSC_INLINE_MATRIX_MUL
	const GR32* const lhsData = in_lhs.GetData();
	const GR32* const rhsData = in_rhs.GetData();
	value[ 0] = (lhsData[ 0] * rhsData[ 0]) + (lhsData[ 1] * rhsData[ 3]) + (lhsData[ 2] * rhsData[ 6]);
	value[ 1] = (lhsData[ 0] * rhsData[ 1]) + (lhsData[ 1] * rhsData[ 4]) + (lhsData[ 2] * rhsData[ 7]);
	value[ 2] = (lhsData[ 0] * rhsData[ 2]) + (lhsData[ 1] * rhsData[ 5]) + (lhsData[ 2] * rhsData[ 8]);

	value[ 3] = (lhsData[ 3] * rhsData[ 0]) + (lhsData[ 4] * rhsData[ 3]) + (lhsData[ 5] * rhsData[ 6]);
	value[ 4] = (lhsData[ 3] * rhsData[ 1]) + (lhsData[ 4] * rhsData[ 4]) + (lhsData[ 5] * rhsData[ 7]);
	value[ 5] = (lhsData[ 3] * rhsData[ 2]) + (lhsData[ 4] * rhsData[ 5]) + (lhsData[ 5] * rhsData[ 8]);

	value[ 6] = (lhsData[ 6] * rhsData[ 0]) + (lhsData[ 7] * rhsData[ 3]) + (lhsData[ 8] * rhsData[ 6]);
	value[ 7] = (lhsData[ 6] * rhsData[ 1]) + (lhsData[ 7] * rhsData[ 4]) + (lhsData[ 8] * rhsData[ 7]);
	value[ 8] = (lhsData[ 6] * rhsData[ 2]) + (lhsData[ 7] * rhsData[ 5]) + (lhsData[ 8] * rhsData[ 8]);
#else
	GMathMatrix<GR32>::MatrixMul(
		in_lhs.GetData(),
		in_rhs.GetData(),
		3,
		3,
		3,
		&value[0]
		);
#endif
	return GMatrix9Float(&value[0]);
}

const GVector3Float operator*(const GVector3Float& in_lhs, const GMatrix9Float& in_rhs)
{
	GR32 value[3];
#ifdef DSC_INLINE_MATRIX_MUL
	const GR32* const lhsData = in_lhs.GetData();
	const GR32* const rhsData = in_rhs.GetData();
	value[ 0] = (lhsData[ 0] * rhsData[ 0]) + (lhsData[ 1] * rhsData[ 3]) + (lhsData[ 2] * rhsData[ 6]);
	value[ 1] = (lhsData[ 0] * rhsData[ 1]) + (lhsData[ 1] * rhsData[ 4]) + (lhsData[ 2] * rhsData[ 7]);
	value[ 2] = (lhsData[ 0] * rhsData[ 2]) + (lhsData[ 1] * rhsData[ 5]) + (lhsData[ 2] * rhsData[ 8]);
#else
	GMathMatrix<GR32>::MatrixMul(
		in_lhs.GetData(),
		in_rhs.GetData(),
		3,
		1,
		3,
		&value[0]
		);
#endif
	return GVector3Float(value[0], value[1], value[2]);
}

const GMatrix9Float& operator*=(GMatrix9Float& in_lhs, const GMatrix9Float& in_rhs)
{
	in_lhs = (in_lhs * in_rhs);
	return in_lhs;
}

/*
  from _Mathematics for computer graphics p.171
 mapping a pair of vectors onto another pair, u,x being one pair, a,y being another of unit vectors at same angle
 u.x = a.y = cos(\), with sin(\) != 0
 a roation matrix sending u,x to a,y given by
               a
 M = [u v w ][ b ]
               c
 d = | x * u | = | y * a | = | sin(\) |
 if 90deg = u to x, cos(\) = 0, sin(\) = 1
 v = ( x * u ) / d
 b = ( y * a ) / d
 w = ( x - u( cos(\) ) ) / d
 c = ( y - a( cos(\) ) ) / d
*/
const GMatrix9Float GMatrix9FloatConstructAtUp(
	const GVector3Float& in_targetAt,
	const GVector3Float& in_targetUp,
	const GVector3Float& in_baseAt,
	const GVector3Float& in_baseUp
	)
{
	GMatrix9Float returnMatrix;

	const GVector3Float crossBaseUpAt = CrossProduct(in_baseUp, in_baseAt);
	const GVector3Float crossTargetUpAt = CrossProduct(in_targetUp, in_targetAt);

	return GMatrix9Float(
		(in_baseAt.m_x * in_targetAt.m_x) + (crossBaseUpAt.m_x * crossTargetUpAt.m_x) + (in_baseUp.m_x * in_targetUp.m_x),
		(in_baseAt.m_y * in_targetAt.m_x) + (crossBaseUpAt.m_y * crossTargetUpAt.m_x) + (in_baseUp.m_y * in_targetUp.m_x),
		(in_baseAt.m_z * in_targetAt.m_x) + (crossBaseUpAt.m_z * crossTargetUpAt.m_x) + (in_baseUp.m_z * in_targetUp.m_x),

		(in_baseAt.m_x * in_targetAt.m_y) + (crossBaseUpAt.m_x * crossTargetUpAt.m_y) + (in_baseUp.m_x * in_targetUp.m_y),
		(in_baseAt.m_y * in_targetAt.m_y) + (crossBaseUpAt.m_y * crossTargetUpAt.m_y) + (in_baseUp.m_y * in_targetUp.m_y),
		(in_baseAt.m_z * in_targetAt.m_y) + (crossBaseUpAt.m_z * crossTargetUpAt.m_y) + (in_baseUp.m_z * in_targetUp.m_y),

		(in_baseAt.m_x * in_targetAt.m_z) + (crossBaseUpAt.m_x * crossTargetUpAt.m_z) + (in_baseUp.m_x * in_targetUp.m_z),
		(in_baseAt.m_y * in_targetAt.m_z) + (crossBaseUpAt.m_y * crossTargetUpAt.m_z) + (in_baseUp.m_y * in_targetUp.m_z),
		(in_baseAt.m_z * in_targetAt.m_z) + (crossBaseUpAt.m_z * crossTargetUpAt.m_z) + (in_baseUp.m_z * in_targetUp.m_z)
		);
}

/*
  http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
*/
const GMatrix9Float GMatrix9FloatConstructAxisAngle(const GVector3Float& in_axis, const GR32 in_angleRad)
{
	const GVector3Float localAxis = Normalise(in_axis);
	const GR32 axis_x = localAxis.m_x;
	const GR32 axis_y = localAxis.m_y;
	const GR32 axis_z = localAxis.m_z;

	const GR32 c = GMath::Cos( in_angleRad );
	const GR32 s = GMath::Sin( in_angleRad );
	const GR32 t = 1.0F - c;

	const GR32 tmp1_01 = axis_x * axis_y * t;
	const GR32 tmp2_01 = axis_z * s;

	const GR32 tmp1_02 = axis_x * axis_z * t;
	const GR32 tmp2_02 = axis_y * s;

	const GR32 tmp1_21 = axis_y * axis_z * t;
	const GR32 tmp2_21 = axis_x * s;

	return GMatrix9Float(
		c + axis_x * axis_x * t, tmp1_01 - tmp2_01, tmp1_02 + tmp2_02,
		tmp1_01 + tmp2_01, c + axis_y * axis_y * t, tmp1_21 - tmp2_21,
		tmp1_02 - tmp2_02, tmp1_21 + tmp2_21, c + axis_z * axis_z * t
		);
}


#include <bundle/error/projection_errors.h>
#include <gmock/gmock.h>
#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <unsupported/Eigen/AutoDiff>

class ReprojectionError2DFixtureBase : public ::testing::Test {
 public:
  typedef Eigen::AutoDiffScalar<Eigen::VectorXd> AScalar;
  ReprojectionError2DFixtureBase() { observed << 0.5, 0.5; }

  constexpr static int size_residual = 2;
  constexpr static int size_point = 3;
  constexpr static int size_rt = 6;

  Vec2d observed;
  double scale{0.1};
  const double point[size_point] = {1.0, 2.0, 3.0};

  AScalar residual_adiff[size_residual];
  AScalar point_adiff[size_point];

  double residuals[size_residual];
  double jac_point[size_residual * size_point];
};

class ReprojectionError2DFixture : public ReprojectionError2DFixtureBase {
 public:
  void SetupADiff(int size, const double* camera, AScalar* camera_adiff) {
    const int total_size = size_point + size_rt + size_rt + size;
    for (int i = 0; i < size_point; ++i) {
      point_adiff[i].value() = point[i];
      point_adiff[i].derivatives() = VecXd::Unit(total_size, i);
    }
    for (int i = 0; i < size_rt; ++i) {
      rt_instance_adiff[i].value() = rt_instance[i];
      rt_instance_adiff[i].derivatives() =
          VecXd::Unit(total_size, size_point + i);
    }
    for (int i = 0; i < size_rt; ++i) {
      rt_camera_adiff[i].value() = rt_camera[i];
      rt_camera_adiff[i].derivatives() =
          VecXd::Unit(total_size, size_point + size_rt + i);
    }
    for (int i = 0; i < size; ++i) {
      camera_adiff[i].value() = camera[i];
      camera_adiff[i].derivatives() =
          VecXd::Unit(total_size, size_point + size_rt + size_rt + i);
    }
  }

  void CheckJacobians(int size, const double* jac_camera) {
    const double eps = 1e-14;
    for (int i = 0; i < size_residual; ++i) {
      for (int j = 0; j < size; ++j) {
        ASSERT_NEAR(
            residual_adiff[i].derivatives()(size_point + size_rt + size_rt + j),
            jac_camera[i * size + j], eps);
      }
    }
    for (int i = 0; i < size_residual; ++i) {
      for (int j = 0; j < size_point; ++j) {
        ASSERT_NEAR(residual_adiff[i].derivatives()(j),
                    jac_point[i * size_point + j], eps);
      }
    }
    for (int i = 0; i < size_residual; ++i) {
      for (int j = 0; j < size_rt; ++j) {
        ASSERT_NEAR(residual_adiff[i].derivatives()(size_point + j),
                    jac_rt_instance[i * size_rt + j], eps);
      }
    }
    for (int i = 0; i < size_residual; ++i) {
      for (int j = 0; j < size_rt; ++j) {
        ASSERT_NEAR(residual_adiff[i].derivatives()(size_point + size_rt + j),
                    jac_rt_camera[i * size_rt + j], eps);
      }
    }
  }

  template <int N>
  void RunTest(const geometry::ProjectionType& type, const double* camera) {
    constexpr int size = N;

    // Autodiff-ed version will be used as reference/expected values
    AScalar camera_adiff[size];
    SetupADiff(size, &camera[0], &camera_adiff[0]);
    bundle::ReprojectionError2D autodiff(type, observed, scale);
    autodiff(camera_adiff, rt_instance_adiff, rt_camera_adiff, point_adiff,
             residual_adiff);

    // We test for analytic evaluation
    double jac_camera[size_residual * size];
    const double* params[] = {camera, rt_camera, rt_instance, point};
    double* jacobians[] = {jac_camera, jac_rt_instance, jac_rt_camera,
                           jac_point};
    bundle::ReprojectionError2DAnalytic<size> analytic(type, observed, scale);
    analytic.Evaluate(params, residuals, &jacobians[0]);

    // Check
    CheckJacobians(size, jac_camera);
  }

  const double rt_instance[size_rt] = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6};
  AScalar rt_instance_adiff[size_rt];
  double jac_rt_instance[size_residual * size_rt];

  const double rt_camera[size_rt] = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6};
  AScalar rt_camera_adiff[size_rt];
  double jac_rt_camera[size_residual * size_rt];
};

TEST_F(ReprojectionError2DFixture, BrownAnalyticErrorEvaluatesOK) {
  constexpr int size = 9;

  // focal, ar, cx, cy, k1, k2, k3, p1, p2
  constexpr std::array<double, size> camera{0.3,   1.0,   0.001,  -0.02, 0.1,
                                            -0.03, 0.001, -0.005, 0.001};
  RunTest<size>(geometry::ProjectionType::BROWN, &camera[0]);
}

TEST_F(ReprojectionError2DFixture, PerspectiveAnalyticErrorEvaluatesOK) {
  constexpr int size = 3;

  // focal, k1, k2
  constexpr std::array<double, size> camera{0.3, 0.1, -0.03};
  RunTest<size>(geometry::ProjectionType::PERSPECTIVE, &camera[0]);
}

TEST_F(ReprojectionError2DFixture, FisheyeAnalyticErrorEvaluatesOK) {
  constexpr int size = 3;

  // focal, k1, k2, k3
  constexpr std::array<double, size> camera{0.3, 0.1, -0.03};
  RunTest<size>(geometry::ProjectionType::FISHEYE, &camera[0]);
}

TEST_F(ReprojectionError2DFixture, FisheyeOpencvAnalyticErrorEvaluatesOK) {
  constexpr int size = 8;

  // focal, ar, cx, cy, k1, k2, k3, k4
  constexpr std::array<double, size> camera{0.3, 1.0,   0.001, -0.02,
                                            0.1, -0.03, 0.001, -0.005};
  RunTest<size>(geometry::ProjectionType::FISHEYE_OPENCV, &camera[0]);
}

TEST_F(ReprojectionError2DFixture, Fisheye62AnalyticErrorEvaluatesOK) {
  constexpr int size = 12;

  // focal, ar, cx, cy, k1, k2, k3, k4, k5, k6, p1, p2
  constexpr std::array<double, size> camera{0.3,  1.0,   0.001, -0.02,
                                            0.1,  -0.03, 0.001, -0.005,
                                            0.01, 0.006, 0.02,  0.003};
  RunTest<size>(geometry::ProjectionType::FISHEYE62, &camera[0]);
}

TEST_F(ReprojectionError2DFixture, Fisheye624AnalyticErrorEvaluatesOK) {
  constexpr int size = 16;

  // focal, ar, cx, cy, k1, k2, k3, k4, k5, k6, p1, p2, s0, s1, s2, s3
  constexpr std::array<double, size> camera{0.3,  1.0,   0.001, -0.02,
                                            0.1,  -0.03, 0.001, -0.005,
                                            0.01, 0.006, 0.02,  0.003,
                                            0.001, -0.009, -0.01, 0.03};
  RunTest<size>(geometry::ProjectionType::FISHEYE624, &camera[0]);
}

TEST_F(ReprojectionError2DFixture, DualAnalyticErrorEvaluatesOK) {
  constexpr int size = 4;

  // transtion, focal, k1, k2
  constexpr std::array<double, size> camera{0.5, 0.3, 0.1, -0.03};
  RunTest<size>(geometry::ProjectionType::DUAL, &camera[0]);
}

class ReprojectionError3DFixture : public ::testing::Test {
 public:
  static constexpr int size = 3;

  typedef Eigen::AutoDiffScalar<Eigen::VectorXd> AScalar;
  ReprojectionError3DFixture() { observed << 0.5, 0.5; }

  void SetupADiff() {
    const int total_size = size_point + size_rt + size_rt;
    for (int i = 0; i < size_point; ++i) {
      point_adiff[i].value() = point[i];
      point_adiff[i].derivatives() = VecXd::Unit(total_size, i);
    }
    for (int i = 0; i < size_rt; ++i) {
      rt_instance_adiff[i].value() = rt_instance[i];
      rt_instance_adiff[i].derivatives() =
          VecXd::Unit(total_size, size_point + i);
    }
    for (int i = 0; i < size_rt; ++i) {
      rt_camera_adiff[i].value() = rt_camera[i];
      rt_camera_adiff[i].derivatives() =
          VecXd::Unit(total_size, size_point + size_rt + i);
    }
  }

  void CheckJacobians() {
    const double eps = 1e-14;
    for (int i = 0; i < size; ++i) {
      for (int j = 0; j < size_point; ++j) {
        ASSERT_NEAR(residual_adiff[i].derivatives()(j),
                    jac_point[i * size_point + j], eps);
      }
    }
    for (int i = 0; i < size; ++i) {
      for (int j = 0; j < size_rt; ++j) {
        ASSERT_NEAR(residual_adiff[i].derivatives()(size_point + j),
                    jac_instance_rt[i * size_rt + j], eps);
      }
    }
    for (int i = 0; i < size; ++i) {
      for (int j = 0; j < size_rt; ++j) {
        ASSERT_NEAR(residual_adiff[i].derivatives()(size_point + size_rt + j),
                    jac_camera_rt[i * size_rt + j], eps);
      }
    }
  }

  constexpr static int size_point = 3;
  constexpr static int size_rt = 6;

  Vec2d observed;
  double scale{0.1};
  const double point[size_point] = {1.0, 2.0, 3.0};
  const double rt_instance[size_rt] = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6};
  const double rt_camera[size_rt] = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6};

  AScalar residual_adiff[size];
  AScalar point_adiff[size_point];
  AScalar rt_instance_adiff[size_rt];
  AScalar rt_camera_adiff[size_rt];

  double residuals[size];
  double jac_instance_rt[size * size_rt];
  double jac_camera_rt[size * size_rt];
  double jac_point[size * size_point];
};

TEST_F(ReprojectionError3DFixture, AnalyticErrorEvaluatesOK) {
  // Autodiff-ed version will be used as reference/expected values
  SetupADiff();
  AScalar dummy_adiff;
  bundle::ReprojectionError3D autodiff(geometry::ProjectionType::SPHERICAL,
                                       observed, scale);
  autodiff(&dummy_adiff, rt_instance_adiff, rt_camera_adiff, point_adiff,
           residual_adiff);

  // We test for analytic evaluation
  double dummy = 0.;
  double dummy_jac[] = {0., 0., 0.};
  const double* params[] = {&dummy, rt_instance, rt_camera, point};
  double* jacobians[] = {&dummy_jac[0], jac_instance_rt, jac_camera_rt,
                         jac_point};
  bundle::ReprojectionError3DAnalytic analytic(
      geometry::ProjectionType::SPHERICAL, observed, scale);
  analytic.Evaluate(params, residuals, &jacobians[0]);

  // Check
  CheckJacobians();
}


/**
 * @file rmsprop_test.cpp
 * @author Marcus Edel
 *
 * Tests the RMSProp optimizer.
 */
#include <mlpack/core.hpp>

#include <mlpack/core/optimizers/rmsprop/rmsprop.hpp>
#include <mlpack/core/optimizers/sgd/test_function.hpp>

#include <mlpack/methods/logistic_regression/logistic_regression.hpp>

#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/init_rules/random_init.hpp>
#include <mlpack/methods/ann/performance_functions/mse_function.hpp>
#include <mlpack/methods/ann/layer/binary_classification_layer.hpp>
#include <mlpack/methods/ann/layer/bias_layer.hpp>
#include <mlpack/methods/ann/layer/linear_layer.hpp>
#include <mlpack/methods/ann/layer/base_layer.hpp>

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

using namespace arma;
using namespace mlpack;
using namespace mlpack::optimization;
using namespace mlpack::optimization::test;

using namespace mlpack::distribution;
using namespace mlpack::regression;

using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(RMSpropTest);

/**
 * Tests the RMSprop optimizer using a simple test function.
 */
BOOST_AUTO_TEST_CASE(SimpleRMSpropTestFunction)
{
  SGDTestFunction f;
  RMSprop<SGDTestFunction> optimizer(f, 1e-3, 0.99, 1e-8, 5000000, 1e-9, true);

  arma::mat coordinates = f.GetInitialPoint();
  optimizer.Optimize(coordinates);

  BOOST_REQUIRE_SMALL(coordinates[0], 0.1);
  BOOST_REQUIRE_SMALL(coordinates[1], 0.1);
  BOOST_REQUIRE_SMALL(coordinates[2], 0.1);
}

/**
 * Run RMSprop on logistic regression and make sure the results are acceptable.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionTest)
{
  // Generate a two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 1.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("9.0 9.0 9.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(3, 1000);
  arma::Row<size_t> responses(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    data.col(i) = g1.Random();
    responses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    data.col(i) = g2.Random();
    responses[i] = 1;
  }

  // Shuffle the dataset.
  arma::uvec indices = arma::shuffle(arma::linspace<arma::uvec>(0,
      data.n_cols - 1, data.n_cols));
  arma::mat shuffledData(3, 1000);
  arma::Row<size_t> shuffledResponses(1000);
  for (size_t i = 0; i < data.n_cols; ++i)
  {
    shuffledData.col(i) = data.col(indices[i]);
    shuffledResponses[i] = responses[indices[i]];
  }

  // Create a test set.
  arma::mat testData(3, 1000);
  arma::Row<size_t> testResponses(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    testData.col(i) = g1.Random();
    testResponses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    testData.col(i) = g2.Random();
    testResponses[i] = 1;
  }

  LogisticRegression<> lr(shuffledData.n_rows, 0.5);

  LogisticRegressionFunction<> lrf(shuffledData, shuffledResponses, 0.5);
  RMSprop<LogisticRegressionFunction<> > rmsprop(lrf);
  lr.Train(rmsprop);

  // Ensure that the error is close to zero.
  const double acc = lr.ComputeAccuracy(data, responses);
  BOOST_REQUIRE_CLOSE(acc, 100.0, 0.3); // 0.3% error tolerance.

  const double testAcc = lr.ComputeAccuracy(testData, testResponses);
  BOOST_REQUIRE_CLOSE(testAcc, 100.0, 0.6); // 0.6% error tolerance.
}

/**
 * Run RMSprop on a feedforward neural network and make sure the results are
 * acceptable.
 */
BOOST_AUTO_TEST_CASE(FeedforwardTest)
{
  // Test on a non-linearly separable dataset (XOR).
  arma::mat input, labels;
  input << 0 << 1 << 1 << 0 << arma::endr
        << 1 << 0 << 1 << 0 << arma::endr;
  labels << 1 << 1 << 0 << 0;

  // Instantiate the first layer.
  LinearLayer<> inputLayer(input.n_rows, 8);
  BiasLayer<> biasLayer(8);
  TanHLayer<> hiddenLayer0;

  // Instantiate the second layer.
  LinearLayer<> hiddenLayer1(8, labels.n_rows);
  TanHLayer<> outputLayer;

  // Instantiate the output layer.
  BinaryClassificationLayer classOutputLayer;

  // Instantiate the feedforward network.
  auto modules = std::tie(inputLayer, biasLayer, hiddenLayer0, hiddenLayer1,
      outputLayer);
  FFN<decltype(modules), decltype(classOutputLayer), RandomInitialization,
      MeanSquaredErrorFunction> net(modules, classOutputLayer);

  RMSprop<decltype(net)> opt(net, 0.03, 0.99, 1e-8, 300 * input.n_cols, -10);

  net.Train(input, labels, opt);

  arma::mat prediction;
  net.Predict(input, prediction);

  BOOST_REQUIRE_EQUAL(prediction(0), 1);
  BOOST_REQUIRE_EQUAL(prediction(1), 1);
  BOOST_REQUIRE_EQUAL(prediction(2), 0);
  BOOST_REQUIRE_EQUAL(prediction(3), 0);
}

BOOST_AUTO_TEST_SUITE_END();


// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>
#include <OpenTissue/core/spline/spline.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>

// Boost Test declaration and Checking macros
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/test/floating_point_comparison.hpp>

typedef OpenTissue::spline::MathTypes<double, size_t>    math_types;
typedef math_types::vector_type                                vector_type;
typedef math_types::matrix_type                                matrix_type;
typedef std::vector<double>                                    knot_container;

BOOST_AUTO_TEST_SUITE(opentissue_spline_init_m_table);

BOOST_AUTO_TEST_CASE(test_init_m_table)
{
  knot_container U;

  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(0.0);  //k = 3
  U.push_back(1.0);
  U.push_back(2.0);
  U.push_back(3.0);
  U.push_back(4.0);  // n = 6  => |P| = 7
  U.push_back(5.0);
  U.push_back(5.0);
  U.push_back(5.0);  // m = 9  => |U| = 10

  // Indices of basis functions belongs to the interval [0..n]

  double const tolerance = 0.00001;

  // u-parameter at half-way
  {
    double const u = 2.5;

    matrix_type M;
    BOOST_CHECK_NO_THROW( OpenTissue::spline::detail::initialize_m_table(4, u, 4, U, M) );

    // Test content of M.
    BOOST_CHECK_CLOSE(M(0,0), 1.0,     tolerance);
    BOOST_CHECK_CLOSE(M(0,1), 1.0/2.0, tolerance);
    BOOST_CHECK_CLOSE(M(0,2), 1.0/8.0, tolerance);
    BOOST_CHECK_CLOSE(M(1,0), 1.0,     tolerance);
    BOOST_CHECK_CLOSE(M(1,1), 1.0/2.0, tolerance);
    BOOST_CHECK_CLOSE(M(1,2), 6.0/8.0, tolerance);
    BOOST_CHECK_CLOSE(M(2,0), 2.0,     tolerance);
    BOOST_CHECK_CLOSE(M(2,1), 2.0,     tolerance);
    BOOST_CHECK_CLOSE(M(2,2), 1.0/8.0, tolerance);

  }

}

BOOST_AUTO_TEST_SUITE_END();


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

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

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

using namespace dealii;

//! Generate a hypercube, and output it as an svg file.
void first_grid(Triangulation<2> &triangulation)
{
  GridGenerator::hyper_cube(triangulation);
  triangulation.refine_global(4);

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

//! Generate a locally refined hyper_shell, and output it as an svg file.
void second_grid(Triangulation<2> &triangulation)
{
  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 10);

  // triangulation.reset_manifold(0);

  for (unsigned int step = 0; step < 5; ++step)
    {
      for (auto &cell : triangulation.active_cell_iterators())
        {
          for (const auto v : cell->vertex_indices())
            {
              const double distance_from_center =
                center.distance(cell->vertex(v));

              if (std::fabs(distance_from_center - inner_radius) <=
                  1e-6 * inner_radius)
                {
                  cell->set_refine_flag();
                  break;
                }
            }
        }

      triangulation.execute_coarsening_and_refinement();
    }

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

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

//! Create an L-shaped domain with one global refinement, and write it on
// `third_grid.vtk`.  Refine the L-shaped mesh adaptively around the re-entrant
// corner three times (after the global refinement you already did), but with a
// twist: refine all cells with the distance between the center of the cell and
// re-entrant corner is smaller than 1/3.
void third_grid(Triangulation<2> &tria)
{
  // Insert code here
  // last year code in comments
  /* Triangulation<2> tr1,tr2,tr3,tr_final;
   const Point<2> p1(0, 0);
   const Point<2> p2(1,2);
   const Point<2> p3(2,1);
   const Point<2> p4(1,0);
   const Point<2> p5(1,1);
   const Point<2> p6(0,1);
   GridGenerator::hyper_rectangle(tr1,p1,p5);
   GridGenerator::hyper_rectangle(tr2,p6,p2);
   GridGenerator::hyper_rectangle(tr3,p4,p3);
   //they cannot be refined!
   GridGenerator::merge_triangulations ({&tr1,&tr2,&tr3},tr_final);*/
  GridGenerator::hyper_L(tria);
  tria.refine_global(1);
  const Point<2> p5(0, 0);
  for (unsigned int step = 0; step < 3; ++step)
    {
      // Active cells are those that are not further refined
      // we need to mark cells for refinement
      for (auto &cell : tria.active_cell_iterators())
        {
          const double distance_from_corner = cell->center().distance({0, 0});
          // choose whatever refinement condition
          if (distance_from_corner < 2.0 / 3.0)
            {
              cell->set_refine_flag();
            }
        }
      // refine global calls this function too
      tria.execute_coarsening_and_refinement();
    } // for steps loop

  // tr_final.refine_global(2); //we want a nice picture :)

  std::ofstream file_var("grid-3.vtk");
  GridOut       grid_out;
  grid_out.write_vtk(tria, file_var);
  std::cout << "Grid written to grid-3.vtk" << std::endl;
}

//! Returns a tuple with number of levels, number of cells, number of active
// cells. Test this with all of  your meshes.
std::tuple<unsigned int, unsigned int, unsigned int>
get_info(const Triangulation<2> &tria)
{
  // Insert code here
  return std::make_tuple(tria.n_levels(),
                         tria.n_cells(),
                         tria.n_active_cells());
}

void
torus_grid()
{
  Triangulation<2, 3> tria;
  GridGenerator::torus(tria, 2, 1);
  tria.refine_global(2);

  std::ofstream out("grid-torus.vtk");
  GridOut       grid_out;
  grid_out.write_vtk(tria, out);
  std::cout << "Grid written to grid-torus.vtk" << std::endl;
}

int
main()
{
  Triangulation<2> triangulation;
  first_grid(triangulation);
  triangulation.clear();
  second_grid(triangulation);
  triangulation.clear();
  third_grid(triangulation);

  torus_grid();
}


/**
 * @file conslawwithsource_main.cc
 * @brief NPDE exam TEMPLATE CODE FILE
 * @author Oliver Rietmann
 * @date 20.07.2020
 * @copyright Developed at SAM, ETH Zurich
 */

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

#include "conslawwithsource.h"

int main() {
  // Initial data
  auto u0 = [](double x) { return (0.0 <= x && x < 1.0) ? 1.0 : 0.0; };

  // Prepare different spacial resolutions
  const int samples = 8;
  Eigen::VectorXi N(samples);
  N(0) = 10;
  for (int i = 1; i < samples; ++i) N(i) = 2 * N(i - 1);

  // Compute total masses at endtime T=3
  Eigen::VectorXd m3(samples);
  for (int i = 0; i < samples; ++i) {
    Eigen::VectorXd m = ConsLawWithSource::traceMass(u0, N(i));
    m3(i) = m(m.size() - 1);
  }

  // Print N vs. total masses
  Eigen::Matrix<double, samples, 2> table;
  std::cout << "          N        m(3)" << std::endl;
  table.col(0) = N.cast<double>();
  table.col(1) = m3;
  std::cout << table << std::endl;

  return 0;
}


#include <dvs_mosaic/poisolve/laplace.h>
#include <dvs_mosaic/reconstruction.h>

#include <boost/multi_array.hpp>
#include <glog/logging.h>

namespace poisson
{

void reconstructBrightnessFromGradientMap(const cv::Mat& grad_map,
                                                cv::Mat* map_reconstructed)
{
  CHECK_EQ(grad_map.type(), CV_32FC2);
  CHECK_GT(grad_map.cols, 0);
  CHECK_GT(grad_map.rows, 0);
  const cv::Size img_size = grad_map.size();

  // Compute the right hand side of Poisson eq.
  // F = dgx/dx + dgy/dy and put it into a boost::multi_array
  const size_t height = img_size.height;
  const size_t width = img_size.width;
  boost::multi_array<double,2> M(boost::extents[height][width]);
  boost::multi_array<double,2> F(boost::extents[height][width]);

  // Compute right hand side (rhs) using one-sided finite differences
  for(size_t i=0; i < height-1; ++i)
  {
    for(size_t j=0; j < width-1; ++j)
    {
      F[i][j] = double( grad_map.at<cv::Vec2f>(i,j+1)[0] - grad_map.at<cv::Vec2f>(i,j)[0]
                      + grad_map.at<cv::Vec2f>(i+1,j)[1] - grad_map.at<cv::Vec2f>(i,j)[1] );
    }
  }
  F[height-1][width-1] = 0.0;

  // Solve for M_xx + M_yy = F using Poisson solver,
  // with constant intensity (zero) boundary conditions
  const double gradient_on_boundary = 0.0;
  pde::poisolve(M, F, 1.0, 1.0, 1.0, 1.0,
                gradient_on_boundary,
                pde::types::boundary::Neumann,false);

  // Fill in output variable
  *map_reconstructed = cv::Mat(img_size, CV_32FC1);
  // FILL IN ...

}

}


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

using namespace boost::numeric::ublas;

int main()
{
	// op(Matrix)
	matrix<std::complex<double> > m (3, 3);
	for (unsigned i = 0; i < m.size1 (); ++ i)
		for (unsigned j = 0; j < m.size2 (); ++ j)
			m (i, j) = std::complex<double> (3 * i + j, 3 * i + j);

	std::cout << "minus \t" << - m << std::endl;
	std::cout << "conj \t" << conj (m) << std::endl;
	std::cout << "real \t" << real (m) << std::endl;
	std::cout << "imag \t" << imag (m) << std::endl;
	std::cout << "trans \t" << trans (m) << std::endl;
	std::cout << "herm \t" << herm (m) << std::endl;

	// Matrix +/- Matrix
	matrix<double> m1 (3, 3), m2 (3, 3);
	for (unsigned i = 0; i < std::min (m1.size1 (), m2.size1 ()); ++ i)
		for (unsigned j = 0; j < std::min (m1.size2 (), m2.size2 ()); ++ j)
			m1 (i, j) = m2 (i, j) = 3 * i + j;

	std::cout << "add \t" << m1 + m2 << std::endl;
	std::cout << "subt \t" << m1 - m2 << std::endl;

	// scalar * Matrix
	matrix<double> mm (3, 3);
	for (unsigned i = 0; i < mm.size1 (); ++ i)
		for (unsigned j = 0; j < mm.size2 (); ++ j)
			mm (i, j) = 3 * i + j;

	std::cout << "2 * matrix \t" << 2.0 * mm << std::endl;
	std::cout << "matrix * 2 \t" << mm * 2.0 << std::endl;

	// Matrix * Vector
	matrix<double> m_ (3, 3);
	vector<double> v (3);
	for (unsigned i = 0; i < std::min (m_.size1 (), v.size ()); ++ i) {
		for (unsigned j = 0; j < m_.size2 (); ++ j)
			m_ (i, j) = 3 * i + j;
		v (i) = i;
	}

	std::cout << "matrix * vector (A * b) \t" << prod (m_, v) << std::endl;
	std::cout << "vector * matrix (A' * b)\t" << prod (v, m_) << std::endl;

	// Matrix * Matrix
	matrix<double> m11 (3, 3), m22 (3, 3);
	for (unsigned i = 0; i < std::min (m11.size1 (), m22.size1 ()); ++ i)
		for (unsigned j = 0; j < std::min (m11.size2 (), m22.size2 ()); ++ j)
			m11 (i, j) = m22 (i, j) = 3 * i + j;

	std::cout << "matrix * matrix (A * B) \t" << prod (m11, m22) << std::endl;
	return 0;
}


#include <string>
#include <vector>
#include <Eigen/Dense>
#include <cmath>
#include <exception>
#include <algorithm>
#include <iostream>
#include <fstream>

#include "naive_bayes.cpp"
#include "eigen_space.cpp"

#define PI 3.141592653589793238462643383279502884L
#define REGULARIZATION_FACTOR 0.5
#define IDEAL_NUM_PRINCIPLE_COMPONENTS 16

#define _l_ std::cout<<__LINE__<<std::endl;

const std::string TRAINING_FILE_NAME = "data/zip.train"; // 7291 items
const std::string TEST_FILE_NAME = "data/zip.test"; // 2007 items

std::vector<ClassificationObject>
ReadData(
  const std::string& fileName,
  const unsigned featureDim,
  const unsigned numItems) {

  std::ifstream fin;
  fin.clear(); fin.open(fileName.c_str());

  if (!fin.good()) {
    throw std::exception();
  }

  std::vector<ClassificationObject> data; data.reserve(numItems);

  for (unsigned i = 0; i < numItems; ++i) {
    ClassificationObject classificationObject;
    double dummy;
    fin >> dummy;
    classificationObject.label = (unsigned) dummy;

    classificationObject.features.resize(featureDim);
    for (unsigned j = 0; j < featureDim; ++j) {
      fin >> classificationObject.features(j);
    }
    data.push_back(classificationObject);
  }

  return data;
}

Eigen::MatrixXi
PerformClassifications(const std::vector<unsigned>& labelSet,
  const std::vector<ClassInfo>& classSummaries,
  const std::vector<ClassificationObject>& testObjects) {

  Eigen::MatrixXi confusionMatrix(labelSet.size(), labelSet.size());
  confusionMatrix.setZero(labelSet.size(), labelSet.size());

  unsigned i = 1;
  unsigned onePercent = testObjects.size() / 100;
  for (const ClassificationObject& object : testObjects) {
    if (i % onePercent == 0) {
      std::cout << i << "%% processed." << std::endl;
    }
    i++;

    unsigned classifiedAs = ClassifyObject(object, classSummaries);
    confusionMatrix(object.label, classifiedAs)++;
  }

  return confusionMatrix;
}

Eigen::MatrixXi
PerformPcaClassifications(const std::vector<unsigned>& labelSet,
  const std::vector<ReducedDimClassInfo>& classSummaries,
  const std::vector<ClassificationObject>& testObjects) {

  Eigen::MatrixXi confusionMatrix(labelSet.size(), labelSet.size());
  confusionMatrix.setZero(labelSet.size(), labelSet.size());

  unsigned i = 1;
  unsigned onePercent = testObjects.size() / 100;
  for (const ClassificationObject& object : testObjects) {
    if (i % onePercent == 0) {
      std::cout << (double)i * 100.0 / testObjects.size() << "% processed." << std::endl;
    }
    i++;

    unsigned classifiedAs = PcaClassifyObject(object, classSummaries);
    confusionMatrix(object.label, classifiedAs)++;
  }

  return confusionMatrix;
}

int
main(const int argc, const char** argv) {
  std::cout << "Reading Data..." << std::endl;
  std::vector<ClassificationObject> trainingData = ReadData(
    TRAINING_FILE_NAME,
    256,
    7291
  );
  std::vector<ClassificationObject> testData = ReadData(
    TEST_FILE_NAME,
    256,
    2007
  );

  std::cout << "Training Models..." << std::endl;
  std::vector<ClassInfo> classSummaries; classSummaries.reserve(10);
  for (unsigned label = 0; label <= 9; ++label) {
    classSummaries.push_back(ComputeClassInfo(trainingData, label, 256));
  }

  std::cout << "Training Reduced Dimensionality Models..." << std::endl;
  std::vector<ReducedDimClassInfo> reducedClassSummaries; reducedClassSummaries.reserve(10);
  for (unsigned label = 0; label <= 9; ++label) {
    reducedClassSummaries.push_back(
      ComputeClassInfoInPcaDimensionReducedSpace(
        trainingData,
        classSummaries[label],
        16
      )
    );
  }

  std::cout << "Classifying Objects..." << std::endl;
  Eigen::MatrixXi confusionMatrix = PerformPcaClassifications(
    {0,1,2,3,4,5,6,7,8,9},
    reducedClassSummaries,
    testData
  );

  std::cout << "Results:" << std::endl;
  std::cout << confusionMatrix << std::endl;

	return 0;
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef FCPPT_MPL_CEIL_DIV_HPP_INCLUDED
#define FCPPT_MPL_CEIL_DIV_HPP_INCLUDED

#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/mpl/integral_c.hpp>
#include <type_traits>
#include <fcppt/config/external_end.hpp>

namespace fcppt
{
namespace mpl
{

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

/**
\brief Calculates a division of integral contants rounded towards infinity

\ingroup fcpptmpl

Calculates Dividend / Divisor rounded towards infinity. For
example, 5 / 3 would result in 2.

\snippet mpl/various.cpp mpl_ceil_div

\tparam Type Must be an unsigned integral type

\tparam Dividend The dividend

\tparam Divisor The divisor
*/
template<
	typename Type,
	Type Dividend,
	Type Divisor
>
struct ceil_div
:
boost::mpl::integral_c<
	Type,
	Dividend
	/
	Divisor
	+
	(
		Dividend %
		Divisor
		?
			1u
		:
			0u
	)
>
{
	static_assert(
		std::is_unsigned<
			Type
		>::value,
		"ceil_div only works on unsigned types"
	);
};

FCPPT_PP_POP_WARNING

}
}

#endif


#include <iostream>

#include <Eigen/Eigen>

#include "slick/math/so2.h"
#include "slick/math/se2.h"

using namespace std;
using namespace Eigen;
using namespace slick;

void test_constructors(){
  cout << "SE2():\n" << SE2() << endl;
  SO2 so2;
  Matrix<SlickScalar,2,1> v2=Matrix<SlickScalar,2,1>::Zero();
  v2[0]=1;
  cout << "SE2(so2,v2):\n" << SE2(so2,v2) << endl;
  Matrix<SlickScalar,3,1> v3 = Matrix<SlickScalar,3,1>::Zero();
  v3[0]=1;
  cout << "SE2(v3):\n" << SE2(v3) << endl;
}

void test_operators(){
  SO2 so2;
  Matrix<SlickScalar,3,1> v3 = Matrix<SlickScalar,3,1>::Zero();
  v3[0]=1;
  Matrix<SlickScalar,3,1> v3Minus = -v3;
  cout << "SE2(v3)*SE2(-v3):\n" << SE2(v3)*SE2(v3Minus) << endl;
  Matrix<SlickScalar,3,3> m3 = Matrix<SlickScalar,3,3>::Identity();
  cout << "SE2(v3)*m3:\n" << SE2(v3)*m3 << endl;
  cout << "m3*SE2(v3):\n" << endl;
  cout << m3*SE2(v3) << endl;
  cout << "SE2(v3).inverse():\n" << SE2(v3).inverse() << endl;
  cout << "SE2(v3).ln():\n" << SE2(v3).ln() << endl;
  cout << "SO2()*SE2(v3):\n" << SO2()*SE2(v3) << endl;
}

int main(int , char ** )
{
  cout << "testing constructors ...\n" << endl;
  test_constructors();
  cout << "testing operators ...\n" << endl;
  test_operators();
  return 0;
}


#include "camsim.hpp"

#include "gtsam/inference/Symbol.h"
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <boost/make_shared.hpp>

using namespace gtsam;
using namespace gtsam::noiseModel;
using symbol_shorthand::X;

/**
 * Unary factor on the unknown pose, resulting from measuring the projection of
 * a known 3D point in the image
 */
class ResectioningFactor_xxx : public NoiseModelFactor1<Pose3>
{
  typedef NoiseModelFactor1<Pose3> Base;

  Cal3_S2::shared_ptr K_; ///< camera's intrinsic parameters
  Point3 P_;              ///< 3D point on the calibration rig
  Point2 p_;              ///< 2D measurement of the 3D point

public:

  /// Construct factor given known point P and its projection p
  ResectioningFactor_xxx(const SharedNoiseModel &model, const Key &key,
                         const Cal3_S2::shared_ptr &calib, const Point2 &p, const Point3 &P) :
    Base(model, key), K_(calib), P_(P), p_(p)
  {
  }

  /// evaluate the error
  virtual Vector evaluateError(const Pose3 &pose, boost::optional<Matrix &> H =
  boost::none) const
  {
    SimpleCamera camera(pose, *K_);
    return camera.project(P_, H, boost::none, boost::none) - p_;
  }
};

/*******************************************************************************
 * Camera: f = 1, Image: 100x100, center: 50, 50.0
 * Pose (ground truth): (Xw, -Yw, -Zw, [0,0,2.0]')
 * Known landmarks:
 *    3D Points: (10,10,0) (-10,10,0) (-10,-10,0) (10,-10,0)
 * Perfect measurements:
 *    2D Point:  (55,45)   (45,45)    (45,55)     (55,55)
 *******************************************************************************/

int gtsam_resection()
{
  /* read camera intrinsic parameters */
  Cal3_S2::shared_ptr calib(new Cal3_S2(1, 1, 0, 50, 50));

  /* 1. create graph */
  NonlinearFactorGraph graph;

  /* 2. add factors to the graph */
  // add measurement factors
  SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(5., .5));
  graph.emplace_shared<ResectioningFactor_xxx>(measurementNoise, X(1), calib,
                                               Point2(55, 45), Point3(10, 10, 0));
  graph.emplace_shared<ResectioningFactor_xxx>(measurementNoise, X(1), calib,
                                               Point2(45, 45), Point3(-10, 10, 0));
  graph.emplace_shared<ResectioningFactor_xxx>(measurementNoise, X(1), calib,
                                               Point2(45, 55), Point3(-10, -10, 0));
  graph.emplace_shared<ResectioningFactor_xxx>(measurementNoise, X(1), calib,
                                               Point2(55, 55), Point3(10, -10, 0));

  /* 3. Create an initial estimate for the camera pose */
  Values initial;
  initial.insert(X(1),
//                 Pose3(Rot3(1, 1, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2)));
                 Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2)));

  graph.print("full graph");

  /* 4. Optimize the graph using Levenberg-Marquardt*/
  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
  result.print("Final result:\n");

  auto camera_f_marker = result.at<Pose3>(X(1));
  std::cout << camera_f_marker.rotation() << camera_f_marker.rotation().xyz() << std::endl;

  std::cout.precision(2);
  Marginals marginals(graph, result);
  std::cout << "x1 covariance:\n" << marginals.marginalCovariance(X(1)) << std::endl;

  std::cout << "initial error = " << graph.error(initial) << std::endl;
  std::cout << "final error = " << graph.error(result) << std::endl;

  return EXIT_SUCCESS;
}


// Copyright (c) 2018-2019, University of Bremen, M. Farzalipour Tabriz
// Copyrights licensed under the 2-Clause BSD License.
// See the accompanying LICENSE.txt file for terms.

#pragma once
#include <armadillo>
#include "arma_io.hpp"
#include <fftw3.h>
#include "general_io.hpp"
#include "spline.hpp"

#define PI datum::pi

//These functions extend the functionality of the included Armadillo library by providing:
// (arma,iostream) << and >> operators for reading from iostreams to armadillo types and vice versa
// fft/ifft functions for cube files: Wrappers for FFTW with MATLAB/Octave scaling convention
//				in FFT functions the forward FFT scales by 1, and the reverse scales by 1/N.
// 3D ndgrid and 3D meshgrid
// 3D spline interpolation
// 3D shift: by number of the elements along one axis or with a relative 3D shift vector
// irowvec to matrix/cube size conversion
// scalar triple product

using namespace arma;

// a (very inefficient) 3D spline interpolation!
cube interp3(const rowvec& x, const rowvec& y, const rowvec& z, const cube& v, const rowvec& xi, const rowvec& yi, const rowvec& zi);
cube interp3(const cube& v, const rowvec& xi, const rowvec& yi, const rowvec& zi);

//Rectangular grid in 3D space
tuple<cube, cube, cube> ndgrid(const rowvec& v1, const rowvec& v2, const rowvec& v3);

//3D meshgrid
tuple<cube, cube, cube> meshgrid(const rowvec& v1, const rowvec& v2, const rowvec& v3);

//shifts a cube by a relative 3D vector [0 1]
cube shift(cube cube_in, rowvec3 shifts);

//Planar average of a cube in the defined direction
//direction: 0,1,2 > x,y,z
vec planar_average(const uword& direction, const cube& cube_in);

//1D FFT of complex data.
//no normalization for forward FFT
cx_vec fft(cx_vec X);

//1D FFT of real data.
//no normalization for forward FFT
cx_vec fft(vec X);

//3D FFT of complex data.
//no normalization for forward FFT
cx_cube fft(cx_cube X);

//3D FFT of real data.
//no normalization for forward FFT
cx_cube fft(cube X);

//1D inverse FFT of complex data.
//normalized by N = X.n_elem
cx_vec ifft(cx_vec X);

//3D inverse FFT of complex data.
//normalized by N = X.n_elem
cx_cube ifft(cx_cube X);

//returns a cube size object from the values inside a vector
SizeCube as_size(const urowvec3& vec);

//returns a matrix size object from the values inside a vector
SizeMat as_size(const urowvec2& vec);

//element-wise fmod
mat fmod(mat mat_in, const double& denom) noexcept;

//element-wise positive fmod
mat fmod_p(mat mat_in, const double& denom) noexcept;

//positive fmod
double fmod_p(double num, const double& denom) noexcept;

//just a simple square! May cause overflows!!
inline double square(const double& input) noexcept {
	return input * input;
}

//Poisson solver in 3D with anisotropic dielectric profiles
//diel is the N*3 matrix of variations in dielectric tensor elements in direction normal to the surface
cx_cube poisson_solver_3D(const cx_cube& rho, mat diel, rowvec3 lengths, uword normal_direction);

//generate a copy of the cube with the elements shifted by N positions along:
//dim=0: each row
//dim=1: each column
//dim=2: each slice
template <typename T>
Cube<T> shift(const Cube<T>& A, const sword& N, const uword& dim) {
	Cube<T> B(arma::size(A));
	const auto index_init = regspace<uvec>(0, A.n_elem - 1);
	imat sub_shift = conv_to<imat>::from(ind2sub(arma::size(A), index_init));
	const uword size = arma::size(A)(dim);
	sub_shift.row(dim).for_each([&N, &size](sword& i) noexcept {
		i += N;
		while (i < 0) i += size;
		i = i % size;
	});

	B(sub2ind(arma::size(A), conv_to<umat>::from(sub_shift))) = A(index_init);

	return B;
}

//Undo a fftshift
template <typename T>
Row<T> ifftshift(const Row<T> &A) {
	return shift(A, -1 * (A.n_elem / 2));
}

//Undo a fftshift
template <typename T>
Cube<T> ifftshift(Cube<T> A) {
	for (uword i = 0; i < 3; ++i) {
		A = shift(A, -1 * (arma::size(A)(i) / 2), i);
	}

	return A;
}

//returns the size of a cube as a rowvec
template<typename T>
urowvec3 SizeVec(const Cube<T>& c) {
	const SizeCube size = arma::size(c);
	return urowvec({ size(0), size(1), size(2) });
}

//returns the size of a matrix as a rowvec
template<typename T>
urowvec2 SizeVec(const Mat<T>& c) {
	const SizeMat size = arma::size(c);
	return urowvec({ size(0), size(1) });
}

//sign of the val as -1/0/+1
template <typename T> int sgn(T val) {
	return (T(0) < val) - (val < T(0));
}


#include <doctest/doctest.h>

#define EIGEN_INITIALIZE_MATRICES_BY_ZERO

#include <Eigen/Core>
#include <ode/ode.hpp>

TEST_CASE("ODE Test")
{
  using method_type    = ode::explicit_method<ode::runge_kutta_4_tableau<float>>;
  using problem_type   = ode::initial_value_problem<float, Eigen::Vector3f>;

  constexpr auto sigma = 10.0f;
  constexpr auto rho   = 28.0f;
  constexpr auto beta  = 8.0f / 3.0f;

  const auto problem   = problem_type
  {
    0.0f,                                         /* t0 */
    Eigen::Vector3f(16.0f, 16.0f, 16.0f),         /* y0 */
    [&] (const float t, const Eigen::Vector3f& y) /* y' = f(t, y) */
    {
      return Eigen::Vector3f(sigma * (y[1] - y[0]), y[0] * (rho - y[2]) - y[1], y[0] * y[1] - beta * y[2]); /* Lorenz system */
    }
  };

  auto iterator = ode::fixed_step_iterator<method_type, problem_type>(problem, 1.0f /* h */);
  for (auto i = 0; i < 1000; ++i)
    ++iterator;
}


#pragma once

#include <crest/geometry/indexed_mesh.hpp>
#include <crest/geometry/biscale_mesh.hpp>
#include <crest/geometry/triangle.hpp>
#include <crest/quadrature/triquad.hpp>
#include <crest/util/eigen_extensions.hpp>

#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace crest
{
    namespace detail
    {
        /**
         * Returns the coefficients of the three linear Lagrange basis functions
         * associated with the triangle.
         *
         * Given basis function l_i for i = 0, 1, 2, then
         * l_i = a_i x + b_i y + c_i
         * where
         *      [ a_0   a_1   a_2 ]
         * B =  [ b_0   b_1   b_2 ]
         *      [ c_0   c_1   c_2 ]
         * @param triangle
         * @return
         */
        template <typename Scalar>
        Eigen::Matrix<Scalar, 3, 3> basis_coefficients_for_triangle(const Triangle<Scalar> & triangle)
        {
            constexpr Scalar one = static_cast<Scalar>(1.0);
            Eigen::Matrix<Scalar, 3, 3> X;
            X <<
              triangle.a.x, triangle.a.y, one,
                    triangle.b.x, triangle.b.y, one,
                    triangle.c.x, triangle.c.y, one;

            return X.partialPivLu().solve(Eigen::Matrix<Scalar, 3, 3>::Identity());
        };
        /**
         * The interpolation of a fine-scale finite element space in the (possibly discontinuous) space of
         * affine functions on a coarse finite element space can be computed by the relation
         * Py = Bx, where y represent the weights in the affine coarse space and x represents the weights
         * in the fine-scale finite element space. This function computes B.
         * @param coarse
         * @param fine
         * @return
         */
        template <typename Scalar>
        Eigen::SparseMatrix<Scalar> build_affine_interpolator_rhs(
                const BiscaleMesh<Scalar, int> & mesh)
        {
            std::vector<Eigen::Triplet<Scalar>> triplets;

            for (int coarse_index = 0; coarse_index < mesh.coarse_mesh().num_elements(); ++coarse_index)
            {
                // The basis functions p_h in the affine space can be expressed as
                // p_h(x, y) = a x + b y + c
                // for some coefficients a, b, c.
                const auto coarse_triangle = mesh.coarse_mesh().triangle_for(coarse_index);
                const Eigen::Matrix<Scalar, 3, 3> coarse_coeff = basis_coefficients_for_triangle(coarse_triangle);
                for (const auto fine_index : mesh.descendants_for(coarse_index))
                {
                    const auto vertex_indices = mesh.fine_mesh().elements()[fine_index].vertex_indices;
                    const auto fine_triangle = mesh.fine_mesh().triangle_for(fine_index);
                    const Eigen::Matrix<Scalar, 3, 3> fine_coeff = basis_coefficients_for_triangle(fine_triangle);

                    // Local coarse basis function i
                    for (size_t i = 0; i < 3; ++i)
                    {
                        // Local fine basis function j
                        for (size_t j = 0; j < 3; ++j)
                        {
                            const auto vertex_index = vertex_indices[j];
                            const auto product = [&](auto x, auto y)
                            {
                                const auto coarse_basis_value =
                                        coarse_coeff(0, i) * x + coarse_coeff(1, i) * y + coarse_coeff(2, i);
                                const auto fine_basis_value =
                                        fine_coeff(0, j) * x + fine_coeff(1, j) * y + fine_coeff(2, j);
                                return coarse_basis_value * fine_basis_value;
                            };
                            const auto inner_product = triquad<2>(product,
                                                                  fine_triangle.a,
                                                                  fine_triangle.b,
                                                                  fine_triangle.c);
                            const auto row = 3 * coarse_index + i;
                            triplets.push_back(Eigen::Triplet<Scalar>(row, vertex_index, inner_product));
                        }
                    }
                }
            }

            const auto num_dof_affine_space = 3 * mesh.coarse_mesh().num_elements();
            Eigen::SparseMatrix<Scalar> B(num_dof_affine_space, mesh.fine_mesh().num_vertices());
            B.setFromTriplets(triplets.cbegin(), triplets.cend());
            return B;
        };

        template <typename Scalar>
        Eigen::SparseMatrix<Scalar> build_affine_interpolator_lhs_inverse(
                const IndexedMesh<Scalar, int> & coarse)
        {
            const auto num_dof_affine_space = 3 * coarse.num_elements();
            Eigen::SparseMatrix<Scalar> P(num_dof_affine_space, num_dof_affine_space);

            // P will be block diagonal with 3x3 blocks, so we can reserve space in advance.
            P.reserve(Eigen::VectorXi::Constant(num_dof_affine_space, 3));

            Eigen::Matrix<Scalar, 3, 3> P_ref;
            P_ref << 2.0 / 24.0, 1.0 / 24.0, 1.0 / 24.0,
                    1.0 / 24.0, 2.0 / 24.0, 1.0 / 24.0,
                    1.0 / 24.0, 1.0 / 24.0, 2.0 / 24.0;

            for (int t = 0; t < coarse.num_elements(); ++t)
            {
                const auto triangle = coarse.triangle_for(t);
                const auto determinant = static_cast<Scalar>(2.0) * crest::area(triangle);
                assert(determinant != static_cast<Scalar>(0));
                Eigen::Matrix<Scalar, 3, 3> P_local = determinant * P_ref.template cast<Scalar>();
                P_local = P_local.inverse().eval();

                for (int j = 0; j < 3; ++j)
                {
                    for (int i = 0; i < 3; ++i)
                    {
                        const auto row = 3 * t + i;
                        const auto col = 3 * t + j;
                        P.insert(row, col) = P_local(i, j);
                    }
                }
            }

            return P;
        };

        /**
         * Given a coarse and fine mesh where the coarse mesh is a subset of the fine mesh (when interpreted as a forest),
         * build a matrix that interpolates functions in the fine finite element space in the
         * (possibly discontinuous) space of affine functions on the coarse space.
         * @param coarse
         * @param fine
         * @return
         */
        template <typename Scalar>
        Eigen::SparseMatrix<Scalar> affine_interpolator(
                const BiscaleMesh<Scalar, int> & mesh)
        {
            const Eigen::SparseMatrix<Scalar> B = build_affine_interpolator_rhs(mesh);
            const Eigen::SparseMatrix<Scalar> P = build_affine_interpolator_lhs_inverse(mesh.coarse_mesh());
            return P * B;
        };

        template <typename Scalar>
        std::vector<unsigned int> count_vertex_occurrences(const IndexedMesh<Scalar, int> & mesh)
        {
            auto count = std::vector<unsigned int>(mesh.num_vertices(), static_cast<Scalar>(0));
            for (const auto & element : mesh.elements())
            {
                for (const auto & v : element.vertex_indices)
                {
                    ++count[v];
                }
            }
            return count;
        }

        /**
         * Given a triangulation T, returns a matrix of dimensions N(T) x [3 * card(T)] which maps functions in the
         * (possibly discontinuous) affine space P_1 to functions in the standard linear finite element space
         * on the mesh.
         * Here, N(T) denotes the number of vertices in T, and card(T) denotes the number of elements in T.
         * @param mesh
         * @return
         */
        template <typename Scalar>
        Eigen::SparseMatrix<Scalar> nodal_average_interpolator(const IndexedMesh<Scalar, int> & mesh)
        {
            const auto occurrences = count_vertex_occurrences(mesh);

            // Reserve space for 1 non-zero per column, since each basis function in the affine space
            // maps to exactly one vertex in the standard finite element space.
            const auto num_dof_affine_space = 3 * mesh.num_elements();
            Eigen::SparseMatrix<Scalar> J(mesh.num_vertices(), num_dof_affine_space);
            J.reserve(Eigen::VectorXi::Constant(num_dof_affine_space, 1));

            for (int t = 0; t < mesh.num_elements(); ++t)
            {
                const auto vertices = mesh.elements()[t].vertex_indices;

                for (size_t v = 0; v < 3; ++v)
                {
                    const auto col = 3 * t + v;
                    const auto vertex_index = vertices[v];
                    const auto cardinality = occurrences[vertex_index];
                    J.insert(vertex_index, col) = static_cast<Scalar>(1.0) / static_cast<Scalar>(cardinality);
                }
            }

            return J;
        };
    }

    template <typename Scalar>
    Eigen::SparseMatrix<Scalar> quasi_interpolator(const BiscaleMesh<Scalar, int> & mesh)
    {
        const auto P = detail::affine_interpolator(mesh);
        const auto J = detail::nodal_average_interpolator(mesh.coarse_mesh());
        return J * P;
    }

}


/**
 * @file kalmen.hpp
 * @author Vahid Bastani
 *
 * Implementation of Kalman filter
 */
#ifndef SSMPACK_FILTER_KALMAN_HPP
#define SSMPACK_FILTER_KALMAN_HPP

#include "ssmkit/distribution/gaussian.hpp"
#include "ssmkit/distribution/conditional.hpp"
#include "ssmkit/process/markov.hpp"
#include "ssmkit/process/memoryless.hpp"
#include "ssmkit/process/hierarchical.hpp"
#include "ssmkit/filter/recursive_bayesian_base.hpp"
#include <armadillo>

namespace ssmkit {
namespace filter {

using process::Hierarchical;
using process::Markov;
using process::Memoryless;
using distribution::Conditional;
using distribution::Gaussian;

/** Kalman filter
 */
template <class STA_MAP, class OBS_MAP>
class Kalman
    : public RecursiveBayesianBase<Kalman<STA_MAP, OBS_MAP>> {

 public:
  //! Type of process object
  using TProcess =
      Hierarchical<Markov<Gaussian, STA_MAP, Gaussian>,
                   Memoryless<Gaussian, OBS_MAP>>;
  //! Type of the posterior state \f$(\hat{\mathbf{x}}, \hat{\mathbf{P}})\f$
  using TCompeleteState =
      std::tuple<arma::vec, arma::mat>;

 private:
  //! The process object
  TProcess process_;
  //! The state transition matrix \f$\mathbf{F}\f$
  const arma::mat &dyn_mat_;
  //! The measurement matrix \f$\mathbf{H}\f$
  const arma::mat &mes_mat_;
  //! The covariance of dynamic noise \f$\mathbf{Q}\f$
  const arma::mat &dyn_cov_;
  //! The covariance of measurement noise \f$\mathbf{R}\f$
  const arma::mat &mes_cov_;
  //! The corrected state vector \f$\mathbf{x}_{t|t}\f$
  arma::vec state_vec_;
  //! The corrected state covariance \f$\mathbf{P}_{t|t}\f$
  arma::mat state_cov_;
  //! The predicted state vector \f$\mathbf{x}_{t|t-1}\f$
  arma::vec p_state_vec_;
  //! The predicted state covariance \f$\mathbf{P}_{t|t-1}\f$
  arma::mat p_state_cov_;

 public:
  /** Construct a Kalman filter
   *
   * Construct a Kalman filter with parameters taken from \p process argument.
   */
  Kalman(const TProcess &process)
      : process_(process),
        dyn_mat_(
            process_.template getProcess<0>().getCPDF().getParamMap().transfer),
        mes_mat_(
            process_.template getProcess<1>().getCPDF().getParamMap().transfer),
        dyn_cov_(process_.template getProcess<0>()
                     .getCPDF()
                     .getParamMap()
                     .covariance),
        mes_cov_(process_.template getProcess<1>()
                     .getCPDF()
                     .getParamMap()
                     .covariance) {}

  /** Prediction
   *
   * Performs the prediction step.
   *
   * \f{equation}{\hat{\mathbf{x}}_{t|t-1} = \mathbf{F}\hat{\mathbf{x}}_{t-1|t-1}+u_d(y^d_1,
   * \cdots, y^d_{N_d}) \f}
   * \f{equation}{\mathbf{P}_{t|t-1} = \mathbf{F}\mathbf{P}_{t-1|t-1}\mathbf{F}^T+\mathbf{Q}\f}
   *
   * @param args... Control variables \f$y^d_1, \cdots, y^d_{N_d}\f$ of the dynamic process, if any.
   */
  template <class... TArgs>
  void predict(const TArgs &... args) {
    // use the map function to pass controls, avoiding control definition
    // is move used here??
    p_state_vec_ =
        std::get<0>(process_.template getProcess<0>().getCPDF().getParamMap()(
            state_vec_, args...));
    p_state_cov_ = dyn_mat_ * state_cov_ * dyn_mat_.t() + dyn_cov_;
  }

  /** Correction
   *
   * Performs correction step.
   *
   * \f{equation}{\tilde{\mathbf{z}}_t=\mathbf{z}_t-\mathbf{H}\hat{\mathbf{x}}_{t|t-1}-u_m(y^m_1, \cdots, y^m_{N_m})\f}
   * \f{equation}{\mathbf {S}_t=\mathbf{H}\mathbf{P}_{t|t-1}\mathbf{H}^T+\mathbf{R} \f}
   * \f{equation}{\mathbf{K}_t=\mathbf{P}_{t|t-1}\mathbf{H}^T\mathbf{S}_t^{-1} \f}
   * \f{equation}{\hat{\mathbf{x}}_{t|t}=\hat{\mathbf{x}}_{t|t-1}+\mathbf{K}_{t}\tilde{\mathbf{z}}_t \f}
   * \f{equation}{\mathbf{P}_{t|t}=(I-\mathbf{K}_t\mathbf{H})\mathbf{P}_{t|t-1}\f}
   *
   * @param measurement Measurement vector \f$\mathbf{z}_t\f$.
   * @param args... Control variables \f$y^m_1, \cdots, y^m_{N_m}\f$ of the measurement process, if any.
   * @return Estimated state \f$(\hat{\mathbf{x}}_{t|t}, \mathbf{P}_{t|t})\f$
   */
  template <class... TArgs>
  TCompeleteState correct(const arma::vec &measurement,
                          const TArgs &... args) {
    arma::vec inovation =
        measurement -
        std::get<0>(process_.template getProcess<1>().getCPDF().getParamMap()(
            p_state_vec_, args...));

    arma::mat inovation_cov = mes_mat_ * p_state_cov_ * mes_mat_.t() + mes_cov_;
    arma::mat kalman_gain =
        p_state_cov_ * mes_mat_.t() * arma::inv_sympd(inovation_cov);

    state_vec_ = p_state_vec_ + kalman_gain * inovation;
    state_cov_ = p_state_cov_ - kalman_gain * mes_mat_ * p_state_cov_;
    return std::make_tuple(state_vec_, state_cov_);
  }
  /** Initialization
   *
   * @return Initial state \f$(\hat{\mathbf{x}}_{0|0}, \mathbf{P}_{0|0})\f$
   */
  TCompeleteState initialize() {
    state_vec_ = process_.template getProcess<0>().getInitialPDF().getMean();
    state_cov_ =
        process_.template getProcess<0>().getInitialPDF().getCovariance();
    return std::make_tuple(state_vec_, state_cov_);
  }
};

template <class STA_MAP, class OBS_MAP>
Kalman<STA_MAP, OBS_MAP> makeKalman(
    Hierarchical<Markov<Gaussian, STA_MAP, Gaussian>,
                 Memoryless<Gaussian, OBS_MAP>> process) {
  return Kalman<STA_MAP, OBS_MAP>(process);
}

} // namespace filter
} // namespace ssmkit

#endif // SSMPACK_FILTER_KALMAN_HPP


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

#include <dlib/matrix.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include "../stl_checked.h"
#include "../array.h"
#include "../rand.h"
#include <dlib/string.h>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.matrix_lu");

    dlib::rand rnd;

// ----------------------------------------------------------------------------------------

    template <typename mat_type>
    const matrix<typename mat_type::type> symm(const mat_type& m) { return m*trans(m); }

// ----------------------------------------------------------------------------------------

    template <typename type>
    const matrix<type> randmat(long r, long c)
    {
        matrix<type> m(r,c);
        for (long row = 0; row < m.nr(); ++row)
        {
            for (long col = 0; col < m.nc(); ++col)
            {
                m(row,col) = static_cast<type>(rnd.get_random_double());
            }
        }

        return m;
    }

    template <typename type, long NR, long NC>
    const matrix<type,NR,NC> randmat()
    {
        matrix<type,NR,NC> m;
        for (long row = 0; row < m.nr(); ++row)
        {
            for (long col = 0; col < m.nc(); ++col)
            {
                m(row,col) = static_cast<type>(rnd.get_random_double());
            }
        }

        return m;
    }

// ----------------------------------------------------------------------------------------

    template <typename matrix_type>
    void test_lu ( const matrix_type& m)
    {
        typedef typename matrix_type::type type;
        const type eps = 10*max(abs(m))*sqrt(std::numeric_limits<type>::epsilon());
        dlog << LDEBUG << "test_lu():  " << m.nr() << " x " << m.nc() << "  eps: " << eps;
        print_spinner();

        lu_decomposition<matrix_type> test(m);

        DLIB_TEST(test.is_square() == (m.nr() == m.nc()));

        DLIB_TEST(test.nr() == m.nr());
        DLIB_TEST(test.nc() == m.nc());

        dlog << LDEBUG << "m.nr(): " << m.nr() << "  m.nc(): " << m.nc();

        type temp;
        DLIB_TEST_MSG( (temp= max(abs(test.get_l()*test.get_u() - rowm(m,test.get_pivot())))) < eps,temp);

        if (test.is_square())
        {
            // none of the matrices we should be passing in to test_lu() should be singular.
            DLIB_TEST_MSG (abs(test.det()) > eps/100, "det: " << test.det() );
            dlog << LDEBUG << "big det: " << test.det();

            DLIB_TEST(test.is_singular() == false);

            matrix<type> m2;
            matrix<type,0,1> col;

            m2 = identity_matrix<type>(m.nr());
            DLIB_TEST_MSG(equal(m*test.solve(m2), m2,eps),max(abs(m*test.solve(m2)- m2)));
            m2 = randmat<type>(m.nr(),5);
            DLIB_TEST_MSG(equal(m*test.solve(m2), m2,eps),max(abs(m*test.solve(m2)- m2)));
            m2 = randmat<type>(m.nr(),1);
            DLIB_TEST_MSG(equal(m*test.solve(m2), m2,eps),max(abs(m*test.solve(m2)- m2)));
            col = randmat<type>(m.nr(),1);
            DLIB_TEST_MSG(equal(m*test.solve(col), col,eps),max(abs(m*test.solve(m2)- m2)));

            // now make us a singular matrix
            if (m.nr() > 1)
            {
                matrix<type> sm(m);
                set_colm(sm,0) = colm(sm,1);

                lu_decomposition<matrix_type> test2(sm);
                DLIB_TEST_MSG( (temp= max(abs(test2.get_l()*test2.get_u() - rowm(sm,test2.get_pivot())))) < eps,temp);

                // these checks are only accurate for small matrices
                if (test2.nr() < 100)
                {
                    DLIB_TEST_MSG(test2.is_singular() == true,"det: " << test2.det());
                    DLIB_TEST_MSG(abs(test2.det()) < eps,"det: " << test2.det());
                }

            }
        }

    }

// ----------------------------------------------------------------------------------------

    void matrix_test_double()
    {

        test_lu(10*randmat<double>(2,2));
        test_lu(10*randmat<double>(1,1));
        test_lu(10*symm(randmat<double>(2,2)));
        test_lu(10*randmat<double>(4,4));
        test_lu(10*randmat<double>(9,4));
        test_lu(10*randmat<double>(3,8));
        test_lu(10*randmat<double>(15,15));
        test_lu(2*symm(randmat<double>(15,15)));
        test_lu(10*randmat<double>(100,100));
        test_lu(10*randmat<double>(137,200));
        test_lu(10*randmat<double>(200,101));

        test_lu(10*randmat<double,2,2>());
        test_lu(10*randmat<double,1,1>());
        test_lu(10*randmat<double,4,3>());
        test_lu(10*randmat<double,4,4>());
        test_lu(10*randmat<double,9,4>());
        test_lu(10*randmat<double,3,8>());
        test_lu(10*randmat<double,15,15>());
        test_lu(10*randmat<double,100,100>());
        test_lu(10*randmat<double,137,200>());
        test_lu(10*randmat<double,200,101>());

        typedef matrix<double,0,0,default_memory_manager, column_major_layout> mat;
        test_lu(mat(3*randmat<double>(4,4)));
        test_lu(mat(3*randmat<double>(9,4)));
        test_lu(mat(3*randmat<double>(3,8)));
    }

// ----------------------------------------------------------------------------------------

    void matrix_test_float()
    {

    // -------------------------------

        test_lu(3*randmat<float>(1,1));
        test_lu(3*randmat<float>(2,2));
        test_lu(3*randmat<float>(4,4));
        test_lu(3*randmat<float>(9,4));
        test_lu(3*randmat<float>(3,8));
        test_lu(3*randmat<float>(137,200));
        test_lu(3*randmat<float>(200,101));

        test_lu(3*randmat<float,1,1>());
        test_lu(3*randmat<float,2,2>());
        test_lu(3*randmat<float,4,3>());
        test_lu(3*randmat<float,4,4>());
        test_lu(3*randmat<float,9,4>());
        test_lu(3*randmat<float,3,8>());
        test_lu(3*randmat<float,137,200>());
        test_lu(3*randmat<float,200,101>());

        typedef matrix<float,0,0,default_memory_manager, column_major_layout> mat;
        test_lu(mat(3*randmat<float>(4,4)));
        test_lu(mat(3*randmat<float>(9,4)));
        test_lu(mat(3*randmat<float>(3,8)));
    }

// ----------------------------------------------------------------------------------------

    class matrix_tester : public tester
    {
    public:
        matrix_tester (
        ) :
            tester ("test_matrix_lu",
                    "Runs tests on the matrix LU component.")
        {
            //rnd.set_seed(cast_to_string(time(0)));
        }

        void perform_test (
        )
        {
            dlog << LINFO << "seed string: " << rnd.get_seed();

            dlog << LINFO << "begin testing with double";
            matrix_test_double();
            dlog << LINFO << "begin testing with float";
            matrix_test_float();
        }
    } a;

}


//
// Created by Jacques Perrault on 10/2/21.
//
#include <catch2/catch.hpp>
#include <iostream>
#include <Eigen/Dense>
#include <3dMethods/pca.h>
using namespace Eigen;
using namespace std;

SCENARIO("pca", "[pca.h]")
{
  MatrixXd mat(4, 3);
  mat << 1, 1, 1, -2, -1, -1, 1.2, 1.2, 1.2, 21, 21, 21;
  auto j = Pca(mat);

  cout << "eigenValues:\n" << get<0>(j) << endl;
  cout << "eigen vectors: \n" << get<1>(j) << endl;
  // make some real test data
}

SCENARIO("findRigidTransform", "[pca.h]")
{
  MatrixXd source(2, 3);
  source << 1, 1, 1, -1, -1, -1;
  MatrixXd target(2, 3);
  target << -1, -1, -1, 2, 2, 2;
  auto out = findRigidTransform(source, target);
  cout << "rotation: \n" << get<0>(out) << endl;
  cout << "translation: \n" << get<1>(out) << endl;
}


#include <Eigen/Core>
#include <catch2/catch.hpp>
#include <memory>
#include <tuple>
#include <vector>
#include "ear/bs2051.hpp"
#include "ear/common/geom.hpp"
#include "ear/common/helpers/eigen_helpers.hpp"
#include "ear/object_based/extent.hpp"

using namespace ear;

TEST_CASE("test_basis") {
  double eps = 1e-6;

  Eigen::Matrix3d expected;
  // cardinal directions
  expected = Eigen::Matrix3d::Identity();
  REQUIRE(calcBasis(cart(0.0, 0.0, 1.0)).isApprox(expected, eps));
  expected << 0, 1, 0,  //
      -1, 0, 0,  //
      0, 0, 1;
  REQUIRE(calcBasis(cart(90.0, 0.0, 1.0)).isApprox(expected, eps));
  expected << 0, -1, 0,  //
      1, 0, 0,  //
      0, 0, 1;
  REQUIRE(calcBasis(cart(-90.0, 0.0, 1.0)).isApprox(expected, eps));
  expected << -1, 0, 0,  //
      0, -1, 0,  //
      0, 0, 1;
  REQUIRE(calcBasis(cart(180.0, 0.0, 1.0)).isApprox(expected, eps));
  expected << 1, 0, 0,  //
      0, 0, 1,  //
      0, -1, 0;
  REQUIRE(calcBasis(cart(0.0, 90.0, 1.0)).isApprox(expected, eps));
  expected << 1, 0, 0,  //
      0, 0, -1,  //
      0, 1, 0;
  REQUIRE(calcBasis(cart(0.0, -90.0, 1.0)).isApprox(expected, eps));

  // slight offset from pole should behave as if pointing forwards
  expected << 1, 0, 0,  //
      0, 0, 1,  //
      0, -1, 0;
  REQUIRE(calcBasis(cart(90.0, 90.0 - 1e-6, 1.0)).isApprox(expected, eps));
  expected << 1, 0, 0,  //
      0, 0, -1,  //
      0, 1, 0;
  REQUIRE(calcBasis(cart(90.0, -90.0 + 1e-6, 1.0)).isApprox(expected, eps));
}

TEST_CASE("test_azimuth_elevation_on_basis") {
  double eps = 1e-6;

  double azimuth, elevation;
  Eigen::Matrix3d basis;
  basis = calcBasis(cart(0.0, 10.0, 1.0));
  std::tie(azimuth, elevation) =
      azimuthElevationOnBasis(basis, cart(0.0, 10.0, 1.0));
  REQUIRE(azimuth == Approx(0.0).margin(eps));
  REQUIRE(elevation == Approx(0.0).margin(eps));
  std::tie(azimuth, elevation) =
      azimuthElevationOnBasis(basis, cart(0.0, 20.0, 1.0));
  REQUIRE(azimuth == Approx(0.0).margin(eps));
  REQUIRE(elevation == Approx(radians(10.0)).margin(eps));
  basis = calcBasis(cart(-10.0, 0.0, 1.0));
  std::tie(azimuth, elevation) =
      azimuthElevationOnBasis(basis, cart(-20.0, 0.0, 1.0));
  REQUIRE(azimuth == Approx(radians(10.0)).margin(eps));
  REQUIRE(elevation == Approx(0.0).margin(eps));
}

TEST_CASE("test_cart_on_basis") {
  Eigen::Matrix3d basis;
  basis = calcBasis(cart(0.0, 10.0, 1.0));
  REQUIRE(
      cartOnBasis(basis, 0.0, radians(10.0)).isApprox(cart(0.0, 20.0, 1.0)));
  basis = calcBasis(cart(-10.0, 0.0, 1.0));
  REQUIRE(
      cartOnBasis(basis, radians(10.0), 0.0).isApprox(cart(-20.0, 0.0, 1.0)));
}

TEST_CASE("test_weight_func") {
  double fade = 10.0;
  double height = 10.0;

  double width;
  double azimuth;
  double expected;
  double actual;
  Eigen::Vector3d point;
  for (auto entry : {std::make_pair(20.0, 0.0), std::make_pair(360.0, 0.0),
                     std::make_pair(360.0, 180.0)}) {
    std::tie(width, azimuth) = entry;
    Eigen::VectorXd elevations = Eigen::VectorXd::LinSpaced(50, -90.0, 90.0);
    for (double elevation : elevations) {
      expected = interp(elevation,
                        Eigen::Vector4d{-(height / 2 + fade), -height / 2,
                                        height / 2, height / 2 + fade},
                        Eigen::Vector4d{0, 1, 1, 0});

      point = cart(azimuth, elevation, 1.0);
      WeightingFunction weightingFunc(cart(0.0, 0.0, 1.0), width, height);
      actual = weightingFunc(point);
      REQUIRE(actual == Approx(expected));
      // Swapped
      point = cart(elevation, azimuth, 1.0);
      WeightingFunction weightingFuncSwap(cart(0.0, 0.0, 1.0), height, width);
      actual = weightingFuncSwap(point);
      REQUIRE(actual == Approx(expected));
    }
  }
  Eigen::VectorXd azimuths = Eigen::VectorXd::LinSpaced(50, -180, 180);
  for (double azimuth : azimuths) {
    double expected = interp(azimuth,
                             Eigen::Vector4d{-(width / 2 + fade), -width / 2,
                                             width / 2, width / 2 + fade},
                             Eigen::Vector4d{0, 1, 1, 0});

    point = cart(azimuth, 0.0, 1.0);
    WeightingFunction weightingFunc(cart(0.0, 0.0, 1.0), width, height);
    actual = weightingFunc(point);
    REQUIRE(actual == Approx(expected));
    // Swapped
    point = cart(0.0, azimuth, 1.0);
    WeightingFunction weightingFuncSwap(cart(0.0, 0.0, 1.0), height, width);
    actual = weightingFuncSwap(point);
    REQUIRE(actual == Approx(expected));
  }
}

TEST_CASE("test_pv") {
  Layout layout = getLayout("9+10+3").withoutLfe();
  std::shared_ptr<PointSourcePanner> psp = configurePolarPanner(layout);
  PolarExtentPanner extentPanner(psp);

  REQUIRE(extentPanner.calcPvSpread(cart(0.0, 0.0, 1.0), 0.0, 0.0) ==
          psp->handle(cart(0.0, 0.0, 1.0)).get());
  REQUIRE(extentPanner.calcPvSpread(cart(10.0, 20.0, 1.0), 0.0, 0.0) ==
          psp->handle(cart(10.0, 20.0, 1.0)).get());

  std::vector<std::pair<Eigen::Vector3d, double>> positions{
      std::make_pair(cart(0.0, 0.0, 1.0), 1e-10),
      std::make_pair(cart(30.0, 10.0, 1.0), 1e-2)};
  for (auto position : positions) {
    Eigen::Vector3d pos = position.first;
    double tol = position.second;
    Eigen::VectorXd spread_pv = extentPanner.calcPvSpread(pos, 20.0, 10.0);
    REQUIRE(spread_pv.norm() == Approx(1.0));
    Eigen::VectorXd vv =
        spread_pv.transpose() * toPositionsMatrix(layout.positions());
    vv /= vv.norm();
    REQUIRE(vv.isApprox(pos, tol));
  }
}


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

using namespace Eigen;

int main(){
    Matrix3f haha;
    haha << 1, 2, 3,
            4, 5, 6,
            7, 8, 9;
    std::cout << haha << std::endl;
}