/*
 * File: fast_hankel_transform.cc
 * Created Date: 2019-09-11
 * Author: Lei Pan
 * Contact: <panlei7@gmail.com>
 *
 * Last Modified: Wednesday September 25th 2019 11:37:38 am
 *
 * MIT License
 *
 * Copyright (c) 2019 Lei Pan
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the 'Software'), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED 'AS IS', WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 * -----
 * HISTORY:
 * Date      	 By	Comments
 * ----------	---
 * ----------------------------------------------------------
 */

#include "fast_hankel_transform.hpp"

#include <Eigen/Dense>
#include <boost/math/special_functions/bessel.hpp>
#include <cmath>
#include <complex>
#include <fftw3.h>
#include <fmt/format.h>
#include <fmt/ostream.h>
#include <iostream>

using namespace Eigen;
using std::abs;
using std::exp;
using std::log;
using std::pow;

FastHankelTransform::FastHankelTransform(int num_sample, double ux, double uy)
    : num_sample_(num_sample), ux_(ux), uy_(uy),
      x_(VectorXd::Zero(num_sample_)), f_(VectorXd::Zero(num_sample_ + 1)),
      phi_(new std::complex<double>[num_sample_ * 2]),
      j1_(new std::complex<double>[num_sample_ * 2]) {
  alpha_ = evaluate_alpha();
  k0_ = evaluate_k0(alpha_);
}

FastHankelTransform::~FastHankelTransform() {
  delete[] phi_;
  delete[] j1_;
}

double FastHankelTransform::evaluate_alpha() {
  auto func = [&](auto a) { return -log(1.0 - exp(-a)) / (num_sample_ - 1); };

  const int maxiter = 100;
  double alpha = 1.0;
  for (auto i = 0; i < maxiter; ++i) {
    alpha = func(alpha);
  }
  return alpha;
}

double FastHankelTransform::evaluate_k0(double alpha) {
  double k0 = (2.0 * exp(alpha) + exp(2.0 * alpha)) /
              (pow(1 + exp(alpha), 2) * (1 - exp(-2.0 * alpha)));
  return k0;
}

VectorXd FastHankelTransform::sampling() {
  x_updated_ = true;
  x_(0) = (1.0 + exp(alpha_)) * exp(-alpha_ * num_sample_) / 2.0;
  for (auto i = 1; i < num_sample_; ++i) {
    x_(i) = x_(0) * exp(alpha_ * i);
  }
  return x_;
}

void FastHankelTransform::set_feval(const Ref<const VectorXd> &feval) {
  assert(x_updated_ == true);
  f_updated_ = true;
  f_.head(num_sample_) = feval;
}

void FastHankelTransform::evaluate_phi() {
  phi_[0] = k0_ * (f_(0) - f_(1)) * exp(alpha_ * (1 - num_sample_));
  for (auto i = 1; i < num_sample_; ++i) {
    phi_[i] = (f_(i) - f_(i + 1)) * exp(alpha_ * (i + 1 - num_sample_));
  }
  for (auto i = num_sample_; i < num_sample_ * 2; ++i) {
    phi_[i] = 0.0;
  }
}

void FastHankelTransform::evaluate_j1() {
  for (auto i = 0; i < num_sample_ * 2; ++i) {
    double x = ux_ * uy_ * x_(0) * exp(alpha_ * (i + 1 - num_sample_));
    j1_[i] = boost::math::cyl_bessel_j(1, x);
  }
}

