#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 (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];
        }
      }
    }
  }
}


//
// Created by david on 2019-05-27.
//

#include <complex.h>
#undef I

#include <Eigen/QR>
#include <Eigen/SVD>
#include <general/class_tic_toc.h>
#include <math/svd.h>

//
// Created by david on 2021-03-26.
//
/*! \brief Performs SVD on a matrix
 *  This function is defined in cpp to avoid long compilation times when having Eigen::BDCSVD included everywhere in headers.
 *  Performs rigorous checks to ensure stability of DMRG.
 *  In some cases Eigen::BCDSVD/JacobiSVD will fail with segfault. Here we use a patched version of Eigen that throws an error
 *  instead so we get a chance to catch it and use lapack svd instead.
 *   \param mat_ptr Pointer to the matrix. Supported are double * and std::complex<double> *
 *   \param rows Rows of the matrix
 *   \param cols Columns of the matrix
 *   \param rank_max Maximum number of singular values
 *   \return The U, S, and V matrices (with S as a vector) extracted from the Eigen::BCDSVD SVD object.
 */
template<typename Scalar>
std::tuple<svd::solver::MatrixType<Scalar>, svd::solver::VectorType<Scalar>, svd::solver::MatrixType<Scalar>, long>
    svd::solver::do_svd_eigen(const Scalar *mat_ptr, long rows, long cols, std::optional<long> rank_max) {
    if(not rank_max.has_value()) rank_max = std::min(rows, cols);

    svd::log->trace("Starting SVD with Eigen");
    Eigen::Map<const MatrixType<Scalar>> mat(mat_ptr, rows, cols);

    if(rows <= 0) throw std::runtime_error(fmt::format("SVD error: rows = {}", rows));
    if(cols <= 0) throw std::runtime_error(fmt::format("SVD error: cols = {}", cols));

#ifndef NDEBUG
    // These are more expensive debugging operations
    if(not mat.allFinite()) throw std::runtime_error("SVD error: matrix has inf's or nan's");
    if(mat.isZero(0)) throw std::runtime_error("SVD error: matrix is all zeros");
    if(mat.isZero(1e-12)) svd::log->warn("Lapacke SVD Warning\n\t Given matrix elements are all close to zero (prec 1e-12)");
#endif

    Eigen::BDCSVD<MatrixType<Scalar>> SVD;

    // Setup the SVD solver
    SVD.setSwitchSize(static_cast<int>(switchsize));
    SVD.setThreshold(threshold);
    bool use_jacobi = std::min(rows, cols) < static_cast<long>(switchsize);
    svd::log->trace("Running SVD with threshold {:.4e} | switchsize {} | size {}", threshold, switchsize, rank_max.value());
    if(use_jacobi) {
        // We only use Jacobi for precision. So we use all the precision we can get.
        svd::log->debug("Running Eigen::JacobiSVD threshold {:.4e} | switchsize {} | rank_max {}", threshold, switchsize, rank_max.value());
        // Run the svd
        t_jac->tic();
        SVD.compute(mat, Eigen::ComputeFullU | Eigen::ComputeFullV | Eigen::FullPivHouseholderQRPreconditioner);
        t_jac->toc();
    } else {
        svd::log->debug("Running Eigen::BDCSVD threshold {:.4e} | switchsize {} | rank_max {}", threshold, switchsize, rank_max.value());
        // Run the svd
        t_svd->tic();
        SVD.compute(mat, Eigen::ComputeThinU | Eigen::ComputeThinV);
        t_svd->toc();
    }
    if(count) count.value()++;
    long max_size = std::min(SVD.singularValues().size(), rank_max.value());
    long rank     = (SVD.singularValues().head(max_size).array() >= threshold).count();
    svd::log->trace("Truncating singular values");
    if(rank == SVD.singularValues().size()) {
        truncation_error = 0;
    } else {
        truncation_error = SVD.singularValues().tail(SVD.singularValues().size() - rank).norm();
    }

    if(SVD.rank() <= 0 or rank == 0 or not SVD.matrixU().leftCols(rank).allFinite() or not SVD.singularValues().head(rank).allFinite() or
       not SVD.matrixV().leftCols(rank).allFinite()) {
        throw std::runtime_error(fmt::format(FMT_COMPILE("Eigen SVD error \n"
                                                         "  svd_threshold    = {:.4e}\n"
                                                         "  Truncation Error = {:.4e}\n"
                                                         "  Rank             = {}\n"
                                                         "  Dims             = ({}, {})\n"
                                                         "  A all finite     : {}\n"
                                                         "  U all finite     : {}\n"
                                                         "  S all finite     : {}\n"
                                                         "  V all finite     : {}\n"),
                                             threshold, truncation_error, rank, rows, cols, mat.allFinite(), SVD.matrixU().leftCols(rank).allFinite(),
                                             SVD.singularValues().head(rank).allFinite(), SVD.matrixV().leftCols(rank).allFinite()));
    }
    svd::log->trace("SVD with Eigen finished successfully");

    return std::make_tuple(SVD.matrixU().leftCols(rank), SVD.singularValues().head(rank), SVD.matrixV().leftCols(rank).adjoint(), rank);
}

//! \relates svd::class_SVD
//! \brief force instantiation of do_svd for type 'double'
template std::tuple<svd::solver::MatrixType<double>, svd::solver::VectorType<double>, svd::solver::MatrixType<double>, long>
    svd::solver::do_svd_eigen(const double *, long, long, std::optional<long>);

using cplx = std::complex<double>;
//! \relates svd::class_SVD
//! \brief force instantiation of do_svd for type 'std::complex<double>'
template std::tuple<svd::solver::MatrixType<cplx>, svd::solver::VectorType<cplx>, svd::solver::MatrixType<cplx>, long>
    svd::solver::do_svd_eigen(const cplx *, long, long, std::optional<long>);


#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
#include <ctime>
#include <climits>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/types/sba/types_six_dof_expsmap.h>

struct
{
    Eigen::Vector3d pose_world;
    float grayscale;
    Measurement(Eigen::Vector3d p, float g) :
};

Eigen::Vector project2Dto3D(int x , int y, int d, float fx, float fy, float cx, float cy, float scale){
    float zz = float(d) / scale;
    float xx = zz * (x - cx) / fx;
    float yy = zz * (y - cy ) / fy;
    return Eigen::Vector3d(xx, yy, zz);
}

Eigen::Vector2d project3Dto2D(float x, float y, float z, float fx, float fy, float cx, float cy){
    float u = fx*x/z + cx;
    float v = fy*y/z + cy;
    return Eigen::Vector2d(u, v);
}

