#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <test/unit/math/rev/scal/fun/util.hpp>
#include <test/unit/math/mix/scal/fun/nan_util.hpp>

TEST(AgradFwdGammaP, FvarVar_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(0.5,1.0);
  fvar<var> z(1.0,1.0);
  fvar<var> a = stan::math::gamma_p(x,z);

  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5,1.0), a.val_.val());
  EXPECT_FLOAT_EQ(-0.18228334, a.d_.val());

  AVEC y = createAVEC(x.val_,z.val_);
  VEC g;
  a.val_.grad(y,g);
  EXPECT_FLOAT_EQ(-0.38983709,g[0]);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0),g[1]);
}
TEST(AgradFwdGammaP, Double_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  double x(0.5);
  fvar<var> z(1.0,1.0);
  fvar<var> a = stan::math::gamma_p(x,z);

  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5,1.0), a.val_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), a.d_.val());

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.val_.grad(y,g);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0),g[0]);
}
TEST(AgradFwdGammaP, FvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(0.5,1.0);
  double z(1.0);
  fvar<var> a = stan::math::gamma_p(x,z);

  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5,1.0), a.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.val_.grad(y,g);
  EXPECT_FLOAT_EQ(-0.38983709,g[0]);
}
TEST(AgradFwdGammaP, FvarVar_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(0.5,1.0);
  fvar<var> z(1.0,1.0);
  fvar<var> a = stan::math::gamma_p(x,z);

  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5,1.0), a.val_.val());
  EXPECT_FLOAT_EQ(-0.18228334, a.d_.val());

  AVEC y = createAVEC(x.val_,z.val_);
  VEC g;
  a.d_.grad(y,g);
  EXPECT_FLOAT_EQ(0.19403456,g[0]);
  EXPECT_FLOAT_EQ(0.096204743,g[1]);
}
TEST(AgradFwdGammaP, Double_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  double x(0.5);
  fvar<var> z(1.0,1.0);
  fvar<var> a = stan::math::gamma_p(x,z);

  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5,1.0), a.val_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), a.d_.val());

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.d_.grad(y,g);
  EXPECT_FLOAT_EQ(-0.31133062,g[0]);
}
TEST(AgradFwdGammaP, FvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(0.5,1.0);
  double z(1.0);
  fvar<var> a = stan::math::gamma_p(x,z);

  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5,1.0), a.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.d_.grad(y,g);
  EXPECT_FLOAT_EQ(-0.21349931,g[0]);
}