VectorXd FastHankelTransform::calculate() {
  assert(f_updated_ == true);
  evaluate_phi();
  evaluate_j1();

  int nsample = num_sample_ * 2;
  fftw_complex *fft_phi =
      (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * nsample);
  fftw_complex *phi = reinterpret_cast<fftw_complex *>(phi_);
  fftw_plan p1 =
      fftw_plan_dft_1d(nsample, phi, fft_phi, FFTW_FORWARD, FFTW_ESTIMATE);
  fftw_execute(p1);

  fftw_complex *fft_j1 =
      (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * nsample);
  fftw_complex *j1 = reinterpret_cast<fftw_complex *>(j1_);
  fftw_plan p2 =
      fftw_plan_dft_1d(nsample, j1, fft_j1, FFTW_BACKWARD, FFTW_ESTIMATE);
  fftw_execute(p2);

  fftw_complex *in =
      (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * nsample);

  for (auto i = 0; i < nsample; ++i) {
    in[i][0] = fft_phi[i][0] * fft_j1[i][0] - fft_phi[i][1] * fft_j1[i][1];
    in[i][1] = fft_phi[i][0] * fft_j1[i][1] + fft_phi[i][1] * fft_j1[i][0];
  }

  fftw_complex *out =
      (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * nsample);

  fftw_plan p3 =
      fftw_plan_dft_1d(nsample, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
  fftw_execute(p3);

  VectorXd ret(num_sample_);
  for (auto i = 0; i < num_sample_; ++i) {
    ret(i) = 2.0 / (x_(i) * pow(ux_, 2) * uy_) * out[i][0] / nsample;
  }

  fftw_destroy_plan(p1);
  fftw_destroy_plan(p2);
  fftw_destroy_plan(p3);
  fftw_free(fft_phi);
  fftw_free(fft_j1);
  fftw_free(in);
  fftw_free(out);

  return ret;
}


#include<NTL/ZZ_p.h>
#include<NTL/ZZ_pXFactoring.h>
#include <NTL/vector.h>
using namespace std;
using namespace NTL;
ZZ encr(ZZ m,ZZ e,ZZ n){
	return PowerMod(m,e,n);
	}

int main()
{
	Vec<ZZ> v;
   ZZ n,p,q,f;
   long L = 20;
   GenGermainPrime(p,L,800);
   GenGermainPrime(q,L,800);

   n = p*q;
   //~ f=(p-1)*(q-1);
   cout <<"P="<< p << "\n";   cout <<"Q="<< q << "\n";
   cout <<"N="<< n << "\n";
   //~ cout <<"F="<< f << "\n";
   //~ cout <<"Modulo Gen Done" << "\n";
   //~
   //~ ZZ e=RandomBnd(f),test,d;
   //~ ZZ gcd_ef=ZZ(20);
   //~ while(gcd_ef!=0){
	   //~ e=RandomBnd(f);
	   //~ gcd_ef=InvModStatus(d,e,f);
	   //~ }
	//~ d=InvMod(e,f);
	//~ test=MulMod(e,d,f);
   //~ cout <<"e="<< e << "\n";
   //~ cout <<"d="<< d << "\n";
   //~ cout <<"test="<< test << "\n";
   //~ cout <<"Key Generation Done" << "\n";
   //~ ZZ m = RandomBnd(n);
   //~ ZZ Enc,Dec;
   //~ Enc = encr(m,e,n);
   //~ Dec = encr(Enc,d,n);
   //~ if(m==Dec){
	//~ cout <<"Success" << "\n";
	   //~ }
   return 0;
}


#ifndef Interpolators__2D_BicubicInterpolator_hpp
#define Interpolators__2D_BicubicInterpolator_hpp

/** @file BicubicInterpolator.hpp
  * @brief
  * @author C.D. Clark III
  * @date 12/27/16
  */

#include "InterpolatorBase.hpp"
#include <boost/range/algorithm/lower_bound.hpp>
#include <boost/range/adaptor/strided.hpp>

/** @class
  * @brief Cubic spline interpolation for for 2D functions.
  * @author C.D. Clark III, Aaron Hoffman
  *
  * This class implements the bicubic spline interpolation method.
  * It is essentially the 2D equivalent of cubic splines for 1D.
  *
  */

namespace _2D {

template<class Real>
class BicubicInterpolator : public InterpolatorBase<BicubicInterpolator<Real>>
{
  public:
    using BASE = InterpolatorBase<BicubicInterpolator<Real>>;
    using VectorType = typename BASE::VectorType;
    using MapType = typename BASE::MapType;

    // types used to view data as 2D coordinates
    using MatrixType    = typename BASE::MatrixType;
    using _2DVectorView = typename BASE::_2DVectorView;
    using _2DMatrixView = typename BASE::_2DMatrixView;

    // types used for 4x4 matrix algebra
    using Matrix44 = Eigen::Matrix<Real,4,4 >;
    using Matrix44Array = Eigen::Array< Matrix44, Eigen::Dynamic, Eigen::Dynamic >;
    using ColVector4 = Eigen::Matrix<Real,4,1 >;
    using RowVector4 = Eigen::Matrix<Real,1,4 >;

  protected:
    using BASE::xView;
    using BASE::yView;
    using BASE::zView;
    using BASE::X;
    using BASE::Y;
    using BASE::Z;

    Matrix44Array a; // naming convention used by wikipedia article (see Wikipedia https://en.wikipedia.org/wiki/Bicubic_interpolation)

  public:

    template<typename I>
    BicubicInterpolator( I n, Real *x, Real *y, Real *z ) {this->setData(n,x,y,z);}

    template<typename X, typename Y, typename Z>
    BicubicInterpolator( X &x, Y &y, Z &z ) {this->setData(x,y,z);}

    BicubicInterpolator():BASE()
    { }

    BicubicInterpolator(const BicubicInterpolator& rhs)
    :BASE(rhs)
    ,a(rhs.a)
    {}

    // copy-swap idiom
    friend void swap( BicubicInterpolator& lhs, BicubicInterpolator& rhs)
    {
      lhs.a.swap(rhs.a);
      swap( static_cast<BASE&>(lhs), static_cast<BASE&>(rhs) );
    }

    BicubicInterpolator& operator=(BicubicInterpolator rhs)
    {
      swap(*this,rhs);
      return *this;
    }

    // methods required by the interface
    Real operator()( Real x, Real y ) const;

  protected:

    void setupInterpolator();
    friend BASE;

};

template<class Real>
void
BicubicInterpolator<Real>::setupInterpolator()
{
  // Interpolation will be done by multiplying the coordinates by coefficients.

  a = Matrix44Array( X->size()-1, Y->size()-1 );

  // We are going to precompute the interpolation coefficients so
  // that we can interpolate quickly. This requires a 4x4 matrix for each "patch".
  Matrix44 Left, Right;

  Left <<  1,  0,  0,  0,
           0,  0,  1,  0,
          -3,  3, -2, -1,
           2, -2,  1,  1;

  Right<<  1,  0, -3,  2,
           0,  0,  3, -2,
           0,  1, -2,  1,
           0,  0, -1,  1;

  for(int i = 0; i < X->size() - 1; i++)
  {
    for( int j = 0; j < Y->size() - 1; j++)
    {
      Matrix44 F;

      Real f00,   f01,   f10,   f11;
      Real fx00,  fx01,  fx10,  fx11;
      Real fy00,  fy01,  fy10,  fy11;
      Real fxy00, fxy01, fxy10, fxy11;

      Real fm, fp;
      int im, ip, jm, jp;

      int iN = X->size();
      int jN = Y->size();

      // function values
      f00 = (*Z)(i    ,j    ); // <<<<<<
      f01 = (*Z)(i    ,j + 1); // <<<<<<
      f10 = (*Z)(i + 1,j    ); // <<<<<<
      f11 = (*Z)(i + 1,j + 1); // <<<<<<

      // need to calculate function values and derivatives
      // at each corner.
      //
      // note: interpolation algorithm is derived for the unit square.
      // so we need to take the derivatives assuming X(i+1) - X(i) = Y(j+1) - Y(j) = 1

      Real xL = (*X)(i+1) - (*X)(i);
      Real yL = (*Y)(j+1) - (*Y)(j);
      Real dx, dy;

      // x derivatives

      im = std::max(i-1,0);
      ip = std::min(i+1,iN-1);

      dx = ((*X)(ip) - (*X)(im))/xL;

      fp = (*Z)(ip,j);
      fm = (*Z)(im,j);
      fx00 = (fp - fm) / dx; // <<<<<<

      fp = (*Z)(ip,j+1);
      fm = (*Z)(im,j+1);
      fx01 = (fp - fm) / dx; // <<<<<<

      im = std::max(i,0);
      ip = std::min(i+2,iN-1);

      dx = ((*X)(ip) - (*X)(im))/xL;

      fp = (*Z)(ip,j);
      fm = (*Z)(im,j);
      fx10 = (fp - fm) / dx; // <<<<<<

      fp = (*Z)(ip,j+1);
      fm = (*Z)(im,j+1);
      fx11 = (fp - fm) / dx; // <<<<<<

      // y derivatives

      jm = std::max(j-1,0);
      jp = std::min(j+1,jN-1);

      dy = ((*Y)(jp) - (*Y)(jm))/yL;

      fp = (*Z)(i,jp);
      fm = (*Z)(i,jm);
      fy00 = (fp - fm) / dy; // <<<<<<

      fp = (*Z)(i+1,jp);
      fm = (*Z)(i+1,jm);
      fy10 = (fp - fm) / dy; // <<<<<<

      jm = std::max(j,0);
      jp = std::min(j+2,jN-1);

      dy = ((*Y)(jp) - (*Y)(jm))/yL;

      fp = (*Z)(i,jp);
      fm = (*Z)(i,jm);
      fy01 = (fp - fm) / yL; // <<<<<<

      fp = (*Z)(i+1,jp);
      fm = (*Z)(i+1,jm);
      fy11 = (fp - fm) / yL; // <<<<<<

      // xy derivatives

      im = std::max(i-1,0);
      ip = std::min(i+1,iN-1);
      jm = std::max(j-1,0);
      jp = std::min(j+1,jN-1);

      dx = ((*X)(ip) - (*X)(im)) / xL;

      dy = ((*Y)(jp) - (*Y)(jm)) / yL;

      fp = ((*Z)(ip,jp) - (*Z)(im,jp))/dx;
      fm = ((*Z)(ip,jm) - (*Z)(im,jm))/dx;
      fxy00 = (fp - fm) / dy; // <<<<<<

      jm = std::max(j,0);
      jp = std::min(j+2,jN-1);

      dy = ((*Y)(jp) - (*Y)(jm))/yL;

      fp = ((*Z)(ip,jp) - (*Z)(im,jp))/dx;
      fm = ((*Z)(ip,jm) - (*Z)(im,jm))/dx;
      fxy01 = (fp - fm) / dy; // <<<<<<

      im = std::max(i,0);
      ip = std::min(i+2,iN-1);
      jm = std::max(j-1,0);
      jp = std::min(j+1,jN-1);

      dx = ((*X)(ip) - (*X)(im)) / xL;

      dy = ((*Y)(jp) - (*Y)(jm)) / yL;

      fp = ((*Z)(ip,jp) - (*Z)(im,jp))/dx;
      fm = ((*Z)(ip,jm) - (*Z)(im,jm))/dx;
      fxy10 = (fp - fm) / dy; // <<<<<<

      jm = std::max(j,0);
      jp = std::min(j+2,jN-1);

      dy = ((*Y)(jp) - (*Y)(jm)) / yL;

      fp = ((*Z)(ip,jp) - (*Z)(im,jp))/dx;
      fm = ((*Z)(ip,jm) - (*Z)(im,jm))/dx;
      fxy11 = (fp - fm) / dy; // <<<<<<

      F <<  f00,  f01,  fy00,  fy01,
            f10,  f11,  fy10,  fy11,
           fx00, fx01, fxy00, fxy01,
           fx10, fx11, fxy10, fxy11;

      a(i,j) = Left * F * Right;
    }
  }
}

template<class Real>
Real
BicubicInterpolator<Real>::operator()( Real x, Real y ) const
{
  BASE::checkData();

  // no extrapolation...
  if( x < this->xView->minCoeff()
   || x > this->xView->maxCoeff()
   || y < this->yView->minCoeff()
   || y > this->yView->maxCoeff() )
  {
    return 0;
  }

  // find the x index that is just to the LEFT of x
  //int i  = Utils::index__last_lt( x, *X, X->size() );
  // NOTE: X data is strided.
  auto xrng = std::make_pair( X->data(), X->data()+X->size()*X->innerStride() ) | boost::adaptors::strided(X->innerStride());
  int i = boost::lower_bound( xrng, x) - boost::begin(xrng) - 1;
  if(i < 0)
    i = 0;

  // find the y index that is just BELOW y
  //int j  = Utils::index__last_lt( y, *Y, Y->size() );
  // NOTE: Y data is NOT strided
  auto yrng = std::make_pair( Y->data(), Y->data()+Y->size() );
  int j = boost::lower_bound( yrng, y) - boost::begin(yrng) - 1;
  if(j < 0)
    j = 0;

  Real xL = (*X)(i+1) - (*X)(i);
  Real yL = (*Y)(j+1) - (*Y)(j);

  // now, create the coordinate vectors (see Wikipedia https://en.wikipedia.org/wiki/Bicubic_interpolation)
  RowVector4 vx;
  vx[0] = 1;                                   // x^0
  vx[1] = (x - (*X)(i))/xL;                    // x^1
  vx[2] = vx[1] * vx[1];                       // x^2
  vx[3] = vx[2] * vx[1];                       // x^3

  ColVector4 vy;
  vy[0] = 1;                                   // y^0
  vy[1] = (y - (*Y)(j))/yL;                    // y^1
  vy[2] = vy[1] * vy[1];                       // y^2
  vy[3] = vy[2] * vy[1];                       // y^3

  // interpolation is just x*a*y

  return vx*a(i,j)*vy;

}

}

#endif // include protector


    #include <Eigen/Dense>
    #include <Eigen/Sparse>
    #include <unsupported/Eigen/MatrixFunctions>
    #include <unsupported/Eigen/KroneckerProduct>

    #include <iostream>
    #include <cmath>

    #define TVMTL_MATRIX_UTILS_DEBUG

    #include <mtvmtl/core/matrix_utils.hpp>

    int main(){

    using namespace Eigen;

    Matrix3d R = Matrix3d::Random();
    Matrix3d M = R + R.transpose() + R.rows()*Matrix3d::Identity();

    std::cout << "Matrix M :\n" << M << std::endl;
    std::cout << "\nlog(M) :\n" << M.log() << std::endl;

    for (int i = 0; i < 3; i++) {
    	for (int j = 0; j < 3; j++) {
	    Matrix3d E = Matrix3d::Zero();
	    MatrixXd D = MatrixXd::Zero(3,3);
	    E(i,j)=1.0;
	    tvmtl::MatrixLogarithmFrechetDerivative(M, E, D);
	    std::cout << "\ndlog(M,E_" << i+1 << j+1 << ") :\n" << D << std::endl;
	    Map<VectorXd> V(D.data(), D.size());
	    std::cout << "\nRowwise Vectorized:\n" << V << std::endl;
    	}
    }

    Matrix<double, 9 ,9>  Result;
    tvmtl::KroneckerDLog(M, Result);
    std::cout << "Full Kronecker Representation of DLog:\n" << Result << std::endl;

    std::cout << "\n\nPermutation Matrix Test:\n" << std::endl;

    PermutationMatrix<9, 9, int> P;
    P.setIdentity();
    for(int i=0; i<3; i++)
	for(int j=0; j<i; j++)
	    P.applyTranspositionOnTheRight(j*3+i,i*3+j);

    std::cout << P*Matrix<double, 9,9>::Identity() << std::endl;

    std::cout << "\n\n Simple Rotation Matrix Test\n" << std::endl;

    Matrix3d s1 = Matrix3d::Identity();
    Matrix3d s2;
    s2 << 0, -1, 0, 1, 0, 0, 0, 0, 1;
    Matrix3d m2 = s1.transpose()*s2;

    std::cout << "\nMatrix 1\n" << s1 << std::endl;
    std::cout << "\nMatrix 2\n" << s2 << std::endl;
    std::cout << "\nDlog Argument\n" << m2 << std::endl;

    tvmtl::KroneckerDLog(m2, Result);
    std::cout << "\n\nFull Kronecker Representation of DLog:\n" << Result << std::endl;

    Matrix3d m3;
    m3 << 4102350, -2446420, 2209640, -2446420,  1458920, -1317710, 2209640, -1317710, 1190180;

    tvmtl::KroneckerDLog(m3, Result);
    std::cout << "\n\nPathological Matrix test:\n" << Result << std::endl;

    return 0;
    }


#include <cmath>
#include <Eigen/Dense>
#include <gtest/gtest.h>
#include <adaptive_filter_rls.h>

class ReferenceFilter
{
public:
    ReferenceFilter( unsigned inputCount )
        : mW( Eigen::VectorXf::Random( inputCount ) )
    {}

    float operator()( Eigen::VectorXf inputs )
    {
        return mW.dot( inputs );
    }

    Eigen::VectorXf mW;
};

class Model
{
    const unsigned kInputCount = 100;
    const float kMaxAmplitude = 1.0f;
    const float kMaxFrequency = 10.0f;
    const float kStep = 0.1f * 1.0f / kMaxFrequency;

public:
    Model()
        : mAmplitude( kMaxAmplitude / 2.0f *
            ( Eigen::VectorXf::Random( kInputCount ).array() + 1.0f ) )
        , mOmega( kMaxFrequency / 2.0f *
            ( Eigen::VectorXf::Random( kInputCount ).array() + 1.0f ) )
        , mInput( kInputCount )
        , mW( Eigen::VectorXf::Random( kInputCount ) )
        , mOutput( 0.0f )
        , mReferenceFilter( kInputCount )
        , mReferenceOutput( 0.0f )
        , mTime( 0.0f )
    {}

    void Update()
    {
        mTime += kStep;
        mInput = mAmplitude.array() * ( mOmega.array() * mTime ).unaryExpr(
            std::ptr_fun< float, float >( std::sin ) );
        mOutput = mW.dot( mInput );
        mReferenceOutput = mReferenceFilter( mInput );
    }

    Eigen::VectorXf mAmplitude;
    Eigen::VectorXf mOmega;
    Eigen::VectorXf mInput;
    Eigen::VectorXf mW;
    float mOutput;
    ReferenceFilter mReferenceFilter;
    float mReferenceOutput;
    float mTime;
};

TEST( AdaptiveFilter, NLMS )
{
    const unsigned kStepCount = 10000;
    const float kTrainStep = 0.1f;

    Model model;
    model.Update();
    float error = model.mReferenceOutput - model.mOutput;
    float initialError = std::fabs( error / model.mOutput );

    for ( int i = 0; i < kStepCount; ++ i )
    {
        model.Update();
        error = model.mReferenceOutput - model.mOutput;
        model.mW += ( kTrainStep * error * model.mInput.transpose() /
            model.mInput.squaredNorm() );
    }

    EXPECT_TRUE( std::fabs( error / model.mOutput ) < initialError );
}

TEST( AdaptiveFilter, RLS )
{
    const unsigned kStepCount = 1000;
    const float kRegularization = 1000.0f;
    const float kForgettingFactor = 0.99f;

    Model model;
    model.Update();
    float error = model.mReferenceOutput - model.mOutput;
    float initialError = std::fabs( error / model.mOutput );

    ESN::AdaptiveFilterRLS filter( model.mInput.size(),
        kForgettingFactor, kRegularization );

    for ( int i = 0; i < kStepCount; ++ i )
    {
        model.Update();
        error = model.mReferenceOutput - model.mOutput;
        filter.Train( model.mW, model.mOutput, model.mReferenceOutput,
            model.mInput );
    }

    EXPECT_LT( std::fabs( error / model.mOutput ), initialError );
}


#include <iostream>
#include <cmath>
#include <math.h>
#include <iostream>
#include <vector>
#include <array>
#include "heatEquation.hpp"
#include "AdaptiveHeat.hpp"
#include "TriDiagMatrix.hpp"
#include "MassMatrix.hpp"
#include "StiffnessMatrix.hpp"
#include <fstream>
#include <string>
#include <boost/math/quadrature/gauss.hpp>
using namespace std;
using namespace boost::math::quadrature;

void AdaptiveHeatEquation::AdaptiveSolver()
{
AnalyticSolutionVec();
mpPreviousSolution = mpAnalyticSolution;
BuildSystemAtTimeStep();
oldmesh.CopySpaceMesh(mpsmesh);
mpsmesh.PrintSpaceNodes();

int m = mptmesh.NumberOfTimeSteps();
for(int j = 0; j<m; j++)
{
    mpcurrenTimeStep = j+1;
    mpcurrentMeshIndex = j;

    BuildSystemAtTimeStep();
    SystemSolver();
    mpPreviousSolution = mpx;

    SaveIntervalsForRefinement();
    SaveIntervalsForCoarsening();
    mpsmesh.BisectIntervals(intervalsForRefinement);
    mpsmesh.CoarsenIntervals(NodesForRemoval);
    std::cout<<mpsmesh.meshsize() <<"\n";
    std::cout<<"\n";
    PrintVector(mpErrorMesh);
}
    std::cout<<mpsmesh.meshsize() <<"\n";
    std::cout<<"\n";
}

void AdaptiveHeatEquation::SaveIntervalsForCoarsening()
{
    BuildErrorMesh();
    NodesForRemoval.clear();
    for(int i=0; i<mpErrorMesh.size()-1; i++)
    {
        if (sqrt(mpErrorMesh.at(i)+mpErrorMesh.at(i+1))<coarseningtol)
        {
            NodesForRemoval.push_back(i+1);
        }
    }

}

void AdaptiveHeatEquation::BuildSystemAtTimeStep()
{
stiff.BuildStiffnessMatrix( mpsmesh );
stiff.MultiplyByScalar( mptmesh.ReadTimeMesh(mpcurrentMeshIndex) );
mass.BuildMassMatrix(mpsmesh);
LHS.AddTwoMatrices( mass, stiff );
}

void AdaptiveHeatEquation::SystemSolver()
{
    BuildRHS();
    LHS.MatrixSolver( mpRHS, mpx );
    oldmesh.CopySpaceMesh(mpsmesh);
}

void AdaptiveHeatEquation::SaveIntervalsForRefinement()
{
    BuildErrorMesh();
    intervalsForRefinement.clear();
    for(int i=0; i<mpErrorMesh.size(); i++)
    {
        if (sqrt(mpErrorMesh.at(i))>tolerance)
        {
            intervalsForRefinement.push_back(i);
        }
    }
}

double AdaptiveHeatEquation::IntegrateBasisWithU( int NodeIndex, double lowerlimit,
                              double upperlimit )
{
    auto SolutionWithBasis = [&](double x)
        { return mpsmesh.TestFunctions( NodeIndex, x)*PiecewiseU(x, oldmesh, mpPreviousSolution); };

    return gauss<double, 7>::integrate(SolutionWithBasis, lowerlimit, upperlimit);
}

    //this function works for refinement and coarsening
void AdaptiveHeatEquation::BuildRHS()
{
mpRHS.clear();
std::vector<double> intervals;

refinedsmesh.CommonMesh(mpsmesh, oldmesh);

double integral;
for (int i = 1; i<mpsmesh.meshsize(); i++)
{
    integral=0;
    refinedsmesh.Range(mpsmesh.ReadSpaceNode(i-1), mpsmesh.ReadSpaceNode(i+1), intervals);

    for(int j = 0; j<intervals.size()-1; j++)
    {
        integral = integral + IntegrateBasisWithU(i, intervals.at(j),
                    intervals.at(j+1));
    }
    mpRHS.push_back(integral);
}
}

Sover class with method to adapt in space and time/AdaptiveHeat.cpp


//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include <boost/gil.hpp>
#include <boost/gil/extension/io/png.hpp>

#include <iostream>
#include <limits>
#include <vector>

namespace gil = boost::gil;

// Demonstrates how to use a Hough transform to identify a circle

// Note this relies on the brute force approach, which today is the only one available in GIL
// The function hough_circle_transform_brute, defined in include/boost/gil/image_processing/hough_transform.cpp,
// accepts a greyscale edge map, the three Hough parameters allowing to do the drawing and the voting,
// an accumulator in the form of an iterator of views of the parameter space and a utility rasterizer to produce the points.
// The example outputs the voting cell of the centre of a circle drawn programatically.
// See also:
// hough_transform_line.cpp - Hough transform to detect lines

int main()
{
    const std::size_t size = 128;
    gil::gray8_image_t input_image(size, size);
    auto input = gil::view(input_image);

    const std::ptrdiff_t circle_radius = 16;
    const gil::point_t circle_center = {64, 64};
    const auto rasterizer = gil::midpoint_circle_rasterizer{};
    std::vector<gil::point_t> circle_points(rasterizer.point_count(circle_radius));
    rasterizer(circle_radius, circle_center, circle_points.begin());
    for (const auto& point : circle_points)
    {
        input(point) = std::numeric_limits<gil::uint8_t>::max();
    }

    const auto radius_parameter =
        gil::hough_parameter<std::ptrdiff_t>::from_step_count(circle_radius, 3, 3);
    const auto x_parameter =
        gil::hough_parameter<std::ptrdiff_t>::from_step_count(circle_center.x, 3, 3);
    const auto y_parameter =
        gil::hough_parameter<std::ptrdiff_t>::from_step_count(circle_center.x, 3, 3);

    std::vector<gil::gray16_image_t> parameter_space_images(
        radius_parameter.step_count,
        gil::gray16_image_t(x_parameter.step_count, y_parameter.step_count));
    std::vector<gil::gray16_view_t> parameter_space_views(parameter_space_images.size());
    std::transform(parameter_space_images.begin(), parameter_space_images.end(),
                   parameter_space_views.begin(),
                   [](gil::gray16_image_t& img)
                   {
                       return gil::view(img);
                   });

    gil::hough_circle_transform_brute(input, radius_parameter, x_parameter, y_parameter,
                                      parameter_space_views.begin(), rasterizer);
    std::cout << parameter_space_views[3](3, 3) << '\n';
}


// based off of the example provided at https://github.com/boostorg/histogram
//
#include <boost/histogram.hpp>
#include <boost/format.hpp> // used here for printing
#include <iostream>

int main() {
    using namespace boost::histogram;

    // make 1d histogram with 4 regular bins from 0 to 2
    auto h = make_histogram( axis::regular<>(4, 0.0, 2.0) );

    // push some values into the histogram
    for (auto&& value : { 0.4, 1.1, 0.3, 1.7, 10. })
      h(value);

    // iterate over bins
    for (auto&& x : indexed(h)) {
      std::cout << boost::format("bin %i [ %.1f, %.1f ): %i\n")
        % x.index() % x.bin().lower() % x.bin().upper() % *x;
    }

    std::cout << std::flush;

    /* program output:

    bin 0 [ 0.0, 0.5 ): 2
    bin 1 [ 0.5, 1.0 ): 0
    bin 2 [ 1.0, 1.5 ): 1
    bin 3 [ 1.5, 2.0 ): 1
    */
}


/***************************************************************************
 *   Software License Agreement (BSD License)                              *
 *   Copyright (C) 2017 by Florian Beck <florian.beck@tuwien.ac.at>        *
 *                                                                         *
 *   Redistribution and use in source and binary forms, with or without    *
 *   modification, are permitted provided that the following conditions    *
 *   are met:                                                              *
 *                                                                         *
 *   1. Redistributions of source code must retain the above copyright     *
 *      notice, this list of conditions and the following disclaimer.      *
 *   2. Redistributions in binary form must reproduce the above copyright  *
 *      notice, this list of conditions and the following disclaimer in    *
 *      the documentation and/or other materials provided with the         *
 *      distribution.                                                      *
 *   3. Neither the name of the copyright holder nor the names of its      *
 *      contributors may be used to endorse or promote products derived    *
 *      from this software without specific prior written permission.      *
 *                                                                         *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS   *
 *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT     *
 *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS     *
 *   FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE        *
 *   COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,  *
 *   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,  *
 *   BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;      *
 *   LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER      *
 *   CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT    *
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY *
 *   WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE           *
 *   POSSIBILITY OF SUCH DAMAGE.                                           *
 ***************************************************************************/

#include "mahalanobis_meas_model.h"
#include <boost/math/distributions/chi_squared.hpp>
#include <assert.h>

MahalanobisMeasModel::MahalanobisMeasModel(double cov_scale)
{
  cov_scale_ = cov_scale;
}

double MahalanobisMeasModel::getProbability(const Ref<const VectorXd>& curr_state, const Ref<const VectorXd>& meas, const Ref<const MatrixXd>& meas_cov, double dt)
{
  // simple measurement model, position directly observable (by detector)
  // state = [x, y, vx, vy]^T
  // z = [x, y]^T
  // C = [1, 0, 0, 0; 0, 1, 0, 0]

  // calculate mahalanobis distance from predicted measurement to actual measurement
  // note: consider current state as deterministic state with no covariance
  // only measurement provides covariance
  // mahalanobis distance is chi_square distributed

  double dist = sqrt((curr_state.block<2, 1>(0, 0) - meas).transpose() *
                    ((meas_cov.block<2, 2>(0, 0) * cov_scale_).inverse()) *
                    (curr_state.block<2, 1>(0, 0) - meas));

  //boost::math::normal norm = boost::math::normal(0, sigma_ * sigma_);
  boost::math::chi_squared chi(2);

  return boost::math::pdf(chi, dist);
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can 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 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef STENCILS_HPP
#define STENCILS_HPP

#include <functional>

#include "Config.hpp"

#include <Eigen/Core>

/*! \typedef DifferentiableFunction
 *  \brief sort of a function pointer to a function of a pair of vectors that can be numerically differentiated
 */
typedef pcm::function<double(const Eigen::Vector3d &, const Eigen::Vector3d &)> DifferentiableFunction;

/*! \brief Calculate directional derivative using a three-point stencil
 *  \param[in] func function to be differentiated
 *  \param[in] arg_1 first point, the directional derivative is calculated with respect to this point
 *  \param[in] arg_2 second point
 *  \param[in] direction the direction in which the directional derivative is to be evaluated
 *  \param[in] step finite difference value for the stencil
 */
inline double threePointStencil(const DifferentiableFunction & func,
        const Eigen::Vector3d & arg_1, const Eigen::Vector3d & arg_2,
        const Eigen::Vector3d & direction, double step = 1.0e-04)
{
    // f(x-h)
    Eigen::Vector3d delta_m1 = arg_1 - direction * step / direction.norm();
    // f(x+h)
    Eigen::Vector3d delta_1  = arg_1 + direction * step / direction.norm();

    Eigen::Vector2d stencil;
    stencil << -0.5, 0.5;
    Eigen::Vector2d function_values;
    function_values << func(delta_m1, arg_2), func(delta_1,  arg_2);

    return (function_values.dot(stencil) / step);
}

/*! \brief Calculate directional derivative using a five-point stencil
 *  \param[in] func function to be differentiated
 *  \param[in] arg_1 first point, the directional derivative is calculated with respect to this point
 *  \param[in] arg_2 second point
 *  \param[in] direction the direction in which the directional derivative is to be evaluated
 *  \param[in] step finite difference value for the stencil
 */
inline double fivePointStencil(const DifferentiableFunction & func,
        const Eigen::Vector3d & arg_1, const Eigen::Vector3d & arg_2,
        const Eigen::Vector3d & direction, double step = 1.0e-04)
{
    // f(x-2h)
    Eigen::Vector3d delta_m2 = arg_1 - 2.0 * direction * step / direction.norm();
    // f(x-h)
    Eigen::Vector3d delta_m1 = arg_1 - direction * step / direction.norm();
    // f(x+h)
    Eigen::Vector3d delta_1  = arg_1 + direction * step / direction.norm();
    // f(x+2h)
    Eigen::Vector3d delta_2  = arg_1 + 2.0 * direction * step / direction.norm();

    Eigen::Vector4d stencil;
    stencil << 1.0/12.0, -2.0/3.0, 2.0/3.0, -1.0/12.0;
    Eigen::Vector4d function_values;
    function_values << func(delta_m2, arg_2), func(delta_m1, arg_2),
                       func(delta_1,  arg_2), func(delta_2,  arg_2);

    return (function_values.dot(stencil) / step);
}

/*! \brief Calculate directional derivative using a seven-point stencil
 *  \param[in] func function to be differentiated
 *  \param[in] arg_1 first point, the directional derivative is calculated with respect to this point
 *  \param[in] arg_2 second point
 *  \param[in] direction the direction in which the directional derivative is to be evaluated
 *  \param[in] step finite difference value for the stencil
 */
inline double sevenPointStencil(const DifferentiableFunction & func,
        const Eigen::Vector3d & arg_1, const Eigen::Vector3d & arg_2,
        const Eigen::Vector3d & direction, double step = 1.0e-04)
{
    // f(x-3h)
    Eigen::Vector3d delta_m3 = arg_1 - 3.0 * direction * step / direction.norm();
    // f(x-2h)
    Eigen::Vector3d delta_m2 = arg_1 - 2.0 * direction * step / direction.norm();
    // f(x-h)
    Eigen::Vector3d delta_m1 = arg_1 - direction * step / direction.norm();
    // f(x+h)
    Eigen::Vector3d delta_1  = arg_1 + direction * step / direction.norm();
    // f(x+2h)
    Eigen::Vector3d delta_2  = arg_1 + 2.0 * direction * step / direction.norm();
    // f(x+3h)
    Eigen::Vector3d delta_3  = arg_1 + 3.0 * direction * step / direction.norm();

    Eigen::Matrix<double, 6, 1> stencil;
    stencil << -1.0/60.0, 3.0/20.0, -3.0/4.0, 3.0/4.0, -3.0/20.0, 1.0/60.0;
    Eigen::Matrix<double, 6, 1> function_values;
    function_values << func(delta_m3, arg_2), func(delta_m2, arg_2), func(delta_m1, arg_2),
                       func(delta_1,  arg_2), func(delta_2,  arg_2), func(delta_3, arg_2);

    return (function_values.dot(stencil) / step);
}

#endif // STENCILS_HPP


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

#include <almath/tools/altrigonometry.h>
#include <almath/tools/almathio.h>
#include <almath/tools/altransformhelpers.h>
#include <boost/math/constants/constants.hpp>
#include <gtest/gtest.h>
#include <stdexcept>

TEST(ALMathTest, moduloPI)
{
  float epsilon = 0.001f;
  float pAngle  = 0.0f;

  // case 0
  pAngle = 0.0f;
  AL::Math::modulo2PIInPlace(pAngle);
  EXPECT_NEAR(pAngle, 0.0f, epsilon);

  pAngle = 0.99f*AL::Math::PI;
  AL::Math::modulo2PIInPlace(pAngle);
  EXPECT_NEAR(pAngle, 0.99f*AL::Math::PI, epsilon);

  pAngle = -0.99f*AL::Math::PI;
  AL::Math::modulo2PIInPlace(pAngle);
  EXPECT_NEAR(pAngle, -0.99f*AL::Math::PI, epsilon);

  // case 1
  pAngle = AL::Math::PI + 0.5f;
  AL::Math::modulo2PIInPlace(pAngle);
  EXPECT_NEAR(pAngle, 0.5f-AL::Math::PI, epsilon);

  pAngle = -AL::Math::PI - 0.5f;
  AL::Math::modulo2PIInPlace(pAngle);
  EXPECT_NEAR(pAngle, -0.5f+AL::Math::PI, epsilon);

  // case 2
  pAngle  = 10.0f*AL::Math::PI + 0.5f;
  AL::Math::modulo2PIInPlace(pAngle);
  EXPECT_NEAR(pAngle, 0.5f, epsilon);

  pAngle  = -10.0f*AL::Math::PI - 0.5f;
  AL::Math::modulo2PIInPlace(pAngle);
  EXPECT_NEAR(pAngle, -0.5f, epsilon);

  // case 3
  pAngle  = 11.0f*AL::Math::PI + 0.5f;
  AL::Math::modulo2PIInPlace(pAngle);
  EXPECT_NEAR(pAngle, 0.5f-AL::Math::PI, epsilon);

  pAngle  = -11.0f*AL::Math::PI - 0.5f;
  AL::Math::modulo2PIInPlace(pAngle);
  EXPECT_NEAR(pAngle, -0.5f+AL::Math::PI, epsilon);

  pAngle = 1.0f;
  EXPECT_NEAR(AL::Math::modulo2PI(pAngle), 1.0f, 1e-4f);
  pAngle = 2 * AL::Math::PI;
  EXPECT_NEAR(AL::Math::modulo2PI(pAngle), 0.0f, 1e-4f);
  pAngle = -5 * AL::Math::PI_2;
  EXPECT_NEAR(AL::Math::modulo2PI(pAngle), -AL::Math::PI_2, 1e-4f);
  pAngle = 3 * AL::Math::PI_2;
  EXPECT_NEAR(AL::Math::modulo2PI(pAngle), -AL::Math::PI_2, 1e-4f);
  pAngle = AL::Math::PI;
  EXPECT_NEAR(AL::Math::modulo2PI(pAngle), AL::Math::PI, 1e-4f);
}

TEST(ALMathTest, mean) {
  EXPECT_THROW(AL::Math::meanAngle(std::vector<float>()),
               std::runtime_error);
  EXPECT_THROW(AL::Math::weightedMeanAngle(std::vector<float>(),
                                           std::vector<float>()),
               std::runtime_error);
  std::vector<float> angles;
  std::vector<float> weights;
  angles.push_back(AL::Math::PI);
  EXPECT_NEAR(AL::Math::modulo2PI(AL::Math::meanAngle(angles) - AL::Math::PI),
              0.f, 1e-3f);
  EXPECT_THROW(AL::Math::weightedMeanAngle(angles, weights),
               std::runtime_error);
  weights.push_back(0.5f);
  EXPECT_NEAR(AL::Math::modulo2PI(AL::Math::weightedMeanAngle(angles, weights)
                                  - AL::Math::PI),
              0.f, 1e-3f);
  angles.push_back(-AL::Math::PI);
  weights.push_back(0.5f);
  EXPECT_NEAR(AL::Math::modulo2PI(AL::Math::meanAngle(angles) - AL::Math::PI),
              0.f, 1e-3f);
  EXPECT_NEAR(AL::Math::modulo2PI(AL::Math::weightedMeanAngle(angles, weights)
                                  - AL::Math::PI),
              0.f, 1e-3f);
  angles[0] = AL::Math::PI_2;
  EXPECT_NEAR(AL::Math::modulo2PI(AL::Math::meanAngle(angles) - 0.75f*AL::Math::PI),
              0.f, 1e-3f);
  EXPECT_NEAR(AL::Math::modulo2PI(AL::Math::weightedMeanAngle(angles, weights)
                                  - 0.75f*AL::Math::PI),
              0.f, 1e-3f);
  angles[0] = 0.f;
  EXPECT_THROW(AL::Math::meanAngle(angles), std::runtime_error);
  EXPECT_THROW(AL::Math::weightedMeanAngle(angles, weights), std::runtime_error);
  angles[0] = 0.f;
  weights[0] = 1.f;
  angles[1] = 0.5f*AL::Math::PI;
  weights[1] = 0.5f;
  EXPECT_NEAR(AL::Math::weightedMeanAngle(angles, weights),
              std::atan(0.5f), 1e-3f);
  weights[1] = 0.f;
  EXPECT_THROW(AL::Math::weightedMeanAngle(angles, weights),
              std::exception);
  weights[1] = -1.f;
  EXPECT_THROW(AL::Math::weightedMeanAngle(angles, weights),
               std::runtime_error);
}

TEST(ALMathTest, clipData)
{
  float pMin  = -0.2f;
  float pMax  = 0.5f;
  float pData = 0.4f;
  EXPECT_FALSE(AL::Math::clipData(pMin, pMax, pData));

  pMin  = -0.2f;
  pMax  = 0.5f;
  pData = 0.6f;
  EXPECT_TRUE(AL::Math::clipData(pMin, pMax, pData));
  EXPECT_NEAR(pData, pMax, 0.0001f);

  pMin  = -0.2f;
  pMax  = 0.5f;
  pData = -0.1f;
  EXPECT_FALSE(AL::Math::clipData(pMin, pMax, pData));

  pMin  = -0.2f;
  pMax  = 0.5f;
  pData = -0.3f;
  EXPECT_TRUE(AL::Math::clipData(pMin, pMax, pData));
  EXPECT_NEAR(pData, pMin, 0.0001f);

  pData = 2.1f;
  EXPECT_TRUE(AL::Math::clipData(1, 2, pData));
  EXPECT_NEAR(pData, static_cast<float>(2), 0.0001f);
}

TEST(ALMathTest, clipDataVector)
{
  const float lEpsilon = 0.0001f;
  std::vector<float> data(5, -1.0f);
  bool isClipped = AL::Math::clipData(0.0f, 1.0f, data);
  EXPECT_TRUE(isClipped);
  for (unsigned int i=0; i<data.size(); ++i)
  {
    EXPECT_NEAR(data[i], 0.0f, lEpsilon);
  }

  data.clear();
  data.push_back(-1.0f);
  data.push_back(1.0f);
  data.push_back(2.0f);
  data.push_back(-0.5f);
  data.push_back(3.0f);
  std::vector<float> expectedData;
  expectedData.push_back(0.0f);
  expectedData.push_back(1.0f);
  expectedData.push_back(1.0f);
  expectedData.push_back(0.0f);
  expectedData.push_back(1.0f);

  isClipped = AL::Math::clipData(0.0f, 1.0f, data);
  EXPECT_TRUE(data.size()==expectedData.size());
  EXPECT_TRUE(isClipped);
  for (unsigned int i=0; i<data.size(); ++i)
  {
    EXPECT_NEAR(data[i], expectedData[i], lEpsilon);
  }

  data.clear();
  data.push_back(0.0f);
  data.push_back(0.1f);
  data.push_back(0.2f);
  data.push_back(0.3f);
  data.push_back(0.4f);
  expectedData = data;
  isClipped = AL::Math::clipData(0.0f, 1.0f, data);
  EXPECT_TRUE(data.size()==expectedData.size());
  EXPECT_FALSE(isClipped);
  for (unsigned int i=0; i<data.size(); ++i)
  {
    EXPECT_NEAR(data[i], expectedData[i], lEpsilon);
  }
}

TEST(ALMathTest, clipDataVectorVector)
{
  const float lEpsilon = 0.0001f;
  std::vector<float> data(5, -1.0f);
  std::vector<std::vector<float> > dataList(5, data);
  bool isClipped = AL::Math::clipData(0.0f, 1.0f, dataList);
  EXPECT_TRUE(isClipped);
  for (unsigned int i=0; i<dataList.size(); ++i)
  {
    for (unsigned int j=0; j<dataList[i].size(); ++j)
    {
      EXPECT_NEAR(dataList[i][j], 0.0f, lEpsilon);
    }
  }
}

TEST(ALMathTest, changeReferencePose2D)
{
  float pTheta = 90.0f*AL::Math::TO_RAD;
  AL::Math::Pose2D pPosIn;
  AL::Math::Pose2D pPosOut;

  pPosIn = AL::Math::Pose2D(10.0f, 0.0f, 0.5f);
  AL::Math::changeReferencePose2D(pTheta, pPosIn, pPosOut);
  EXPECT_TRUE(pPosOut.isNear(AL::Math::Pose2D(0.0f, 10.0f, 0.5f), 0.001f));

  pPosIn = AL::Math::Pose2D(0.0f, 10.0f, 0.5f);
  AL::Math::changeReferencePose2D(pTheta, pPosIn, pPosOut);
  EXPECT_TRUE(pPosOut.isNear(AL::Math::Pose2D(-10.0f, 0.0f, 0.5f), 0.001f));

  pPosIn = AL::Math::Pose2D(-10.0f, 0.0f, 0.5f);
  AL::Math::changeReferencePose2D(pTheta, pPosIn, pPosOut);
  EXPECT_TRUE(pPosOut.isNear(AL::Math::Pose2D(0.0f, -10.0f, 0.5f), 0.001f));

  pPosIn = AL::Math::Pose2D(0.0f, -10.0f, 0.5f);
  AL::Math::changeReferencePose2D(pTheta, pPosIn, pPosOut);
  EXPECT_TRUE(pPosOut.isNear(AL::Math::Pose2D(10.0f, 0.0f, 0.5f), 0.001f));
}

TEST(ALMathTest, changeReferencePose2DInPlace)
{
  float pTheta = 90.0f*AL::Math::TO_RAD;
  AL::Math::Pose2D pPosIn;

  pPosIn = AL::Math::Pose2D(10.0f, 0.0f, 0.5f);
  AL::Math::changeReferencePose2DInPlace(pTheta, pPosIn);
  EXPECT_TRUE(pPosIn.isNear(AL::Math::Pose2D(0.0f, 10.0f, 0.5f), 0.001f));

  pPosIn = AL::Math::Pose2D(0.0f, 10.0f, 0.5f);
  AL::Math::changeReferencePose2DInPlace(pTheta, pPosIn);
  EXPECT_TRUE(pPosIn.isNear(AL::Math::Pose2D(-10.0f, 0.0f, 0.5f), 0.001f));

  pPosIn = AL::Math::Pose2D(-10.0f, 0.0f, 0.5f);
  AL::Math::changeReferencePose2DInPlace(pTheta, pPosIn);
  EXPECT_TRUE(pPosIn.isNear(AL::Math::Pose2D(0.0f, -10.0f, 0.5f), 0.001f));

  pPosIn = AL::Math::Pose2D(0.0f, -10.0f, 0.5f);
  AL::Math::changeReferencePose2DInPlace(pTheta, pPosIn);
  EXPECT_TRUE(pPosIn.isNear(AL::Math::Pose2D(10.0f, 0.0f, 0.5f), 0.001f));
}

TEST(ALMathTest, position3DFromPosition6D)
{
  AL::Math::Position6D pPose6d        = AL::Math::Position6D(0.1f, 0.2f, 0.3f, 0.4f, 0.5f, 0.6f);
  AL::Math::Position3D pPos3dExpected = AL::Math::Position3D(0.1f, 0.2f, 0.3f);

  AL::Math::Position3D pPose3dResult = AL::Math::position3DFromPosition6D(pPose6d);

  EXPECT_TRUE(pPose3dResult.isNear(pPos3dExpected, 0.0001f));
}

TEST(ALMathTest, position2DFromPose2D)
{
  const AL::Math::Pose2D pPose2d(0.1f, 0.2f, 0.3f);
  const AL::Math::Position2D pPos2dExpected(0.1f, 0.2f);

  const AL::Math::Position2D& pPose2dResult =
      AL::Math::position2DFromPose2D(pPose2d);

  EXPECT_TRUE(pPose2dResult.isNear(pPos2dExpected, 0.0001f));
}

TEST(ALMathTest, pose2DFromPosition2DInPlace)
{
  const AL::Math::Position2D pPosition2d(0.1f, 0.2f);
  const AL::Math::Pose2D pPos2dExpected(0.1f, 0.2f, 0.5f);

  AL::Math::Pose2D pPose2dResult = AL::Math::Pose2D(10.0f, 20.0f, 30.0f);
  AL::Math::pose2DFromPosition2DInPlace(pPosition2d, 0.5f, pPose2dResult);

  EXPECT_TRUE(pPose2dResult.isNear(pPos2dExpected, 0.0001f));
}

TEST(ALMathTest, pose2DFromPosition2D)
{
  const AL::Math::Position2D pPosition2d(0.1f, 0.2f);
  const AL::Math::Pose2D pPos2dExpected(0.1f, 0.2f, 0.0f);

  const AL::Math::Pose2D& pPose2dResult =
      AL::Math::pose2DFromPosition2D(pPosition2d);

  EXPECT_TRUE(pPose2dResult.isNear(pPos2dExpected, 0.0001f));

  const AL::Math::Position2D pPosition2d2(0.1f, 0.2f);
  const AL::Math::Pose2D pPos2dExpected2(0.1f, 0.2f, 0.5f);

  const AL::Math::Pose2D& pPose2dResult2 =
      AL::Math::pose2DFromPosition2D(pPosition2d2, 0.5f);

  EXPECT_TRUE(pPose2dResult2.isNear(pPos2dExpected2, 0.0001f));
}

TEST(ALMathTest, Position6DFromVelocity6D)
{
  const AL::Math::Velocity6D pVIn =
      AL::Math::Velocity6D(0.1f, 0.2f, 0.3f, 0.4f, 0.5f, 0.6f);
  const AL::Math::Position6D pPosIn =
      AL::Math::position6DFromVelocity6D(pVIn);

  const AL::Math::Position6D pPosOut =
      AL::Math::Position6D(0.1f, 0.2f, 0.3f, 0.4f, 0.5f, 0.6f);

  EXPECT_TRUE(pPosIn.isNear(pPosOut, 0.0001f));
}

TEST(ALMathTest, position2DFromPose2DInPlace)
{
  const AL::Math::Pose2D pose2D = AL::Math::Pose2D(1.0f, 2.0f, 3.0f);
  AL::Math::Position2D position2D = AL::Math::Position2D(10.0f, 20.0f);

  AL::Math::position2DFromPose2DInPlace(pose2D, position2D);
  EXPECT_TRUE(position2D.isNear(AL::Math::Position2D(1.0f, 2.0f), 0.0001f));
}

TEST(ALMathTest, variousOperator)
{
  //inline Position3D operator*(
  //  const Rotation&   pRot,
  //  const Position3D& pPos)
  AL::Math::Rotation   pRot;
  AL::Math::Position3D pPos3D;
  AL::Math::Position3D pPosIn = pRot*pPos3D;
  AL::Math::Position3D pPosOut = AL::Math::Position3D();
  EXPECT_TRUE(pPosIn.isNear(pPosOut, 0.0001f));

  pRot = AL::Math::Rotation();
  pPos3D = AL::Math::Position3D(1.0f, -1.0f, 0.5f);
  pPosIn = pRot*pPos3D;
  pPosOut = AL::Math::Position3D(1.0f, -1.0f, 0.5f);
  EXPECT_TRUE(pPosIn.isNear(pPosOut, 0.0001f));

  pRot = AL::Math::Rotation::fromRotX(0.5f);
  pPos3D = AL::Math::Position3D(1.0f, -1.0f, 0.5f);
  pPosIn = pRot*pPos3D;
  pPosOut = AL::Math::Position3D(1.00000000000000f, -1.11729533119247f, -0.04063425765902f);
  EXPECT_TRUE(pPosIn.isNear(pPosOut, 0.0001f));

  //inline Velocity6D operator*(
  //  const float       pK,
  //  const Position6D& pDelta)
  float pK = 10.0f;
  AL::Math::Position6D pPos6D = AL::Math::Position6D(1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f);
  AL::Math::Velocity6D pVel6DIn = pK*pPos6D;
  AL::Math::Velocity6D pVel6DOut = AL::Math::Velocity6D(10.0f, 20.0f, 30.0f, 40.0f, 50.0f, 60.0f);
  EXPECT_TRUE(pVel6DIn.isNear(pVel6DOut, 0.0001f));

  //Velocity3D operator* (const Rotation&   pRot, const Velocity3D& pVel);
  AL::Math::Velocity3D pVIn1 = AL::Math::Velocity3D(0.5f, 0.3f, 0.1f);
  AL::Math::Rotation   pRot1 = AL::Math::rotationFromRotZ(AL::Math::PI_2);
  AL::Math::Velocity3D pVIn2 = pRot1*pVIn1;
  AL::Math::Velocity3D pVOut = AL::Math::Velocity3D(-0.3f, 0.5f, 0.1f);
  EXPECT_TRUE(pVIn2.isNear(pVOut, 0.0001f));

}

TEST(ALMathTest, isLeft)
{
}

TEST(ALMathTest, FilterPosition6D)
{
}

TEST(ALMathTest, AxisMaskToPosition6DOn)
{
}

TEST(ALMathTest, AxisMaskToPosition6DOff)
{
}

TEST(ALMathTest, AxisMaskToVelocity6DOn)
{
}

TEST(ALMathTest, RotationFromAngleDirection)
{
  AL::Math::Rotation pRotOut;
  float pTheta = 0.0f;
  AL::Math::Position3D pPos = AL::Math::Position3D(0.0f, 0.0f, 0.0f);

  // ****** test 0 ****** //
  ASSERT_THROW(AL::Math::rotationFromAngleDirection(pTheta, pPos), std::runtime_error);

  // ****** test 1 ****** //
  pTheta = 0.0f;
  pPos = AL::Math::Position3D(1.0f, 0.0f, 0.0f);
  pRotOut = AL::Math::rotationFromAngleDirection(pTheta, pPos);
  EXPECT_TRUE(pRotOut.isNear(AL::Math::Rotation()));

  // ****** test 2 ****** //
  pTheta = 0.0f;
  pPos = AL::Math::Position3D(0.0f, 1.0f, 0.0f);
  pRotOut = AL::Math::rotationFromAngleDirection(pTheta, pPos);
  EXPECT_TRUE(pRotOut.isNear(AL::Math::Rotation()));

  // ****** test 3 ****** //
  pTheta = 0.0f;
  pPos = AL::Math::Position3D(0.0f, 0.0f, 1.0f);
  pRotOut = AL::Math::rotationFromAngleDirection(pTheta, pPos);
  EXPECT_TRUE(pRotOut.isNear(AL::Math::Rotation()));

  // ****** test 4 ****** //
  pTheta = 15.0f*AL::Math::TO_RAD;
  pPos = AL::Math::Position3D(1.0f, 0.0f, 0.0f);
  pRotOut = AL::Math::rotationFromAngleDirection(pTheta, pPos);
  EXPECT_TRUE(pRotOut.isNear(AL::Math::Rotation::fromRotX(pTheta)));
  EXPECT_FALSE(pRotOut.isNear(AL::Math::Rotation()));

  // ****** test 5 ****** //
  pTheta = 15.0f*AL::Math::TO_RAD;
  pPos = AL::Math::Position3D(0.0f, 1.0f, 0.0f);
  pRotOut = AL::Math::rotationFromAngleDirection(pTheta, pPos);
  EXPECT_TRUE(pRotOut.isNear(AL::Math::Rotation::fromRotY(pTheta)));
  EXPECT_FALSE(pRotOut.isNear(AL::Math::Rotation()));

  // ****** test 6 ****** //
  pTheta = 15.0f*AL::Math::TO_RAD;
  pPos = AL::Math::Position3D(0.0f, 0.0f, 1.0f);
  pRotOut = AL::Math::rotationFromAngleDirection(pTheta, pPos);
  EXPECT_TRUE(pRotOut.isNear(AL::Math::Rotation::fromRotZ(pTheta)));
  EXPECT_FALSE(pRotOut.isNear(AL::Math::Rotation()));

  EXPECT_TRUE(AL::Math::Rotation::fromRotXPi().isNear(
                  AL::Math::Rotation::fromRotX(
                     boost::math::constants::pi<float>())));
  EXPECT_TRUE(AL::Math::Rotation::fromRotYPi().isNear(
                  AL::Math::Rotation::fromRotY(
                     boost::math::constants::pi<float>())));
  EXPECT_TRUE(AL::Math::Rotation::fromRotZPi().isNear(
                  AL::Math::Rotation::fromRotZ(
                     boost::math::constants::pi<float>())));
  EXPECT_TRUE(AL::Math::Rotation::fromRotZHalfPi().isNear(
                  AL::Math::Rotation::fromRotZ(
                     0.5f * boost::math::constants::pi<float>())));
}

TEST(ALMathTest, quaternionOperator)
{
  AL::Math::Quaternion pQuat;
  AL::Math::Position3D pPos;
  AL::Math::Position3D result;
  result = pQuat * pPos;
  EXPECT_NEAR(pPos.x, 0.0f, 1e-6f);
  EXPECT_NEAR(pPos.y, 0.0f, 1e-6f);
  EXPECT_NEAR(pPos.z, 0.0f, 1e-6f);

  // Test 90° rotation around the x axis.
  pQuat = AL::Math::Quaternion(0.7071f, 0.7071f, 0.0f, 0.0f);
  pPos = AL::Math::Position3D(0.0f, 1.0f, 0.0f);
  result = pQuat * pPos;
  EXPECT_NEAR(result.x, 0.0f, 1e-4f);
  EXPECT_NEAR(result.y, 0.0f, 1e-4f);
  EXPECT_NEAR(result.z, 1.0f, 1e-4f);

  // Test 90° rotation around the y axis.
  pQuat = AL::Math::Quaternion(0.7071f, 0.0f, 0.7071f, 0.0f);
  pPos = AL::Math::Position3D(0.0f, 0.0f, 1.0f);
  result = pQuat * pPos;
  EXPECT_NEAR(result.x, 1.0f, 1e-4f);
  EXPECT_NEAR(result.y, 0.0f, 1e-4f);
  EXPECT_NEAR(result.z, 0.0f, 1e-4f);

  // Test 90° rotation around the z axis.
  pQuat = AL::Math::Quaternion(0.7071f, 0.0f, 0.0f, 0.7071f);
  pPos = AL::Math::Position3D(1.0f, 0.0f, 0.0f);
  result = pQuat * pPos;
  EXPECT_NEAR(result.x, 0.0f, 1e-4f);
  EXPECT_NEAR(result.y, 1.0f, 1e-4f);
  EXPECT_NEAR(result.z, 0.0f, 1e-4f);

}

TEST(ALMathTest, position6DFromPose2DInPlace)
{
  const AL::Math::Pose2D pPose2d = AL::Math::Pose2D(0.1f, 0.2f, 0.3f);
  const AL::Math::Position6D pPose6dExpected =
      AL::Math::Position6D(0.1f, 0.2f, 0.0f, 0.0f, 0.0f, 0.3f);

  AL::Math::Position6D pPose6dComputed =
      AL::Math::Position6D(10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f);
  AL::Math::position6DFromPose2DInPlace(
        pPose2d,
        pPose6dComputed);

  EXPECT_TRUE(pPose6dComputed.isNear(pPose6dExpected, 0.0001f));
}

TEST(ALMathTest, position6DFromPose2D)
{
  AL::Math::Pose2D pPose2d = AL::Math::Pose2D(0.1f, 0.2f, 0.3f);
  AL::Math::Position6D pPose6dExpected = AL::Math::Position6D(0.1f, 0.2f, 0.0f, 0.0f, 0.0f, 0.3f);

  AL::Math::Position6D pPose6dComputed = AL::Math::position6DFromPose2D(pPose2d);

  EXPECT_TRUE(pPose6dComputed.isNear(pPose6dExpected, 0.0001f));
}

TEST(ALMathTest, pose2DFromPosition6DInPlace)
{
  AL::Math::Position6D pPose6d = AL::Math::Position6D(0.1f, 0.2f, 0.3f, 0.4f, 0.5f, 0.6f);
  AL::Math::Pose2D pPose2dExpected = AL::Math::Pose2D(0.1f, 0.2f, 0.6f);

  AL::Math::Pose2D pPose2dComputed;
  AL::Math::pose2DFromPosition6DInPlace(pPose6d, pPose2dComputed);

  EXPECT_TRUE(pPose2dComputed.isNear(pPose2dExpected, 0.0001f));
}

TEST(ALMathTest, pose2DFromPosition6D)
{
  AL::Math::Position6D pPose6d = AL::Math::Position6D(0.1f, 0.2f, 0.3f, 0.4f, 0.5f, 0.6f);
  AL::Math::Pose2D pPose2dExpected = AL::Math::Pose2D(0.1f, 0.2f, 0.6f);

  AL::Math::Pose2D pPose2dComputed = AL::Math::pose2DFromPosition6D(pPose6d);

  EXPECT_TRUE(pPose2dComputed.isNear(pPose2dExpected, 0.0001f));
}

TEST(ALMathTest, position6DFromPosition3DInPlace)
{
  const AL::Math::Position3D position3D =
      AL::Math::Position3D(0.1f, 0.2f, 0.3f);

  AL::Math::Position6D position6D =
      AL::Math::Position6D(10.1f, 10.2f, 10.0f, 10.0f, 10.0f, 10.3f);

  AL::Math::position6DFromPosition3DInPlace(position3D, position6D);

  EXPECT_TRUE(position6D.isNear(
                AL::Math::Position6D(0.1f, 0.2f, 0.3f, 0.0f, 0.0f, 0.0f), 0.0001f));
}

TEST(ALMathTest, position6DFromPosition3D)
{
  const AL::Math::Position3D position3D =
      AL::Math::Position3D(0.1f, 0.2f, 0.3f);

  const AL::Math::Position6D position6D =
      AL::Math::position6DFromPosition3D(position3D);

  EXPECT_TRUE(position6D.isNear(
                AL::Math::Position6D(0.1f, 0.2f, 0.3f, 0.0f, 0.0f, 0.0f), 0.0001f));
}

TEST(ALMathTest, multiplicationPose2DPosition2D)
{
  AL::Math::Pose2D pVal;
  AL::Math::Position2D pPos;
  AL::Math::Position2D pExpected;
  AL::Math::Position2D pResult;

  pVal      = AL::Math::Pose2D(0.0f, 0.0f, AL::Math::PI_2);
  pPos      = AL::Math::Position2D(1.0f, 0.0f);
  pExpected = AL::Math::Position2D(0.0f, 1.0f);
  pResult   = pVal*pPos;
  EXPECT_TRUE(pResult.isNear(pExpected, 0.0001f));

  pVal      = AL::Math::Pose2D(0.0f, 0.0f, AL::Math::PI_2);
  pPos      = AL::Math::Position2D(0.0f, 1.0f);
  pExpected = AL::Math::Position2D(-1.0f, 0.0f);
  pResult   = pVal*pPos;
  EXPECT_TRUE(pResult.isNear(pExpected, 0.0001f));

  pVal      = AL::Math::Pose2D(1.0f, 1.0f, -AL::Math::PI_2);
  pPos      = AL::Math::Position2D(1.0f, 0.0f);
  pExpected = AL::Math::Position2D(1.0f, 0.0f);
  pResult   = pVal*pPos;
  EXPECT_TRUE(pResult.isNear(pExpected, 0.0001f));
}

TEST(ALMathTest, quaternionFromRotation3D)
{
  for (unsigned int i=0; i<360; ++i)
  {
    const float angleX = static_cast<float>(i)*AL::Math::TO_RAD;
    const AL::Math::Quaternion quatX =
        AL::Math::quaternionFromAngleAndAxisRotation(angleX, 1.0f, 0.0f, 0.0f);
    for (unsigned int j=0; j<360; ++j)
    {
      const float angleY = static_cast<float>(j)*AL::Math::TO_RAD;
      const AL::Math::Quaternion quatY =
          AL::Math::quaternionFromAngleAndAxisRotation(angleY, 0.0f, 1.0f, 0.0f);
      for (unsigned int k=0; k<360; ++k)
      {
        const float angleZ = static_cast<float>(k)*AL::Math::TO_RAD;
        const AL::Math::Quaternion quatZ =
            AL::Math::quaternionFromAngleAndAxisRotation(angleZ, 0.0f, 0.0f, 1.0f);

        const AL::Math::Quaternion quatExpected = quatZ*quatY*quatX;
        const AL::Math::Rotation3D rot3DZYX(angleX, angleY, angleZ);
        const AL::Math::Quaternion quatResult =
            AL::Math::quaternionFromRotation3D(rot3DZYX);
        EXPECT_TRUE(quatResult.isNear(quatExpected, 0.001f));
      }
    }
  }
}

TEST(ALMathTest, rotation3DFromQuaternion1)
{
  const float lAngleX = 0.5f;
  const float lAngleY = -0.3f;
  AL::Math::Quaternion quat =
      AL::Math::quaternionFromAngleAndAxisRotation(lAngleY, 0.0f, 1.0f , 0.0f)*
      AL::Math::quaternionFromAngleAndAxisRotation(lAngleX, 1.0f, 0.0f , 0.0f);

  AL::Math::Rotation3D lRotation3D;
  AL::Math::rotation3DFromQuaternion(quat, lRotation3D);
  EXPECT_NEAR(lRotation3D.wx, lAngleX, 0.0001f);
  EXPECT_NEAR(lRotation3D.wy, lAngleY, 0.0001f);

  AL::Math::Quaternion lQuat;
  AL::Math::quaternionFromRotation3D(lRotation3D, lQuat);
  EXPECT_NEAR(lQuat.w, quat.w, 0.0001f);
  EXPECT_NEAR(lQuat.x, quat.x, 0.0001f);
  EXPECT_NEAR(lQuat.y, quat.y, 0.0001f);
  EXPECT_NEAR(lQuat.z, quat.z, 0.0001f);

  // new test jory
  AL::Math::Rotation3D rotation3DRef(0.0213f, 0.0139f, 0.02619f);
  quat = AL::Math::Quaternion(0.99983f, 0.01056f, 0.00712f, 0.01302f);
  quat = quat.normalize();
  AL::Math::Rotation3D rotation3D;
  AL::Math::rotation3DFromQuaternion(quat, rotation3D);
  EXPECT_TRUE(rotation3D.isNear(rotation3DRef, 0.0001f));

  rotation3DRef = AL::Math::Rotation3D(-0.00350f, 0.0f, 0.0314f);
  quat = AL::Math::Quaternion(0.99988f, -0.00175f,
                              -0.00003f, 0.01571f);

  AL::Math::rotation3DFromQuaternion(quat, rotation3D);

  EXPECT_TRUE(rotation3D.isNear(rotation3DRef, 0.0001f));

  const float wx = -180.0f*AL::Math::TO_RAD;
  const float wy = -90.0f*AL::Math::TO_RAD;
  const float wz = -175.0f*AL::Math::TO_RAD;
  quat =
      AL::Math::quaternionFromAngleAndAxisRotation(wz, 0.0f, 0.0f, 1.0f)*
      AL::Math::quaternionFromAngleAndAxisRotation(wy, 0.0f, 1.0f, 0.0f)*
      AL::Math::quaternionFromAngleAndAxisRotation(wx, 1.0f, 0.0f, 0.0f);
  const AL::Math::Rotation3D rot3D = AL::Math::rotation3DFromQuaternion(quat);
  EXPECT_TRUE(rot3D.wx == rot3D.wx);
  EXPECT_TRUE(rot3D.wy == rot3D.wy);
  EXPECT_TRUE(rot3D.wz == rot3D.wz);
}

TEST(ALMathTest, rotation3DFromQuaternion2)
{
  // function quaternionFromRotation3D must be check before
  for (int i=-180; i<180; ++i)
  {
    const float angleX = static_cast<float>(i)*AL::Math::TO_RAD;
    const AL::Math::Quaternion quatX =
        AL::Math::quaternionFromAngleAndAxisRotation(angleX, 1.0f, 0.0f, 0.0f);
    for (int j=-180; j<180; ++j)
    {
      const float angleY = static_cast<float>(j)*AL::Math::TO_RAD;
      const AL::Math::Quaternion quatY =
          AL::Math::quaternionFromAngleAndAxisRotation(angleY, 0.0f, 1.0f, 0.0f);
      for (int k=-180; k<180; ++k)
      {
        const float angleZ = static_cast<float>(k)*AL::Math::TO_RAD;
        const AL::Math::Quaternion quatZ =
            AL::Math::quaternionFromAngleAndAxisRotation(angleZ, 0.0f, 0.0f, 1.0f);

        const AL::Math::Quaternion quatExpected = quatZ*quatY*quatX;
        const AL::Math::Rotation3D rot3D =
            AL::Math::rotation3DFromQuaternion(quatExpected);

        const AL::Math::Quaternion quatResult =
            AL::Math::quaternionFromRotation3D(rot3D);

        EXPECT_TRUE(quatExpected.isNear(quatResult, 0.001f));
      }
    }
  }
}

TEST(ALMathTest, quaternionPosition3DFromPosition6D)
{
  // function quaternionFromRotation3D must be check before
  const float lEpsilon = 0.001f;
  const AL::Math::Position6D pos6D =
      AL::Math::Position6D(0.1f, 0.2f, 0.3f, 0.4f, 0.5f, 0.6f);
  AL::Math::Quaternion qua;
  AL::Math::Position3D pos3D;

  AL::Math::quaternionPosition3DFromPosition6D(pos6D, qua, pos3D);

  const AL::Math::Quaternion quaExpected =
      AL::Math::quaternionFromRotation3D(AL::Math::Rotation3D(0.4f, 0.5f, 0.6f));
  EXPECT_TRUE(qua.isNear(quaExpected, lEpsilon));
  EXPECT_TRUE(pos3D.isNear(AL::Math::Position3D(0.1f, 0.2f, 0.3f), lEpsilon));
}

TEST(ALMathTest, pointMassRotationalInertia)
{
  AL::Math::Position3D pos123(1.f, 2.f, 3.f);
  std::vector<float> inertia;

  pointMassRotationalInertia(0.f, pos123, inertia);
  ASSERT_EQ(9, inertia.size());
  ASSERT_EQ(std::vector<float>(9, 0.f), inertia);

  pointMassRotationalInertia(1.f, AL::Math::Position3D(), inertia);
  ASSERT_EQ(std::vector<float>(9, 0.f), inertia);

  float m10 = 10.f;
  pointMassRotationalInertia(m10, pos123, inertia);
  // check symmetry
  ASSERT_TRUE(inertia[1] == inertia[3]);
  ASSERT_TRUE(inertia[2] == inertia[6]);
  ASSERT_TRUE(inertia[5] == inertia[7]);
  // check values
  ASSERT_EQ(m10*(4+9), inertia[0]);
  ASSERT_EQ(m10*(1+9), inertia[4]);
  ASSERT_EQ(m10*(1+4), inertia[8]);
  ASSERT_EQ(-m10*1*2, inertia[1]);
  ASSERT_EQ(-m10*1*3, inertia[2]);
  ASSERT_EQ(-m10*2*3, inertia[5]);
}


// This example consists of a single static target which sits in one
// place and does not move; in effect it has a "GPS" which measures
// its position

#include <Eigen/StdVector>
#include <iostream>
#include <stdint.h>

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/stuff/sampler.h>

#include "targetTypes3D.hpp"

using namespace Eigen;
using namespace std;
using namespace g2o;

int main()
{
  // Set up the optimiser
  SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  // Create the block solver - the dimensions are specified because
  // 3D observations marginalise to a 3D estimate
  typedef BlockSolver<BlockSolverTraits<3, 3> > BlockSolver_3_3;
  BlockSolver_3_3::LinearSolverType* linearSolver
      = new LinearSolverCholmod<BlockSolver_3_3::PoseMatrixType>();
  BlockSolver_3_3* blockSolver
      = new BlockSolver_3_3(linearSolver);
  OptimizationAlgorithmGaussNewton* solver
    = new OptimizationAlgorithmGaussNewton(blockSolver);
  optimizer.setAlgorithm(solver);

  // Sample the actual location of the target
  Vector3d truePoint(sampleUniform(-500, 500),
                     sampleUniform(-500, 500),
                     sampleUniform(-500, 500));

  // Construct vertex which corresponds to the actual point of the target
  VertexPosition3D* position = new VertexPosition3D();
  position->setId(0);
  optimizer.addVertex(position);

  // Now generate some noise corrupted measurements; for simplicity
  // these are uniformly distributed about the true target. These are
  // modelled as a unary edge because they do not like to, say,
  // another node in the map.
  int numMeasurements = 10;
  double noiseLimit = sqrt(12.);
  double noiseSigma = noiseLimit*noiseLimit / 12.0;

  for (int i = 0; i < numMeasurements; i++)
    {
      Vector3d measurement = truePoint +
        Vector3d(sampleUniform(-0.5, 0.5) * noiseLimit,
                 sampleUniform(-0.5, 0.5) * noiseLimit,
                 sampleUniform(-0.5, 0.5) * noiseLimit);
      GPSObservationPosition3DEdge* goe = new GPSObservationPosition3DEdge();
      goe->setVertex(0, position);
      goe->setMeasurement(measurement);
      goe->setInformation(Matrix3d::Identity() / noiseSigma);
      optimizer.addEdge(goe);
    }

  // Configure and set things going
  optimizer.initializeOptimization();
  optimizer.setVerbose(true);
  optimizer.optimize(5);

  cout << "truePoint=\n" << truePoint << endl;

  cerr <<  "computed estimate=\n" << dynamic_cast<VertexPosition3D*>(optimizer.vertices().find(0)->second)->estimate() << endl;

  //position->setMarginalized(true);

  SparseBlockMatrix<MatrixXd> spinv;

  optimizer.computeMarginals(spinv, position);

  //optimizer.solver()->computeMarginals();

  // covariance
  //
  cout << "covariance\n" << spinv << endl;

  cout << spinv.block(0,0) << endl;

}


#include <NTL/MatPrime.h>

NTL_START_IMPL

MatPrimeTablesType MatPrimeTables;
// a truly GLOBAL variable, shared among all threads

// For now, we use the same logic as for IsFFTPrime,
// which is good enough

static
long IsMatPrime(long n)
{
   long  m, x, y, z;
   long j, k;

   if (n <= 1 || n >= NTL_SP_BOUND) return 0;

   if (n % 2 == 0) return 0;

   if (n % 3 == 0) return 0;

   if (n % 5 == 0) return 0;

   if (n % 7 == 0) return 0;

   m = n - 1;
   k = 0;
   while ((m & 1) == 0) {
      m = m >> 1;
      k++;
   }

   for (;;) {
      x = RandomBnd(n);

      if (x == 0) continue;
      z = PowerMod(x, m, n);
      if (z == 1) continue;

      x = z;
      j = 0;
      do {
         y = z;
         z = MulMod(y, y, n);
         j++;
      } while (j != k && z != 1);

      if (z != 1 || y !=  n-1) return 0;

      if (j == k)
         break;
   }

   /* x^{2^k} = 1 mod n, x^{2^{k-1}} = -1 mod n */

   long TrialBound;

   TrialBound = m >> k;
   if (TrialBound > 0) {
      if (!ProbPrime(n, 5)) return 0;

      /* we have to do trial division by special numbers */

      TrialBound = SqrRoot(TrialBound);

      long a, b;

      for (a = 1; a <= TrialBound; a++) {
         b = (a << k) + 1;
         if (n % b == 0) return 0;
      }
   }

   return 1;
}

static
void NextMatPrime(long& q, long index)
{
   static long m = NTL_MatPrime_NBITS-1;
   static long k = 0;
   // m and k are truly GLOBAL variables, shared among
   // all threads.  Access is protected by a critical section
   // guarding MatPrimeTables

   static long last_index = -1;
   static long last_m = 0;
   static long last_k = 0;

   if (index == last_index) {
      // roll back m and k...part of a simple error recovery
      // strategy if an exception was thrown in the last
      // invocation of UseMatPrime...probably of academic
      // interest only

      m = last_m;
      k = last_k;
   }
   else {
      last_index = index;
      last_m = m;
      last_k = k;
   }

   long cand;

   for (;;) {
      if (k == 0) {
         m--;
         if (m < 3) ResourceError("ran out of matrix primes");
         k = 1L << (NTL_MatPrime_NBITS-m-2);
      }

      k--;

      cand = (1L << (NTL_MatPrime_NBITS-1)) + (k << (m+1)) + (1L << m) + 1;

      if (!IsMatPrime(cand)) continue;
      q = cand;
      return;
   }
}

void InitMatPrimeInfo(MatPrimeInfo& info, long q)
{
   info.q = q;
   info.context = zz_pContext(q);
}

void UseMatPrime(long index)
{
   if (index < 0) LogicError("invalid matrix prime index");
   if (index >= NTL_MAX_MATPRIMES) ResourceError("matrix prime index too large");

   if (index+1 >= NTL_NSP_BOUND) ResourceError("matrix prime index too large");
   // largely acacedemic, but it is a convenient assumption

   do {  // NOTE: thread safe lazy init
      MatPrimeTablesType::Builder bld(MatPrimeTables, index+1);
      long amt = bld.amt();
      if (!amt) break;

      long first = index+1-amt;
      // initialize entries first..index

      long i;
      for (i = first; i <= index; i++) {
         UniquePtr<MatPrimeInfo> info;
         info.make();

         long q, w;
         NextMatPrime(q, i);

         InitMatPrimeInfo(*info, q);
         bld.move(info);
      }

   } while (0);
}

#ifndef NTL_MatPrime_HALF_SIZE_STRATEGY

void build(MatPrime_crt_helper& H, const ZZ& P)
{
   ZZ B, M, M1, M2, M3;
   long n, i;
   long q, t;
   mulmod_t qinv;

   sqr(B, P);
   mul(B, B, NTL_MatPrimeLimit);
   LeftShift(B, B, NTL_MatPrimeFudge);

   set(M);
   n = 0;
   while (M <= B) {
      UseMatPrime(n);
      q = GetMatPrime(n);
      n++;
      mul(M, M, q);
   }

   double fn = double(n);

   if (8.0*fn*(fn+48) > NTL_FDOUBLE_PRECISION)
      ResourceError("modulus too big");

   H.NumPrimes = n;
   H.sz = P.size();
   H.prime.SetLength(n);
   H.prime_recip.SetLength(n);
   H.u.SetLength(n);
   H.uqinv.SetLength(n);
   H.ZZ_red_struct.SetLength(n);

   H.coeff.SetSize(n, P.size());

   H.montgomery_struct.init(P, ZZ(n) << NTL_MatPrime_NBITS);

   ZZ qq, rr;

   DivRem(qq, rr, M, P);

   NegateMod(H.MinusMModP, rr, P);

   H.montgomery_struct.adjust(H.MinusMModP);

   for (i = 0; i < n; i++) {
      q = GetMatPrime(i);
      qinv = MatPrimeTables[i]->context.ModulusInverse();

      long tt = rem(qq, q);

      mul(M2, P, tt);
      add(M2, M2, rr);
      div(M2, M2, q);  // = (M/q) rem p

      div(M1, M, q);
      t = rem(M1, q);
      t = InvMod(t, q);

      // montgomery
      H.montgomery_struct.adjust(M2);

      H.prime[i] = q;
      H.prime_recip[i] = 1/double(q);
      H.u[i] = t;
      H.uqinv[i] = PrepMulModPrecon(H.u[i], q, qinv);
      H.ZZ_red_struct[i] = &MatPrimeTables[i]->context.ZZ_red_struct();
      H.coeff[i] = M2;
   }

   H.cost = double(H.sz)*double(n);

}

void reduce(const MatPrime_crt_helper& H, const ZZ& value, MatPrime_residue_t *remainders,
            MatPrime_crt_helper_scratch& scratch)
{
   long n = H.NumPrimes;
   const sp_ZZ_reduce_struct *const *red_struct = H.ZZ_red_struct.elts();

   for (long i = 0; i < n; i++)
      remainders[i] = red_struct[i]->rem(value);
}

void reconstruct(const MatPrime_crt_helper& H, ZZ& value, const MatPrime_residue_t *remainders,
                 MatPrime_crt_helper_scratch& scratch)
{
   ZZ& t = scratch.t;

   long nprimes = H.NumPrimes;
   const long *u = H.u.elts();
   const long *prime = H.prime.elts();
   const mulmod_precon_t  *uqinv = H.uqinv.elts();
   const double *prime_recip = H.prime_recip.elts();

   double y = 0.0;

   QuickAccumBegin(t, H.sz);
   for (long i = 0; i < nprimes; i++) {
      long r = MulModPrecon(remainders[i], u[i], prime[i], uqinv[i]);
      y += double(r)*prime_recip[i];
      QuickAccumMulAdd(t, H.coeff[i], r);
   }

   long q = long(y + 0.5);
   QuickAccumMulAdd(t, H.MinusMModP, q);

   QuickAccumEnd(t);

   // montgomery
   H.montgomery_struct.eval(value, t);

}

#else

void build(MatPrime_crt_helper& H, const ZZ& P)
{
   ZZ B, M, M1, M2, M3;
   long n, i, j;
   long q, t;
   mulmod_t qinv;

   sqr(B, P);
   mul(B, B, NTL_MatPrimeLimit);
   LeftShift(B, B, NTL_MatPrimeFudge);

   set(M);
   n = 0;
   while (M <= B) {
      UseMatPrime(n);
      q = GetMatPrime(n);
      n++;
      mul(M, M, q);
   }

   double fn = double(n);

   if (8.0*fn*(fn+48) > NTL_FDOUBLE_PRECISION)
      ResourceError("modulus too big");

   long n_half_ceil = (n+1)/2;

   H.NumPrimes = n;
   H.sz = P.size();
   H.prime.SetLength(n);
   H.prime_recip.SetLength(n);
   H.u.SetLength(n);
   H.uqinv.SetLength(n);
   H.red_struct.SetLength(n);

   H.ZZ_red_struct.SetLength(n_half_ceil);
   H.coeff.SetSize(n_half_ceil, P.size());

   H.montgomery_struct.init(P, ZZ(n) << (2*NTL_MatPrime_NBITS));

   for (i = 0; i < n; i++) {
      q = GetMatPrime(i);
      qinv = MatPrimeTables[i]->context.ModulusInverse();

      div(M1, M, q);
      t = rem(M1, q);
      t = InvMod(t, q); // = (M/q)^{-1} rem q

      H.prime[i] = q;
      H.prime_recip[i] = 1/double(q);
      H.u[i] = t;
      H.uqinv[i] = PrepMulModPrecon(H.u[i], q, qinv);
      H.red_struct[i] = MatPrimeTables[i]->context.red_struct();

   }

   ZZ qq, rr;
   DivRem(qq, rr, M, P);
   NegateMod(H.MinusMModP, rr, P);
   H.montgomery_struct.adjust(H.MinusMModP);

   for (i = 0, j = 0; i < n; i += 2, j++) {
      q = GetMatPrime(i);
      if (i+1 < n) q *= GetMatPrime(i+1);

      long tt = rem(qq, q);

      mul(M2, P, tt);
      add(M2, M2, rr);
      div(M2, M2, q);  // = (M/q) rem p

      // montgomery
      H.montgomery_struct.adjust(M2);

      H.ZZ_red_struct[j].build(q);
      H.coeff[j] = M2;
   }

   H.cost = double(H.sz)*double(n_half_ceil);
}

void reduce(const MatPrime_crt_helper& H, const ZZ& value, MatPrime_residue_t *remainders,
            MatPrime_crt_helper_scratch& scratch)
{
   long n = H.NumPrimes;
   const sp_ZZ_reduce_struct *ZZ_red_struct = H.ZZ_red_struct.elts();
   const sp_reduce_struct *red_struct = H.red_struct.elts();
   const long *prime = H.prime.elts();

   long i = 0, j = 0;
   for (; i <= n-2; i += 2, j++) {
      unsigned long t = ZZ_red_struct[j].rem(value);
      remainders[i] = rem(t, prime[i], red_struct[i]);
      remainders[i+1] = rem(t, prime[i+1], red_struct[i+1]);
   }
   if (i < n) {
      remainders[i] = ZZ_red_struct[j].rem(value);
   }
}

void reconstruct(const MatPrime_crt_helper& H, ZZ& value, const MatPrime_residue_t *remainders,
                 MatPrime_crt_helper_scratch& scratch)
{
   ZZ& t = scratch.t;

   long nprimes = H.NumPrimes;
   const long *u = H.u.elts();
   const long *prime = H.prime.elts();
   const mulmod_precon_t  *uqinv = H.uqinv.elts();
   const double *prime_recip = H.prime_recip.elts();

   double y = 0.0;

   QuickAccumBegin(t, H.sz);

   long i = 0, j = 0;
   for (; i <= nprimes-2; i += 2, j++) {
      long r0 = MulModPrecon(remainders[i], u[i], prime[i], uqinv[i]);
      long r1 = MulModPrecon(remainders[i+1], u[i+1], prime[i+1], uqinv[i+1]);
      y += double(r0)*prime_recip[i] + double(r1)*prime_recip[i+1];
      long r = r0*prime[i+1] + r1*prime[i];
      QuickAccumMulAdd(t, H.coeff[j], r);
   }

   if (i < nprimes) {
      long r = MulModPrecon(remainders[i], u[i], prime[i], uqinv[i]);
      y += double(r)*prime_recip[i];
      QuickAccumMulAdd(t, H.coeff[j], r);
   }

   long q = long(y + 0.5);
   QuickAccumMulAdd(t, H.MinusMModP, q);

   QuickAccumEnd(t);

   // montgomery
   H.montgomery_struct.eval(value, t);

}

#endif

// Facillitates PIMPL in ZZ_p.h
void MatPrime_crt_helper_deleter(MatPrime_crt_helper* p)
{
   delete p;
}

NTL_END_IMPL

#if 0

NTL_CLIENT

int main()
{
   ZZ P;
   RandomLen(P, 8000);

   MatPrime_crt_helper H;
   build(H, P);

   MatPrime_crt_helper_scratch scratch;

   long nprimes = H.GetNumPrimes();

   cerr << nprimes << "\n";

   ZZ a, b, c, d, e;
   RandomBnd(a, P);
   RandomBnd(b, P);
   RandomBnd(c, P);
   RandomBnd(d, P);

   e = (a*b + c*d) % P;

   Vec<MatPrime_residue_t> avec, bvec, cvec, dvec, evec;

   avec.SetLength(nprimes);
   bvec.SetLength(nprimes);
   cvec.SetLength(nprimes);
   dvec.SetLength(nprimes);
   evec.SetLength(nprimes);

   reduce(H, a, avec.elts(), scratch);
   reduce(H, b, bvec.elts(), scratch);
   reduce(H, c, cvec.elts(), scratch);
   reduce(H, d, dvec.elts(), scratch);

   for (long i = 0; i < nprimes; i++) {
      long q = GetMatPrime(i);
      evec[i] = AddMod(MulMod(avec[i], bvec[i], q), MulMod(cvec[i], dvec[i], q), q);
   }

   ZZ e1;

   reconstruct(H, e1, evec.elts(), scratch);

   if (e == e1)
      cerr << "PASS\n";
   else
      cerr << "FAIL\n";
}

#endif


/*
 * LinearMixedModel.hpp
 *
 *  Created on: Mar 17, 2016
 *      Author: Aditya Gautam (agautam1@andrew.cmu.edu)
 */

#ifndef SRC_MODEL_LINEARMIXEDMODEL_HPP_
#define SRC_MODEL_LINEARMIXEDMODEL_HPP_

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <iostream>
#include <vector>
#include <math.h>
#include <cstdlib>
#include <iostream>

#ifdef BAZEL
#include "Math/Math.hpp"
#include "Model.hpp"
#else
#include "../Math/Math.hpp"
#include "../Models/Model.hpp"
#endif

using namespace std;
using namespace Eigen;

class LinearMixedModel : public Model {
protected:

    // Training data
//    MatrixXf X;
//    MatrixXf Y;

    // Dimensions of the data
    long n; // Number of samples
    long d; // Number of input features

    //Similary matrix and SVD
    MatrixXf K;
    MatrixXf S;
    MatrixXf U;
    void decomposition();

//    MatrixXf beta; // d*1
    MatrixXf mau;  // Coeff matrix of similarity matrix.
    float lambda_optimized; // Value at which log likelihood is max
    float sigma;
    bool initFlag;

public:

    // Constructor
    LinearMixedModel();
    LinearMixedModel(const unordered_map<string, string>& options);

    //Setters and Getters
    long get_num_samples();
    long get_X_features();
    float get_lambda();
    float getSigma();
    void set_lambda(float);
    void set_S(MatrixXf);
    void set_U(MatrixXf);
    void setX(const MatrixXf&);
    void setXY(MatrixXf, MatrixXf);
    void setXYK(MatrixXf, MatrixXf, MatrixXf);
    void setUS(MatrixXf, MatrixXf);
    MatrixXf getBeta();
    //Supporting functions
    // Final Objective of the LLM : Obtain beta matrix.
    void calculate_beta(float);
    void calculate_sigma(float);
    float get_log_likelihood_value(float);
    void find_max_log_likelihood();

    void set_num_samples(int num_samples);
    // Search objective functions
    float f(float);
    void assertReadyToRun();
    void init();
};

#endif /* SRC_MODEL_LINEARMIXEDMODEL_HPP_ */


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

#include <fcppt/math/matrix/componentwise_equal.hpp>
#include <fcppt/math/matrix/logarithm.hpp>
#include <fcppt/math/matrix/row.hpp>
#include <fcppt/math/matrix/static.hpp>
#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/test/unit_test.hpp>
#include <iostream>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_matrix_logarithm
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::matrix::static_<
		double,
		3,
		3
	>
	matrix_type;

	matrix_type const t(
		fcppt::math::matrix::row(
			23.6045,-7.38906,23.6045
		),
		fcppt::math::matrix::row(
			-16.2155,14.7781,-23.6045
		),
		fcppt::math::matrix::row(
			30.9936,7.38906,30.9936
		)
	);

	double const epsilon{
		0.1
	};

	BOOST_CHECK((
	        fcppt::math::matrix::componentwise_equal(
			fcppt::math::matrix::logarithm(
				t,
				1e-4,
				1.0e-9,
				1.0e-6
			),
		        matrix_type(
				fcppt::math::matrix::row(
					2.0, -1.0, 1.0
				),
				fcppt::math::matrix::row(
					0.0, 3.0, -1.0
				),
				fcppt::math::matrix::row(
					2.0, 1.0, 3.0
				)
			),
		        epsilon
		)
	));
}


#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <vector>
#include <list>

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/lapack/geqrf.hpp>
#include <boost/numeric/bindings/lapack/orgqr.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/vector_of_vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>

#ifdef OPENMP
#include <omp.h>
#endif

void boostbuild_Alpert_matrix(std::vector<std::vector<double> >p, std::vector<int> ki, boost::numeric::ublas::compressed_matrix <double> &  bU , int J,int N);
void build_Alpert_Sampling_product(boost::numeric::ublas::compressed_matrix <double> U, boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> P, boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> &T);


#include <trajopt_sco/solver_interface.hpp>
#include <Eigen/Core>

namespace sco {

using Eigen::MatrixXd;
using Eigen::VectorXd;

#if 0
typedef vector<AffExpr> ExprVector;
Matrix3d leftCrossProdMat(const Vector3d& x);
Matrix3d rightCrossProdMat(const Vector3d& x);

ExprVector exprMatMult(const MatrixXd& A, const VarVector& x);
ExprVector exprMatMult(const MatrixXd& A, const ExprVector& x);

ExprVector exprCross(const VectorXd& x, const VarVector& y);
ExprVector exprCross(const VarVector& x, const VectorXd& y);
ExprVector exprCross(const VectorXd& x, const ExprVector& y);
ExprVector exprCross(const ExprVector& x, const VectorXd& y);

#endif
AffExpr varDot(const VectorXd& x, const VarVector& v);
AffExpr exprDot(const VectorXd& x, const AffExprVector& v);
#if 0
QuadExpr varNorm2(const VarVector& v);
QuadExpr exprNorm2(const ExprVector& v);
#endif
}


//
// Created by Liuyu Jin on Jun 9, 2016.
//

#ifndef ALGORITHMS_LINEARREGRESSION_HPP
#define ALGORITHMS_LINEARREGRESSION_HPP

#include <Eigen/Dense>
#include <unordered_map>
//#include "Model.hpp"

#ifdef BAZEL
#include "model/ModelOptions.hpp"
#include "Model.hpp"
#else
#include "../model/ModelOptions.hpp"
#include "../model/Model.hpp"
#endif

using namespace Eigen;

class ICLasso : public virtual Model {
private:
    MatrixXf X; //n * p
    MatrixXf Y; //n * 1
    float lambda;
    float lambda1;
    float lambda2;
    float gamma;
    MatrixXf Beta; //p * 1
    MatrixXf Theta;

public:
    //constructor
    ICLasso();
    void set_X(MatrixXf);
    void set_Y(MatrixXf);
    void set_XY(MatrixXf, MatrixXf);
    void set_lambda1(float);
    void set_lambda2(float);
    void set_gamma(float);
    void set_theta(MatrixXf);
    float cost();
    void optimize_theta();

};

#endif //ALGORITHMS_LINEARREGRESSION_HPP


#include <Eigen/Core>
#include <Eigen/SparseCholesky>
#include <Euclid/MeshUtil/CGALMesh.h>
#include <Euclid/MeshUtil/EigenMesh.h>
#include <Euclid/Topology/MeshTopology.h>
#include <igl/cotmatrix.h>
#include <igl/vector_area_matrix.h>
#include <igl/repdiag.h>

namespace Euclid
{

namespace _impl
{

template<typename Mesh, typename T>
void boundary_matrix(const Mesh& mesh,
                     Eigen::SparseMatrix<T>& B,
                     Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& E)
{
    using Triplet = Eigen::Triplet<T>;
    std::vector<Triplet> triplets;
    auto vimap = get(CGAL::vertex_index, mesh);

    auto nv = mesh.number_of_vertices();
    auto sz = nv * 2;
    B.resize(sz, sz);
    E.setZero(sz, 2);

    auto bnds = boundary_components(mesh);
    int nb = 0;
    for (auto h : bnds) {
        auto hi = h;
        do {
            auto v = target(hi, mesh);
            auto vi = get(vimap, v);
            hi = next(hi, mesh);
            triplets.emplace_back(vi, vi, 1.0);
            triplets.emplace_back(nv + vi, nv + vi, 1.0);
            E(vi, 0) = 1.0;
            E(nv + vi, 1) = 1.0;
            ++nb;
        } while (hi != h);
        E /= std::sqrt(nb + 0.0);
    }
    B.setFromTriplets(triplets.begin(), triplets.end());
    B.makeCompressed();
}

} // namespace _impl

template<typename Mesh, typename VertexUVMap>
void spectral_conformal_parameterization(Mesh& mesh, VertexUVMap uvm)
{
    using Point_2 = typename boost::property_traits<VertexUVMap>::value_type;
    using FT = typename CGAL::Kernel_traits<Point_2>::Kernel::FT;
    using SpMat = Eigen::SparseMatrix<FT>;
    using Mat = Eigen::Matrix<FT, Eigen::Dynamic, Eigen::Dynamic>;
    using Vec = Eigen::Matrix<FT, Eigen::Dynamic, 1>;
    constexpr const int MAX_ITERS = 30;

    // convert mesh
    std::vector<FT> positions;
    std::vector<int> indices;
    extract_mesh<3>(mesh, positions, indices);
    Mat V;
    Eigen::MatrixXi F;
    make_mesh<3>(V, F, positions, indices);

    // assemble matrices
    SpMat L, LD, A, LC, B;
    Mat E;
    igl::cotmatrix(V, F, L);
    igl::repdiag(L, 2, LD);
    igl::vector_area_matrix(F, A);
    LC = -LD + 2.0 * A;
    _impl::boundary_matrix(mesh, B, E);

    // inverse power iterations
    Vec fidler = Vec::Random(LC.rows());
    Eigen::SimplicialLDLT<SpMat> solver(LC);
    for (int i = 0; i < MAX_ITERS; ++i) {
        fidler = (B - E * E.transpose()) * fidler;
        fidler = solver.solve(fidler);
        fidler.normalize();
    }

    // assemble results
    auto nv = mesh.number_of_vertices();
    auto vimap = get(CGAL::vertex_index, mesh);
    for (auto v : vertices(mesh)) {
        auto i = get(vimap, v);
        auto x = fidler(i);
        auto y = fidler(nv + i);
        put(uvm, v, Point_2(x, y));
    }
}

template<typename Mesh>
template<typename VertexUVMap,
         typename VertexIndexMap,
         typename VertexParameterizedMap>
CGAL::Surface_mesh_parameterization::Error_code
SCP_parameterizer_3<Mesh>::parameterize(TriangleMesh& mesh,
                                        halfedge_descriptor bhd,
                                        VertexUVMap uvmap,
                                        VertexIndexMap vimap,
                                        VertexParameterizedMap vpmap)
{
    spectral_conformal_parameterization(mesh, uvmap);
    return CGAL::Surface_mesh_parameterization::OK;
}

} // namespace Euclid


/* Copyright 2015 The TensorFlow Authors. All Rights Reserved.

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

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

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

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

#include "eigen_spatial_convolutions.h"
#include "eigen_cuboid_convolution.h"

void test_conv2d () {
  const int input_depth = 3;
  const int input_rows = 227;
  const int input_cols = 227;
  const int num_batches = 1;
  const int output_depth = 96;
  const int patch_rows = 11;
  const int patch_cols = 11;
  const int output_rows = input_rows - patch_rows + 1;
  const int output_cols = input_cols - patch_cols + 1;

  using namespace Eigen;

  Tensor<float, 4, RowMajor> input(num_batches, input_cols, input_rows, input_depth);
  Tensor<float, 4, RowMajor> kernel(patch_cols, patch_rows, input_depth, output_depth);
  Tensor<float, 4, RowMajor> result(num_batches, output_cols, output_rows, output_depth);

  input = input.constant(11.0f) + input.random();
  kernel = kernel.constant(2.0f) + kernel.random();

  using namespace std::chrono;
  milliseconds t0 = duration_cast< milliseconds >(system_clock::now().time_since_epoch());
  result = SpatialConvolution(input, kernel, 4, 4, PADDING_SAME);
  milliseconds t1 = duration_cast< milliseconds >(system_clock::now().time_since_epoch());
  int difference = std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count();
  std::cout << difference << " ms" << std::endl;
}

void test_conv2d_back_input () {

}

int main()
{
  std::cout << "Hello World!\n";

  test_conv2d ();

  return 0;
}


#include "FastqIterator.hpp"
#include <iostream>
#include <stdexcept>
#include "span.hpp"

using std::runtime_error;
using std::cout;
using std::cerr;

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

using namespace boost::accumulators;

typedef accumulator_set<float, features<tag::count, tag::mean, tag::variance>> stats_accumulator;

using overlap_analysis::quality_char_to_error_probability;
using overlap_analysis::FastqIterator;
using overlap_analysis::FastqElement;

bool classify_palindromic_q_scores(span<char> qualities){
    bool is_palindromic = false;

    stats_accumulator left_stats;
    stats_accumulator right_stats;

    auto length = qualities.size();

    if (length < 6){
        return is_palindromic;
    }

    size_t midpoint = length/2;

    for (size_t i=0; i<midpoint; i++){
        auto q = qualities[i];
        auto p = quality_char_to_error_probability(q);

        left_stats(p);
    }

    for (size_t i=midpoint; i<length; i++){
        auto q = qualities[i];
        auto p = quality_char_to_error_probability(q);

        right_stats(p);
    }

    float left_mean = mean(left_stats);
    float left_variance = variance(left_stats);

    float right_mean = mean(right_stats);
    float right_variance = variance(right_stats);

//    cout << left_mean << '\t' << left_variance << '\n';
//    cout << right_mean << '\t' << right_variance << '\n';

    if (right_mean - left_mean > 0.09 and right_mean >= 0.15){
        if (right_variance > left_variance and right_variance > 0.025){
            is_palindromic = true;
        }
    }

    return is_palindromic;
}

void split_palindrome_by_quality(FastqElement& element, vector<FastqElement>& result){
    span<char> q_scores(&element.quality_string.front(), &element.quality_string.back());

    bool is_palindromic = classify_palindromic_q_scores(q_scores);

    if (not is_palindromic){
        cout << '@' << element.name << '\n';
        cout << element.sequence << '\n';
        cout << '+' << '\n';
        cout << element.quality_string << '\n';
    }
}

int main(int argc, char **argv){
    if (argc == 1 or argc > 2){
        cerr << "Usage: split_palindromes_by_quality /path/to/file.fastq\n";
        return 1;
    }

    path fastq_path = argv[1];
    FastqIterator iterator(fastq_path);
    FastqElement e;
    vector<FastqElement> result;

    size_t i = 0;
    while (iterator.next_element(e)) {
        if (i % 100 == 0) {
            cerr << "\33[2K\r" << i << std::flush;
        }

        split_palindrome_by_quality(e, result);

        i++;
    }
    cerr << '\n';

    return 0;
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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 <boost/config.hpp>

#include <algorithm>
#include <vector>
#include <utility>
#include <iostream>

#include <boost/graph/visitors.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/neighbor_bfs.hpp>
#include <boost/property_map/property_map.hpp>

/*

  Sample Output:

  0 --> 2
  1 --> 1 3 4
  2 --> 1 3 4
  3 --> 1 4
  4 --> 0 1
  distances: 0 2 1 2 1
  parent[0] = 0
  parent[1] = 2
  parent[2] = 0
  parent[3] = 2
  parent[4] = 0

*/

using namespace boost;

template <class ParentDecorator>
struct print_parent {
  print_parent(const ParentDecorator& p_) : p(p_) { }
  template <class Vertex>
  void operator()(const Vertex& v) const {
    std::cout << "parent[" << v << "] = " <<  p[v]  << std::endl;
  }
  ParentDecorator p;
};

template <class DistanceMap, class PredecessorMap, class ColorMap>
class distance_and_pred_visitor : public neighbor_bfs_visitor<>
{
  typedef typename property_traits<ColorMap>::value_type ColorValue;
  typedef color_traits<ColorValue> Color;
public:
  distance_and_pred_visitor(DistanceMap d, PredecessorMap p, ColorMap c)
    : m_distance(d), m_predecessor(p), m_color(c) { }

  template <class Edge, class Graph>
  void tree_out_edge(Edge e, const Graph& g) const
  {
    typename graph_traits<Graph>::vertex_descriptor
      u = source(e, g), v = target(e, g);
    put(m_distance, v, get(m_distance, u) + 1);
    put(m_predecessor, v, u);
  }
  template <class Edge, class Graph>
  void tree_in_edge(Edge e, const Graph& g) const
  {
    typename graph_traits<Graph>::vertex_descriptor
      u = source(e, g), v = target(e, g);
    put(m_distance, u, get(m_distance, v) + 1);
    put(m_predecessor, u, v);
  }

  DistanceMap m_distance;
  PredecessorMap m_predecessor;
  ColorMap m_color;
};

int main(int , char* [])
{
  typedef adjacency_list<
    mapS, vecS, bidirectionalS,
    property<vertex_color_t, default_color_type>
  > Graph;

  typedef property_map<Graph, vertex_color_t>::type
    ColorMap;

  Graph G(5);
  add_edge(0, 2, G);
  add_edge(1, 1, G);
  add_edge(1, 3, G);
  add_edge(1, 4, G);
  add_edge(2, 1, G);
  add_edge(2, 3, G);
  add_edge(2, 4, G);
  add_edge(3, 1, G);
  add_edge(3, 4, G);
  add_edge(4, 0, G);
  add_edge(4, 1, G);

  typedef Graph::vertex_descriptor Vertex;

  // Array to store predecessor (parent) of each vertex. This will be
  // used as a Decorator (actually, its iterator will be).
  std::vector<Vertex> p(num_vertices(G));
  // VC++ version of std::vector has no ::pointer, so
  // I use ::value_type* instead.
  typedef std::vector<Vertex>::value_type* Piter;

  // Array to store distances from the source to each vertex .  We use
  // a built-in array here just for variety. This will also be used as
  // a Decorator.
  typedef graph_traits<Graph>::vertices_size_type size_type;
  size_type d[5];
  std::fill_n(d, 5, 0);

  // The source vertex
  Vertex s = *(vertices(G).first);
  p[s] = s;
  distance_and_pred_visitor<size_type*, Vertex*, ColorMap>
    vis(d, &p[0], get(vertex_color, G));
  neighbor_breadth_first_search
    (G, s, visitor(vis).
     color_map(get(vertex_color, G)));

  print_graph(G);

  if (num_vertices(G) < 11) {
    std::cout << "distances: ";
#ifdef BOOST_OLD_STREAM_ITERATORS
    std::copy(d, d + 5, std::ostream_iterator<int, char>(std::cout, " "));
#else
    std::copy(d, d + 5, std::ostream_iterator<int>(std::cout, " "));
#endif
    std::cout << std::endl;

    std::for_each(vertices(G).first, vertices(G).second,
                  print_parent<Piter>(&p[0]));
  }

  return 0;
}


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

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/log2.hpp>
#include <boost/simd/function/std.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>

STF_CASE_TPL (" log2",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::log2;

  using r_t = decltype(log2(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(log2(bs::Inf<T>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(log2(bs::Minf<T>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(log2(bs::Nan<T>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(log2(bs::Mone<T>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(log2(bs::Zero<T>()), bs::Minf<r_t>(), 0);
#endif
  STF_ULP_EQUAL(log2(bs::One<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(log2(T(2)), T(1), 0);
  STF_ULP_EQUAL(log2(T(8)), T(3), 0);
  STF_ULP_EQUAL(log2(T(64)), T(6), 0);
}

STF_CASE_TPL (" log2int",  STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::log2;

  using r_t = decltype(log2(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_ULP_EQUAL(log2(bs::One<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(log2(T(2)), T(1), 0);
  STF_ULP_EQUAL(log2(T(8)), T(3), 0);
  STF_ULP_EQUAL(log2(T(64)), T(6), 0);
}

STF_CASE_TPL (" log2 std",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::log2;
  using bs::std_;

  using r_t = decltype(bs::std_(log2)(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(bs::std_(log2)(bs::Inf<T>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(bs::std_(log2)(bs::Minf<T>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(bs::std_(log2)(bs::Nan<T>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(bs::std_(log2)(bs::Mone<T>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(bs::std_(log2)(bs::Zero<T>()), bs::Minf<r_t>(), 0);
#endif
  STF_ULP_EQUAL(bs::std_(log2)(bs::One<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(bs::std_(log2)(T(2)), T(1), 0);
  STF_ULP_EQUAL(bs::std_(log2)(T(8)), T(3), 0);
  STF_ULP_EQUAL(bs::std_(log2)(T(64)), T(6), 0);
}


#include "util/number_util.h"

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

namespace metternich::number {

double degree_to_radian(const double degree)
{
	return degree * boost::math::constants::pi<double>() / 180.;
}

}