class EdgeSE3ProjectUVDirect: public: BaseUnaryEdge<1, doublem VertexSE3Expmap>{
    private:
        Eigen::Vector3d x_world_;
        float cx_ = 0, cy_ = 0, fx_ = 0, fy_ = 0;
        Mat* image_ = nullptr;

    protected:
    float getPixelValue(float x, float y){
        uchar* data = & data[int(y) * issmage_->step + int(x)];
        float xx = x - floor(x);
        float yy = y - floor(y);
        return float (
                   ( 1-xx ) * ( 1-yy ) * data[0] +
                   xx* ( 1-yy ) * data[1] +
                   ( 1-xx ) *yy*data[ image_->step ] +
                   xx*yy*data[image_->step+1]);
    }
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeSE3ProjectUVDirect(Eigen::Vector3d point, float fx, float fy, float cx, float cy, Mat *img):
                x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image ) {}

    virtual void computeError(){
        const VertexSE3Expmap *v = static_cast<const VertexSE3Expmap*> (_vertices[0]);
        Eigen::Vector3d x_local = v->estimate().map(x_world_);
        float x = x_local[0]*fx_/x_local[2] + cx_;
        float y = x_local[1] * fy_/x_local[2] + cy_;

        if (x-4 < 0 || x + 4 > image_->cols || y-4 < 0 || y + 4 > image_->rows) {
            _error (0, 0) = 0.0;
            this->setLevel(1);
        }
        else{
            _error(0, 0) = getPixelValue(x, y) - _measurement;
        }
    }

    virtual void linearOplus(){
        if(level() = 1){
            _jacobianOplusXi = Eigen:Matrix<double, 1, 6>::Zero();
            return;
        }

        VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> (_vertices[0]);
        Eigen::Vector3d xyz_trans = vtx->estimate().map(x_world_);

        double x = xyz_trans[0];
        double y = xyz_trans[1];

        double invz = 1.0 / xyz_trans[2];
        double invz_2 = invz*invz;

        float u = x*fx_*invz + cx_;
        float v = y*fy_*invz + cy_;

        // Jacobian from SE3 to uv, the matrix is (2, 6)
        // In g2o, the Lie algebra is translation in first three
        Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;

        jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
        jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
        jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
        jacobian_uv_ksai ( 0,3 ) = invz *fx_;
        jacobian_uv_ksai ( 0,4 ) = 0;
        jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;

        jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
        jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
        jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
        jacobian_uv_ksai ( 1,3 ) = 0;
        jacobian_uv_ksai ( 1,4 ) = invz *fy_;
        jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;

        Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
        // simple image gradient
        jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
        jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;

        _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;

    }
}

bool poseEstimationDirect( const vector<Measurement> & measurements, Mat& gray; Eigen::Matrix3f &K, Eigen::Isometry3d &Tcw){
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;
    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense<DirectBlock::PoseMatrixType>();
    DirectBlock *solver_ptr = new DirectBlock(linearSolver);

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);
<<<<<<< HEAD

=======

>>>>>>> d3aa7eab7a66633ee6b6716534884a999f4bfb7a
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setEstimate(g2o::SE3Quat(Tcw.rotation(), Tcw.translation()));
    pose->setId(0);
    optimizer.addVertex(pose);

    int id = 1;
    for ( Measurement mm: measurements){
        EdgeSE3ProjectUVDirect* edge = new EdgeSE3ProjectUVDirect(
            mm.pose_world,
            K(0,0), K(1,1), K(0,2), K(1,2), gray
        );
        edge->setVertex(0, pose);
        edge->setMeasurement(mm.grayscale);       //based on our assumption of garyscale consistency
        edge->setInformation(Eigen::Matrix<double, 1,1>::Identity());
        edge->setId(id++);
        optimizer.addEdge(edge);
    }

    cout << "edge number in a graph: " << optimizer.edges().size() << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);
    Tcw = pose->estimate();
}

int main(int argc, char** argv){
    if ( argc != 2 )
    {
        cout<<"usage: useLK path_to_dataset"<<endl;
        return 1;
    }
    srand ( ( unsigned int ) time ( 0 ) );
    string path_to_dataset = argv[1];
    string associate_file = path_to_dataset + "/associate.txt";

    ifstream fin ( associate_file );

    string rgb_file, depth_file, time_rgb, time_depth;
    cv::Mat color, depth, gray;
    vector<Measurement> measurements;
    // Camera intrinstic parameters
    float cx = 325.5;
    float cy = 253.5;
    float fx = 518.0;
    float fy = 519.0;
    float depth_scale = 1000.0;
    Eigen::Matrix3f K;
    K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;

    Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();

    cv::Mat prev_color;
    for(int index = 0; index<10;index++){
        fin>> time_rgb >> rgb_file >> time_depth >> depth_file;
        color = cv::imread(path_to_dataset+"/"+rgb_file);
        depth = imread(path_to_dataset + "/" + depth_file, -1);
        if(color.data == nullptr || depth.data == nullptr) continue;

        cvtColor(color, gray, cv::COLOR_BGR2GRAY);

        if(index == 0){
            vector<cv::KeyPoint> keypoints;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect(color, keypoints);
            for (auto kp: keypoints){
               if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
                    continue;
                ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];
                if ( d==0 )
                    continue;
                Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
                float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
                measurements.push_back ( Measurement ( p3d, grayscale ) );
            }
            prev_color = color.clone();
            continue;
        }
        // 使用直接法计算相机运动
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        poseEstimationDirect ( measurements, &gray, K, Tcw );
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
        cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl;
        cout<<"Tcw="<<Tcw.matrix() <<endl;

        // plot the feature points
        cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );
        prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );
        color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );
        for ( Measurement m:measurements )
        {
            if ( rand() > RAND_MAX/5 )
                continue;
            Eigen::Vector3d p = m.pos_world;
            Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );
            Eigen::Vector3d p2 = Tcw*m.pos_world;
            Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );
            if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )
                continue;

            float b = 255*float ( rand() ) /RAND_MAX;
            float g = 255*float ( rand() ) /RAND_MAX;
            float r = 255*float ( rand() ) /RAND_MAX;
            cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );
            cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );
            cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), cv::Scalar ( b,g,r ), 1 );
        }
        cv::imshow ( "result", img_show );
        cv::waitKey ( 0 );

    }
    return 0;
    }
}