TEST(AgradFwdGammaP, FvarFvarVar_FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::gamma_p;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x,y);

  EXPECT_FLOAT_EQ(gamma_p(0.5,1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0.40753385, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.38983709, g[0]);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), g[1]);
}
TEST(AgradFwdGammaP, Double_FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::gamma_p;

  double x(0.5);

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x,y);

  EXPECT_FLOAT_EQ(gamma_p(0.5,1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), g[0]);
}
TEST(AgradFwdGammaP, FvarFvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::gamma_p;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  double y(1.0);

  fvar<fvar<var> > a = gamma_p(x,y);

  EXPECT_FLOAT_EQ(gamma_p(0.5,1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.38983709, g[0]);
}

TEST(AgradFwdGammaP, FvarFvarVar_FvarFvarVar_2ndDeriv_x) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::gamma_p;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x,y);

  EXPECT_FLOAT_EQ(gamma_p(0.5,1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0.40753385, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.21349931, g[0]);
  EXPECT_FLOAT_EQ(0.40753537, g[1]);
}
TEST(AgradFwdGammaP, FvarFvarVar_FvarFvarVar_2ndDeriv_y) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::gamma_p;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x,y);

  EXPECT_FLOAT_EQ(gamma_p(0.5,1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0.40753385, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(0.40753385, g[0]);
  EXPECT_FLOAT_EQ(-0.31133062, g[1]);
}
TEST(AgradFwdGammaP, Double_FvarFvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::gamma_p;

  double x(0.5);

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x,y);

  EXPECT_FLOAT_EQ(gamma_p(0.5,1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.31133062, g[0]);
}
TEST(AgradFwdGammaP, FvarFvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::gamma_p;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  double y(1.0);

  fvar<fvar<var> > a = gamma_p(x,y);

  EXPECT_FLOAT_EQ(gamma_p(0.5,1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.21349931, g[0]);
}

TEST(AgradFwdGammaP, FvarFvarVar_FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::gamma_p;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x,y);

  EXPECT_FLOAT_EQ(gamma_p(0.5,1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0.40753385, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.22403987, g[0]);
  EXPECT_FLOAT_EQ(-0.40374705, g[1]);
}
TEST(AgradFwdGammaP, Double_FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::gamma_p;

  double x(0.5);

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;
  y.val_.d_ = 1.0;

  fvar<fvar<var> > a = gamma_p(x,y);

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(0.57077283, g[0]);
}
TEST(AgradFwdGammaP, FvarFvarVar_Double_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::gamma_p;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;
  x.d_.val_ = 1.0;

  double y(1.0);

  fvar<fvar<var> > a = gamma_p(x,y);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(0.5462361, g[0]);
}

struct gamma_p_fun {
  template <typename T0, typename T1>
  inline
  typename boost::math::tools::promote_args<T0,T1>::type
  operator()(const T0 arg1,
             const T1 arg2) const {
    return gamma_p(arg1,arg2);
  }
};

TEST(AgradFwdGammaP, nan) {
  gamma_p_fun gamma_p_;
  test_nan_mix(gamma_p_,3.0,5.0,false);
}


#ifndef HAMILTONIANS_XXZ_HPP
#define HAMILTONIANS_XXZ_HPP
#include <Eigen/Eigen>
#include <nlohmann/json.hpp>

class XXZ
{
private:
	int n_;
	double J_;
	double Delta_;
	double sign_;

public:

	XXZ(int n, double J, double Delta, bool signRule = false)
		: n_(n), J_(J), Delta_(Delta)
	{
		if(signRule)
			sign_ = -1.0;
		else
			sign_ = 1.0;
	}

	nlohmann::json params() const
	{
		return nlohmann::json
		{
			{"name", "XXZ"},
			{"n", n_},
			{"J", J_},
			{"Delta", Delta_},
			{"sign_rule", int(sign_)}
		};
	}

	template<class State>
	typename State::Scalar operator()(const State& smp) const
	{
		typename State::Scalar s = 0.0;
		//Nearest-neighbor
		for(int i = 0; i < n_; i++)
		{
			int zz = smp.sigmaAt(i)*smp.sigmaAt((i+1)%n_);
			s += J_*Delta_*zz; //zz
			s += sign_*J_*(1-zz)*smp.ratio(i, (i+1)%n_); //xx+yy
		}
		return s;
	}

	std::map<uint32_t, double> operator()(uint32_t col) const
	{
		std::map<uint32_t, double> m;
		for(int i = 0; i < n_; i++)
		{
			int b1 = (col >> i) & 1;
			int b2 = (col >> ((i+1)%n_)) & 1;
			int zz = (1-2*b1)*(1-2*b2);
			long long int x = (1 << i) | (1 << ((i+1)%(n_)));
			m[col] += J_*Delta_*zz;
			m[col ^ x] += sign_*J_*(1 - zz);
		}
		return m;
	}
};
#endif//HAMILTONIANS_XXZ_HPP


////////////////////////////////////////////////////////////////////
//
// $Id: QEM.hxx 2021/06/05 14:25:18 kanai Exp $
//
// Copyright (c) 2021 Takashi Kanai
// Released under the MIT license
//
////////////////////////////////////////////////////////////////////

#ifndef _QEM_HXX
#define _QEM_HXX 1

#include <vector>
#include <cmath>
#include <cstring>
using namespace std;

#include "VertexL.hxx"
#include "FaceL.hxx"

#if 1
#include <Eigen/Dense>
#include <Eigen/Cholesky>
//using namespace Eigen;
#endif

#define DEFAULT_QEM_SIZE 10

class QEM {

  std::vector<double> mat_;

 public:

  QEM(){
    resize( DEFAULT_QEM_SIZE );
    init();
  };
  QEM(int size) { resize( size ); init(); };
  QEM( VertexL* vt ) {
    resize( DEFAULT_QEM_SIZE );
    init();
    QEM q;
    q.construct(vt);
    add(q);
  };
  QEM( HalfedgeL* he ) {
    resize( DEFAULT_QEM_SIZE );
    init();
    QEM q;
    q.construct(he);
    add(q);
  };
  QEM( HalfedgeL* he, double penalty ) {
    resize( DEFAULT_QEM_SIZE );
    init();
    QEM q; q.construct_boundary( he, penalty );
    add(q);
  };
  // copy constructor
  QEM( const QEM& q ) {
    resize( q.size() );
    set(q);
  };

  ~QEM(){};

  int size() const { return mat_.size(); };
  void resize( int n ) { mat_.resize( n ); };
  void init() { for ( unsigned int i = 0; i < mat_.size(); ++i ) mat_[i]=0.0; };
  void clear() { for ( unsigned int i = 0; i < mat_.size(); ++i ) mat_[i]=0.0; };

  void optimizePoint( VertexL* vt ) {
    optimizePoint( vt->point() );
  };
  bool optimizePoint( Point3d& );
  // matrix inverse
  bool inverse( QEM& );

  double error( Point3d& );

  // vertex QEM
  void construct( VertexL* vt ) {
    QEM q;
    construct( vt, q );
    add(q);
  };
  void construct( VertexL*, QEM& );

  // face QEM
  void construct( HalfedgeL* he ) {
    QEM q;
    construct( he, q );
    add(q);
  };
  void construct( HalfedgeL*, QEM& );
  void construct( FaceL*, QEM& );

  void construct_boundary( HalfedgeL* he, double penalty ) {
    QEM q;
    construct_boundary( he, penalty, q );
    add(q);
  };
  void construct_boundary( HalfedgeL*, double, QEM& );

  void setMat( int i, double val ) {
//     if ( val < .0 ) cout << i << " " << val << endl;
    mat_[i] = val;
  };
  //void set( VertexL* );

  void set( const QEM& q ) {
    for ( unsigned int i = 0; i < mat_.size(); ++i )
      mat_[i] = q.mat(i);
  };
  void mul( Point3d& p ) {
    Point3d q;
    mul(p, q);
    p.set(q);
  };

  void mul( Point3d& p, Point3d& q ) {
    q.x = mat_[0]*p.x + mat_[3]*p.y + mat_[4]*p.z;
    q.y = mat_[3]*p.x + mat_[1]*p.y + mat_[5]*p.z;
    q.z = mat_[4]*p.x + mat_[5]*p.y + mat_[2]*p.z;
  };

  void add( const QEM& q1, const QEM& q2 ) {
    for (unsigned int i = 0; i < mat_.size(); ++i )
      mat_[i] += (q1.mat(i) + q2.mat(i));
  };

  void add( const QEM& q ) {
    for (unsigned int i = 0; i < mat_.size(); ++i )
      mat_[i] += q.mat(i);
  };

  bool equals( const QEM& q ) {
    return ( (mat_[0] == q.mat(0)) && (mat_[1] == q.mat(1)) &&
	     (mat_[2] == q.mat(2)) && (mat_[3] == q.mat(3)) &&
	     (mat_[4] == q.mat(4)) && (mat_[5] == q.mat(5)) &&
	     (mat_[6] == q.mat(6)) && (mat_[7] == q.mat(7)) &&
	     (mat_[8] == q.mat(8)) && (mat_[9] == q.mat(9)) );
  };

  void scale( double t ) {
    for (unsigned int i = 0; i < mat_.size(); ++i )
      mat_[i] *= t;
  };

  double sum() {
    double t = .0;
    for ( unsigned int i = 0; i < mat_.size(); ++i ) t += mat_[i];
    return t;
  };

  void copy( QEM& q ) {
    for ( unsigned int i = 0; i < mat_.size(); ++i )
      q.setMat(i, mat_[i]);
  };

  //
  // operators
  //
  QEM& operator=( const QEM& q ) {
    set(q);
    return *this;
  };

  QEM& operator+=( const QEM& q ) {
    add( q );
    return *this;
  };

  bool operator==(const QEM& q ) {
    return equals(q);
  };

  friend std::ostream& operator <<( std::ostream& o, const QEM& q ) {
    return o << "( " << q.mat(0) << ", " << q.mat(1) << ", " << q.mat(2) << ", " << q.mat(3) << ", " << q.mat(4) << " " << q.mat(5) << " " << q.mat(6) << " " << q.mat(7) << " " << q.mat(8) << " " << q.mat(9) << " )\n";
  };

  double mat(int i) const { return mat_[i]; };
  //std::vector<double>* mat() { return &mat_; };

  bool invert( double* m, double* invOut ) {

    double inv[16];
    inv[0] = m[5]  * m[10] * m[15] -
             m[5]  * m[11] * m[14] -
             m[9]  * m[6]  * m[15] +
             m[9]  * m[7]  * m[14] +
             m[13] * m[6]  * m[11] -
             m[13] * m[7]  * m[10];

    inv[4] = -m[4]  * m[10] * m[15] +
              m[4]  * m[11] * m[14] +
              m[8]  * m[6]  * m[15] -
              m[8]  * m[7]  * m[14] -
              m[12] * m[6]  * m[11] +
              m[12] * m[7]  * m[10];

    inv[8] = m[4]  * m[9] * m[15] -
             m[4]  * m[11] * m[13] -
             m[8]  * m[5] * m[15] +
             m[8]  * m[7] * m[13] +
             m[12] * m[5] * m[11] -
             m[12] * m[7] * m[9];

    inv[12] = -m[4]  * m[9] * m[14] +
               m[4]  * m[10] * m[13] +
               m[8]  * m[5] * m[14] -
               m[8]  * m[6] * m[13] -
               m[12] * m[5] * m[10] +
               m[12] * m[6] * m[9];

    inv[1] = -m[1]  * m[10] * m[15] +
              m[1]  * m[11] * m[14] +
              m[9]  * m[2] * m[15] -
              m[9]  * m[3] * m[14] -
              m[13] * m[2] * m[11] +
              m[13] * m[3] * m[10];

    inv[5] = m[0]  * m[10] * m[15] -
             m[0]  * m[11] * m[14] -
             m[8]  * m[2] * m[15] +
             m[8]  * m[3] * m[14] +
             m[12] * m[2] * m[11] -
             m[12] * m[3] * m[10];

    inv[9] = -m[0]  * m[9] * m[15] +
              m[0]  * m[11] * m[13] +
              m[8]  * m[1] * m[15] -
              m[8]  * m[3] * m[13] -
              m[12] * m[1] * m[11] +
              m[12] * m[3] * m[9];

    inv[13] = m[0]  * m[9] * m[14] -
              m[0]  * m[10] * m[13] -
              m[8]  * m[1] * m[14] +
              m[8]  * m[2] * m[13] +
              m[12] * m[1] * m[10] -
              m[12] * m[2] * m[9];

    inv[2] = m[1]  * m[6] * m[15] -
             m[1]  * m[7] * m[14] -
             m[5]  * m[2] * m[15] +
             m[5]  * m[3] * m[14] +
             m[13] * m[2] * m[7] -
             m[13] * m[3] * m[6];

    inv[6] = -m[0]  * m[6] * m[15] +
              m[0]  * m[7] * m[14] +
              m[4]  * m[2] * m[15] -
              m[4]  * m[3] * m[14] -
              m[12] * m[2] * m[7] +
              m[12] * m[3] * m[6];

    inv[10] = m[0]  * m[5] * m[15] -
              m[0]  * m[7] * m[13] -
              m[4]  * m[1] * m[15] +
              m[4]  * m[3] * m[13] +
              m[12] * m[1] * m[7] -
              m[12] * m[3] * m[5];

    inv[14] = -m[0]  * m[5] * m[14] +
               m[0]  * m[6] * m[13] +
               m[4]  * m[1] * m[14] -
               m[4]  * m[2] * m[13] -
               m[12] * m[1] * m[6] +
               m[12] * m[2] * m[5];

    inv[3] = -m[1] * m[6] * m[11] +
              m[1] * m[7] * m[10] +
              m[5] * m[2] * m[11] -
              m[5] * m[3] * m[10] -
              m[9] * m[2] * m[7] +
              m[9] * m[3] * m[6];

    inv[7] = m[0] * m[6] * m[11] -
             m[0] * m[7] * m[10] -
             m[4] * m[2] * m[11] +
             m[4] * m[3] * m[10] +
             m[8] * m[2] * m[7] -
             m[8] * m[3] * m[6];

    inv[11] = -m[0] * m[5] * m[11] +
               m[0] * m[7] * m[9] +
               m[4] * m[1] * m[11] -
               m[4] * m[3] * m[9] -
               m[8] * m[1] * m[7] +
               m[8] * m[3] * m[5];

    inv[15] = m[0] * m[5] * m[10] -
              m[0] * m[6] * m[9] -
              m[4] * m[1] * m[10] +
              m[4] * m[2] * m[9] +
              m[8] * m[1] * m[6] -
              m[8] * m[2] * m[5];

    double det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];

    // cout << "det " << det << endl;
    if (det == 0) return false;

    det = 1.0 / det;
    for (int i = 0; i < 16; i++)
      {
        invOut[i] = inv[i] * det;
      }

    return true;
  };

  void cholesky4( double* A, double* L ) {

    //double *L = (double*)calloc(n * n, sizeof(double));
    //if (L == NULL)
    //exit(EXIT_FAILURE);
    for ( int i = 0; i < 16; ++i ) L[i] = .0;

    int n = 4;
    for (int i = 0; i < n; i++)
      for (int j = 0; j < (i+1); j++) {
        double s = 0;
        for (int k = 0; k < j; k++)
          s += L[i * n + k] * L[j * n + k];
        L[i * n + j] = (i == j) ?
          std::sqrt(A[i * n + i] - s) :
          (1.0 / L[j * n + j] * (A[i * n + j] - s));
      }
    // cout << "cholesky ";
    // for ( int m = 0; m < 16; ++m )
    //   cout << L[m] << " ";
    // cout << endl;
  };

  void toInvR( double* Rinv ) {

    Eigen::MatrixXd A(4,4);
    A << mat_[0], mat_[3], mat_[4], mat_[6],
      mat_[3], mat_[1], mat_[5], mat_[7],
      mat_[4], mat_[5], mat_[2], mat_[8],
      mat_[6], mat_[7], mat_[8], mat_[9];

    // Cholesky decomposition
    Eigen::LLT<Eigen::MatrixXd> lltOfA(A);
    Eigen::MatrixXd L = lltOfA.matrixL();
    // Inverse
    Eigen::MatrixXd Linv = L.inverse();

    // OpenGL is column-major
    Eigen::MatrixXd Linvtrans = Linv.transpose();
    memcpy( Rinv, Linvtrans.data(), sizeof(double) * 16 );

#if 0
    // OpenGL is column-major
    for ( int i = 0; i < 4; ++i )
      for ( int j = 0; j < 4; ++j )
        Rinv[ 4 * i + j ] = Linv(i,j);
#endif

#if 0
    double a[16];
    Eigen::MatrixXd Linvtrans = Linv.transpose();
    memcpy( a, Linvtrans.data(), sizeof(double) * 16 );

    cout << "Rinv ";
    for ( int i = 0; i < 4; ++i )
      for ( int j = 0; j < 4; ++j )
        cout << Rinv[ 4 * i + j ] << " ";
    cout << endl;

    cout << "a ";
    for ( int i = 0; i < 4; ++i )
      for ( int j = 0; j < 4; ++j )
        cout << a[ 4 * i + j ] << " ";
    cout << endl;
#endif

#if 0
    double p[16];
    p[0]  = mat_[0]; p[1]  = mat_[3]; p[2]  = mat_[4]; p[3]  = mat_[6];
    p[4]  = mat_[3]; p[5]  = mat_[1]; p[6]  = mat_[5]; p[7]  = mat_[7];
    p[8]  = mat_[4]; p[9]  = mat_[5]; p[10] = mat_[2]; p[11] = mat_[8];
    p[12] = mat_[6]; p[13] = mat_[7]; p[14] = mat_[8]; p[15] = mat_[9];

    // cout << "p ";
    // for ( int m = 0; m < 16; ++m )
    //   cout << p[m] << " ";
    // cout << endl;

    // Cholesky decomposition
    double R[16];
    cholesky4( p, R );

    // inverse R
    if ( invert( R, Rinv ) == false )
      {
        cerr << "error in matrix inverse." << endl;
      }
#endif

  };

protected:

  double operator[](size_t index) {
    return mat_[index];
  };

};

#if 0
#ifdef __cplusplus
extern "C" {
#endif

// functions
extern void optimize_vector(Sped *, double *);
extern BOOL InvertSymMatrix3(double *, double *);
extern double calc_error(double *, Vec *);
extern void optimize_2subvector(Sped *, double *);
extern void make_qeminfmatrix(Sped *, double *, double *);
extern void optimize_infvector(Sped *, double *);
extern double calc_inferror(double *, Vec *);
extern void make_vqem2submatrix(Sped *, double *, double, double *);
extern void make_eqem2submatrix(Sped *, double, double *);
extern void loop_odd_vertex(Spvt *, Spvt *, Spvt *, Spvt *, int, double *, Vec *);
extern void loop_even_vertex(Sped *, int *, double *, double *, Vec *, BOOL);
extern void loop_even_vertex_vj(Spvt *, Sped *, int *, double *, double *, Vec *);
extern Sped *ppdedge_first_ring_edge(Sped *);
extern Sped *ppdedge_next_ring_edge(Sped *, Sped *);
extern Sped *ppdedge_prev_ring_edge(Sped *, Sped *);
extern Spvt *ppdedge_ring_another_vt(Sped *, Sped *);
extern Spvt *ppdedge_first_ring_vertex(Sped *, Sped **);
extern Spvt *ppdedge_last_ring_vertex( Sped *, Sped ** );
extern Spvt *ppdedge_next_ring_vertex(Spvt *, Sped *, Sped *, Sped **);
extern Spvt *ppdedge_prev_ring_vertex(Spvt *, Sped *, Sped *, Sped **);
extern Spvt *ppdedge_opposite_vertex(Sped *, Spvt *, Spvt *);
extern void qem_matrix4(double, Vec *, double *, double *);
extern void edge_ring_test(Sppd *);
extern void edge_ring_vertex_test(Sppd *);

// parameters for QEM evaluation
#define EVAL_VERTEX 0
#define EVAL_SLP    1
#define EVAL_2SUB   2

#ifdef __cplusplus
}
#endif
#endif

#endif // _QEM_HXX


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 boost.simd.arithmetic toolbox - sqrt/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.arithmetic components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 30/11/2010
///
#include <nt2/include/functions/sqrt.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/include/functions/extract.hpp>
#include <nt2/include/functions/unary_minus.hpp>
#include <nt2/include/constants/cnan.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/sqrt_2o_2.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/complex/imaginary.hpp>
#include <nt2/sdk/complex/meta/as_imaginary.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>

NT2_TEST_CASE_TPL ( abs_cplx__1_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef std::complex<T>                              cT;
  typedef native<T ,ext_t>                             vT;
  typedef native<cT ,ext_t>                           vcT;
  typedef typename nt2::meta::as_imaginary<T>::type   ciT;
  typedef native<ciT ,ext_t>                         vciT;
  typedef typename nt2::meta::as_dry<T>::type          dT;
  typedef native<dT ,ext_t>                           vdT;
  double ulpd;
  ulpd=0.0;

  {
    typedef vcT r_t;
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Inf<vcT>())[0], cT(nt2::Inf<cT>()),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Minf<vcT>())[0], cT(0, nt2::Inf<T>()),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Mone<vcT>())[0], cT(0, 1),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Nan<vcT>())[0], cT(nt2::Cnan<cT>()),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::One<vcT>())[0], cT(nt2::One<cT>()),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Zero<vcT>())[0], cT(nt2::Zero<cT>()),0);
  }
  {
    typedef vcT r_t;
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Inf<vciT>())[0],  cT(nt2::Inf<T>(), nt2::Inf<T>()),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Minf<vciT>())[0], cT(nt2::Inf<T>(),nt2::Minf<T>()),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Mone<vciT>())[0], cT(nt2::Sqrt_2o_2<T>(), -nt2::Sqrt_2o_2<T>()) ,0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Nan<vciT>())[0],  cT(nt2::Nan<T>(), nt2::Nan<T>()),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::One<vciT>())[0],  cT(nt2::Sqrt_2o_2<T>(), nt2::Sqrt_2o_2<T>()),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Zero<vciT>())[0], nt2::Zero<cT>(),0);
  }
  {
    typedef vcT r_t;
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Inf<vdT>())[0], nt2::Inf<cT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Minf<vdT>())[0], cT(nt2::Zero<T>(), nt2::Inf<T>()),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Mone<vdT>())[0], cT(nt2::Zero<T>(), nt2::One<T>()),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Nan<vdT>())[0], nt2::Nan<cT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::One<vdT>())[0], nt2::One<cT>(),0);
    NT2_TEST_ULP_EQUAL(nt2::sqrt(nt2::Zero<vdT>())[0], nt2::Zero<cT>(),0);
  }
} // end of test for floating_


// Bring in my package's API, which is what I'm testing
#include <bsplines/BSpline.hpp>