/*
 neighbours.cxx

 Copyright (c) 2018 Guy Skinner

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "neighbour.hxx"
#include "utils.hxx"

Neighbour::Neighbour(unsigned long nbas,
                     ublas::matrix<double> plat,
                     ublas::matrix<double> basis) {
  NumberOfAtoms = nbas;
  LatticeVectors = plat;
  BasisVectors = basis;
}

Neighbour::~Neighbour() {
}

void Neighbour::SetInverseLattice(ublas::matrix<double> iplat,
                                  ublas::matrix<double> xbasis) {
  InverseLatticeVectors = iplat;
  InverseBasisVectors = xbasis;
}

void Neighbour::GetSiteList(double rmax) {

  /* Create Site List */
  ublas::vector<long> rcut(3);
  ublas::vector<long> sext(3);

  for (auto i = 0; i < 3; i++) {
    ublas::matrix_row<ublas::matrix<double>> p(LatticeVectors,i);
    rcut(i) = static_cast<long>(rmax/ublas::norm_2(p)+2);
    sext(i) = 2*rcut(i) + 1;
  }

  SupercellExtension = sext;
  /* Linked-List Block Size */
  Neighbour::LinkedListBlocks(rmax);

  ublas::vector<double> cent(3);
  for (auto i = 0; i < 3; i++) cent(i) = 0.5*(sext(i) - 1);
  Centre = cent;

  //long product = boost::accumulate(sext,1,std::multiplies<long>());
  long smax = NumberOfAtoms*product(sext);
  long blks = boost::accumulate(Blocks,1,std::multiplies<long>());

  ublas::matrix<double> sites(smax,3);
  ublas::vector<long> basptr(smax);
  ublas::vector<long> linkl(smax);
  ublas::vector<long> head(blks);
  ublas::vector<double> sitex(3);

  /* Make Sure Zero Vectors */
  head.clear();
  linkl.clear();

  ublas::matrix_row<ublas::matrix<double>> p0(LatticeVectors,0);
  ublas::matrix_row<ublas::matrix<double>> p1(LatticeVectors,1);
  ublas::matrix_row<ublas::matrix<double>> p2(LatticeVectors,2);

  long icnt = 0;
  for (auto i = 0; i < NumberOfAtoms; i++) {
    ublas::matrix_row<ublas::matrix<double>> b(BasisVectors,i);
    ublas::matrix_row<ublas::matrix<double>> xb(InverseBasisVectors,i);
    for (auto j = -rcut(0); j <= rcut(0); j++) {
      for (auto k = -rcut(1); k <= rcut(1); k++) {
        for (auto l = -rcut(2); l <= rcut(2); l++) {
          for (auto a = 0; a < 3; a++) {
            sites(icnt,a) = b(a) + j*p0(a) + k*p1(a) + l*p2(a);
            sitex(a) = xb(a);
          }
          basptr(icnt) = i;
          sitex(0) += static_cast<double>(j);
          sitex(1) += static_cast<double>(k);
          sitex(2) += static_cast<double>(l);
          unsigned long icell = FindCell(sitex);
          linkl(icnt) = head(icell);
          head(icell) = icnt;
          icnt++;
        }
      }
    }
  }

  Sites = sites;
  BasisPointers = basptr;
  Head = head;
  LinkedList = linkl;

}

void Neighbour::LinkedListBlocks(double rmax) {

  /* Decompose Lattice Vector Matrix into Lattice Vectors */
  ublas::matrix_row<ublas::matrix<double>> p1(LatticeVectors,0);
  ublas::matrix_row<ublas::matrix<double>> p2(LatticeVectors,1);
  ublas::matrix_row<ublas::matrix<double>> p3(LatticeVectors,2);

  /* Cross Product of Lattice Vectors */
  ublas::vector<double> c1 = cross3(p2,p3);
  ublas::vector<double> c2 = cross3(p3,p1);
  ublas::vector<double> c3 = cross3(p1,p2);

  ublas::vector<double> w(3);
  w(0) = ublas::inner_prod(p1,c1)/ublas::norm_2(c1);
  w(1) = ublas::inner_prod(p2,c2)/ublas::norm_2(c2);
  w(2) = ublas::inner_prod(p3,c3)/ublas::norm_2(c3);

  ublas::vector<long> blks(3);
  for (auto i = 0; i < 3; i++) {
    blks(i) = static_cast<long>(SupercellExtension(i)*w(i)/rmax);
    if (blks(i) < 3) blks(i) = 3;
  }

  Blocks = blks;

}

void Neighbour::GetNhbrList(double rmax) {

  /* Calculate Site List */
  Neighbour::GetSiteList(rmax);

  ublas::vector<long> tot(NumberOfAtoms);
  ublas::vector<long> ptrs(650*NumberOfAtoms);

  unsigned long nsum = 0;
  for (auto i = 0; i < NumberOfAtoms; i++) {
    tot(i) = 0;
    ublas::matrix_row<ublas::matrix<double>> b(BasisVectors,i);
    ublas::matrix_row<ublas::matrix<double>> bx(InverseBasisVectors,i);
    unsigned long icell = FindCell(bx);
    for (auto imx = -1; imx <= 1; imx++) {
      for (auto imy = -Blocks(0); imy <= Blocks(0); imy += Blocks(0)) {
        for (auto imz = -Blocks(0)*Blocks(1);
             imz <= Blocks(0)*Blocks(1);
             imz += Blocks(0)*Blocks(1)) {
          long jcell = icell + imx + imy + imz;
          long j = Head(jcell);
          while (j != 0) {
            ublas::matrix_row<ublas::matrix<double>> site(Sites,j);
            ublas::vector<double> ra = site - b;
            double r = ublas::norm_2(ra);
            if (r <= rmax and r > 0.0) {
              ptrs(nsum) = j;
              tot(i)++;
              nsum++;
            }
            j = LinkedList(j);
          }
        }
      }
    }
  }

  Total = tot;
  Pointers = ptrs;

}

unsigned long Neighbour::FindCell(ublas::vector<double> sitex){

  /* Find Block that Atom Belongs In */
  ublas::vector<long> ixs(3);
  ublas::vector<double> scent = sitex + Centre;

  for (auto i = 0; i < 3; i++) {
    ixs(i) = static_cast<long>(Blocks(i)*scent(i)/SupercellExtension(i));
    if (ixs(i) > Blocks(i)) ixs(i) = Blocks(i)-1;
  }

  unsigned long icell = ixs(0) + Blocks(0)*ixs(1) + Blocks(1)*Blocks(0)*ixs(2);
  return icell;

}


// [[Rcpp::depends(BH)]]

#include <boost/multi_array.hpp>
#include <Rcpp.h>
#include <fstream>
#include <iostream>

using namespace Rcpp;
using namespace boost;