// Bring in gtest
#include <gtest/gtest.h>
#include <boost/cstdint.hpp>

// Helpful functions from schweizer_messer
#include <sm/eigen/NumericalDiff.hpp>
#include <sm/eigen/gtest.hpp>

#include <boost/tuple/tuple.hpp>

using namespace bsplines;

struct BSplineJacobianFunctor {
    // Necessary for eigen fixed sized type member variables.
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef Eigen::VectorXd input_t;
    typedef Eigen::MatrixXd jacobian_t;
    typedef Eigen::VectorXd value_t;
    typedef double scalar_t;

    BSplineJacobianFunctor(BSpline bs, double t, int d) : bs_(bs), t_(t), d_(d) {}

    input_t update(const input_t& x, int c, double delta) {
        input_t xnew = x;
        xnew[c] += delta;
        return xnew;
    }

    Eigen::VectorXd operator()(const Eigen::VectorXd& c) {
        bs_.setLocalCoefficientVector(t_, c);
        return bs_.evalD(t_, d_);
    }

    BSpline bs_;
    double t_;
    int d_;
};

// Check that the Jacobian calculation is correct.
TEST(SplineTestSuite, testBSplineJacobian) {
    const int segments = 2;
    for (int order = 2; order < 10; order++) {
        BSpline bs(order);
        int nk = bs.numKnotsRequired(segments);
        std::vector<double> knots;
        for (int i = 0; i < nk; i++) {
            knots.push_back(i);
        }

        for (int dim = 1; dim < 4; dim++) {
            int nc = bs.numCoefficientsRequired(segments);
            Eigen::MatrixXd C = Eigen::MatrixXd::Random(dim, nc);
            bs.setKnotsAndCoefficients(knots, C);

            for (int derivative = 0; derivative < order; derivative++) {
                for (double t = bs.t_min(); t < bs.t_max(); t += 0.1) {
                    BSplineJacobianFunctor f(bs, t, derivative);
                    sm::eigen::NumericalDiff<BSplineJacobianFunctor> nd(f);
                    Eigen::MatrixXd estJ = nd.estimateJacobian(bs.localCoefficientVector(t));
                    Eigen::MatrixXd J;
                    Eigen::VectorXd v;
                    boost::tie(v, J) = bs.evalDAndJacobian(t, derivative);

                    sm::eigen::assertNear(J, estJ, 1e-5, SM_SOURCE_FILE_POS);
                }
            }
        }
    }
}

TEST(SplineTestSuite, testCoefficientMap) {
    const int order = 4;
    const int segments = 10;
    const int dim = 5;
    BSpline bs(order);
    int nk = bs.numKnotsRequired(segments);
    int nc = bs.numCoefficientsRequired(segments);

    std::vector<double> knots;
    for (int i = 0; i < nk; i++) {
        knots.push_back(i);
    }

    Eigen::MatrixXd C = Eigen::MatrixXd::Random(dim, nc);
    bs.setKnotsAndCoefficients(knots, C);

    const Eigen::MatrixXd& CC = bs.coefficients();
    for (int i = 0; i < bs.numVvCoefficients(); i++) {
        Eigen::Map<Eigen::VectorXd> m = bs.vvCoefficientVector(i);
        // Test pass by value...
        Eigen::Map<Eigen::Matrix<double, 5, 1> > m2 = bs.fixedSizeVvCoefficientVector<dim>(i);
        for (int r = 0; r < m.size(); ++r) {
            ASSERT_TRUE(&m[r] == &CC(r, i));
            ASSERT_TRUE(&m[r] == &m2[r]);
            m[r] = rand();
            ASSERT_EQ(m[r], CC(r, i));
            ASSERT_EQ(m[r], m2[r]);
        }
    }
}

TEST(SplineTestSuite, testGetBi) {
    const int order = 4;
    const int segments = 10;
    const double startTime = 0;
    const double endTime = 5;
    const int numTimeSteps = 43;
    BSpline bs(order);
    bs.initConstantSpline(startTime, endTime, segments, Eigen::VectorXd::Zero(1));

    for (int i = 0; i <= numTimeSteps; i++) {
        double t = startTime + (endTime - startTime) * ((double)i / numTimeSteps);
        Eigen::VectorXd localBiVector = bs.getLocalBiVector(t);
        SM_ASSERT_NEAR(std::runtime_error, localBiVector.sum(), 1.0, 1e-10,
                       "the bis at a given time should always sum up to 1")
        Eigen::VectorXd biVector = bs.getBiVector(t);
        SM_ASSERT_NEAR(std::runtime_error, localBiVector.sum(), 1.0, 1e-10,
                       "the bis at a given time should always sum up to 1")

        Eigen::VectorXd cumulativeBiVector = bs.getCumulativeBiVector(t);
        Eigen::VectorXd localCumulativeBiVector = bs.getLocalCumulativeBiVector(t);

        Eigen::VectorXi localCoefficientVectorIndices = bs.localCoefficientVectorIndices(t);
        int firstIndex = localCoefficientVectorIndices[0];
        SM_ASSERT_EQ(std::runtime_error, localCoefficientVectorIndices.size(), order,
                     "localCoefficientVectorIndices has to have exactly " << order << " entries");

        for (int j = 0; j < order; j++) {
            SM_ASSERT_EQ(std::runtime_error, localCoefficientVectorIndices[j], firstIndex + j,
                         "localCoefficientVectorIndices have to be successive");
        }

        for (int j = 0, n = biVector.size(); j < n; j++) {
            if (j < firstIndex) {
                SM_ASSERT_EQ(std::runtime_error, biVector[j], 0, "");
                SM_ASSERT_EQ(std::runtime_error, cumulativeBiVector[j], 1, "");
            } else if (j < firstIndex + order) {
                SM_ASSERT_EQ(std::runtime_error, biVector[j], localBiVector[j - firstIndex],
                             "localBiVector should be a slice of the biVector");
                SM_ASSERT_EQ(std::runtime_error, cumulativeBiVector[j], localCumulativeBiVector[j - firstIndex],
                             "localCumulativeBiVector must be a slice of the cumulativeBiVector");
                SM_ASSERT_NEAR(
                    std::runtime_error, cumulativeBiVector[j],
                    localBiVector.segment(j - firstIndex, order - (j - firstIndex)).sum(), 1e-13,
                    "cumulativeBiVector must be the sum of the localBiVector where it overlaps, but it is not at "
                        << (j - firstIndex) << " (localBiVector=" << localBiVector << ")");
            } else {
                SM_ASSERT_EQ(std::runtime_error, biVector[j], 0, "at position " << j);
                SM_ASSERT_EQ(std::runtime_error, cumulativeBiVector[j], 0, "at position " << j);
            }
        }
    }
}

// TEST(SplineTestSuite, testBSplineCubic)
// {
//   double knots_d[] = {-2.0, -1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0 };
//   double control_d[] = { 0.0,1.0, 2.0, 3.0 };

//   std::vector<double> knots;
//   knots.insert(knots.begin(),knots_d,knots_d + (sizeof(knots_d)/sizeof(double)));

//   std::vector<double> control;
//   control.insert(control.begin(),control_d,control_d + (sizeof(control_d)/sizeof(double)));

//   BSpline<double,4> s(knots,control);

//   for(double i = 1.0; i < 2.0; i += 0.1)
//     {
//       //std::cout << "s(" << i << ") = " << s.eval(i) << std::endl;
//       ASSERT_DOUBLE_EQ(s.eval(i),i);
//       //std::cout << "D1 s(" << i << ") = " << s.evalD(i,1) << std::endl;
//       //std::cout << "D2 s(" << i << ") = " << s.evalD(i,2) << std::endl;
//       //std::cout << "D3 s(" << i << ") = " << s.evalD(i,3) << std::endl;
//       //std::cout << "D4 s(" << i << ") = " << s.evalD(i,4) << std::endl;
//     }

//   // Check the bounds of evaluation.
//   // Lower bound.
//   ASSERT_THROW(s.eval(0.999),Exception);
//   ASSERT_NO_THROW(s.eval(1.0));
//   // Upper bound
//   ASSERT_NO_THROW(s.eval(1.999));
//   ASSERT_THROW(s.eval(2.0),Exception);

// }

// TEST(SplineTestSuite, testBSplineLinear)
// {
//   double knots_d[] = {-1.0, 0.0, 1.0, 2.0, 3.0, 4.0 };
//   double control_d[] = { 0.0,1.0, 2.0, 3.0 };

//   std::vector<double> knots;
//   knots.insert(knots.begin(),knots_d,knots_d + (sizeof(knots_d)/sizeof(double)));

//   std::vector<double> control;
//   control.insert(control.begin(),control_d,control_d + (sizeof(control_d)/sizeof(double)));

//   BSpline<double,2> s(knots,control);

//   for(double i = 0.0; i < 3.0; i += 0.1)
//     {
//       //std::cout << "s(" << i << ") = " << s.eval(i) << std::endl;
//       ASSERT_DOUBLE_EQ(s.eval(i),i);
//     }

//   // Check the bounds of evaluation.
//   // Lower bound.
//   ASSERT_THROW(s.eval(-0.0001),Exception);
//   ASSERT_NO_THROW(s.eval(0.0));
//   // Upper bound
//   ASSERT_NO_THROW(s.eval(2.999));
//   ASSERT_THROW(s.eval(3.0),Exception);

// }

// TEST(SplineTestSuite, testBSplineQuadratic)
// {
//   double knots_d[] = {-1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0 };
//   double control_d[] = { 0.5,1.5, 2.5, 3.5 };

//   std::vector<double> knots;
//   knots.insert(knots.begin(),knots_d,knots_d + (sizeof(knots_d)/sizeof(double)));

//   std::vector<double> control;
//   control.insert(control.begin(),control_d,control_d + (sizeof(control_d)/sizeof(double)));

//   BSpline<double,3> s(knots,control);

//     for(double i = 1.0; i < 3.0; i += 0.1)
//     {
//       //std::cout << "s(" << i << ") = " << s.eval(i) << std::endl;
//       ASSERT_DOUBLE_EQ(s.eval(i),i);
//     }

//     // Check the bounds of evaluation.
//   // Lower bound.
//   ASSERT_THROW(s.eval(0.999),Exception);
//   ASSERT_NO_THROW(s.eval(1.0));
//   // Upper bound
//   ASSERT_NO_THROW(s.eval(2.999));
//   ASSERT_THROW(s.eval(3.0),Exception);
// }


/**
 * @author  Daniel Maturana
 * @year    2015
 *
 * @attention Copyright (c) 2015
 * @attention Carnegie Mellon University
 * @attention All rights reserved.
 *
 **@=*/

#include <iostream>

#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>

#include <Eigen/Core>

#include <scrollgrid/scrollgrid3.hpp>

#include <gtest/gtest.h>

using namespace boost::assign;
using namespace Eigen;
using namespace ca;

TEST(scrollgrid3, hash_roundtrip1) {

  Vector3f center(-10, 20, -30);
  Vec3Ix dim(200, 100, 400);
  float res = 0.15;
  ca::ScrollGrid3f grid3( center, dim, res);

  std::vector<grid_ix_t> ix;
  ix += -10, -3, 0, 4, 11;

  BOOST_FOREACH(grid_ix_t i, ix) {
    BOOST_FOREACH(grid_ix_t j, ix) {
      BOOST_FOREACH(grid_ix_t k, ix) {
        Vec3Ix gix(i, j, k);
        uint64_t hix = grid3.grid_to_hash(gix);
        Vec3Ix gix2 = grid3.hash_to_grid(hix);
        EXPECT_EQ(gix, gix2);
      }
    }
  }

}

TEST(scrollgrid3, hash_roundtrip2) {

  Vector3f center(-10, 20, -30);
  Vec3Ix dim(21, 31, 41);
  float res = 0.15;
  ca::ScrollGrid3f grid3( center, dim, res);

  std::vector<grid_ix_t> ix;
  ix += -10, -3, 0, 4, 11;

  BOOST_FOREACH(grid_ix_t i, ix) {
    BOOST_FOREACH(grid_ix_t j, ix) {
      BOOST_FOREACH(grid_ix_t k, ix) {
        Vec3Ix gix(i, j, k);
        uint64_t hix = grid3.grid_to_hash(gix);
        Vec3Ix gix2 = grid3.hash_to_grid(hix);
        EXPECT_EQ(gix, gix2);
      }
    }
  }

}