// [[Rcpp::export]]
NumericVector cohort_sim(const List s_fp) {
  const List s_ss = s_fp["ss"];
  const int n_year(s_ss["PROJ_YEARS"]), n_sex(2), n_age(66), n_agr(9), n_cd4(7);
  const int n_ts(10);
  const double dt(0.1);

  // Map single age indices (0=15..65=80) to age groups
  const int a2g[n_age] = {
    0, 0, 1, 1, 1, 2, 2, 2, 2, 2, // 15-16 = 0, 17-19 = 1, 20-24 = 2
    3, 3, 3, 3, 3, 4, 4, 4, 4, 4, // 25-29 = 3, 30-34 = 4
    5, 5, 5, 5, 5, 6, 6, 6, 6, 6, // 35-39 = 5, 40-44 = 6
    7, 7, 7, 7, 7, 8, 8, 8, 8, 8, // 45-49 = 7, 50-54 = 8
    8, 8, 8, 8, 8, 8, 8, 8, 8, 8, // 55-64 = 8
    8, 8, 8, 8, 8, 8, 8, 8, 8, 8, // 65-74 = 8
    8, 8, 8, 8, 8, 8};            // 75-80 = 8

  multi_array_ref<double, 3> surv(REAL(s_fp["Sx"]), extents[n_year][n_sex][n_age]);
  multi_array_ref<double, 3> inci(REAL(s_fp["new_inf"]), extents[n_year][n_sex][n_age]);
  multi_array_ref<double, 3> in_dist(REAL(s_fp["cd4_initdist"]), extents[n_sex][n_agr][n_cd4]);
  multi_array_ref<double, 3> in_prog(REAL(s_fp["cd4_prog"]), extents[n_sex][n_agr][n_cd4-1]);
  multi_array_ref<double, 3> in_mort(REAL(s_fp["cd4_mort"]), extents[n_sex][n_agr][n_cd4]);

  multi_array<double, 3> dist(extents[n_sex][n_age][n_cd4]);
  multi_array<double, 3> prog(extents[n_sex][n_age][n_cd4-1]);
  multi_array<double, 3> mort(extents[n_sex][n_age][n_cd4]);

  double hivpop[n_year * n_sex * n_age * n_cd4];
  multi_array_ref<double, 4> Y(hivpop, extents[n_year][n_sex][n_age][n_cd4]);
  double influx[n_cd4], efflux[n_cd4];

  memset(hivpop, 0, n_year * n_sex * n_age * n_cd4 * sizeof(double));

  for (int si(0); si < n_sex; ++si) {
    for (int ai(0); ai < n_age; ++ai) {
      for (int hi(0); hi < n_cd4; ++hi) dist[si][ai][hi] = in_dist[si][a2g[ai]][hi];
      for (int hi(0); hi < n_cd4; ++hi) mort[si][ai][hi] = in_mort[si][a2g[ai]][hi] * dt; // 1.0 - exp(-in_mort[si][a2g[ai]][hi] * dt);
      for (int hi(0); hi < n_cd4-1; ++hi) prog[si][ai][hi] = in_prog[si][a2g[ai]][hi] * dt; // 1.0 - exp(-in_prog[si][a2g[ai]][hi] * dt);
    }
  }

  // Year 1 simulation
  for (int si(0); si < n_sex; ++si) {
    for (int ai(0); ai < n_age; ++ai) {
      for (int hi(0); hi < n_cd4; ++hi) {
        Y[0][si][ai][hi] = inci[0][si][ai] * dist[si][ai][hi];
      }
    }
  }

  // Subsequent year simulation
  for (int yi(1); yi < n_year; ++yi) {

    // Aging and non-HIV mortality
    for (int si(0); si < n_sex; ++si) {
      for (int hi(0); hi < n_cd4; ++hi) {
        for (int ai(1); ai < n_age; ++ai) Y[yi][si][ai][hi] = Y[yi-1][si][ai-1][hi] * surv[yi][si][ai];
        Y[yi][si][n_age-1][hi] += Y[yi-1][si][n_age-1][hi] * surv[yi][si][n_age-1];
      }
    }

    // HIV disease progression and mortality
    for (int ti(0); ti < n_ts; ++ti) {
      for (int si(0); si < n_sex; ++si) {
        for (int ai(0); ai < n_age; ++ai) {
          efflux[n_cd4-1] = Y[yi][si][ai][n_cd4-1] * mort[si][ai][n_cd4-1];
          for (int hi(0); hi < n_cd4-1; ++hi) efflux[hi] = Y[yi][si][ai][hi] * (prog[si][ai][hi] + mort[si][ai][hi]);
          for (int hi(1); hi < n_cd4; ++hi) influx[hi] = Y[yi][si][ai][hi-1] * prog[si][ai][hi-1];
          for (int hi(0); hi < n_cd4; ++hi) Y[yi][si][ai][hi] += influx[hi] - efflux[hi];
        }
      }
    }

    // Distribute new HIV infections
    for (int si(0); si < n_sex; ++si) {
      for (int ai(0); ai < n_age; ++ai) {
        for (int hi(0); hi < n_cd4; ++hi) Y[yi][si][ai][hi] += inci[yi][si][ai] * dist[si][ai][hi];
      }
    }
  }

  NumericVector rval(n_year * n_sex * n_age * n_cd4);
  for (int i(0); i < rval.length(); ++i) rval[i] = hivpop[i];
  return rval; // in R, convert to array via array(rval, c(n_cd4, n_age, n_sex, n_year))
}


/*
 * ietl.cpp
 *
 *  Created on: Jan 19, 2012
 *      Author: david
 */

#include "../Common.hpp"
#include "../as_range.hpp"
#include <boost/graph/adjacency_list.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <ietl/interface/ublas.h>
#include <ietl/vectorspace.h>
#include <ietl/lanczos.h>
#include <boost/random.hpp>
#include <boost/limits.hpp>
#include <limits>
#include <iostream>
#include <vector>
#include <cmath>

namespace graphseg { namespace detail {

std::vector<EigenComponent> solver_ietl(const SparseMatrix& A, unsigned int num_ev)
{
	typedef boost::numeric::ublas::symmetric_matrix<
		double, boost::numeric::ublas::lower> Matrix;
	typedef boost::numeric::ublas::vector<double> Vector;

	int N = A.dim;
	Matrix mat(N, N);
	for(int i=0;i<N;i++)
		for(int j=0;j<=i;j++)
			mat(i,j) = 0;
	for(auto& e : A.entries)
		mat(e.i,e.j) = e.weight;

	typedef ietl::vectorspace<Vector> Vecspace;
	typedef boost::lagged_fibonacci607 Gen;

	Vecspace vec(N);
	Gen mygen;
	ietl::lanczos<Matrix,Vecspace> lanczos(mat,vec);

	// Creation of an iteration object:
	int max_iter = 10*N;
	double rel_tol = 50*std::numeric_limits<double>::epsilon();
	double abs_tol = 0.00001f;// std::pow(std::numeric_limits<double>::epsilon(),2./3);
	std::cout << "Computation of 2 lowest converged eigenvalues\n\n";
	std::cout << "-----------------------------------\n\n";
	int n_lowest_eigenval = num_ev;
	std::vector<double> eigen;
	std::vector<double> err;
	std::vector<int> multiplicity;
	ietl::lanczos_iteration_nlowest<double> iter(max_iter, n_lowest_eigenval, rel_tol, abs_tol);
	try{
		lanczos.calculate_eigenvalues(iter,mygen);
		//lanczos.more_eigenvalues(iter);
		eigen = lanczos.eigenvalues();
		err = lanczos.errors();
		multiplicity = lanczos.multiplicities();
		std::cout<<"number of iterations: "<<iter.iterations()<<"\n";
	}
	catch (std::runtime_error& e) {
		std::cout << e.what() << "\n";
	}

  // Printing eigenvalues with error & multiplicities:
	std::cout << "#        eigenvalue            error         multiplicity\n";
	std::cout.precision(10);
	for (int i=0;i<eigen.size();++i)
		std::cout << i << "\t" << eigen[i] << "\t" << err[i] << "\t" << multiplicity[i] << "\n";

	// call of eigenvectors function follows:
	std::cout << "\nEigen vectors computations for lowest eigenvalues:\n\n";
	auto ew_begin = eigen.begin();
	while(*ew_begin <= 0.0f) ew_begin ++;
	auto ew_end = ew_begin + num_ev;
	std::vector<Vector> eigenvectors; // for storing the eigen vectors.
	ietl::Info<double> info; // (m1, m2, ma, eigenvalue, residualm, status).

	try {
		lanczos.eigenvectors(ew_begin,ew_end,std::back_inserter(eigenvectors),info,mygen);
	}
	catch (std::runtime_error& e) {
		std::cout << e.what() << "\n";
	}

	// std::cout << "Printing eigenvectors:\n\n";
	// for(std::vector<Vector>::iterator it = eigenvectors.begin();it!=eigenvectors.end();it++){
	// 	std::copy((it)->begin(),(it)->end(),std::ostream_iterator<double>(std::cout,", "));
	// 	std::cout << "\n\n";
	// }
	std::cout << " Information about the eigenvector computations:\n\n";
	for(int i = 0; i < info.size(); i++) {
		std::cout << " m1(" << i+1 << "): " << info.m1(i) << ", m2(" << i+1 << "): "
			<< info.m2(i) << ", ma(" << i+1 << "): " << info.ma(i) << " eigenvalue("
			<< i+1 << "): " << info.eigenvalue(i) << " residual(" << i+1 << "): "
			<< info.residual(i) << " error_info(" << i+1 << "): "
			<< info.error_info(i) << "\n\n";
	}

  	std::vector<EigenComponent> solution(eigenvectors.size());
	for(unsigned int i=0; i<solution.size(); i++) {
		EigenComponent& cmp = solution[i];
		cmp.eigenvalue = *(ew_begin + i);
#ifdef SPECTRAL_VERBOSE
		std::cout << "Eigenvalue " << i << ": " << cmp.eigenvalue << std::endl;
		cmp.eigenvector = Eigen::VectorXf(N);
#endif
		for(unsigned int j=0; j<N; j++) {
			// convert back to generalized eigenvalue problem!
			cmp.eigenvector[j] = eigenvectors[i][j];
		}
	}
#ifdef SPECTRAL_VERBOSE
	std::cout << "Sparse Solver: returning" << std::endl;
#endif
	return solution;
}

}}


#ifndef _HIGHER_ORDER_ENERGY_HPP_
#define _HIGHER_ORDER_ENERGY_HPP_

/*
 * higher-order-energy.hpp
 *
 * Copyright 2012 Alexander Fix
 * See LICENSE.txt for license information
 *
 * A representation for higher order pseudo-boolean energy functions
 *
 * Built up by calls to AddVar and AddTerm
 * Main operation is the reduction to quadratic form, ToQuadratic()
 * ToQuadratic() impements the reduction described in
 *  A Graph Cut Algorithm for Higher Order Markov Random Fields
 *  Alexander Fix, Artinan Gruber, Endre Boros, Ramin Zabih, ICCV 2011
 *
 * Template parameters:
 *  Energy   The result type of the energy function
 *  D        The maximum degree of any monomial
 *
 * Example usage:
 *
 * To represent the function
 *      f(x_0, ..., x_3) = 7x_0 + 9x_1 - 8x_2 + 13x_3 + 27x_1x_2x_3
 *                       - 31x_1x_3x_4 + 18x_1x_2x_3x_4
 * we can do the following:
 *
 * HigherOrderEnergy<int, 4> f;
 * f.AddVars(4);
 * f.AddUnaryTerm(0, 7);
 * f.AddUnaryTerm(1, 9);
 * f.AddUnaryTerm(2, -8);
 * f.AddUnaryTerm(3, 13);
 *
 * VarId term1[] = {1, 2, 3};
 * VarId term2[] = {1, 3, 4};
 * VarId term3[] = {1, 2, 3, 4};
 * f.AddTerm(27, 3, term1);
 * f.AddTerm(-31, 3, term2);
 * f.AddTerm(18, 4, term3);
 *
 *
 * Then, we can get an equivalent quadratic form (by adding additional
 * variables and performing transformations) by calling ToQuadratic. We can
 * solve this using QPBO, or any other quadratic optimizer that has the same
 * interface.
 *
 * QPBO<int> qr;
 * f.ToQuadratic(qr);
 * qr.Solve();
 *
 *
 */

#include <vector>
#include <list>
#include <boost/foreach.hpp>

template <typename R, int D>
class HigherOrderEnergy {
    public:
        typedef int VarId;

        // Constructs empty HigherOrderEnergy with no variables or terms
        HigherOrderEnergy();

        // Adds variables to the HigherOrderEnergy. Variables must be added
        // before any terms referencing them can be added
        VarId AddVar();
        VarId AddVars(int n);
        VarId NumVars() const { return _varCounter; }

        // Adds a monomial to the HigherOrderEnergy. degree must be <= D
        // vars is an array of length at least degree, with the indices of
        // the corresponding variables or literals in the monomial
        void AddTerm(R coeff, int degree, const VarId vars[]);

        void AddUnaryTerm(VarId v, R coeff);

        // Reduces the HigherOrderEnergy to quadratic form
        // NOTE: THIS IS A DESTRUCTIVE OPERATION, so do not rely on the
        // HigherOrderEnergy being in any useful state after this operation.
        //
        // This is a templated function, so it will work with any class
        // implementing the necessary interface. See quadratic-rep.hpp for
        // the minimal requirements.
        template <typename QR>
        void ToQuadratic(QR &qr);