TEST(scrollgrid3, scroll1) {

  Vector3f center(-10, 20, -30);
  Vec3Ix dim(21, 31, 41);
  float res = 0.15;
  ca::ScrollGrid3f grid3( center, dim, res);

  EXPECT_EQ( grid3.scroll_offset()[0] , 0 );
  EXPECT_EQ( grid3.scroll_offset()[1] , 0 );
  EXPECT_EQ( grid3.scroll_offset()[2] , 0 );

  {
    ca::Vec3Ix gix(0, 0, 1);
    grid3.scroll( gix );
  }

  EXPECT_EQ( grid3.scroll_offset()[0] , 0 );
  EXPECT_EQ( grid3.scroll_offset()[1] , 0 );
  EXPECT_EQ( grid3.scroll_offset()[2] , 1 );

  {
    ca::Vec3Ix gix(0, 0, -1);
    grid3.scroll( gix );
  }

  EXPECT_EQ( grid3.scroll_offset()[0] , 0 );
  EXPECT_EQ( grid3.scroll_offset()[1] , 0 );
  EXPECT_EQ( grid3.scroll_offset()[2] , 0 );

}

TEST(scrollgrid3, scroll2) {

  Vector3f center(-10, 20, -30);
  Vec3Ix dim(21, 31, 41);
  float res = 0.15;
  ca::ScrollGrid3f grid3( center, dim, res);

  ca::Vec3Ix gix(2, 0, 0);
  grid3.scroll( gix );

  EXPECT_FLOAT_EQ( grid3.center()[0], center[0]+2*res );

}

TEST(scrollgrid3, scroll3) {

  Vector3f center(-10, 20, -30);
  Vec3Ix dim(21, 31, 41);
  float res = 0.15;
  ca::ScrollGrid3f grid3( center, dim, res);

  EXPECT_EQ( grid3.first_i() , 0 );
  EXPECT_EQ( grid3.first_j() , 0 );
  EXPECT_EQ( grid3.first_k() , 0 );

  ca::Vec3Ix gix(2, 0, 0);
  grid3.scroll( gix );

  EXPECT_EQ( grid3.first_i() , 2 );
  EXPECT_EQ( grid3.first_j() , 0 );
  EXPECT_EQ( grid3.first_k() , 0 );

  ca::Vec3Ix gix2(0, -2, 0);
  grid3.scroll( gix2 );

  EXPECT_EQ( grid3.first_i() , 2 );
  EXPECT_EQ( grid3.first_j() , -2 );
  EXPECT_EQ( grid3.first_k() , 0 );

}

TEST(scrollgrid3, scroll4) {

  Vector3f center(-10, 20, -30);
  Vec3Ix dim(21, 31, 41);
  float res = 0.15;
  ca::ScrollGrid3f grid3( center, dim, res);

  EXPECT_EQ( grid3.dim_i() , grid3.last_i() - grid3.first_i() );
  EXPECT_EQ( grid3.dim_j() , grid3.last_j() - grid3.first_j() );
  EXPECT_EQ( grid3.dim_k() , grid3.last_k() - grid3.first_k() );

  ca::Vec3Ix gix(2, 0, 0);
  grid3.scroll( gix );

  EXPECT_EQ( grid3.dim_i() , grid3.last_i() - grid3.first_i() );
  EXPECT_EQ( grid3.dim_j() , grid3.last_j() - grid3.first_j() );
  EXPECT_EQ( grid3.dim_k() , grid3.last_k() - grid3.first_k() );

}

TEST(scrollgrid3, grid_to_mem1) {

  Vector3f center(-1, 2, -3);
  Vec3Ix dim(21, 31, 41);
  float res = 0.5;
  ca::ScrollGrid3f grid3( center, dim, res);

#if 0
  std::cerr << "grid3.first_i() = " << grid3.first_i() << std::endl;
  std::cerr << "grid3.first_j() = " << grid3.first_j() << std::endl;
  std::cerr << "grid3.first_k() = " << grid3.first_k() << std::endl;

  std::cerr << "grid3.last_i() = " << grid3.last_i() << std::endl;
  std::cerr << "grid3.last_j() = " << grid3.last_j() << std::endl;
  std::cerr << "grid3.last_k() = " << grid3.last_k() << std::endl;
#endif

  mem_ix_t mix1 = grid3.grid_to_mem( ca::Vec3Ix(2, 1, 4) );
  mem_ix_t mix2 = grid3.grid_to_mem( 2, 1, 4 );

  EXPECT_EQ(mix1, mix2);
}

TEST(scrollgrid3, grid_to_mem2) {

  Vector3f center(-1, 2, -3);
  Vec3Ix dim(21, 31, 41);
  float res = 0.5;
  ca::ScrollGrid3f grid3( center, dim, res);

  std::vector<grid_ix_t> ix;
  ix += 0, 3, 4, 10, 11;

  BOOST_FOREACH(grid_ix_t i, ix) {
    BOOST_FOREACH(grid_ix_t j, ix) {
      BOOST_FOREACH(grid_ix_t k, ix) {
        mem_ix_t mix1 = grid3.grid_to_mem_slow( i, j, k);
        mem_ix_t mix2 = grid3.grid_to_mem( i, j, k );
        EXPECT_EQ(mix1, mix2);
      }
    }
  }
}

TEST(scrollgrid3, grid_to_mem3) {

  Vector3f center(-1, 2, -3);
  Vec3Ix dim(21, 31, 41);
  float res = 0.5;
  ca::ScrollGrid3f grid3( center, dim, res);

  std::vector<grid_ix_t> ix;
  ix += 0, 3, 4, 10, 11;

  BOOST_FOREACH(grid_ix_t i, ix) {
    BOOST_FOREACH(grid_ix_t j, ix) {
      BOOST_FOREACH(grid_ix_t k, ix) {
        mem_ix_t mix1 = grid3.grid_to_mem_slow( i, j, k);
        mem_ix_t mix2 = grid3.grid_to_mem( i, j, k );
        EXPECT_EQ(mix1, mix2);
      }
    }
  }
}

TEST(scrollgrid3, grid_to_mem4) {

  Vector3f center(-1, 2, -3);
  Vec3Ix dim(21, 31, 41);
  float res = 0.5;
  ca::ScrollGrid3f grid3( center, dim, res);

  ca::Vec3Ix gix( 10, 14,  21);
  mem_ix_t mix = grid3.grid_to_mem(gix);

  grid3.scroll( ca::Vec3Ix( 0, 0, 1) );
  EXPECT_EQ( mix , grid3.grid_to_mem(gix) );
  EXPECT_EQ( mix , grid3.grid_to_mem_slow(gix) );

  grid3.scroll( ca::Vec3Ix( 0, 0, -1) );
  EXPECT_EQ( mix , grid3.grid_to_mem(gix) );
  EXPECT_EQ( mix , grid3.grid_to_mem_slow(gix) );

  grid3.scroll( ca::Vec3Ix( 1, 0, 1) );
  EXPECT_EQ( mix , grid3.grid_to_mem(gix) );
  EXPECT_EQ( mix , grid3.grid_to_mem_slow(gix) );

  grid3.scroll( ca::Vec3Ix( -2, 0, 1) );
  EXPECT_EQ( mix , grid3.grid_to_mem(gix) );
  EXPECT_EQ( mix , grid3.grid_to_mem_slow(gix) );

  grid3.scroll( ca::Vec3Ix( 0, 3, -4) );
  EXPECT_EQ( mix , grid3.grid_to_mem(gix) );
}

TEST(scrollgrid3, grid_to_mem5) {

  Vector3f center(-10, 20, -30);
  Vec3Ix dim(200, 100, 400);
  float res = 0.15;
  ca::ScrollGrid3f grid3( center, dim, res);

  grid3.scroll( ca::Vec3Ix(2, 0, 0) );

  std::vector<grid_ix_t> ix;
  ix += -10, -3, 0, 4, 11;

  BOOST_FOREACH(grid_ix_t i, ix) {
    BOOST_FOREACH(grid_ix_t j, ix) {
      BOOST_FOREACH(grid_ix_t k, ix) {
        Vec3Ix gix(i, j, k);
        gix += (grid3.scroll_offset() + grid3.radius_ijk());
        mem_ix_t mix = grid3.grid_to_mem_slow(gix);
        mem_ix_t mix2 = grid3.grid_to_mem(gix);
        EXPECT_EQ( mix, mix2 );
      }
    }
  }
}

TEST(scrollgrid3, mem_roundtrip1) {

  Vector3f center(-10, 20, -30);
  Vec3Ix dim(200, 100, 400);
  float res = 0.15;
  ca::ScrollGrid3f grid3( center, dim, res);

  std::vector<grid_ix_t> ix;
  ix += -10, -3, 0, 4, 11;

  BOOST_FOREACH(grid_ix_t i, ix) {
    BOOST_FOREACH(grid_ix_t j, ix) {
      BOOST_FOREACH(grid_ix_t k, ix) {
        Vec3Ix gix(i, j, k);
        gix += (grid3.scroll_offset() + grid3.radius_ijk());
        mem_ix_t mix = grid3.grid_to_mem(gix);
        Vec3Ix gix2 = grid3.mem_to_grid(mix);
        EXPECT_TRUE( gix.cwiseEqual(gix2).all() );

      }
    }
  }
}

TEST(scrollgrid3, mem_roundtrip2) {

  Vector3f center(-10, 20, -30);
  Vec3Ix dim(200, 100, 400);
  float res = 0.15;
  ca::ScrollGrid3f grid3( center, dim, res);

  grid3.scroll( ca::Vec3Ix(2, 0, 0) );

  std::vector<grid_ix_t> ix;
  ix += -10, -3, 0, 4, 11;

  BOOST_FOREACH(grid_ix_t i, ix) {
    BOOST_FOREACH(grid_ix_t j, ix) {
      BOOST_FOREACH(grid_ix_t k, ix) {
        Vec3Ix gix(i, j, k);
        gix += (grid3.scroll_offset() + grid3.radius_ijk());
        mem_ix_t mix = grid3.grid_to_mem(gix);
        //Vec3Ix gix2 = grid3.mem_to_grid_undo_wrap(mix);
        Vec3Ix gix2 = grid3.mem_to_grid(mix);

        EXPECT_EQ( gix[0], gix2[0] );
        EXPECT_EQ( gix[1], gix2[1] );
        EXPECT_EQ( gix[2], gix2[2] );

      }
    }
  }

}

TEST(scrollgrid3, mem_roundtrip3) {

  Vector3f center(-10, 20, -30);
  Vec3Ix dim(200, 100, 400);
  float res = 0.15;
  ca::ScrollGrid3f grid3( center, dim, res);

  grid3.scroll( ca::Vec3Ix(202, 0, 0) );

  std::vector<grid_ix_t> ix;
  ix += -10, -3, 0, 4, 11;

  BOOST_FOREACH(grid_ix_t i, ix) {
    BOOST_FOREACH(grid_ix_t j, ix) {
      BOOST_FOREACH(grid_ix_t k, ix) {
        Vec3Ix gix(i, j, k);
        gix += (grid3.scroll_offset() + grid3.radius_ijk());
        mem_ix_t mix = grid3.grid_to_mem(gix);
        //Vec3Ix gix2 = grid3.mem_to_grid_undo_wrap(mix);
        Vec3Ix gix2 = grid3.mem_to_grid(mix);
        EXPECT_EQ( gix[0], gix2[0] );
        EXPECT_EQ( gix[1], gix2[1] );
        EXPECT_EQ( gix[2], gix2[2] );
        //EXPECT_TRUE( gix.cwiseEqual(gix2).all() );

      }
    }
  }

}

#if 0
struct TestClearCellsFun : ca::ClearCellsFun {
  TestClearCellsFun(ca::Vec3Ix* last_start, ca::Vec3Ix* last_finish) {
  }
  virtual void operator()(const ca::Vec3Ix& start,
                          const ca::Vec3Ix& finish) const {
    *last_start = start;
    *last_finish = finish;
  }
  ca::Vec3Ix *last_start_, *last_finish_;
};

TEST(scrollgrid3, big_offset1) {
  // when scroll offset is bigger than grid dimensions

  Vector3f center(0.f, 0.f, 0.f);
  Vec3Ix dim(20, 10, 40);
  float res = 0.15;
  ca::ScrollGrid3f grid3( center, dim, res);

  grid3.scroll( ca::Vec3Ix(25, 0, 0) );

  std::vector<grid_ix_t> ix;
  ix += -10, -3, 0, 4, 11;

  BOOST_FOREACH(grid_ix_t i, ix) {
    BOOST_FOREACH(grid_ix_t j, ix) {
      BOOST_FOREACH(grid_ix_t k, ix) {
        Vec3Ix gix(i, j, k);
        gix += (grid3.scroll_offset() + grid3.radius_ijk());
        mem_ix_t mix = grid3.grid_to_mem(gix);
        //Vec3Ix gix2 = grid3.mem_to_grid_undo_wrap(mix);
        Vec3Ix gix2 = grid3.mem_to_grid(mix);
        EXPECT_EQ( gix[0], gix2[0] );
        EXPECT_EQ( gix[1], gix2[1] );
        EXPECT_EQ( gix[2], gix2[2] );
        //EXPECT_TRUE( gix.cwiseEqual(gix2).all() );

      }
    }
  }
}
#endif

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


/*
@copyright Louis Dionne 2015
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/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/orderable.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>
using namespace boost::hana;

int main() {

{

//! [less.than]
BOOST_HANA_CONSTEXPR_CHECK(all_of(tuple_c<int, 1, 2, 3, 4>, less.than(5)));

BOOST_HANA_CONSTANT_CHECK(all_of(tuple_c<int, 1, 2, 3, 4>, less_equal.than(int_<4>)));
//! [less.than]

}{

//! [greater]
BOOST_HANA_CONSTEXPR_CHECK(greater(4, 1));
BOOST_HANA_CONSTANT_CHECK(!greater(int_<1>, int_<3>));
//! [greater]

}{

//! [greater_equal]
BOOST_HANA_CONSTEXPR_CHECK(greater_equal(4, 1));
BOOST_HANA_CONSTEXPR_CHECK(greater_equal(1, 1));
BOOST_HANA_CONSTANT_CHECK(!greater_equal(int_<1>, int_<2>));
//! [greater_equal]

}{

//! [less]
BOOST_HANA_CONSTEXPR_CHECK(less(1, 4));
BOOST_HANA_CONSTANT_CHECK(!less(int_<3>, int_<2>));
//! [less]

}{

//! [less_equal]
BOOST_HANA_CONSTEXPR_CHECK(less_equal(1, 4));
BOOST_HANA_CONSTEXPR_CHECK(less_equal(1, 1));
BOOST_HANA_CONSTANT_CHECK(!less_equal(int_<3>, int_<2>));
//! [less_equal]

}{

//! [max]
BOOST_HANA_CONSTEXPR_CHECK(max(1, 4) == 4);
BOOST_HANA_CONSTANT_CHECK(max(int_<7>, int_<5>) == int_<7>);
//! [max]

}{

//! [min]
BOOST_HANA_CONSTEXPR_CHECK(min(1, 4) == 1);
BOOST_HANA_CONSTANT_CHECK(min(int_<7>, int_<5>) == int_<5>);
//! [min]

}{

//! [ordering]
BOOST_HANA_CONSTEXPR_LAMBDA auto sorted = sort_by(ordering(sizeof_), tuple_t<
    char[3], char[1], char[2], char[15]
>);
BOOST_HANA_CONSTANT_CHECK(sorted == tuple_t<
    char[1], char[2], char[3], char[15]
>);
//! [ordering]

}

}


#ifndef CALIBRATE_HPP
#define CALIBRATE_HPP

#include <vector>

#include <Eigen/Geometry>

// Assuming that the photos have been distributed roughly evenly around the
// circle and that the rotation axis was close to the global vertical axis.
// The returned vector is the camera space +y vector in IMU coordinates. y is down on the image.
Eigen::Vector3f get_y_axis(std::vector<Eigen::Vector3f> const& photo_gravity);

// Assumes that the camera x axis was roughly orthogonal to the world vertcal axis and
// the photos were taken while rotating exactly around the camera x axis.
Eigen::Vector3f get_x_axis(std::vector<Eigen::Vector3f> const& photo_gravity);

// Returns the rotation from IMU frame to Camera frame.
Eigen::Quaternionf calibrate(std::ifstream& recording_infile,
                             int x_session_index, int y_session_index,
                             int nr_skip_measurements);

#endif // CALIBRATE_HPP


// Copyright (c) 2017 Graphcore Ltd. All rights reserved.
#include <boost/multi_array.hpp>
#include <cassert>
#include <poplibs_test/GeneralMatrixAdd.hpp>
#include <poplibs_test/GeneralMatrixMultiply.hpp>
#include <poplibs_test/Lstm.hpp>
#include <poplibs_test/NonLinearity.hpp>
#include <unordered_map>

// Fwd state array indices
#define LSTM_FWD_STATE_FORGET_GATE 2
#define LSTM_FWD_STATE_CAND_TANH 3
#define LSTM_FWD_STATE_INPUT_GATE 4
#define LSTM_FWD_STATE_OUTPUT_GATE 5
#define LSTM_FWD_STATE_OUTPUT_TANH 6

using IndexRange = boost::multi_array_types::index_range;
using Array1dRef = boost::multi_array_ref<double, 1>;
using Array2dRef = boost::multi_array_ref<double, 2>;
using Array2d = boost::multi_array<double, 2>;
using Array3dRef = boost::multi_array_ref<double, 3>;
using Array4dRef = boost::multi_array_ref<double, 4>;
using Array3d = boost::multi_array<double, 3>;

using namespace poplibs_test;

/**
 * Process a given unit type within an LSTM given its weights and biases.
 * The non-linearity is also specified although it may be derived from the unit
 */
static void processBasicLstmUnit(const Array2dRef prevOutput,
                                 const Array2dRef input,
                                 const Array3dRef weightsInput,
                                 const Array3dRef weightsOutput,
                                 const Array2dRef biases, Array2dRef output,
                                 unsigned lstmUnitOffset,
                                 popnn::NonLinearityType nonLinearityType) {
  const auto batchSize = prevOutput.shape()[0];
  const auto outputSize = prevOutput.shape()[1];

  /* split weight into two parts:
   * 1) part which weighs only the previous output
   * 2) part which weighs only the input
   */
  Array2d weightsOutputUnit = weightsOutput[lstmUnitOffset];
  Array2d weightsInputUnit = weightsInput[lstmUnitOffset];

  gemm::generalMatrixMultiply(prevOutput, weightsOutputUnit, output, output,
                              1.0, 0, false, false);
  gemm::generalMatrixMultiply(input, weightsInputUnit, output, output, 1.0, 1.0,
                              false, false);
  /* add bias */
  for (auto b = 0U; b != batchSize; ++b) {
    for (auto i = 0U; i != outputSize; ++i) {
      output[b][i] += biases[lstmUnitOffset][i];
    }
  }

  /* apply non-linearity */
  nonLinearity(nonLinearityType, output);
}

static std::unordered_map<BasicLstmCellUnit, unsigned>
getCellMapping(const std::vector<BasicLstmCellUnit> &cellOrder) {
  // build a mapping of the order that the gates are stored in.
  std::unordered_map<BasicLstmCellUnit, unsigned> cellMapping;
  for (unsigned i = 0; i < cellOrder.size(); ++i) {
    auto gate = cellOrder.at(i);
    cellMapping.insert(std::make_pair(gate, i));
  }

  return cellMapping;
}