        void Clear();

    private:
        struct Term
        {
            R coeff;
            int degree;
            VarId vars[D];

            Term(R _coeff, int _degree, const VarId _vars[])
                : coeff(_coeff), degree(_degree)
            {
                for (int i = 0; i < degree; ++i)
                    vars[i] = _vars[i];
            }

            bool operator<(const Term& t) const;
            bool operator==(const Term& t) const;

            std::string ToString() const;

        };
        static int Compare(int d1,
                const VarId vars1[],
                int d2,
                const VarId vars2[]);

        struct VarRecord {
            VarRecord(VarId id)
                : _id(id), _positiveTerms(0), _higherOrderTerms(0),
                _quadraticTerms(0), _sumDegrees(0), _terms(), _coeff(0) { }
            VarId _id;
            int _positiveTerms;
            int _higherOrderTerms;
            int _quadraticTerms;
            int _sumDegrees;
            std::list<Term> _terms;
            R _coeff;

            void PrintTerms() const;
        };

        R _constantTerm;

        void RemoveTerm(Term* tp);
        void _EliminatePositiveTerms();
        template <typename QR>
        void _ReduceNegativeTerms(QR& qr);
        void _ReportMultilinearStats();

        size_t NumTerms() const {
            size_t numTerms = 0;
            BOOST_FOREACH(const VarRecord& vr, _varRecords)
                numTerms += vr._terms.size();
            return numTerms;
        }

        VarId _varCounter;

        typedef std::vector<VarRecord> varRecordVec_t;
        varRecordVec_t _varRecords;
};

template <typename R, int D>
inline HigherOrderEnergy<R, D>::HigherOrderEnergy()
    : _constantTerm(0), _varCounter(0), _varRecords()
{ }

template <typename R, int D>
inline typename HigherOrderEnergy<R, D>::VarId
HigherOrderEnergy<R, D>::AddVar() {
    VarRecord vr(_varCounter);
    _varRecords.push_back(vr);
    return _varCounter++;
}

template <typename R, int D>
inline typename HigherOrderEnergy<R, D>::VarId
HigherOrderEnergy<R, D>::AddVars(int n) {
    VarId firstVar = _varCounter;
    for (int i = 0; i < n; ++i)
        this->AddVar();
    return firstVar;
}

template <typename R, int D>
inline void
HigherOrderEnergy<R, D>::AddTerm(R coeff, int d, const VarId vars[]) {
    if(coeff == 0) {
        return;
    } else if (d == 0) {
        _constantTerm += coeff;
        return;
    } else if (d == 1) {
        _varRecords[vars[0]]._coeff += coeff;
        return;
    } else {
        VarRecord& smallestVarRec = _varRecords[vars[0]];
        typename std::list<Term>::iterator it = smallestVarRec._terms.begin();
        int compareVars = 1;
        while (it != smallestVarRec._terms.end()) {
            compareVars = Compare(d, vars, it->degree, it->vars);
            if (compareVars == 0) {
                break;
            } else if (compareVars < 0) {
                break;
            } else {
                ++it;
            }
        }
        if (compareVars == 0) {
            it->coeff += coeff;
        } else {
            if (d > 2) {
                smallestVarRec._higherOrderTerms++;
                smallestVarRec._sumDegrees += d;
            } else {
                smallestVarRec._quadraticTerms++;
            }
            smallestVarRec._terms.insert(it, Term(coeff, d, vars));
        }
        if (coeff > 0)
            smallestVarRec._positiveTerms++;
        return;
    }
}

template <typename R, int D>
inline void HigherOrderEnergy<R, D>::AddUnaryTerm(VarId var, R coeff) {
    _varRecords[var]._coeff += coeff;
}

template <typename R, int D>
void HigherOrderEnergy<R, D>::_EliminatePositiveTerms() {
    size_t numVars = _varRecords.size();
    for (size_t varIndex = 0; varIndex < numVars; ++varIndex) {
        R positiveSum = 0;
        VarId newPosVar = AddVar();

        VarRecord& vr = _varRecords[varIndex];

        typename std::list<Term>::iterator termIt = vr._terms.begin();
        VarId newVars[D];
        while (termIt != vr._terms.end()) {
            Term& t = *termIt;
            //std::cout << "\t" << t.ToString() << std::endl;
            typename std::list<Term>::iterator currIt = termIt;
            ++termIt;

            if (t.coeff > 0) {
                positiveSum += t.coeff;
                for (int i = 0; i < t.degree - 1; ++i) {
                    newVars[i] = t.vars[i+1];
                    assert(newVars[i] >= vr._id);
                }
                AddTerm(t.coeff, t.degree - 1, newVars);
                newVars[t.degree - 1] = newPosVar;
                AddTerm(-t.coeff, t.degree, newVars);
                if (t.degree == 2) {
                    vr._quadraticTerms--;
                } else {
                    vr._higherOrderTerms--;
                    vr._sumDegrees -= t.degree;
                }
                vr._terms.erase(currIt);
            }
        }
        VarId quadratic[2];
        quadratic[0] = vr._id;
        quadratic[1] = newPosVar;
        AddTerm(positiveSum, 2, quadratic);
    }
}

template <typename R, int D>
template <typename QR>
void HigherOrderEnergy<R, D>::_ReduceNegativeTerms(QR& qr) {
    // Estimate expected size of quadratic problem. Only nodes/edges are
    // created below, so we can count them ahead of time
    int expectedVars = _varCounter;
    int expectedEdges = 0;
    BOOST_FOREACH(const VarRecord& vr, _varRecords) {
        expectedVars += vr._higherOrderTerms;
        expectedEdges += vr._quadraticTerms;
        expectedEdges += vr._sumDegrees;
    }

    //std::cout << "\tExpected Vars: " << expectedVars << "\tExpected Edges: " << expectedEdges << std::endl;

    qr.SetMaxEdgeNum(expectedEdges);
    qr.AddNode(_varCounter);

    // Term-by-term reduction from Friedman & Drineas
    BOOST_FOREACH(VarRecord& vr, _varRecords) {
        BOOST_FOREACH(Term& t, vr._terms) {
            if (t.degree == 2) {
                qr.AddPairwiseTerm(t.vars[0], t.vars[1], 0, 0, 0, t.coeff);
            } else {
                typename QR::NodeId w = qr.AddNode();
                assert(t.coeff <= 0);
                for (int i = 0; i < t.degree; ++i) {
                    qr.AddPairwiseTerm(t.vars[i], w, 0, 0, 0, t.coeff);
                }
                qr.AddUnaryTerm(w, 0, t.coeff*(1-t.degree));
            }
        }
    }
    BOOST_FOREACH(VarRecord& vr, _varRecords) {
        qr.AddUnaryTerm(vr._id, 0, vr._coeff);
    }
}