void poplibs_test::lstm::basicLstmCellForwardPass(
    const Array3dRef input, const Array2dRef biases,
    const Array2dRef prevOutput, const Array3dRef weightsInput,
    const Array3dRef weightsOutput, Array2dRef prevCellState, Array4dRef state,
    const std::vector<BasicLstmCellUnit> &cellOrder) {
  const auto sequenceSize = state.shape()[1];
  const auto batchSize = state.shape()[2];
  const auto outputSize = state.shape()[3];
#ifndef NDEBUG
  const auto inputSize = input.shape()[2];
#endif
  assert(state.shape()[0] == LSTM_NUM_FWD_STATES);
  assert(weightsInput.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(weightsInput.shape()[1] == inputSize);
  assert(weightsInput.shape()[2] == outputSize);
  assert(weightsOutput.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(weightsOutput.shape()[1] == outputSize);
  assert(weightsOutput.shape()[2] == outputSize);
  assert(prevCellState.shape()[0] == batchSize);
  assert(prevCellState.shape()[1] == outputSize);
  assert(biases.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(biases.shape()[1] == outputSize);
  assert(prevOutput.shape()[0] == batchSize);
  assert(prevOutput.shape()[1] == outputSize);

  auto cellMapping = getCellMapping(cellOrder);

  for (auto s = 0U; s != sequenceSize; ++s) {
    Array2d ysm1 = s == 0 ? state[LSTM_FWD_STATE_ACTS_IDX][s]
                          : state[LSTM_FWD_STATE_ACTS_IDX][s - 1];
    Array2d csm1 = s == 0 ? state[LSTM_FWD_STATE_CELL_STATE_IDX][s]
                          : state[LSTM_FWD_STATE_CELL_STATE_IDX][s - 1];
    Array2d prevOutputThisStep = s == 0 ? prevOutput : ysm1;
    Array2d cellState = s == 0 ? prevCellState : csm1;
    Array2d inputThisStep = input[s];

    /* forget gate */
    Array2d forgetGate(boost::extents[batchSize][outputSize]);
    processBasicLstmUnit(prevOutputThisStep, inputThisStep, weightsInput,
                         weightsOutput, biases, forgetGate,
                         cellMapping.at(BASIC_LSTM_CELL_FORGET_GATE),
                         popnn::NonLinearityType::SIGMOID);
    state[LSTM_FWD_STATE_FORGET_GATE][s] = forgetGate;

    /* input gate */
    Array2d inputGate(boost::extents[batchSize][outputSize]);
    processBasicLstmUnit(prevOutputThisStep, inputThisStep, weightsInput,
                         weightsOutput, biases, inputGate,
                         cellMapping.at(BASIC_LSTM_CELL_INPUT_GATE),
                         popnn::NonLinearityType::SIGMOID);
    state[LSTM_FWD_STATE_INPUT_GATE][s] = inputGate;

    /* new candidate contribution to this cell */
    Array2d candidate(boost::extents[batchSize][outputSize]);
    processBasicLstmUnit(prevOutputThisStep, inputThisStep, weightsInput,
                         weightsOutput, biases, candidate,
                         cellMapping.at(BASIC_LSTM_CELL_CANDIDATE),
                         popnn::NonLinearityType::TANH);
    state[LSTM_FWD_STATE_CAND_TANH][s] = candidate;

    /* output gate */
    Array2d outputGate(boost::extents[batchSize][outputSize]);
    processBasicLstmUnit(prevOutputThisStep, inputThisStep, weightsInput,
                         weightsOutput, biases, outputGate,
                         cellMapping.at(BASIC_LSTM_CELL_OUTPUT_GATE),
                         popnn::NonLinearityType::SIGMOID);
    state[LSTM_FWD_STATE_OUTPUT_GATE][s] = outputGate;

    poplibs_test::gemm::hadamardProduct(forgetGate, cellState, cellState);
    poplibs_test::gemm::hadamardProduct(inputGate, candidate, candidate);
    poplibs_test::axpby::add(cellState, candidate, cellState);

    /* need to maintain the cell state for next step */
    Array2d outputThisStep = cellState;
    nonLinearity(popnn::NonLinearityType::TANH, outputThisStep);
    state[LSTM_FWD_STATE_OUTPUT_TANH][s] = outputThisStep;
    gemm::hadamardProduct(outputThisStep, outputGate, outputThisStep);

    state[LSTM_FWD_STATE_ACTS_IDX][s] = outputThisStep;
    state[LSTM_FWD_STATE_CELL_STATE_IDX][s] = cellState;
  }
}

static void computeGradients(const Array2dRef weightIn,
                             const Array2dRef weightPrev, const Array2dRef grad,
                             Array2dRef gradIn, Array2dRef gradPrev, bool acc) {
  double k = acc ? 1.0 : 0.0;
  gemm::generalMatrixMultiply(grad, weightIn, gradIn, gradIn, 1.0, k, false,
                              true);
  gemm::generalMatrixMultiply(grad, weightPrev, gradPrev, gradPrev, 1.0, k,
                              false, true);
}

void poplibs_test::lstm::basicLstmCellBackwardPass(
    const Array3dRef weightsInput, const Array3dRef weightsOutput,
    const Array3dRef gradsNextLayer, const Array2dRef prevCellState,
    const Array4dRef fwdState, Array4dRef bwdState, Array3dRef gradsPrevLayer,
    const std::vector<BasicLstmCellUnit> &cellOrder) {
  const auto sequenceSize = fwdState.shape()[1];
  const auto batchSize = fwdState.shape()[2];
  const auto outputSize = fwdState.shape()[3];
  const auto inputSize = gradsPrevLayer.shape()[2];

  assert(fwdState.shape()[0] == LSTM_NUM_FWD_STATES);
  assert(bwdState.shape()[0] == LSTM_NUM_BWD_STATES);
  assert(weightsInput.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(weightsInput.shape()[1] == inputSize);
  assert(weightsInput.shape()[2] == outputSize);
  assert(weightsOutput.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(weightsOutput.shape()[1] == outputSize);
  assert(weightsOutput.shape()[2] == outputSize);
  assert(prevCellState.shape()[0] == batchSize);
  assert(prevCellState.shape()[1] == outputSize);
  assert(fwdState.shape()[1] == sequenceSize);
  assert(fwdState.shape()[2] == batchSize);
  assert(fwdState.shape()[3] == outputSize);
  assert(bwdState.shape()[1] == sequenceSize);
  assert(bwdState.shape()[2] == batchSize);
  assert(bwdState.shape()[3] == outputSize);
  assert(gradsNextLayer.shape()[0] == sequenceSize);
  assert(gradsNextLayer.shape()[1] == batchSize);
  assert(gradsNextLayer.shape()[2] == outputSize);
  assert(gradsPrevLayer.shape()[0] == sequenceSize);
  assert(gradsPrevLayer.shape()[1] == batchSize);

  auto cellMapping = getCellMapping(cellOrder);

  // gradient of cell state for this step
  Array2d gradCellState(boost::extents[batchSize][outputSize]);
  for (auto it = gradCellState.data(),
            end = gradCellState.data() + gradCellState.num_elements();
       it != end; ++it) {
    *it = 0;
  }

  // gradient of output of this step
  Array2d gradOutput(boost::extents[batchSize][outputSize]);
  for (auto it = gradCellState.data(),
            end = gradCellState.data() + gradCellState.num_elements();
       it != end; ++it) {
    *it = 0;
  }

  for (auto i = sequenceSize; i != 0; --i) {
    const auto s = i - 1;

    Array2d sumGradOut(boost::extents[batchSize][outputSize]);
    Array2d gradOut = gradsNextLayer[s];
    axpby::add(gradOut, gradOutput, sumGradOut);

    Array2d actOutGate = fwdState[LSTM_FWD_STATE_OUTPUT_GATE][s];
    Array2d gradAtOTanhInp(boost::extents[batchSize][outputSize]);
    gemm::hadamardProduct(actOutGate, sumGradOut, gradAtOTanhInp);

    Array2d actTanhOutGate = fwdState[LSTM_FWD_STATE_OUTPUT_TANH][s];
    Array2d gradAtOutGate(boost::extents[batchSize][outputSize]);
    ;

    gemm::hadamardProduct(actTanhOutGate, sumGradOut, gradAtOutGate);

    bwdNonLinearity(popnn::NonLinearityType::TANH, actTanhOutGate,
                    gradAtOTanhInp);

    bwdNonLinearity(popnn::NonLinearityType::SIGMOID, actOutGate,
                    gradAtOutGate);

    Array2dRef gradAtCellStateSum = gradAtOTanhInp;
    axpby::add(gradAtOTanhInp, gradCellState, gradAtCellStateSum);

    Array2d actInpGate = fwdState[LSTM_FWD_STATE_INPUT_GATE][s];
    Array2d gradAtCand(boost::extents[batchSize][outputSize]);
    ;
    gemm::hadamardProduct(actInpGate, gradAtCellStateSum, gradAtCand);
    Array2d actCand = fwdState[LSTM_FWD_STATE_CAND_TANH][s];
    Array2d gradAtInpGate(boost::extents[batchSize][outputSize]);
    ;
    gemm::hadamardProduct(actCand, gradAtCellStateSum, gradAtInpGate);
    bwdNonLinearity(popnn::NonLinearityType::TANH, actCand, gradAtCand);
    bwdNonLinearity(popnn::NonLinearityType::SIGMOID, actInpGate,
                    gradAtInpGate);

    Array2d actForgetGate = fwdState[LSTM_FWD_STATE_FORGET_GATE][s];
    gemm::hadamardProduct(actForgetGate, gradAtCellStateSum, gradCellState);

    Array2d pCellAct(boost::extents[batchSize][outputSize]);

    if (s == 0) {
      pCellAct = prevCellState;
    } else {
      pCellAct = fwdState[LSTM_FWD_STATE_CELL_STATE_IDX][s - 1];
    }
    Array2d gradAtForgetGate(boost::extents[batchSize][outputSize]);
    ;

    gemm::hadamardProduct(pCellAct, gradAtCellStateSum, gradAtForgetGate);
    bwdNonLinearity(popnn::NonLinearityType::SIGMOID, actForgetGate,
                    gradAtForgetGate);

    Array2d gradIn(boost::extents[batchSize][inputSize]);
    ;
    Array2d weightsInUnit =
        weightsInput[cellMapping.at(BASIC_LSTM_CELL_FORGET_GATE)];
    Array2d weightsOutUnit =
        weightsOutput[cellMapping.at(BASIC_LSTM_CELL_FORGET_GATE)];
    computeGradients(weightsInUnit, weightsOutUnit, gradAtForgetGate, gradIn,
                     gradOutput, false);
    weightsInUnit = weightsInput[cellMapping.at(BASIC_LSTM_CELL_INPUT_GATE)];
    weightsOutUnit = weightsOutput[cellMapping.at(BASIC_LSTM_CELL_INPUT_GATE)];
    computeGradients(weightsInUnit, weightsOutUnit, gradAtInpGate, gradIn,
                     gradOutput, true);
    weightsInUnit = weightsInput[cellMapping.at(BASIC_LSTM_CELL_OUTPUT_GATE)];
    weightsOutUnit = weightsOutput[cellMapping.at(BASIC_LSTM_CELL_OUTPUT_GATE)];
    computeGradients(weightsInUnit, weightsOutUnit, gradAtOutGate, gradIn,
                     gradOutput, true);
    weightsInUnit = weightsInput[cellMapping.at(BASIC_LSTM_CELL_CANDIDATE)];
    weightsOutUnit = weightsOutput[cellMapping.at(BASIC_LSTM_CELL_CANDIDATE)];
    computeGradients(weightsInUnit, weightsOutUnit, gradAtCand, gradIn,
                     gradOutput, true);

    gradsPrevLayer[s] = gradIn;

    // save bwd state for weight update
    bwdState[BASIC_LSTM_CELL_FORGET_GATE][s] = gradAtForgetGate;
    bwdState[BASIC_LSTM_CELL_INPUT_GATE][s] = gradAtInpGate;
    bwdState[BASIC_LSTM_CELL_OUTPUT_GATE][s] = gradAtOutGate;
    bwdState[BASIC_LSTM_CELL_CANDIDATE][s] = gradAtCand;
  }
}

void poplibs_test::lstm::basicLstmCellParamUpdate(
    const Array3dRef prevLayerActs, const Array4dRef fwdState,
    const Array2dRef outputActsInit, const Array4dRef bwdState,
    Array3dRef weightsInputDeltas, Array3dRef weightsOutputDeltas,
    Array2dRef biasDeltas, const std::vector<BasicLstmCellUnit> &cellOrder) {
  const auto sequenceSize = prevLayerActs.shape()[0];
  const auto batchSize = prevLayerActs.shape()[1];
  const auto inputSize = prevLayerActs.shape()[2];
  const auto outputSize = fwdState.shape()[3];

  assert(fwdState.shape()[0] == LSTM_NUM_FWD_STATES);
  assert(fwdState.shape()[1] == sequenceSize);
  assert(fwdState.shape()[2] == batchSize);
  assert(outputActsInit.shape()[0] == batchSize);
  assert(outputActsInit.shape()[1] == outputSize);
  assert(bwdState.shape()[0] == LSTM_NUM_BWD_STATES);
  assert(bwdState.shape()[1] == sequenceSize);
  assert(bwdState.shape()[2] == batchSize);
  assert(bwdState.shape()[3] == outputSize);
  assert(weightsInputDeltas.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(weightsInputDeltas.shape()[1] == inputSize);
  assert(weightsInputDeltas.shape()[2] == outputSize);
  assert(weightsOutputDeltas.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(weightsOutputDeltas.shape()[1] == outputSize);
  assert(weightsOutputDeltas.shape()[2] == outputSize);
  assert(biasDeltas.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(biasDeltas.shape()[1] == outputSize);

  auto cellMapping = getCellMapping(cellOrder);

  for (auto it = weightsInputDeltas.data(),
            end = weightsInputDeltas.data() + weightsInputDeltas.num_elements();
       it != end; ++it) {
    *it = 0;
  }
  for (auto it = weightsOutputDeltas.data(),
            end =
                weightsOutputDeltas.data() + weightsOutputDeltas.num_elements();
       it != end; ++it) {
    *it = 0;
  }
  for (auto it = biasDeltas.data(),
            end = biasDeltas.data() + biasDeltas.num_elements();
       it != end; ++it) {
    *it = 0;
  }

  for (auto i = sequenceSize; i != 0; --i) {
    const auto s = i - 1;
    Array2d outActs(boost::extents[batchSize][outputSize]);
    if (s == 0) {
      outActs = outputActsInit;
    } else {
      outActs = fwdState[LSTM_FWD_STATE_ACTS_IDX][s - 1];
    }
    Array2d inActs = prevLayerActs[s];
    for (auto i = 0; i != BASIC_LSTM_CELL_NUM_UNITS; ++i) {
      const auto unit = static_cast<BasicLstmCellUnit>(i);

      Array2d grad = bwdState[i][s];
      Array2d wInputDeltasUnit(boost::extents[inputSize][outputSize]);

      gemm::generalMatrixMultiply(inActs, grad, wInputDeltasUnit,
                                  wInputDeltasUnit, 1.0, 0, true, false);
      for (auto ic = 0u; ic != inputSize; ++ic) {
        for (auto oc = 0u; oc != outputSize; ++oc) {
          weightsInputDeltas[cellMapping.at(unit)][ic][oc] +=
              wInputDeltasUnit[ic][oc];
        }
      }
      Array2d wOutputDeltasUnit(boost::extents[outputSize][outputSize]);

      gemm::generalMatrixMultiply(outActs, grad, wOutputDeltasUnit,
                                  wOutputDeltasUnit, 1.0, 0, true, false);
      for (auto oc1 = 0u; oc1 != outputSize; ++oc1) {
        for (auto oc2 = 0u; oc2 != outputSize; ++oc2) {
          weightsOutputDeltas[cellMapping.at(unit)][oc1][oc2] +=
              wOutputDeltasUnit[oc1][oc2];
        }
      }

      for (auto oc = 0u; oc != outputSize; ++oc) {
        for (auto b = 0u; b != batchSize; ++b) {
          biasDeltas[cellMapping.at(unit)][oc] += grad[b][oc];
        }
      }
    }
  }
}


/* MIT License
 *
 * Copyright (c) 2020 Aleksa Ilic <aleksa.d.ilic@gmail.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
*/

#include <algorithm>
#include <numeric>
#include <iostream>
#include <string>
#include <tuple>
#include <regex>

#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>
#include <boost/rational.hpp>
#include <boost/optional/optional.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/math/constants/constants.hpp>

#ifdef TESTING_ENABLED
#define CATCH_CONFIG_MAIN
#include <catch.hpp>
#include <prettyprint.hpp>
#else
#endif

#include <fort.hpp>

static constexpr auto kVersion = "v0.2.1";
static constexpr auto kProgramName = "OPNA_1: Continued Fraction Generator";
static constexpr auto kUnderlineType = '-';

// -- CHANGE THESE TYPES FOR MORE PRECISION
namespace mp = boost::multiprecision;
using IntType = mp::int1024_t;
using FloatType = mp::number<mp::cpp_dec_float<256>>;
using Fraction = boost::rational<IntType>;
using DecimalNumber = std::tuple<IntType, FloatType>;

enum class Field {
    ITERATION, INDICES, FRACTION, EVALUATED_FRACTION, DIFFERENCE
};

constexpr const char *FieldToString(Field field) {
    switch (field) {
        case Field::ITERATION:
            return "Iteration";
        case Field::INDICES:
            return "Indices";
        case Field::FRACTION:
            return "Fraction";
        case Field::EVALUATED_FRACTION:
            return "Evaluated fraction";
        case Field::DIFFERENCE:
            return "Difference";
    }
}

struct Config {
    size_t precision = 14;
    size_t iterations = 14;
    IntType max_denominator = std::numeric_limits<IntType>::max();
    bool find_in_between = false;
    bool headerless = false;
    const struct ft_border_style *table_style = FT_NICE_STYLE;
    std::vector<Field> displayed_fields = {Field::ITERATION, Field::INDICES,
                                           Field::FRACTION, Field::EVALUATED_FRACTION,
                                           Field::DIFFERENCE};
};
static const Config kDefaultConfig;

/// Pretty prints DecimalNumber to stream
std::ostream &operator<<(std::ostream &os, const DecimalNumber &number) {
    return os << '(' << std::get<0>(number) << ',' << std::get<1>(number) << ')';
}

/// Performs substring operation with range check without throwing errors
/// \param string String to be processed
/// \param position Position from which to extract
/// \param length How many characters are to be processed
/// \return Extracted substring if range check passes or boost::none
boost::optional<std::string>
SafeSubstring(const std::string &string, size_t position, size_t length = std::string::npos) {
    if (position >= string.length())
        return boost::none;
    else
        return string.substr(position, length);
}

/// Splits decimal number into whole and fraction part
/// \param numer Number to be parsed
/// \return Tuple containing whole and fraction
DecimalNumber ParseDecimalNumber(FloatType number) {
    auto a = mp::floor(number);
    auto d = number - a;
    return std::make_tuple(a.convert_to<IntType>(), d);
}

/// Split decimal number into whole and fraction part
/// \param number Number to be parsed
/// \return Tuple containing whole and fraction
DecimalNumber ParseDecimalNumber(std::string number) {
    if (number == "pi") {
        return ParseDecimalNumber(boost::math::constants::pi<FloatType>());
    } else if (number == "phi") {
        return ParseDecimalNumber(boost::math::constants::phi<FloatType>());
    } else if (number == "e") {
        return ParseDecimalNumber(boost::math::constants::e<FloatType>());
    } else if (number == "catalan") {
        return ParseDecimalNumber(boost::math::constants::catalan<FloatType>());
    }

    auto dot_index = number.find('.');
    if (dot_index == std::string::npos) {
        return std::make_tuple(std::stol(number), 0);
    }
    std::string a = number.substr(0, dot_index);
    std::string d = "0." + SafeSubstring(number, dot_index + 1).get_value_or("0");

    return std::make_tuple(std::stoll(a), FloatType(d));
}

/// Generates continued fraction indices
/// \param indices Generated indices from previous iterations
/// \param total_iterations Total number of iterations to run (max indices size)
/// \return Generated indices
std::vector<DecimalNumber> ContinuedFractionIndices(std::vector<DecimalNumber> &indices, size_t total_iterations) {
    if (indices.size() == 0) {
        throw std::invalid_argument("Indices must not be empty");
    }

    for (int i = indices.size(); i < total_iterations; i++) {
        auto[a, d] = indices.back();
        indices.push_back(ParseDecimalNumber(FloatType(1) / d));
    }

    return indices;
}

/// Generates continued fraction indices
/// \param number Decimal number for which to calculate indices
/// \param iterations Number of iterations
/// \return Generated indices
std::vector<DecimalNumber> ContinuedFractionIndices(DecimalNumber number, size_t iterations) {
    std::vector<DecimalNumber> indices;
    indices.reserve(iterations);

    indices.push_back(number);
    return ContinuedFractionIndices(indices, iterations);
}

/// Evaluate continued fraction indices to get fractional representation
/// \param indices Continued fraction indices
/// \return Evaluated fraction
Fraction ContinuedFractionEvaluator(std::vector<DecimalNumber> indices) {
    return std::accumulate(std::next(indices.rbegin()),
                           indices.rend(),
                           Fraction(std::get<0>(indices.back()), 1),
                           [](Fraction accumulator, DecimalNumber number) {
                               return 1 / accumulator + std::get<0>(number);
                           });
}

enum class EvaluatedType {
    I, II
};
struct EvaluatedIteration {
    std::vector<DecimalNumber> indices;
    Fraction fraction;
    FloatType evaluated_fraction;
    FloatType diff;
    EvaluatedType type;
};

/// Evaluates single iteration from indices and populates all relevant fields
/// \param number_searched Number for which we are deducing optimal indices
/// \param indices Generated continued fraction indices
/// \param type Type of iteration (First order/Second order)
/// \return
EvaluatedIteration
EvaluateIteration(FloatType number_searched, const std::vector<DecimalNumber> &indices,
                  EvaluatedType type) {
    Fraction fraction = ContinuedFractionEvaluator(indices);
    if (fraction < 0) {
        throw std::overflow_error(
                "Underlying type overflow. Compile with larger integer and/or floating point types.");
    }
    auto evaluated_fraction = FloatType(fraction.numerator()) / FloatType(fraction.denominator());
    auto diff = number_searched - evaluated_fraction;
    return EvaluatedIteration{indices, fraction, evaluated_fraction, diff, type};
}

void PrintEvaluatedIteration(fort::char_table &table, const EvaluatedIteration &iteration, const Config &config) {
    for (const auto &header : config.displayed_fields) {
        std::ostringstream oss;
        switch (header) {
            case Field::ITERATION:
                table << iteration.indices.size();
                break;
            case Field::INDICES:
                oss << '[';
                for (int j = 0; j < iteration.indices.size(); j++) {
                    oss << std::get<0>(iteration.indices[j]);
                    if (j < iteration.indices.size() - 1)
                        oss << ',';
                }
                oss << ']';
                table << oss.str();
                break;
            case Field::FRACTION:
                oss << iteration.fraction;
                if (iteration.type == EvaluatedType::I)
                    oss << '*';
                table << oss.str();
                break;
            case Field::EVALUATED_FRACTION:
                table << iteration.evaluated_fraction.str(config.precision, std::ios::fixed);
                break;
            case Field::DIFFERENCE:
                table << std::regex_replace(iteration.diff.str(config.precision, std::ios::scientific), std::regex("e"),
                                            " * 10^");
                break;
        }
    }
    table << fort::endr;
}

void PrintEvaluationTableHeader(fort::char_table &table, const Config &config) {
    if (!config.headerless) {
        table << fort::header;
        for (const auto &field : config.displayed_fields) {
            table << FieldToString(field);
        }
        table << fort::endr;
    }
}

void PrintEvaluationTable(const std::vector<EvaluatedIteration> &iterations, const Config &config) {
    fort::char_table table;
    table.set_border_style(config.table_style);

    auto diff_it = std::find(config.displayed_fields.begin(), config.displayed_fields.end(), Field::DIFFERENCE);
    PrintEvaluationTableHeader(table, config);

    size_t counter = 1;
    for (const auto &iteration : iterations) {
        PrintEvaluatedIteration(table, iteration, config);
        if(diff_it != config.displayed_fields.end()){
            table[counter++][std::distance(config.displayed_fields.begin(), diff_it)].set_cell_text_align(fort::text_align::right);
        }
    }

    std::cout << table.to_string();
}

/// Find indices of in-between approximations (Indices of second order)
/// \param number_searched Number for which we are deducing second order indices
/// \param indices Calculated continued fraction indices of the first order
/// \param diff Evaluated difference of indices of the first order
/// \return Vector of evaluated iterations of approximations of the second order
std::vector<EvaluatedIteration>
FindInBetweenApproximations(FloatType number_searched, std::vector<DecimalNumber> indices, FloatType diff) {
    auto from = IntType(1);
    auto to = std::get<0>(indices.back());
    indices.pop_back();

    std::vector<EvaluatedIteration> iterations;
    for (auto i = from; i < to; i++) {
        indices.push_back(std::make_tuple(i, 0));
        auto candidate = EvaluateIteration(number_searched, indices, EvaluatedType::II);
        if (mp::abs(candidate.diff) < mp::abs(diff)) {
            iterations.push_back(candidate);
        }
        indices.pop_back();
    }
    return iterations;
}

/// Processes decimal number with given configuration. Throws overflow error.
/// \param number Decimal number to be processed
/// \param config Main program configuration
/// \return Vector of evaluated iterations
std::vector<EvaluatedIteration>
ProcessDecimalNumber(DecimalNumber number, const Config &config) {
    const auto number_searched = FloatType(std::get<0>(number)) + FloatType(std::get<1>(number));
    std::vector<DecimalNumber> indices = ContinuedFractionIndices(number, 1);
    std::vector<EvaluatedIteration> evaluated_iterations;

    auto fraction = ContinuedFractionEvaluator(indices);
    auto evaluated_fraction = FloatType(fraction.numerator()) / FloatType(fraction.denominator());
    evaluated_iterations.push_back(
            {indices, fraction, evaluated_fraction, number_searched - evaluated_fraction, EvaluatedType::I});

    try {
        for (size_t iteration_number = 2; iteration_number <= config.iterations; iteration_number++) {
            ContinuedFractionIndices(indices, iteration_number);
            auto evaluated_iteration = EvaluateIteration(number_searched, indices, EvaluatedType::I);
            if (mp::abs(evaluated_iteration.diff) > mp::abs(evaluated_iterations.back().diff)) {
                throw std::overflow_error(
                        "More iterations result in bigger deviation. Compile with larger integer and/or floating point types.");
            }
            if (config.find_in_between) {
                auto in_between_aproximations = FindInBetweenApproximations(number_searched, indices,
                                                                            evaluated_iterations.back().diff);
                evaluated_iterations.insert(evaluated_iterations.end(), in_between_aproximations.begin(),
                                            in_between_aproximations.end());
            }
            evaluated_iterations.push_back(evaluated_iteration);
            if (evaluated_iterations.back().fraction.denominator() > config.max_denominator) {
                evaluated_iterations.erase(std::lower_bound(evaluated_iterations.begin(), evaluated_iterations.end(),
                                                            config.max_denominator,
                                                            [](const auto &iteration, const auto &max_denominator) {
                                                                return iteration.fraction.denominator() <=
                                                                       max_denominator;
                                                            }), evaluated_iterations.end());
                break;
            }
        }
    } catch (const std::overflow_error &err) {
        std::cerr << err.what() << " Stopped at iteration: " << indices.size() << std::endl;
    }

    std::sort(evaluated_iterations.begin(), evaluated_iterations.end(),
              [](auto &lhs, auto &rhs) {
                  return mp::abs(lhs.diff) >= mp::abs(rhs.diff);
              });

    return evaluated_iterations;
}

/// Retrieves program's base name from the exec path
static const char *GetProgramName(const char *path) {
    const char *last = path;
    while (*path++) {
        last = (*path == '/' || *path == '\\') ? path : last;
    }
    return last;
}

#ifdef TESTING_ENABLED
TEST_CASE("GetProgramName") {
    constexpr auto kExamplePath1 = "/home/ilic/opna-1/opna_1";
    constexpr auto kExamplePath2 = "C:\\Users\\My Documents\\opna_1";
    constexpr auto kExpectedName1 = "/opna_1";
    constexpr auto kExpectedName2 = "\\opna_1";

    REQUIRE(strcmp(GetProgramName(kExamplePath1), kExpectedName1) == 0);
    REQUIRE(strcmp(GetProgramName(kExamplePath2), kExpectedName2) == 0);
}
TEST_CASE("ContinuedFractionEvaluator") {
    const auto kExampleNumber = boost::math::constants::pi<FloatType>();
    const std::vector<Fraction> kExpectedResults = {{3,        1},
                                                    {22,       7},
                                                    {333,      106},
                                                    {355,      113},
                                                    {103993,   33102},
                                                    {104348,   33215},
                                                    {208341,   66317},
                                                    {312689,   99532},
                                                    {833719,   265381},
                                                    {1146408,  364913},
                                                    {4272943,  1360120},
                                                    {5419351,  1725033},
                                                    {80143857, 25510582}};
    constexpr auto kDegree = 13;
    std::vector<Fraction> results(kDegree);
    std::generate(results.begin(), results.end(), [n = 1, kExampleNumber]() mutable {
        auto indices = ContinuedFractionIndices(ParseDecimalNumber(kExampleNumber), n++);
        return ContinuedFractionEvaluator(indices);
    });

    REQUIRE(std::equal(kExpectedResults.begin(), kExpectedResults.end(), results.begin(), [](auto lhs, auto rhs) {
        INFO("Checking fraction " << lhs << " and " << rhs);
        CHECK(lhs == rhs);
        return lhs == rhs;
    }));
}
TEST_CASE("ContinuedFractionIndices") {
    const auto kExampleNumber = boost::math::constants::pi<FloatType>();
    const std::vector<IntType> kExpectedResults = {3, 7, 15, 1, 292, 1, 1, 1, 2, 1, 3, 1, 14, 2, 1, 1, 2, 2, 2, 2, 1,
                                                   84,
                                                   2, 1, 1, 15, 3, 13, 1, 4, 2, 6, 6, 99, 1, 2, 2, 6, 3, 5, 1, 1, 6, 8,
                                                   1,
                                                   7, 1, 2, 3, 7, 1, 2, 1, 1, 12, 1, 1, 1, 3, 1, 1, 8, 1, 1, 2, 1, 6, 1,
                                                   1,
                                                   5, 2, 2, 3, 1, 2, 4, 4, 16, 1, 161, 45, 1, 22, 1, 2, 2, 1};
    auto indices = ContinuedFractionIndices(ParseDecimalNumber(kExampleNumber), kExpectedResults.size());
    REQUIRE(std::equal(kExpectedResults.begin(), kExpectedResults.end(), indices.begin(), [](auto lhs, auto rhs) {
        INFO("Checking number " << lhs << " and " << rhs);
        auto comparison = lhs == std::get<0>(rhs);
        CHECK(comparison);
        return comparison;
    }));
}
TEST_CASE("ParseDecimalNumber") {
    REQUIRE(ParseDecimalNumber("3.14") == std::make_tuple(3, FloatType("0.14")));
    REQUIRE(ParseDecimalNumber(1.5L) == std::make_tuple(1, 0.5L));
    REQUIRE(ParseDecimalNumber("125") == std::make_tuple(125, 0.0));
    REQUIRE_THROWS(ParseDecimalNumber("NaN"));
}
#else

int main(int argc, const char *argv[]) {
    namespace po = boost::program_options;

    Config config = kDefaultConfig;

    auto program_name = GetProgramName(argv[0]);
    std::string number;
    std::string table_style;
    std::string fields;

    // Declare the supported options.
    po::options_description desc("Allowed options");
    desc.add_options()
            ("help", "print help message")
            ("version", "print version information")
            ("examples", "show examples")
            ("table", "print evaluation table of every iteration")
            ("table_style", po::value<std::string>(&table_style), "Specify table style: nice|double|simple|empty")
            ("headerless", "Do not display header when showing results")
            ("fields", po::value<std::string>(&fields),
             "Comma delimited list of fields to be shown: iter,ind,frac,eval,diff")
            ("maxdenominator", po::value<IntType>(&config.max_denominator),
             "maximum denominator up to which to iterate")
            ("inbetween", "show best in-between continual fraction approximations as well")
            ("iterations", po::value<size_t>(&config.iterations)->default_value(kDefaultConfig.iterations),
             "number of iterations")
            ("precision", po::value<size_t>(&config.precision)->default_value(kDefaultConfig.precision),
             "how many decimals should result have")
            ("number", po::value<std::string>(&number), "number to be parsed");

    po::positional_options_description pos_desc;
    pos_desc.add("number", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(pos_desc).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << "Usage: ." << program_name << " [OPTIONS] <NUMBER|CONSTANT> \n"
                  << desc << '\n'
                  << "Allowed constants: pi, phi, e, catalan \n";
        return 0;
    }

    if (vm.count("version")) {
        std::ostringstream name_header;
        name_header << kProgramName << ' ' << kVersion;

        std::string underline(name_header.str().size(), kUnderlineType);

        std::cout << name_header.str() << "\n"
                  << underline << "\n"
                  << "Integer type w/: " << std::numeric_limits<IntType>::digits << "bits\n"
                  << "Floating type w/: " << std::numeric_limits<FloatType>::digits10 << "digits\n";
        return 0;
    }

    if (vm.count("examples")) {
        fort::char_table table;
        table.set_border_style(FT_EMPTY_STYLE);

        const std::vector<std::tuple<std::string, std::string>> examples = {
                std::make_tuple("pi",
                                "Evaluates pi with default (" + std::to_string(config.iterations) + ") iterations"),
                std::make_tuple("pi --table --iterations 5 --precision 10",
                                "Print evaluation table with 5 iterations and up to 10 decimal places for pi expansion"),
                std::make_tuple("phi --table --maxdenominator 400 --inbetween",
                                "Print evaluation table for phi where maximum fraction approximation denominator is less than or equal to 400"),
                std::make_tuple("phi --table --inbetween",
                                "Print evaluation table for phi with default (" + std::to_string(config.iterations) +
                                ") iterations and also find best in-between approximations")
        };

        table << fort::header
              << "" << "Command" << "Description" << fort::endr;

        for (size_t i = 0; i < examples.size(); i++) {
            const auto&[command, description] = examples[i];
            table << i + 1 << std::string(".") + program_name + " " + command << description << fort::endr;
        }

        std::cout << table.to_string();
        return 0;
    }

    if (!vm.count("number")) {
        std::cerr << "You need to specify a number. Use --help for usage information \n";
        return 1;
    }

    if (vm.count("inbetween")) {
        config.find_in_between = true;
    }

    if (vm.count("headerless")) {
        config.headerless = true;
    }

    if (vm.count("table_style")) {
        if (boost::iequals(table_style, "nice")) {
            config.table_style = FT_NICE_STYLE;
        } else if (boost::iequals(table_style, "simple")) {
            config.table_style = FT_SIMPLE_STYLE;
        } else if (boost::iequals(table_style, "double")) {
            config.table_style = FT_DOUBLE2_STYLE;
        } else if (boost::iequals(table_style, "empty")) {
            config.table_style = FT_EMPTY_STYLE;
        } else {
            std::cerr << "Incorrect table style supplied: " << table_style << ". Use --help for usage information \n";
            return 1;
        }
    }

    if (vm.count("fields")) {
        config.displayed_fields.clear();
        std::vector<std::string> field_vector;
        boost::split(field_vector, fields, [](char c) { return c == ','; });
        for (const auto &field:field_vector) {
            if (boost::iequals(field, "iter")) {
                config.displayed_fields.push_back(Field::ITERATION);
            } else if (boost::iequals(field, "ind")) {
                config.displayed_fields.push_back(Field::INDICES);
            } else if (boost::iequals(field, "frac")) {
                config.displayed_fields.push_back(Field::FRACTION);
            } else if (boost::iequals(field, "eval")) {
                config.displayed_fields.push_back(Field::EVALUATED_FRACTION);
            } else if (boost::iequals(field, "diff")) {
                config.displayed_fields.push_back(Field::DIFFERENCE);
            } else {
                std::cerr << "Incorrect field name supplied: " << field << ". Use --help for usage information \n";
                return 1;
            }
        }
    }

    auto evaluated_iterations = ProcessDecimalNumber(ParseDecimalNumber(number), config);
    if (vm.count("table")) {
        PrintEvaluationTable(evaluated_iterations, config);
    } else {
        const auto &eval = evaluated_iterations.back();

        fort::char_table table;
        table.set_border_style(config.table_style);
        PrintEvaluationTableHeader(table, config);
        PrintEvaluatedIteration(table, eval, config);

        std::cout << table.to_string();
    }
}

#endif


#include <boost/test/unit_test.hpp>
#include "algorithms/dp/collect_max_gold_from_grid.hpp"

BOOST_AUTO_TEST_SUITE(TestCollectMaxGold)

    BOOST_AUTO_TEST_CASE(test_input) {
        {
            const int expected = 24;
            const std::vector<std::vector<int>> grid = {
                {0, 6, 0},
                {5, 8, 7},
                {0, 9, 0}
            };

            Algo::DP::CollectMaxGold collector;
            BOOST_CHECK(expected == collector.getMaximumGold(grid));
        }

        {
            const int expected = 60;
            const std::vector<std::vector<int>> grid = {
                {1,0,7,0,0,0},
                {2,0,6,0,1,0},
                {3,5,6,7,4,2},
                {4,3,1,0,2,0},
                {3,0,5,0,20,0}
            };

            Algo::DP::CollectMaxGold collector;
            BOOST_CHECK(expected == collector.getMaximumGold(grid));
        }
    }

BOOST_AUTO_TEST_SUITE_END()


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

  Copyright 2016 NumScale SAS

  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/constant/sqrteps.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/as.hpp>
#include <scalar_test.hpp>

STF_CASE_TPL( "Check sqrteps behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::sqrteps;
  using boost::simd::Sqrteps;

  STF_TYPE_IS(decltype(Sqrteps<T>()), T);
  STF_EQUAL(Sqrteps<T>(), T(1));
  STF_EQUAL(sqrteps( as(T{}) ),T(1));
}

STF_CASE_TPL( "Check sqrteps behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::sqrteps;
  using boost::simd::Sqrteps;
  using boost::simd::Eps;

  STF_TYPE_IS(decltype(Sqrteps<T>()), T);
  auto z1 = Sqrteps<T>();
  STF_ULP_EQUAL(z1*z1, Eps<T>(), 0.5);
  auto z2 = sqrteps( as(T{}) );
  STF_ULP_EQUAL(z2*z2, Eps<T>(), 0.5);

}


#include <iostream>
#include <sys/time.h>
#include <Eigen/Core>
#include <fstream>
#include <cmath>

#include "celerite/celerite.h"
#include "celerite/carma.h"
#include "celerite/utils.h"
#include "../include/KF.h"
#include "../include/ndsho.h"
#include "../include/dsho.h"

using namespace Eigen;
using namespace std;
#define TWOPI 6.283185307179586

// This program benchmarks a single DSHO using celerite and gpstate

// Timer for the benchmark.
double get_timestamp ()
{
  struct timeval now;
  gettimeofday (&now, NULL);
  return double(now.tv_usec) * 1.0e-6 + double(now.tv_sec);
}

// Function to read a whitespace separated file
std::vector<double> load_csv (const std::string & path) {
    std::ifstream indata;
    indata.open(path);
    std::string line;
    std::vector<double> values;
    uint rows = 0;
    while (std::getline(indata, line)) {
        std::stringstream lineStream(line);
        std::string cell;
        while (std::getline(lineStream, cell, ' ')) {
            //cout << std::stod(cell) << endl;
            values.push_back(std::stod(cell));
        }
        ++rows;
    }
    return values;
}

int main ()
{

  //std::vector<double> values = load_csv("two_comp_dsho.txt");
  std::vector<double> values = load_csv("GPtest_dsin.txt");

  VectorXd times = Map<VectorXd, 0, InnerStride<2> > (values.data(), 10000);
  VectorXd yi = Map<VectorXd, 0, InnerStride<2> > (values.data()+1, 10000);
  VectorXd yierr = VectorXd::Ones(yi.size());
  yierr *= 0.05;
  VectorXd diagi = yierr.array()*yierr.array();

  //set up the single DSHO parameters we use as a base
  double omega0 = TWOPI;
  double Q = 1000000.0;
  double varf = 0.05 * 0.05;

  Eigen::VectorXd alpha_real, beta_real;

  size_t N = 2;
  Eigen::VectorXd omega0_arr(N), Q_arr(N), varf_arr(N);
  Eigen::VectorXd alpha_complex_real_arr(N), alpha_complex_imag_arr(N), beta_complex_real_arr(N), beta_complex_imag_arr(N);
  double log_likelihood=0.0, celerite_ll=0.0, exact_likelihood=0.0;

  int nterms = 3;

  for (size_t i=0; i<N; i++)
  {
    if (i == 0)
    {
      omega0_arr(i) = omega0;
    }
    else
    {
      omega0_arr(i) = omega0 + static_cast<double>(i);
    }
    Q_arr(i) = Q;
    varf_arr(i) = varf;
    Eigen::VectorXd carma_arparams(nterms);
    Eigen::VectorXd carma_maparams(nterms-1);
    carma_arparams << omega0_arr(i)*omega0_arr(i), omega0_arr(i)/Q, 1.0;
    carma_maparams << 1.0, 0.0;

    double temp = std::sqrt(4.0*Q_arr(i)*Q_arr(i) - 1.0);
    //double S0 = varf_arr(i)* std::pow(Q_arr(i),-2) * std::sqrt(M_PI) / std::sqrt(2);
    double S0 = varf_arr(i)* std::pow(omega0_arr(i),-4) * std::sqrt(M_PI) / std::sqrt(2);
    alpha_complex_real_arr(i) = S0 * omega0_arr(i) * Q_arr(i);
    alpha_complex_imag_arr(i) = S0 * omega0_arr(i) * Q_arr(i) / temp;
    beta_complex_real_arr(i) = 0.5 * omega0_arr(i) / Q_arr(i);
    beta_complex_imag_arr(i) = 0.5 * temp * omega0_arr(i) / Q_arr(i);
  }

  celerite::solver::CholeskySolver<double> solver;

  solver.compute(0.0, alpha_real, beta_real, alpha_complex_real_arr, alpha_complex_imag_arr, beta_complex_real_arr, beta_complex_imag_arr, times, diagi);
  celerite_ll = -0.5*(solver.dot_solve(yi) + solver.log_determinant() + times.rows() * log(2.0 * M_PI));

  gpstate::n_dsho::N_DSHOSolver ndsho(times,yi,yierr,omega0_arr, Q_arr, varf_arr);
  log_likelihood = ndsho.KF_log_likelihood();

  // Calculate known likelihood
  for (size_t k=0; k<times.size(); k++)
  {
      double delta2 = (yi(k)-sin(omega0_arr(0)*times(double(k))) - sin(omega0_arr(1)*times(double(k)))) / 0.05;
      //double delta2 = 0.0;
      exact_likelihood += ( (-0.5*log(TWOPI*0.05*0.05)) - 0.5*delta2*delta2 );
  }
  // Print the results.
  std::cout << N;
  std::cout << " ";
  std::cout << celerite_ll;
  std::cout << " ";
  std::cout << log_likelihood;
  std::cout << " ";
  std::cout << exact_likelihood;
  std::cout << " ";
  std::cout << "\n";

}