template <typename R, int D>
template <typename QR>
inline void HigherOrderEnergy<R, D>::ToQuadratic(QR& qr) {
    _EliminatePositiveTerms();
    _ReduceNegativeTerms(qr);
}

template <typename R, int D>
inline int HigherOrderEnergy<R, D>::Compare(int d1, const VarId vars1[], int d2, const VarId vars2[]) {
    if (d1 < d2)
        return -1;
    if (d1 > d2)
        return 1;
    for (int index = 0; index < d1; ++index) {
        if (vars1[index] != vars2[index])
            return (vars1[index] < vars2[index]) ? -1 : 1;
    }
    return 0;
}

template <typename R, int D>
inline void HigherOrderEnergy<R, D>::Clear() {
    _varRecords.clear();
};

#endif


/**
 * @file k_means_clustering_engine.hpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-06-26
 */
#ifndef PAAL_K_MEANS_CLUSTERING_ENGINE_HPP
#define PAAL_K_MEANS_CLUSTERING_ENGINE_HPP

#include "paal/utils/type_functions.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/irange.hpp"

#include <boost/range/combine.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/range/algorithm/for_each.hpp>

#include <vector>
#include <random>
#include <iostream>

namespace paal {

/**
 * @param lrange
 * @param rrange
 * @tparam RangeLeft
 * @tparam RangeRight
 */
template <class RangeLeft, class RangeRight>
auto distance_square(RangeLeft && lrange, RangeRight && rrange) {
    assert(!boost::empty(lrange));
    assert(boost::distance(lrange) == boost::distance(rrange));

    //TODO change to sum_functors when generic lambdas appears
    decltype(*std::begin(lrange) * *std::begin(rrange)) dist{};
    for (auto point_pair : boost::combine(lrange, rrange)) {
        auto diff = boost::get<0>(point_pair) - boost::get<1>(point_pair);
        dist += diff * diff;
    }
    return dist;
}

/**
 * @param point
 * @param centers
 * @tparam Point
 * @tparam Centers
 */
template <class Point, class Centers>
auto closest_to(Point && point, Centers && centers){
    using coor_t = range_to_elem_t<Point>;
    auto dist = std::numeric_limits<coor_t>::max();
    int new_center = 0;
    for (auto center : centers | boost::adaptors::indexed()){
        auto new_dist = distance_square(center.value(), point);

        if (new_dist < dist) {
            dist = new_dist;
            new_center = center.index();
        }
    }
    return new_center;
}

///k means visitor
struct k_means_visitor {
    /**
    * @param last_center
    * @param new_center
    * @tparam Center
    * @tparam New_center
    */
    template <class Center, class New_center>
    void move_center(Center &last_center, New_center &new_center) {};
    ///new iteration
    void new_iteration() {};
};

/**
 * @param points
 * @param centers
 * @param centroid functor return centroid of set of samples
 * @param closest_to
 * @param result pairs of point and id of cluster
 * (number from 0,1,2 ...,k-1)
 * @param c_equal
 * @param visitor
 * @tparam Points
 * @tparam Centers
 * @tparam Centroid
 * @tparam ClosestTo
 * @tparam OutputIterator
 * @tparam CentroidEqual
 * @tparam Visitor
 */
template <class Points,
          class Centers,
          class Centroid,
          class ClosestTo, class OutputIterator,
          class CentroidEqual = utils::equal_to,
          class Visitor=k_means_visitor >
auto k_means(Points &&points, Centers & centers,
             Centroid centroid, ClosestTo closest_to,
             OutputIterator result,
             CentroidEqual c_equal = CentroidEqual{},
             Visitor visitor=Visitor{}) {
    using point_t = range_to_elem_t<Points>;
    using points_bag = std::vector<point_t>;

    std::vector<points_bag> cluster_points;
    cluster_points.resize(centers.size());
    bool zm;
    do {
        visitor.new_iteration();
        zm = false;
        boost::for_each(cluster_points, std::mem_fn(&points_bag::clear));

        for (auto && point : points) {
            cluster_points[closest_to(point)].push_back(point);
        }

        for (auto point : cluster_points | boost::adaptors::indexed()) {
            if(point.value().empty()) continue;
            auto && old_center = centers[point.index()];
            auto && new_center = centroid(point.value());
            if (!c_equal(new_center, old_center)) {
                visitor.move_center(old_center, new_center);
                old_center = new_center;
                zm = true;
            }
        }
    } while (zm == true);
    for (int cur_cluster : irange(cluster_points.size())) {
        for (auto const & point : cluster_points[cur_cluster]) {
            *result = std::make_pair(point, cur_cluster);
            ++result;
        }
    }
    return centers;
}

/**
 * @param points
 * @param number_of_centers
 * @tparam Points
 */
template <typename Points, typename OutputIterator, typename RNG = std::default_random_engine>
auto get_random_centers(Points &&points, int number_of_centers, OutputIterator out,
                        RNG && rng = std::default_random_engine{}) {

    std::vector<int> centers(points.size());
    boost::iota(centers, 0);
    std::shuffle(centers.begin(),centers.end(), rng);
    centers.resize(number_of_centers);
    for (auto && center : centers) {
        *out=points[center];
        ++out;
    }
}

/**
 * @param points
 * @param number_of_clusters
 * @tparam Points
 */
template <typename Points, typename RNG = std::default_random_engine>
auto get_random_clusters(Points &&points, int number_of_clusters,
                         RNG && rng = std::default_random_engine{}) {
    std::vector<typename std::decay<Points>::type> clusters(number_of_clusters);
    std::uniform_int_distribution<> dis(0, number_of_clusters - 1);

    for (auto o : points) {
        clusters[distribution(rng)].push_back(o);
    }
    return clusters;
}

} //!paal

#endif /* PAAL_K_MEANS_CLUSTERING_ENGINE_HPP */


///////////////////////////////////////////////////////////////////////////////
// ordered_sample.hpp                                                        //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_EMPIRICAL_DISTRIBUTION_ORDERED_SAMPLE_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_EMPIRICAL_DISTRIBUTION_ORDERED_SAMPLE_HPP_ER_2010
#include <map>
#include <functional>

#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/accumulator.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace empirical_distribution{

    // Usage:
    //     namespace ns = empirical_distribution;
    //     accumulator_set<T,stats<ns::tag::ordered_sample> > acc;
    //     acc = boost:for_each(samples,acc);
    //     ns::extract::ordered_sample(acc);
    // The result of the last statement is a sorted map associating each value
    // in the sample (of type T) with their number of occurences in the sample.

namespace impl{

    template<typename T>
    class ordered_sample : public boost::accumulators::accumulator_base{
        typedef std::less<T> comp_;
        typedef std::size_t size_;
        typedef boost::accumulators::dont_care dont_care_;
        typedef std::map<T,size_,comp_> map_;

        public:

        // See accumulator_set for convention naming sample_type
        typedef T 		sample_type;
        typedef size_ 	size_type;

        // non-const because map::operator[](key) returns a non-const
        typedef map_& result_type;

        ordered_sample(dont_care_){}

        template<typename Args>
        void operator()(const Args& args){
        	++(this->freq[
            		static_cast<T>(
                        args[boost::accumulators::sample]
                    )
                ]
            );
        }

        // Returns the entire distribution, represented by a map
        result_type result(dont_care_)const{
            return (this->freq);
        }

        private:
        mutable map_ freq;
	};

}// impl

namespace tag
{

    struct ordered_sample
      : boost::accumulators::depends_on<>
    {
      typedef statistics::detail::empirical_distribution::
      	impl::ordered_sample<boost::mpl::_1> impl;
    };
}// tag

namespace result_of{

    template<typename AccSet>
    struct ordered_sample : boost::accumulators::detail::extractor_result<
        AccSet,
        boost::statistics::detail::empirical_distribution::tag::ordered_sample
    >{};

}// result_of

namespace extract
{

  	template<typename AccSet>
    typename boost::statistics::detail::empirical_distribution
    	::result_of::template ordered_sample<AccSet>::type
  	ordered_sample(AccSet const& acc)
    {
    	typedef boost::statistics::detail::empirical_distribution
    		::tag::ordered_sample the_tag;
        return boost::accumulators::extract_result<the_tag>(acc);
  	}

}// extract

using extract::ordered_sample;

}// empirical_distribution
}// detail
}// statistics
}// boost

#endif


#include "CubicInterpolation/CubicSplines.h"
#include "CubicInterpolation/InterpolantBuilder.h"

#include <boost/math/differentiation/finite_difference.hpp>
#include <boost/math/interpolators/cardinal_cubic_b_spline.hpp>
#include <boost/serialization/access.hpp>
#include <vector>

namespace cubic_splines {

/**
 * @brief Storage class to write and load the interpolation tables from  disk.
 * After reading and writing the object will be destructed.
 */
template <typename T> struct CubicSplines<T>::StorageData {
  std::vector<T> y;
  T lower_lim_derivate;
  T upper_lim_derivate;

  friend class boost::serialization::access;
  template <class Archive> void serialize(Archive &ar, const unsigned int) {
    ar &y;
    ar &lower_lim_derivate;
    ar &upper_lim_derivate;
  };

public:
  StorageData() = default;

  template <typename T1>
  StorageData(T1 const &_y, T _lower_lim_derivate, T _upper_lim_derivate)
      : y(_y.begin(), _y.end()), lower_lim_derivate(_lower_lim_derivate),
        upper_lim_derivate(_upper_lim_derivate){};

  auto to_runtime_data() const {
    return RuntimeData(y, lower_lim_derivate, upper_lim_derivate);
  }
};

template <typename T>
struct CubicSplines<T>::RuntimeData : public CubicSplines<T>::StorageData {

  boost::math::interpolators::cardinal_cubic_b_spline<T> spline;

  RuntimeData() = default;

  template <typename T1>
  RuntimeData(T1 _data, T _lower_lim_derivate, T _upper_lim_derivate)
      : StorageData(_data, _lower_lim_derivate, _upper_lim_derivate),
        spline(_data.data(), _data.size(), 0, 1, _lower_lim_derivate,
               _upper_lim_derivate) {}

  auto to_storage_data() const { return StorageData(*this); };
};

template <typename T>
CubicSplines<T>::CubicSplines(CubicSplines::RuntimeData _data)
    : data(::std::make_unique<CubicSplines::RuntimeData>(_data)) {}

template <typename T>
CubicSplines<T>::CubicSplines(Definition const &def, std::string path,
                              std::string filename) {
  try {
    auto storage_data = load<CubicSplines>(path, filename);
    *this = CubicSplines(storage_data.to_runtime_data());
  } catch (std::system_error const &ex) {
    if (ex.code().value() != ENOENT)
      throw(ex);
    *this = CubicSplines(def);
    save(data->to_storage_data(), path, filename);
  }
}

template <typename T>
CubicSplines<T>::CubicSplines(Definition const &def)
    : data(::std::make_unique<CubicSplines::RuntimeData>()) {
  using boost::math::differentiation::finite_difference_derivative;
  auto func = [&def](T x) {
    auto fx = def.f(x);
    if (def.f_trafo)
      fx = def.f_trafo->transform(fx);
    return fx;
  };
  auto y = std::vector<T>(def.axis->required_nodes());
  for (size_t n = 0; n < y.size(); ++n)
    y[n] = func(def.axis->back_transform(n));
  auto f_derivate = [func, axis = def.axis.get()](T t) {
    return func(axis->back_transform(t));
  };
  auto diff_low = finite_difference_derivative(f_derivate, static_cast<T>(0));
  auto diff_up = finite_difference_derivative(f_derivate, static_cast<T>(y.size() - 1));
  data = ::std::make_unique<CubicSplines::RuntimeData>(y, diff_low, diff_up);
}

template <typename T> T CubicSplines<T>::evaluate(T x) const { return data->spline(x); };

template <typename T> T CubicSplines<T>::prime(T x) const {
  return data->spline.prime(x);
};

template <typename T> T CubicSplines<T>::double_prime(T x) const {
  return data->spline.double_prime(x);
};
} // namespace cubic_splines

template class cubic_splines::CubicSplines<float>;
template class cubic_splines::CubicSplines<double>;


#include <vector>
#include <boost/math/distributions/uniform.hpp>
#include "uniform_dist.h"

stochastic::UniformDistribution::UniformDistribution(double lower, double upper)
  : Distribution(),
    lower_bound_{lower},
    upper_bound_{upper},
    distribution_{lower, upper}
{}

std::vector<double> stochastic::UniformDistribution::cumulative_dist_func(
    const std::vector<double>& locations) const {
  std::vector<double> evaluations(locations.size());

  for (unsigned int i = 0; i < locations.size(); ++i) {
    evaluations[i] = cdf(distribution_, locations[i]);
  }

  return evaluations;
}

std::vector<double> stochastic::UniformDistribution::inv_cumulative_dist_func(
    const std::vector<double>& probabilities) const {
  std::vector<double> evaluations(probabilities.size());

  for (unsigned int i = 0; i < probabilities.size(); ++i) {
    evaluations[i] = quantile(distribution_, probabilities[i]);
  }

  return evaluations;
}