/* Copyright 2017 Battelle Energy Alliance, LLC

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

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

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/
/*
 *
 *  Created on: April 28, 2012
 *      Author: MANDD
 *
 *      Tests  : None for the custom
 *
 *      Problems : None
 *      Issues  : None
 *      Complaints : None
 *      Compliments : None
 *
 *      source: Numerical Recipes in C++ 3rd edition
 *
 */

#include <sstream>
#include <fstream>
#include <ctime>
#include <cstdlib>
#include <vector>
#include <iostream>
#include <string>
#include <iostream>
#include <cmath> // to use erfc error function
#include <ctime> // for rand() and srand()
#include <cstdio>

#include "distribution_1D.h"
#include "distributionFunctions.h"

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

//#include <Eigen/Dense>
#include <Eigen/SVD>
#define throwError(msg) { std::cerr << "\n\n" << msg << "\n\n"; throw std::runtime_error("Error"); }

#define _USE_MATH_DEFINES

/*
extern "C" {
    // LU decomoposition of a general matrix
    void dgetrf_(int* M, int *N, double* A, int* lda, int* IPIV, int* INFO);

    // generate inverse of a matrix given its LU decomposition
    void dgetri_(int* N, double* A, int* lda, int* IPIV, double* WORK, int* lwork, int* INFO);
}
*/

typedef boost::numeric::ublas::matrix<double> matrixDouble;

void matrixConversionBoost(const std::vector<std::vector<double> > & original, matrixDouble & converted)
{
  converted.resize(original.size(),original.at(0).size());
  for(unsigned int r=0; r < original.size(); r++)
  {
    for(unsigned int c=0; c < original.at(r).size(); c++)
    {
      converted(r,c)  = original.at(r).at(c);
    }
  }
}

/*
void matrixConversion(std::vector<std::vector<double> > original, double converted[]){
        if (original.size() == original[0].size()){
                int dimensions = original.size();
                for (int r=0; r<dimensions; r++)
                        for (int c=0; c<dimensions; c++){
                                converted[r*dimensions+c] = original[r][c];
                        }
        }else
                throwError("Error in matrixConversion: matrix is not squared.");
}
*/

void matrixBackConversionBoost(const matrixDouble & original, std::vector<std::vector<double> > & converted)
{
  for(unsigned int r=0; r < original.size1(); r++)
  {
    for(unsigned int c = 0; c < original.size2(); c++)
    {
      converted.at(r).at(c) = original(r,c);
    }
  }
}

/*
void matrixBackConversion(double original[], std::vector<std::vector<double> > converted){
        int dimensions = int(sizeof(original)/sizeof(double));
        dimensions = sqrt(dimensions);

        for (int r=0; r<dimensions; r++)
                for (int c=0; c<dimensions; c++)
                        converted[r][c] = original[r*dimensions+c];
}
*/

/*
//http://stackoverflow.com/questions/3519959/computing-the-inverse-of-a-matrix-using-lapack-in-c
void inverseMatrix(double* a, int n)
{
    int *IPIV = new int[n+1];
    int LWORK = n*n;
    double *WORK = new double[LWORK];
    int INFO;

    dgetrf_(&n,&n,a,&n,IPIV,&INFO);
    dgetri_(&n,a,&n,IPIV,WORK,&LWORK,&INFO);

    delete IPIV;
    delete WORK;
}
*/

//Roughly based on http://savingyoutime.wordpress.com/2009/09/21/c-matrix-inversion-boostublas/ and libs/numeric/ublas/test/test_lu.cpp
void invertMatrixBoost(matrixDouble & a, matrixDouble & aInverted)
{
  boost::numeric::ublas::permutation_matrix<std::size_t> pm(a.size1());

  boost::numeric::ublas::lu_factorize<matrixDouble,
                                      boost::numeric::ublas::permutation_matrix<> >(a, pm);

  aInverted.assign(boost::numeric::ublas::identity_matrix<double>(a.size1()));

  boost::numeric::ublas::lu_substitute(a, pm, aInverted);
}

void computeInverse(const std::vector<std::vector<double> > & matrix, std::vector<std::vector<double> > & inverse){
        int dimensions = matrix.size();
        matrixDouble A(dimensions,dimensions),inverted(dimensions,dimensions);
        matrixConversionBoost(matrix, A);
        invertMatrixBoost(A,inverted);
        matrixBackConversionBoost(inverted, inverse);
}

// Convert the vector of covariance to vector of vector of covariance
void  vectorToMatrix(unsigned int &rows,unsigned int &columns,std::vector<double> &vec_matrix, std::vector<std::vector<double> > &cov_matrix) {
        /** Input Parameter
         * vec_matrix: covariance matrix stored in a vector
         * Output Parameter
         * rows: the first dimension of the covariance matrix
         * columns: the second dimension of the covariance matrix
         * cov_matrix: covariance matrix stored in vector<vector<double> >
         */
        unsigned int dimensions = vec_matrix.size();
        dimensions = std::lround(std::sqrt(dimensions));
        rows = dimensions;
        columns = dimensions;
        if(rows*columns != vec_matrix.size())
                      throwError("MultivariateNormal error: covariance matrix in is not a square matrix.");
        for (unsigned int row = 0; row < rows; ++row) {
                std::vector<double> temp;
                for (unsigned int colm = 0; colm < columns; ++colm) {
                        temp.push_back(vec_matrix.at(colm+row*columns));
                }
                cov_matrix.push_back(temp);
        }
}

//See for example http://en.wikipedia.org/wiki/LU_decomposition or
// http://programmingexamples.net/wiki/CPP/Boost/Math/uBLAS/determinant
double getDeterminantBoost(matrixDouble & a)
{
  boost::numeric::ublas::permutation_matrix<std::size_t> pm(a.size1());
  double determinant = 1.0;

  int result = boost::numeric::ublas::lu_factorize<matrixDouble,boost::numeric::ublas::permutation_matrix<> >(a, pm);

  if(result)
  {
    return 0.0;
  }
  //det(a) = det(P^-1)det(L)det(U) = (-1)^S*(product(u_ii))
  //Where S is the number of row exchanges
  for(unsigned int i = 0; i < a.size1(); i++)
  {
    //Multiple by current diagonal entry
    determinant *= a(i,i);
    if(i != pm(i))
    {
      //Found a row exchange
      determinant *= -1;
    }

  }
  return determinant;
}

double getDeterminant(std::vector<std::vector<double> > matrix){
        int dimensions = matrix.size();
        matrixDouble A(dimensions,dimensions);

        matrixConversionBoost(matrix, A);

        return getDeterminantBoost(A);

}

/*
double getDeterminant(std::vector<std::vector<double> > matrix){
        int dimensions = matrix.size();
        double A [dimensions*dimensions];

        matrixConversion(matrix, A);

    int *IPIV = new int[dimensions+1];
    int LWORK = dimensions*dimensions;
    double *WORK = new double[LWORK];
    int INFO;

    dgetrf_(&dimensions,&dimensions,A,&dimensions,IPIV,&INFO);

    double determinant =1;

    for(int index=0; index<dimensions; index++)
        determinant *= A[index*dimensions];

    delete IPIV;
    delete WORK;

        return determinant;
}
*/

void svdDecomposition(const std::vector<std::vector<double> > &matrix, std::vector<std::vector<double> > &left_singular_vectors, std::vector<std::vector<double> > &right_singular_vectors, std::vector<double> &singular_values, std::vector<std::vector<double> > &transformed_matrix) {
  /**
   * This function compute the singular value decomposition for given matrix
   * Input Parameters
   * matrix: provided data
   * Output Parameters
   * left_singular_vectors: stores the left singular vectors for given matrix
   * right_singular_vectors: stores the right singular vectors for given matrix
   * singular_values: stores the singular values for given matrix
   * transformed_matrix: stores the transformation matrix
   */
  unsigned int row = matrix.size();
  unsigned int col = matrix.at(0).size();
  unsigned int dim = 0;
  if(row > col) {
    dim = col;
  }else {
    dim = row;
  }
  Eigen::MatrixXd A(row,col);
  Eigen::MatrixXd U(row,row);
  Eigen::MatrixXd V(col,col);
  Eigen::MatrixXd X(row,dim);
  Eigen::VectorXd S(dim);
  matrixConversionToEigenType(matrix,A);
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(A,Eigen::ComputeFullU | Eigen::ComputeFullV);
  U = svd.matrixU();
  V = svd.matrixV();
  S = svd.singularValues();
  for(unsigned int i = 0; i < dim; ++i) {
    X.col(i) = U.col(i)*sqrt(S(i));
  }
  matrixConversionToCxxVVectorType(U,left_singular_vectors);
  matrixConversionToCxxVVectorType(V,right_singular_vectors);
  vectorConversionToCxxVectorType(S,singular_values);
  matrixConversionToCxxVVectorType(X,transformed_matrix);
}

void getInverseTransformedMatrix(const std::vector<std::vector<double> > &left_singular_vectors, std::vector<double> &singular_values, std::vector<std::vector<double> > &inverse_transformed_matrix) {
  /**
   * This function compute the inverse transformation matrix
   * Input Parameters
   * left_singular_vectors: stores the left singular vectors for given matrix
   * singular_values: stores the singular values for given matrix
   * Output Parameters
   * inverse_transformed_matrix: stores the inverse transformation matrix
   */
  unsigned int row = left_singular_vectors.size();
  unsigned int col = left_singular_vectors.at(0).size();
  unsigned int dim = singular_values.size();
  Eigen::MatrixXd U(row,col);
  Eigen::MatrixXd inverseX(row,dim);
  matrixConversionToEigenType(left_singular_vectors,U);
  for (unsigned int i = 0; i < dim; ++i) {
    if (singular_values.at(i) == 0) {
      inverseX.col(i) = U.col(i) * 0.0;
    } else {
      inverseX.col(i) = U.col(i) * (1.0/sqrt(singular_values.at(i)));
    }
  }
  matrixConversionToCxxVVectorType(inverseX.transpose(),inverse_transformed_matrix);
}

void svdDecomposition(const std::vector<std::vector<double> > &matrix, std::vector<std::vector<double> > &left_singular_vectors, std::vector<std::vector<double> > &right_singular_vectors, std::vector<double> &singular_values, std::vector<std::vector<double> > &transformed_matrix, unsigned int rank) {
  /**
   * This function compute the singular value decomposition for given matrix
   * Input Parameters
   * matrix: provided data
   * Output Parameters
   * left_singular_vectors: stores the left singular vectors for given matrix
   * right_singular_vectors: stores the right singular vectors for given matrix
   * singular_values: stores the singular values for given matrix
   * rank: used for truncated svd, the number of singular values that will be kept for truncated svd
   */
  unsigned int row = matrix.size();
  unsigned int col = matrix.at(0).size();
  unsigned int dim = 0;
  if(row > col) {
    dim = col;
  }else {
    dim = row;
  }
  Eigen::MatrixXd A(row,col);
  Eigen::MatrixXd U(row,row);
  Eigen::MatrixXd V(col,col);
  Eigen::MatrixXd X(row,rank);
  Eigen::VectorXd S(dim);
  matrixConversionToEigenType(matrix,A);
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(A,Eigen::ComputeFullU | Eigen::ComputeFullV);
  U = svd.matrixU();
  V = svd.matrixV();
  S = svd.singularValues();
  for(unsigned int i = 0; i < rank; ++i) {
    X.col(i) = U.col(i)*sqrt(S(i));
  }
  // transform and store the matrix for the truncated svd
  matrixConversionToCxxVVectorType(U.block(0,0,row,rank),left_singular_vectors);
  matrixConversionToCxxVVectorType(V.block(0,0,col,rank),right_singular_vectors);
  vectorConversionToCxxVectorType(S.head(rank),singular_values);
  matrixConversionToCxxVVectorType(X,transformed_matrix);
}

void  computeNearestSymmetricMatrix(const std::vector<std::vector<double> > &matrix, std::vector<std::vector<double> > &symmetric_matrix) {
  /**
   * This function used to compute the nearest symmetric matrix
   * Input Parameters
   * matrix: std::vector<std::vector<double> >, given matrix
   * Output Parameters
   * symmetric_matrix: std::vector<std::vector<double> >, the computed symmetric matrix
   */
  unsigned int row = matrix.size();
  unsigned int col = matrix.at(0).size();
  if (row != col) {
    throwError("The provided matrix is not a square matrix!" );
  }
  Eigen::MatrixXd A(row,col);
  Eigen::MatrixXd B(col,row);
  matrixConversionToEigenType(matrix,A);
  B = A.transpose();
  A = (A + B)*0.5;
  matrixConversionToCxxVVectorType(A,symmetric_matrix);
}

void resetSingularValues(std::vector<std::vector<double> > &left_singular_vectors, std::vector<std::vector<double> > &right_singular_vectors, std::vector<double> &singular_values,std::vector<std::vector<double> > &transformed_matrix) {
  /**
   * used to reset singular values
   * Input Parameters
   * left_singular_vectors: std::vector<std::vector<double> >, the left singular vectors
   * right_singular_vectors: std::vector<std::vector<double> >, the right singular vectors
   * singular_values: std::vector<double>, the singular values
   * transformed_matrix: std::vector<vector<double> >, the transformation matrix
   * Output Parameters
   * singular_values: std::vector<double>, the modified singular values
   * transformed_matrix: std::vector<vector<double> >, the updated transformation matrix
   */
  unsigned int row1 = left_singular_vectors.size();
  unsigned int col1 = left_singular_vectors.at(0).size();
  unsigned int row2 = right_singular_vectors.size();
  unsigned int col2 = right_singular_vectors.at(0).size();
  unsigned int rank = transformed_matrix.at(0).size();
  if (row1 != row2 && col1 != col2) {
    throwError("The provided matrices should have the same shape!" );
  }
  Eigen::MatrixXd A(row1,col1);
  Eigen::MatrixXd B(row2,col2);
  Eigen::MatrixXd X(row1,rank);
  matrixConversionToEigenType(left_singular_vectors,A);
  matrixConversionToEigenType(right_singular_vectors,B);
  double tol;
  tol = 1.0E-15;
  if (singular_values.at(0) == 0.0) {
    throwError("The provided covariance matrix is zero matrix!")
  }
  for (unsigned int i = 0; i < singular_values.size(); ++i) {
    if ((A.col(i) + B.col(i)).lpNorm<1>()/row1 < tol) {
      singular_values.at(i) = 0.0;
    }
    if (singular_values.at(i)/singular_values.at(0) < tol) {
      singular_values.at(i) = 0.0;
    }
  }
  for(unsigned int i = 0; i < rank; ++i) {
    X.col(i) = A.col(i)*sqrt(singular_values.at(i));
  }
  transformed_matrix.clear();
  matrixConversionToCxxVVectorType(X,transformed_matrix);
}

void matrixConversionToEigenType(std::vector<std::vector<double> > original, Eigen::MatrixXd &converted) {
  /**
   * This function convert the data from type std::vector<std::vector<double> > to Eigen::MatrixXd
   * Input Parameters
   * original: provided data with the type of std::vector<std::vector<double> >
   * Output Parameters
   * converted: output data with the type of Eigen::MatrixXd
   */
  converted.resize(original.size(),original.at(0).size());
  for(unsigned int row = 0; row < original.size(); ++row) {
    for(unsigned int col = 0; col < original.at(0).size(); ++col) {
      if(original.at(0).size() != original.at(row).size()) {
        throwError("The matrix stored in the C++ vector container with different lenght of columns");
      }
      converted(row,col) = original.at(row).at(col);
    }
  }
}

void matrixConversionToCxxVVectorType(const Eigen::MatrixXd & original, std::vector<std::vector<double> > &converted) {
  /**
   * This function convert the data from type Eigen::MatrixXd to type std::vector<double>
   * Input Parameters
   * original: provided data with the type of Eigen::MatrixXd
   * Output Parameters
   * converted: output data with the type of std::vector<std::vector<double> >
   */
  for(unsigned int row = 0; row < original.rows(); ++row) {
    std::vector<double> temp;
    for(unsigned int col = 0; col < original.cols(); ++col) {
      temp.push_back(original(row,col));
    }
    converted.push_back(temp);
  }
}

void vectorConversionToCxxVectorType(const Eigen::VectorXd & original, std::vector<double> &converted) {
  /**
   * This function convert the data from type Eigen::VectorXd to type std::vector<double>
   * Input Parameters
   * original: provided data with the type of Eigen::VectorXd
   * Output Parameters
   * converted: output data with the type of std::vector<double>
   */
  for(unsigned int dim = 0; dim < original.rows(); ++dim) {
    converted.push_back(original(dim));
  }
}

// void nrerror(const char error_text[]){     // added const to avoid "warning: deprecated conversion from string constant to *char
// /* Numerical Recipes standard error handler */
//  fprintf(stderr,"Numerical Recipes run-time error...\n");
//  fprintf(stderr,"%s\n",error_text);
//  fprintf(stderr,"...now exiting to system...\n");
// }
//
// double gammp(double a, double x){
// /* high level function for incomplete gamma function */
//         void gcf(double *gammcf,double a,double x,double *gln);
//         void gser(double *gamser,double a,double x,double *gln);
//         double gamser,gammcf,gln;
//         if(x < 0.0 || a <= 0.0) nrerror("Invalid arg in gammp");
//         if(x < (a+1.0)){
// /* here I change routine so that it returns \gamma(a,x)
//         or P(a,x)-just take out comments to get P(a,x) vs \gamma(a,x)-
//         to get latter use the exp(log(.)+gln) expression */
//                gser(&gamser,a,x,&gln);
// //      return exp(log(gamser)+gln);
//                return gamser;
//         }
//         else{
//                gcf(&gammcf,a,x,&gln);
// //      return exp(log(1.0-gammcf)+gln);
//                return 1.0-gammcf;
//         }
// }
//
// double loggam(double xx)
// {
//         double x,y,tmp,ser;
//         static double cof[6]={76.18009172947146, -86.50532032941677,
//                24.01409824083091,-1.231739572450155, 0.001208650973866179,
//                -5.395239384953e-006};
//         int j;
//         y=x=xx;
//         tmp=x+5.5;
//         tmp -= (x+0.5)*log(tmp);
//         ser=1.000000000190015;
//         for(j=0;j<=5;j++) ser += cof[j]/++y;
//         return -tmp+log(2.506628274631*ser/x);
// }
//
// #define ITMAX 100
// #define EPSW 3.0e-7
//
// void gser(double *gamser,double a,double x,double *gln){
//         int n;
//         double sum,del,ap;
//         *gln=loggam(a);
//         if(x <= 0.0){
//                if(x < 0.0) nrerror("x less than 0 in routine gser");
//                *gamser=0.0;
//                return;
//         }
//         else{
//                ap=a;
//                del=sum=1.0/a;
//                for(n=1;n<=ITMAX;n++){
//                       ++ap;
//                       del *= x/ap;
//                       sum += del;
//                       if(fabs(del) < fabs(sum)*EPSW){
//    *gamser=sum*exp(-x+a*log(x)-(*gln));
//    return;
//                       }
//                }
//                nrerror("a too large, ITMAX too small in routine gser");
//                return;
//         }
// }
//
//
// #define FPMIN 1.0e-30
//
// void gcf(double *gammcf,double a,double x,double *gln){
//         int i;
//         double an,b,c,d,del,h;
//         *gln=loggam(a);
//         b=x+1.0-a;
//         c=1.0/FPMIN;
//         d=1.0/b;
//         h=d;
//         for(i=1;i<=ITMAX;i++){
//                an = -i*(i-a);
//                b += 2.0;
//                d=an*d+b;
//                if(fabs(d) < FPMIN) d=FPMIN;
//                c=b+an/c;
//                if(fabs(c) < FPMIN) c=FPMIN;
//                d=1.0/d;
//                del=d*c;
//                h *= del;
//                if(fabs(del-1.0) < EPSW) break;
//         }
//         if(i > ITMAX) nrerror("a too large, ITMAX too small in gcf");
//         *gammcf=exp(-x+a*log(x)-(*gln))*h;
// }
//
// // Gamma function
// // source http://www.crbond.com/math.htm
// double gammaFunc(double x){
//  int i,k,m;
//  double ga,gr,r,z;
//
//  static double g[] = {
//   1.0,
//   0.5772156649015329,
//                 -0.6558780715202538,
//                 -0.420026350340952e-1,
//   0.1665386113822915,
//                 -0.421977345555443e-1,
//                 -0.9621971527877e-2,
//   0.7218943246663e-2,
//                 -0.11651675918591e-2,
//                 -0.2152416741149e-3,
//   0.1280502823882e-3,
//                 -0.201348547807e-4,
//                 -0.12504934821e-5,
//   0.1133027232e-5,
//                 -0.2056338417e-6,
//   0.6116095e-8,
//   0.50020075e-8,
//                 -0.11812746e-8,
//   0.1043427e-9,
//   0.77823e-11,
//                 -0.36968e-11,
//   0.51e-12,
//                 -0.206e-13,
//                 -0.54e-14,
//   0.14e-14};
//
//  if (x > 171.0) return 1e308;    // This value is an overflow flag.
//  if (x == (int)x) {
//   if (x > 0.0) {
//    ga = 1.0;               // use factorial
//    for (i=2;i<x;i++) {
//                                 ga *= i;
//    }
//                       }
//                       else
//    ga = 1e308;
//               }
//               else {
//   if (fabs(x) > 1.0) {
//    z = fabs(x);
//    m = (int)z;
//    r = 1.0;
//    for (k=1;k<=m;k++)
//     r *= (z-k);
//    z -= m;
//   }
//   else
//    z = x;
//   gr = g[24];
//   for (k=23;k>=0;k--)
//    gr = gr*z+g[k];
//
//   ga = 1.0/(gr*z);
//   if (fabs(x) > 1.0) {
//    ga *= r;
//    if (x < 0.0)
//     ga = -M_PI/(x*ga*sin(M_PI*x));
//   }
//  }
//  return ga;
// }
//
//
// // Beta function
// double betaFunc(double alpha, double beta){
//  double value=gammaFunc(alpha)*gammaFunc(beta)/gammaFunc(alpha+beta);
//  return value;
// }
//
//
// // log gamma using the Lanczos approximation
// double logGamma(double x) {
// const double c[8] = { 676.5203681218851, -1259.1392167224028,
//                       771.32342877765313, -176.61502916214059,
//                       12.507343278686905, -0.13857109526572012,
//                       9.9843695780195716e-6, 1.5056327351493116e-7 };
// double sum = 0.99999999999980993;
// double y = x;
// for (int j = 0; j < 8; j++)
//        sum += c[j] / ++y;
// return log(sqrt(2*3.14159) * sum / x) - (x + 7.5) + (x + 0.5) * log(x + 7.5);
// }
//
// // helper function for incomplete beta
// // computes continued fraction
//
// double betaContFrac(double a, double b, double x) {
//  const int MAXIT = 1000;
//  const double EPS = 3e-7;
//  double qab = a + b;
//  double qap = a + 1;
//  double qam = a - 1;
//  double c = 1;
//  double d = 1 - qab * x / qap;
//  if (fabs(d) < FPMIN) d = FPMIN;
//  d = 1 / d;
//  double h = d;
//  int m;
//  for (m = 1; m <= MAXIT; m++) {
//                int m2 = 2 * m;
//                double aa = m * (b-m) * x / ((qam + m2) * (a + m2));
//                d = 1 + aa * d;
//                if (fabs(d) < FPMIN) d = FPMIN;
//                c = 1 + aa / c;
//                if (fabs(c) < FPMIN) c = FPMIN;
//                d = 1 / d;
//                h *= (d * c);
//                aa = -(a+m) * (qab+m) * x / ((a+m2) * (qap+m2));
//                d = 1 + aa * d;
//                if (fabs(d) < FPMIN) d = FPMIN;
//                c = 1 + aa / c;
//                if (fabs(c) < FPMIN) c = FPMIN;
//                d = 1 / d;
//                double del = d*c;
//                h *= del;
//                if (fabs(del - 1) < EPS) break;
//  }
//  if (m > MAXIT) {
//                cerr << "betaContFrac: too many iterations\n";
//  }
//  return h;
// }
//
// // incomplete beta function
// // must have 0 <= x <= 1
// double betaInc(double a, double b, double x) {
//        if (x == 0)
//  return 0;
//        else if (x == 1)
//  return 1;
//        else {
//  double logBeta = logGamma(a+b) - logGamma(a) - logGamma(b)
//                + a * log(x) + b * log(1-x);
//  if (x < (a+1) / (a+b+2))
//                return exp(logBeta) * betaContFrac(a, b, x) / a;
//  else
//                return 1 - exp(logBeta) * betaContFrac(b, a, 1-x) / b;
//        }
// }
//
// double normRNG(double mu, double sigma, double RNG) {
//  static bool deviateAvailable=false;                        //        flag
//  static float storedDeviate;                        //        deviate from previous calculation
//  double polar, rsquared, var1, var2;
//  //srand(time(NULL));
//  //srand((unsigned)time(0));
//  //srand(time(0));
//  //Ran ran(time(0));
//  //        If no deviate has been stored, the polar Box-Muller transformation is
//  //        performed, producing two independent normally-distributed random
//  //        deviates.  One is stored for the next round, and one is returned.
//  if (!deviateAvailable) {
//
//   //        choose pairs of uniformly distributed deviates, discarding those
//   //        that don't fall within the unit circle
//   do {
//    var1 = 2.0*( RNG ) - 1.0;
//    var2 = 2.0*( RNG ) - 1.0;
//
//    //var1=2.0*( ran.doub() ) - 1.0;
//    //var2=2.0*( ran.doub() ) - 1.0;
//
//    rsquared=var1*var1+var2*var2;
//   } while ( rsquared>=1.0 || rsquared == 0.0);
//
//   //        calculate polar transformation for each deviate
//   polar=sqrt(-2.0*log(rsquared)/rsquared);
//
//   //        store first deviate and set flag
//   storedDeviate=var1*polar;
//   deviateAvailable=true;
//
//   //        return second deviate
//   return var2*polar*sigma + mu;
//  }
//
//  //        If a deviate is available from a previous call to this function, it is
//  //        returned, and the flag is set to false.
//  else {
//   deviateAvailable=false;
//   return storedDeviate*sigma + mu;
//  }
// }
//
//
// void loadData(double** data, int dimensionality, int cardinality, string filename) {
//        int x, y;
//
//        ifstream in(filename.c_str());
//
//        if (!in) {
//          cout << "Cannot open file.\n";
//          return;
//        }
//
//        for (y = 0; y < cardinality; y++) {
//          for (x = 0; x < dimensionality; x++) {
//            in >> data[y][x];
//          }
//        }
//
//        in.close();
// }
//
// double calculateCustomPdf(double position, double fitting, double** data_set, int number_samples){
//  double value=-1;
//  double min;
//  double max;
//
//  for (int i=1; i<number_samples; i++){
//   max=data_set[i][1];
//   min=data_set[i-1][1];
//
//   if((position>min)&(position<max)){
//    if (fitting==1)
//     value=data_set[i-1][2];
//    else
//     value=data_set[i-1][2]+(data_set[i][2]-data_set[i-1][2])/(data_set[i][1]-data_set[i-1][1])*(position-data_set[i-1][1]);
//   }
//   else
//    perror ("The following error occurred: distribution sampled out of its boundaries");
//  }
//
//  return value;
// }
//
// double calculateCustomCDF(double position, double fitting, double** data_set, int number_samples){
//  double value=-1;
//  double min;
//  double max;
//  double cumulative=0;
//
//  for (int i=1; i<number_samples; i++){
//   max=data_set[i][1];
//   min=data_set[i-1][1];
//
//   if((position>min)&(position<max)){
//    if (fitting==1)
//     value=cumulative+data_set[i-1][2]*(position-data_set[i-1][1]);
//    else{
//     double pdfValueInPosition =data_set[i-1][2]+(data_set[i][2]-data_set[i-1][2])/(data_set[i][1]-data_set[i-1][1])*(position-data_set[i-1][1]);
//     value=cumulative + (pdfValueInPosition+data_set[i-1][2])*(position-data_set[i-1][1])/2;
//    }
//   }
//   else
//    perror ("The following error occurred: distribution sampled out of its boundaries");
//
//   if (fitting==1)
//    cumulative=cumulative+data_set[i-1][2]*(data_set[i][1]-data_set[i-1][1]);
//   else
//    cumulative=cumulative+(data_set[i][2]+data_set[i-1][2])*(data_set[i][1]-data_set[i-1][1])/2;
//  }
//
//  return value;
// }
//
//        double stdGammaRNG(double shape);
//
// double gammaRNG(double shape, double scale){
//  double value=scale * stdGammaRNG(shape);
//  return value;
// }
//
//        double rkGauss();
//
// double stdGammaRNG(double shape)
// {
//          double b, c;
//          double U, V, X, Y;
//
//          if (shape == 1.0)
//          {
//              return -log(1.0 - rand());
//          }
//          else if (shape < 1.0)
//          {
//              for (;;)
//              {
//                  U = rand();
//                  V = -log(1.0 - rand());
//                  if (U <= 1.0 - shape)
//                  {
//                      X = pow(U, 1./shape);
//                      if (X <= V)
//                      {
//                          return X;
//                      }
//                  }
//                  else
//                  {
//                      Y = -log((1-U)/shape);
//                      X = pow(1.0 - shape + shape*Y, 1./shape);
//                      if (X <= (V + Y))
//                      {
//                          return X;
//                      }
//                  }
//              }
//          }
//          else
//          {
//              b = shape - 1./3.;
//              c = 1./sqrt(9*b);
//              for (;;)
//              {
//                  do
//                  {
//                      X = rkGauss();
//                      V = 1.0 + c*X;
//                  } while (V <= 0.0);
//
//                  V = V*V*V;
//                  U = rand();
//                  if (U < 1.0 - 0.0331*(X*X)*(X*X))
//                      return (b*V);
//                  if (log(U) < 0.5*X*X + b*(1. - V + log(V)))
//                      return (b*V);
//              }
//          }
// }
//
// double rkGauss() {
//  double f, x1, x2, r2;
//
//  do {
//   x1 = 2.0*rand() - 1.0;
//   x2 = 2.0*rand() - 1.0;
//   r2 = x1*x1 + x2*x2;
//  }
//  while (r2 >= 1.0 || r2 == 0.0);
//
//  /* Box-Muller transform */
//  f = sqrt(-2.0*log(r2)/r2);
//  return f*x2;
//
// }
//
//
// double betaRNG(double alpha, double beta){
//  // To be updated
//  double value=0;
//  return value;
// }
//
// double modifiedLogFunction(double x){
//          if (x <= -1.0)
//          {
//              std::stringstream os;
//              os << "Invalid input argument (" << x << "); must be greater than -1.0";
//              throw std::invalid_argument( os.str() );
//          }
//
//          if (fabs(x) > 1e-4)
//              return log(1.0 + x);
//          else
//              return (-0.5*x + 1.0)*x;
// }

        double abramStegunApproximation(double t)
        {
            // Abramowitz and Stegun formula

            double c[] = {2.515517, 0.802853, 0.010328};
            double d[] = {1.432788, 0.189269, 0.001308};
            return t - ((c[2]*t + c[1])*t + c[0]) /
                       (((d[2]*t + d[1])*t + d[0])*t + 1.0);
        }


#include<iostream>
#include <armadillo>

using namespace std;
using namespace arma;

int main()
{
    int nz=600;
    int nx=400;

    arma::Mat<float> model_ori(nz,nx,fill::zeros);
    arma::Mat<float> model_str(nz,nx,fill::zeros);

    model_ori.load("./model.dat",raw_binary);
    model_ori.reshape(nz,nx);

    for(int ix=0; ix<nx; ix++)
    {
        for(int iz=1; iz<nz; iz++)
        {
            if(model_ori(iz,ix)-model_ori(iz-1,ix)>20)
            {
                model_str(iz,ix)=1;
            }
        }
    }
    model_str.save("model_str.dat",raw_binary);

    return 0;
}


/**
 * \file LMSFilter.cpp
 */

#include <ATK/Adaptive/LMSFilter.h>

#include <complex>
#include <cstdint>
#include <stdexcept>

#include <Eigen/Core>

#include <ATK/Core/TypeTraits.h>
#include <ATK/Core/Utilities.h>

namespace ATK
{
  template<typename DataType_>
  class LMSFilter<DataType_>::LMSFilterImpl
  {
  public:
    using wType = Eigen::Matrix<DataType_, Eigen::Dynamic, 1>;
    using xType = Eigen::Map<const wType>;

    wType w;
    /// Memory factor
    double alpha = 0.99;
    /// line search
    double mu = 0.05;

    explicit LMSFilterImpl(gsl::index size)
    :w(wType::Zero(size))
    {
    }

    using UpdateFunction = void (LMSFilterImpl::*)(const xType& x, DataType error);

    void update(const xType& x, DataType error)
    {
      w = static_cast<DataType>(alpha) * w + static_cast<DataType>(mu) * error * x;
    }

    void update_normalized(const xType& x, DataType error)
    {
      w = static_cast<DataType>(alpha) * w + static_cast<DataType>(mu) * error * x / (std::numeric_limits<DataType>::epsilon() + static_cast<DataType>(x.squaredNorm()));
    }

    void update_signerror(const xType& x, DataType error)
    {
      w = static_cast<DataType>(alpha) * w + static_cast<DataType>(mu) * error / (std::numeric_limits<DataType>::epsilon() + std::abs(error)) * x;
    }

    void update_signdata(const xType& x, DataType error)
    {
      w = static_cast<DataType>(alpha) * w.array() + static_cast<DataType>(mu) * error * x.array() / (x.cwiseAbs().template cast<DataType>().array() + static_cast<DataType>(std::numeric_limits<DataType>::epsilon()));
    }

    void update_signsign(const xType& x, DataType error)
    {
      w = static_cast<DataType>(alpha) * w.array() + static_cast<DataType>(mu) * error / (std::numeric_limits<DataType>::epsilon() + std::abs(error)) * x.array() / (x.cwiseAbs().template cast<DataType>().array() + static_cast<DataType>(std::numeric_limits<DataType>::epsilon()));
    }

    UpdateFunction select(Mode mode)
    {
      switch (mode)
      {
      case Mode::NORMAL:
        return &LMSFilterImpl::update;
      case Mode::NORMALIZED:
        return &LMSFilterImpl::update_normalized;
      case Mode::SIGNERROR:
        return &LMSFilterImpl::update_signerror;
      case Mode::SIGNDATA:
        return &LMSFilterImpl::update_signdata;
      case Mode::SIGNSIGN:
        return &LMSFilterImpl::update_signsign;
      default:
          throw std::range_error("Wrong mode for LMS filter");
      }
    }
  };

  template<typename DataType_>
  LMSFilter<DataType_>::LMSFilter(gsl::index size)
  :Parent(2, 1), impl(std::make_unique<LMSFilterImpl>(size))
  {
    input_delay = size - 1;
  }

  template<typename DataType_>
  LMSFilter<DataType_>::~LMSFilter()
  {
  }

  template<typename DataType_>
  void LMSFilter<DataType_>::set_size(gsl::index size)
  {
    if(size == 0)
    {
      throw RuntimeError("Size must be strictly positive");
    }

    input_delay = size - 1;
    impl = std::make_unique<LMSFilterImpl>(size);
  }

  template<typename DataType_>
  gsl::index LMSFilter<DataType_>::get_size() const
  {
    return input_delay + 1;
  }

  template<typename DataType_>
  void LMSFilter<DataType_>::set_memory(double memory)
  {
    if (memory >= 1)
    {
      throw ATK::RuntimeError("Memory must be less than 1");
    }
    if (memory <= 0)
    {
      throw ATK::RuntimeError("Memory must be strictly positive");
    }

    impl->alpha = memory;
  }

  template<typename DataType_>
  double LMSFilter<DataType_>::get_memory() const
  {
    return impl->alpha;
  }

  template<typename DataType_>
  void LMSFilter<DataType_>::set_mu(double mu)
  {
    if (mu >= 1)
    {
      throw ATK::RuntimeError("Mu must be less than 1");
    }
    if (mu <= 0)
    {
      throw ATK::RuntimeError("Mu must be strictly positive");
    }

    impl->mu = mu;
  }

  template<typename DataType_>
  double LMSFilter<DataType_>::get_mu() const
  {
    return impl->mu;
  }

  template<typename DataType_>
  void LMSFilter<DataType_>::set_mode(Mode mode)
  {
    this->mode = mode;
  }

  template<typename DataType_>
  typename LMSFilter<DataType_>::Mode LMSFilter<DataType_>::get_mode() const
  {
    return mode;
  }

  template<typename DataType_>
  void LMSFilter<DataType_>::process_impl(gsl::index size) const
  {
    const DataType* ATK_RESTRICT input = converted_inputs[0];
    const DataType* ATK_RESTRICT ref = converted_inputs[1];
    DataType* ATK_RESTRICT output = outputs[0];

    auto update_function = impl->select(mode);

    for(gsl::index i = 0; i < size; ++i)
    {
      typename LMSFilterImpl::xType x(input - input_delay + i, input_delay + 1, 1);
      output[i] = impl->w.conjugate().dot(x);
      if(learning)
      {
        (impl.get()->*update_function)(x, TypeTraits<DataType>::conj(ref[i] - output[i]));
      }
    }
  }

  template<typename DataType_>
  const DataType_* LMSFilter<DataType_>::get_w() const
  {
    return impl->w.data();
  }

  template<typename DataType_>
  void LMSFilter<DataType_>::set_w(gsl::not_null<const DataType_*> w)
  {
    impl->w = Eigen::Map<const typename LMSFilterImpl::wType>(w.get(), get_size());
  }

  template<typename DataType_>
  void LMSFilter<DataType_>::set_learning(bool learning)
  {
    this->learning = learning;
  }

  template<typename DataType_>
  bool LMSFilter<DataType_>::get_learning() const
  {
    return learning;
  }

  template class LMSFilter<double>;
#if ATK_ENABLE_INSTANTIATION
  template class LMSFilter<float>;
  template class LMSFilter<std::complex<float>>;
  template class LMSFilter<std::complex<double>>;
#endif
}


// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html

#include <iostream>
#include <unordered_map>

#include "opencv2/core/base.hpp"
#include "opencv2/core/types.hpp"

#if defined(HAVE_EIGEN)
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>

#include "opencv2/core/eigen.hpp"
#endif

namespace cv
{
namespace kinfu
{
/*!
 * \class BlockSparseMat
 * Naive implementation of Sparse Block Matrix
 */
template<typename _Tp, int blockM, int blockN>
struct BlockSparseMat
{
    struct Point2iHash
    {
        size_t operator()(const cv::Point2i& point) const noexcept
        {
            size_t seed                     = 0;
            constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
            seed ^= std::hash<int>()(point.x) + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
            seed ^= std::hash<int>()(point.y) + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
            return seed;
        }
    };
    typedef Matx<_Tp, blockM, blockN> MatType;
    typedef std::unordered_map<Point2i, MatType, Point2iHash> IDtoBlockValueMap;

    BlockSparseMat(int _nBlocks) : nBlocks(_nBlocks), ijValue() {}

    MatType& refBlock(int i, int j)
    {
        Point2i p(i, j);
        auto it = ijValue.find(p);
        if (it == ijValue.end())
        {
            it = ijValue.insert({ p, Matx<_Tp, blockM, blockN>::zeros() }).first;
        }
        return it->second;
    }

    Mat diagonal()
    {
        // Diagonal max length is the number of columns in the sparse matrix
        int diagLength = blockN * nBlocks;
        cv::Mat diag   = cv::Mat::zeros(diagLength, 1, CV_32F);

        for (int i = 0; i < diagLength; i++)
        {
            diag.at<float>(i, 0) = refElem(i, i);
        }
        return diag;
    }

    float& refElem(int i, int j)
    {
        Point2i ib(i / blockM, j / blockN), iv(i % blockM, j % blockN);
        return refBlock(ib.x, ib.y)(iv.x, iv.y);
    }

#if defined(HAVE_EIGEN)
    Eigen::SparseMatrix<_Tp> toEigen() const
    {
        std::vector<Eigen::Triplet<double>> tripletList;
        tripletList.reserve(ijValue.size() * blockM * blockN);
        for (auto ijv : ijValue)
        {
            int xb = ijv.first.x, yb = ijv.first.y;
            MatType vblock = ijv.second;
            for (int i = 0; i < blockM; i++)
            {
                for (int j = 0; j < blockN; j++)
                {
                    float val = vblock(i, j);
                    if (abs(val) >= NON_ZERO_VAL_THRESHOLD)
                    {
                        tripletList.push_back(Eigen::Triplet<double>(blockM * xb + i, blockN * yb + j, val));
                    }
                }
            }
        }
        Eigen::SparseMatrix<_Tp> EigenMat(blockM * nBlocks, blockN * nBlocks);
        EigenMat.setFromTriplets(tripletList.begin(), tripletList.end());
        EigenMat.makeCompressed();

        return EigenMat;
    }
#endif
    size_t nonZeroBlocks() const { return ijValue.size(); }

    static constexpr float NON_ZERO_VAL_THRESHOLD = 0.0001f;
    int nBlocks;
    IDtoBlockValueMap ijValue;
};

//! Function to solve a sparse linear system of equations HX = B
//! Requires Eigen
static bool sparseSolve(const BlockSparseMat<float, 6, 6>& H, const Mat& B, Mat& X, Mat& predB)
{
    bool result = false;
#if defined(HAVE_EIGEN)
    Eigen::SparseMatrix<float> bigA = H.toEigen();
    Eigen::VectorXf bigB;
    cv2eigen(B, bigB);

    Eigen::SparseMatrix<float> bigAtranspose = bigA.transpose();
    if(!bigA.isApprox(bigAtranspose))
    {
        CV_Error(Error::StsBadArg, "H matrix is not symmetrical");
        return result;
    }

    Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>> solver;

    solver.compute(bigA);
    if (solver.info() != Eigen::Success)
    {
        std::cout << "failed to eigen-decompose" << std::endl;
        result = false;
    }
    else
    {
        Eigen::VectorXf solutionX = solver.solve(bigB);
        Eigen::VectorXf predBEigen = bigA * solutionX;
        if (solver.info() != Eigen::Success)
        {
            std::cout << "failed to eigen-solve" << std::endl;
            result = false;
        }
        else
        {
            eigen2cv(solutionX, X);
            eigen2cv(predBEigen, predB);
            result = true;
        }
    }
#else
    std::cout << "no eigen library" << std::endl;
    CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented");
#endif
    return result;
}
}  // namespace kinfu
}  // namespace cv


#include <map>
#include <numeric>

#include <Eigen/Dense>

#include "BingoCpp/acyclic_graph.h"
#include "BingoCpp/backend.h"
#include "BingoCpp/backend_nodes.h"

const int NODE_IDX = 0;
const int OP_1 = 1;
const int OP_2 = 2;

namespace bingo {
namespace {

Eigen::ArrayXXd reverse_eval(const std::pair<int, int>& deriv_shape,
                             const int deriv_wrt_node,
                             const std::vector<Eigen::ArrayXXd>& forward_eval,
                             const Eigen::ArrayX3i& stack) {
  int num_samples = deriv_shape.first;
  int num_features = deriv_shape.second;
  int stack_depth = stack.rows();

  Eigen::ArrayXXd derivative = Eigen::ArrayXXd::Zero(num_samples, num_features);
  std::vector<Eigen::ArrayXXd> reverse_eval(stack_depth);
  for (int row = 0; row < stack_depth; row++) {
      reverse_eval[row] = Eigen::ArrayXd::Zero(num_samples);
  }

  reverse_eval[stack_depth-1] = Eigen::ArrayXd::Ones(num_samples);
  for (int i = stack_depth - 1; i >= 0; i--) {
    int node = stack(i, NODE_IDX);
    int param1 = stack(i, OP_1);
    int param2 = stack(i, OP_2);
    if (node == deriv_wrt_node) {
      derivative.col(param1) += reverse_eval[i];
    } else {
      reverse_eval_function(node, i, param1, param2, forward_eval, reverse_eval);
    }
  }
  return derivative;
}

Eigen::ArrayXXd reverse_eval_with_mask(const std::pair<int, int>& deriv_shape,
                                       const int deriv_wrt_node,
                                       const std::vector<Eigen::ArrayXXd>& forward_eval,
                                       const Eigen::ArrayX3i& stack,
                                       const std::vector<bool>& mask) {
  int num_samples = deriv_shape.first;
  int num_features = deriv_shape.second;
  int stack_depth = stack.rows();

  Eigen::ArrayXXd derivative = Eigen::ArrayXXd::Zero(num_samples, num_features);
  std::vector<Eigen::ArrayXXd> reverse_eval(stack_depth);
  for (int row = 0; row < stack_depth; row++) {
    if (mask[row]) {
      reverse_eval[row] = Eigen::ArrayXd::Zero(num_samples);
    }
  }

  reverse_eval[stack_depth-1] = Eigen::ArrayXd::Ones(num_samples);
  for (int i = stack_depth - 1; i >= 0; i--) {
    if (mask[i]) {
      int node = stack(i, NODE_IDX);
      int param1 = stack(i, OP_1);
      int param2 = stack(i, OP_2);
      if (node == deriv_wrt_node) {
        derivative.col(param1) += reverse_eval[i];
      } else {
        reverse_eval_function(node, i, param1, param2, forward_eval, reverse_eval);
      }
    }
  }
  return derivative;
}

std::vector<Eigen::ArrayXXd> forward_eval(
    const Eigen::ArrayX3i& stack,
    const Eigen::ArrayXXd& x,
    const Eigen::VectorXd& constants) {
  std::vector<Eigen::ArrayXXd> _forward_eval(stack.rows());

  for (int i = 0; i < stack.rows(); ++i) {
    int node = stack(i, NODE_IDX);
    int op1 = stack(i, OP_1);
    int op2 = stack(i, OP_2);
    _forward_eval[i] = forward_eval_function(
      node, op1, op2, x, constants, _forward_eval);
  }
  return _forward_eval;
}

std::vector<Eigen::ArrayXXd> forward_eval_with_mask(
    const Eigen::ArrayX3i& stack,
    const Eigen::ArrayXXd& x,
    const Eigen::VectorXd& constants,
    const std::vector<bool>& mask) {
  std::vector<Eigen::ArrayXXd> _forward_eval(stack.rows());

  for (int i = 0; i < stack.rows(); ++i) {
    if (mask[i]) {
      int node = stack(i, NODE_IDX);
      int op1 = stack(i, OP_1);
      int op2 = stack(i, OP_2);
      _forward_eval[i] = forward_eval_function(
        node, op1, op2, x, constants, _forward_eval);
    }
  }
  return _forward_eval;
}

std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> _evaluate_with_derivative(
    const Eigen::ArrayX3i& stack,
    const Eigen::ArrayXXd& x,
    const Eigen::VectorXd& constants,
    const bool param_x_or_c) {
  std::vector<Eigen::ArrayXXd> _forward_eval = forward_eval(
      stack, x, constants);

  std::pair<int, int> deriv_shape;
  int deriv_wrt_node;
  if (param_x_or_c) {  // true = x
    deriv_shape = std::make_pair(x.rows(), x.cols());
    deriv_wrt_node = 0;
  } else {  // false = c
    deriv_shape = std::make_pair(x.rows(), constants.size());
    deriv_wrt_node = 1;
  }

  Eigen::ArrayXXd derivative = reverse_eval(
      deriv_shape, deriv_wrt_node, _forward_eval, stack);
  return std::make_pair(_forward_eval.back(), derivative);
}

std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> evaluate_with_derivative_and_mask(
    const Eigen::ArrayX3i& stack,
    const Eigen::ArrayXXd& x,
    const Eigen::VectorXd& constants,
    const std::vector<bool>& mask,
    const bool param_x_or_c) {
  std::vector<Eigen::ArrayXXd> forward_eval = forward_eval_with_mask(
      stack, x, constants, mask);

  std::pair<int, int> deriv_shape;
  int deriv_wrt_node;
  if (param_x_or_c) {  // true = x
    deriv_shape = std::make_pair(x.rows(), x.cols());
    deriv_wrt_node = 0;
  } else {  // false = c
    deriv_shape = std::make_pair(x.rows(), constants.size());
    deriv_wrt_node = 1;
  }

  Eigen::ArrayXXd derivative = reverse_eval_with_mask(
      deriv_shape, deriv_wrt_node, forward_eval, stack, mask);
  return std::make_pair(forward_eval.back(), derivative);
}
} // namespace

bool is_cpp() {
    return true;
}

Eigen::ArrayXXd evaluate(const Eigen::ArrayX3i& stack,
                         const Eigen::ArrayXXd& x,
                         const Eigen::VectorXd& constants) {
  std::vector<Eigen::ArrayXXd> _forward_eval = forward_eval(
      stack, x, constants);
  return _forward_eval.back();
}

std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> evaluate_with_derivative(
    const Eigen::ArrayX3i& stack,
    const Eigen::ArrayXXd& x,
    const Eigen::VectorXd& constants,
    const bool param_x_or_c) {
  return _evaluate_with_derivative(stack, x, constants, param_x_or_c);
}

Eigen::ArrayXXd simplify_and_evaluate(const Eigen::ArrayX3i& stack,
                                    const Eigen::ArrayXXd& x,
                                    const Eigen::VectorXd& constants) {
  std::vector<bool> mask = get_utilized_commands(stack);
  std::vector<Eigen::ArrayXXd> forward_eval = forward_eval_with_mask(
      stack, x, constants, mask);
  return forward_eval.back();
}

std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> simplify_and_evaluate_with_derivative(
    const Eigen::ArrayX3i& stack,
    const Eigen::ArrayXXd& x,
    const Eigen::VectorXd& constants,
    const bool param_x_or_c) {
  std::vector<bool> mask = get_utilized_commands(stack);
  return evaluate_with_derivative_and_mask(stack, x, constants, mask, param_x_or_c);
}

std::vector<bool> get_utilized_commands(const Eigen::ArrayX3i& stack) {
  std::vector<bool> used_commands(stack.rows());
  used_commands.back() = true;
  int stack_size = stack.rows();
  for (int i = 1; i < stack_size; i++) {
    int row = stack_size - i;
    int node = stack(row, NODE_IDX);
    int param1 = stack(row, OP_1);
    int param2 = stack(row, OP_2);
    if (used_commands[row] && node > 1) {
      used_commands[param1] = true;
      if (AcyclicGraph::has_arity_two(node)) {
        used_commands[param2] = true;
      }
    }
  }
  return used_commands;
}

Eigen::ArrayX3i simplify_stack(const Eigen::ArrayX3i& stack) {
  std::vector<bool> used_command = get_utilized_commands(stack);
  std::map<int, int> reduced_param_map;
  int num_commands = 0;
  num_commands = std::accumulate(used_command.begin(), used_command.end(), 0);
  Eigen::ArrayX3i new_stack(num_commands, 3);

  for (int i = 0, j = 0; i < stack.rows(); ++i) {
    if (used_command[i]) {
      new_stack(j, 0) = stack(i, 0);
      if (AcyclicGraph::is_terminal(new_stack(j, 0))) {
        new_stack(j, 1) = stack(i, 1);
        new_stack(j, 2) = stack(i, 2);
      } else {
        new_stack(j, 1) = reduced_param_map[stack(i, 1)];
        if (AcyclicGraph::has_arity_two(new_stack(j, 0))) {
          new_stack(j, 2) = reduced_param_map[stack(i, 2)];
        } else {
          new_stack(j, 2) = new_stack(j, 1);
        }
      }
      reduced_param_map[i] = j;
      ++j;
    }
  }
  return new_stack;
}

int get_arity(int node) {
  if (AcyclicGraph::is_terminal(node)) return 0;
  return AcyclicGraph::has_arity_two(node) ? 2 : 1;
}
} // namespace bingo


#include <iostream>
// #include <boost/gil.hpp>
#include <boost/gil/extension/histogram/histogram.hpp>
#include <boost/gil/extension/histogram/histogram_algorithms.hpp>
#include <boost/gil/extension/io/jpeg.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <boost/gil/color_base.hpp>
#include <type_traits>
#include <boost/gil/detail/mp11.hpp>
#include <boost/mp11.hpp>

using namespace boost::gil;

int main() {
    rgb8_image_t img;
    read_and_convert_image("test.png", img, png_tag());
    histogram<gray8_pixel_t> hist;
    fill_histogram<rgb8_view_t, gray8_pixel_t>(view(img), hist);
    save_histogram_img(hist, "test_histogram.jpg");
    rgb8_image_t equalized_img(img.dimensions());
    equalise_histogram(view(img), view(equalized_img));
    write_view("test_equalized.jpg", view(equalized_img), jpeg_tag{});
    return 0;
}


#include <Sphere.h>

#include <Eigen/Dense>

#include <Util.h>

#include <cmath>
#include <cstdio>

void Sphere::generate_geometry(int width, int height) {
    int  gNumVertices  = 0;
    int  gNumTriangles = 0;
    int* gIndexBuffer  = nullptr;

    float theta, phi;
    int t;

    gNumVertices    = (height - 2) * width + 2;
    gNumTriangles   = (height - 2) * (width - 1) * 2;

    // TODO: Allocate an array for gNumVertices vertices.
    Eigen::Vector3f* vertices = new Eigen::Vector3f[gNumVertices];

    gIndexBuffer = new int[3*gNumTriangles];

    t = 0;
    for (int j = 1; j < height-1; ++j) {
        for (int i = 0; i < width; ++i) {
            theta = (float) j / (height-1) * M_PI;
            phi   = (float) i / (width-1)  * M_PI * 2;

            float   x   = sinf(theta) * cosf(phi);
            float   y   = cosf(theta);
            float   z   = -sinf(theta) * sinf(phi);

            // TODO: Set vertex t in the vertex array to {x, y, z}.
            vertices[t] = Eigen::Vector3f(x, y, z);
            t++;
        }
    }

    // TODO: Set vertex t in the vertex array to {0, 1, 0}.
    vertices[t] = Eigen::Vector3f(0, 1, 0);
    t++;

    // TODO: Set vertex t in the vertex array to {0, -1, 0}.
    vertices[t] = Eigen::Vector3f(0, -1, 0);
    t++;

    t = 0;
    for (int j = 0; j < height-3; ++j) {
        for (int i = 0; i < width-1; ++i) {
            gIndexBuffer[t++] = j*width + i;
            gIndexBuffer[t++] = (j+1)*width + (i+1);
            gIndexBuffer[t++] = j*width + (i+1);
            gIndexBuffer[t++] = j*width + i;
            gIndexBuffer[t++] = (j+1)*width + i;
            gIndexBuffer[t++] = (j+1)*width + (i+1);
        }
    }
    for (int i = 0; i < width-1; ++i) {
        gIndexBuffer[t++] = (height-2)*width;
        gIndexBuffer[t++] = i;
        gIndexBuffer[t++] = i + 1;
        gIndexBuffer[t++] = (height-2)*width + 1;
        gIndexBuffer[t++] = (height-3)*width + (i+1);
        gIndexBuffer[t++] = (height-3)*width + i;
    }

    // Create triangles
    for (int i = 0; i < gNumTriangles; i++) {
        Eigen::Vector3f a = vertices[gIndexBuffer[3*i + 0]];
        Eigen::Vector3f b = vertices[gIndexBuffer[3*i + 1]];
        Eigen::Vector3f c = vertices[gIndexBuffer[3*i + 2]];

        triangles.push_back(Triangle(a, b, c));
    }

    delete[] gIndexBuffer;
    delete[] vertices;
}

void Sphere::transform_geometry(const Eigen::Matrix4f& xform) {
    for (auto& tri : triangles) {
        // Transformed Cartesian vertex coordinates
        auto a = vec4to3(xform * Eigen::Vector4f(tri.a[0], tri.a[1], tri.a[2], 1.0f));
        auto b = vec4to3(xform * Eigen::Vector4f(tri.b[0], tri.b[1], tri.b[2], 1.0f));
        auto c = vec4to3(xform * Eigen::Vector4f(tri.c[0], tri.c[1], tri.c[2], 1.0f));

        // Transform vertices
        tri.a = a;
        tri.b = b;
        tri.c = c;

        // Transform normal
        tri.n = ((b-a).cross(c-a)).normalized();

        // Transform vertex normals
        tri.an = (a-center).normalized();
        tri.bn = (b-center).normalized();
        tri.cn = (c-center).normalized();
    }
}


/*! \file demo_boxplot_full.cpp
    \brief Demonstration of more options for Boxplots.
    \details Quickbook markup so can be included in documentation.

    \author Jacob Voytko and Paul A. Bristow
    \date Feb 2009
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2008

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// An example to demonstrate nearly all boxplot options.
// See also demo_boxplot_simple.cpp and demo_boxplot.cpp for a narrow range of use.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.

// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[demo_boxplot_full_1

/*`
Boxplot is a  convenient way of graphically depicting groups of numerical data
through their five-number summaries.
Show 1st quartile, median and 3rd quartile as a box,
minimum and maximum non-outlier values as whiskers,
and outliers and extreme outliers.

See [@http://en.wikipedia.org/wiki/Boxplot boxplot] and

Some Implementations of the Boxplot
Michael Frigge, David C. Hoaglin and Boris Iglewicz
The American Statistician, Vol. 43, No. 1 (Feb., 1989), pp. 50-54

First we need a few includes to use Boost.Plot.
*/

#include <vector>
using std::vector;
#include <cmath>
using ::sin;
#include <boost/svg_plot/svg_boxplot.hpp>

#include <boost/array.hpp>
  using boost::array;

#include <iostream>
using std::cout;
using std::endl;

/*`Use two functions, 1/x and sin(x), to simulate distributions.
*/

double f(double x)
{ // Effectively 1/x.
  return 50 / x;
}

double g(double x)
{ // Effectively sin(x).
  return 40 + 25 * sin(x * 50);
}
//] [demo_boxplot_full_1]

int main()
{
  using namespace boost::svg;
  try
  {
//[demo_boxplot_full_2]
/*`10 values are computed and stored in two std:: vectors.
*/
  std::vector<double> data1;
  std::vector<double> data2;

  cout.precision(2);
  for(double i = 0.1; i < 10; i += 0.1)
  {   // Fill our vectors with 100 values:
    double fv = f(i);
    double gv = g(i);
    // cout << i << ' ' << fv << ' ' << gv << endl;
    data1.push_back(fv);
    data2.push_back(gv);
  }

  /*`Other containers, for example array, can be used too:
  */

  //const boost::array<double, 0> data0;
  //const boost::array<double, 10> data3 = {20., 30., 40., 45., 47., 50., 55., 60., 70, 80.};

/*`A new boxplot is contructed and very many settings added.
This is only to show their use and is intended to be visible, if totally tasteless!
*/
  svg_boxplot my_boxplot;

  //my_boxplot.plot(data0, "data0"); // Produces warning: "Message from thrown exception was: Data series data0 is empty!"

  //my_boxplot.plot(data3); // Produces warning: "Data series has no title!"

  my_boxplot.background_border_color(darkblue);
  my_boxplot.background_color(azure);

  my_boxplot  // Title and axes labels.
    .title("Boxplots of 1/x and sin(x) Functions")
    .x_label("Functions")
    .y_label("Population Size");

  my_boxplot.y_range(0, 100)  // Y-Axis information.
    .y_minor_tick_length(2)
    .y_major_interval(20);

/*`Many attributes of boxplots can be changed from the 'built-in' defaults, for example: */

   my_boxplot.whisker_length(25.).box_width(10)
     .box_fill(lime)
     .box_border(blue)
     .box_fill(lightblue)
     .median_color(red). median_width(2)
     .axis_color(orange).axis_width(4)
     .outlier_color(red)
     .outlier_fill(yellow)
     .outlier_shape(square)
     .outlier_size(5)
     .median_values_on(true)
     .outlier_values_on(true)
     .extreme_outlier_values_on(true)
     .extreme_outlier_color(brown)
     .extreme_outlier_shape(diamond)
     .extreme_outlier_size(10)
    ;

   cout << my_boxplot.outlier_color() << endl; // red
   cout << my_boxplot.outlier_size() << endl; // size 10
   cout << my_boxplot.outlier_shape() << endl; // square

   //cout << my_boxplot.outlier_style.size() << endl; // doesn't work???

/*` Applies to all boxplots, unless changed for any individual plots, for example, change colors for data1 only:*/

   my_boxplot.plot(data1, "data1")
     .whisker_length(50.)
     .min_whisker_width(4).min_whisker_color(red)
     .max_whisker_width(7).max_whisker_color(green)
     .box_width(10)
     .box_fill(yellow)
     .box_border(magenta)
     .median_color(blue). median_width(5)
     .axis_color(lime). axis_width(1)
     .outlier_color(blue)
     .outlier_fill(yellow)
     .outlier_shape(cone)
     .outlier_size(10)
     .extreme_outlier_color(red)
     .extreme_outlier_fill(green)
     .extreme_outlier_shape(circlet)
     .extreme_outlier_size(10)
     //.extreme_outlier_values_on(true) not implemented.
;

   // my_boxplot.plot(data1, "test").box_style().fill_color(pink).stroke_color(green);
   // Once box_style() has been used to chain box styles, one can no longer chain to other non-box items, which is limiting.
   // So convenience functions are provided for many (but not all) features like: .box_fill(pink), box_border(green)...
   // Similar restrictions follow
   // my_boxplot.plot(data1, "test").box_width(10).whisker_length(5).median_style().stroke_color(purple);

/*`Add the two data series containers, and their labels, to the plot.
*/

  my_boxplot.plot(data1, "[50/x]");
  my_boxplot.plot(data2, "[sin(x*50)]");

/*
  cout << "my_boxplot.title " << my_boxplot.title() << endl;
  cout << "my_boxplot.x_label_text "<< my_boxplot.x_label_text() << endl;
  cout << "my_boxplot.y_label_text " << my_boxplot.y_label_text() << endl;

  cout << "my_boxplot.background_color " << my_boxplot.background_color() << endl;
  cout << "my_boxplot.background_border_color " << my_boxplot.background_border_color() << endl;
  cout << "my_boxplot.plot_background_color " << my_boxplot.plot_background_color() << endl;
  cout << "my_boxplot.plot_border_color " << my_boxplot.plot_border_color() << endl;
 */

/*`Finally write the SVG plot to a file.
*/
  my_boxplot.write("demo_boxplot_full.svg");

/*`You can view the plot (in all its 'glory') at demo_boxplot_full.svg."
*/

//] [demo_boxplot_full_2]
  }
  catch(const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n  " << e.what() << std::endl;
  }
  return 0;
} // int main()

/*

Output:
demo_boxplot_full.cpp
Linking...
Embedding manifest...
Autorun "j:\Cpp\SVG\debug\demo_boxplot_full.exe"
RGB(255,0,0)
5
2
Build Time 0:03

*/


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

#include "y2019/vision/target_finder.h"

#include "aos/vision/blob/move_scale.h"
#include "aos/vision/blob/stream_view.h"
#include "aos/vision/blob/transpose.h"
#include "aos/vision/debug/debug_framework.h"
#include "aos/vision/math/vector.h"
#include "gflags/gflags.h"

using aos::vision::ImageRange;
using aos::vision::ImageFormat;
using aos::vision::RangeImage;
using aos::vision::AnalysisAllocator;
using aos::vision::BlobList;
using aos::vision::Vector;
using aos::vision::Segment;
using aos::vision::PixelRef;

DEFINE_int32(camera, 10, "The camera to use the intrinsics for");

namespace y2019 {
namespace vision {

std::vector<PixelRef> GetNColors(size_t num_colors) {
  std::vector<PixelRef> colors;
  for (size_t i = 0; i < num_colors; ++i) {
    int quadrent = i * 6 / num_colors;
    uint8_t alpha = (256 * 6 * i - quadrent * num_colors * 256) / num_colors;
    uint8_t inv_alpha = 255 - alpha;
    switch (quadrent) {
      case 0:
        colors.push_back(PixelRef{255, alpha, 0});
        break;
      case 1:
        colors.push_back(PixelRef{inv_alpha, 255, 0});
        break;
      case 2:
        colors.push_back(PixelRef{0, 255, alpha});
        break;
      case 3:
        colors.push_back(PixelRef{0, inv_alpha, 255});
        break;
      case 4:
        colors.push_back(PixelRef{alpha, 0, 255});
        break;
      case 5:
        colors.push_back(PixelRef{255, 0, inv_alpha});
        break;
    }
  }
  return colors;
}

class FilterHarness : public aos::vision::FilterHarness {
 public:
 FilterHarness() {
   *(target_finder_.mutable_intrinsics()) = GetCamera(FLAGS_camera)->intrinsics;
 }
  aos::vision::RangeImage Threshold(aos::vision::ImagePtr image) override {
    return target_finder_.Threshold(image);
  }

  void InstallViewer(aos::vision::BlobStreamViewer *viewer) override {
    viewer_ = viewer;
    viewer_->SetScale(2.0);
    overlays_.push_back(&overlay_);
    overlays_.push_back(target_finder_.GetOverlay());
    viewer_->view()->SetOverlays(&overlays_);
  }

  void DrawBlob(const RangeImage &blob, PixelRef color) {
    if (viewer_) {
      BlobList list;
      list.push_back(blob);
      viewer_->DrawBlobList(list, color);
    }
  }

  bool HandleBlobs(BlobList imgs, ImageFormat fmt) override {
    const CameraGeometry camera_geometry = GetCamera(FLAGS_camera)->geometry;
    imgs_last_ = imgs;
    fmt_last_ = fmt;
    // reset for next drawing cycle
    for (auto &overlay : overlays_) {
      overlay->Reset();
    }

    if (draw_select_blob_ || draw_raw_poly_ || draw_components_ ||
        draw_raw_target_ || draw_raw_IR_ || draw_results_) {
      printf("_____ New Image _____\n");
    }

    const int num_pixels = target_finder_.PixelCount(&imgs);
    printf("Number pixels: %d\n", num_pixels);

    // Remove bad blobs.
    target_finder_.PreFilter(&imgs);

    // Find polygons from blobs.
    ::std::vector<Polygon> raw_polys;
    for (const RangeImage &blob : imgs) {
      // Convert blobs to contours in the corrected space.
      ContourNode *contour = target_finder_.GetContour(blob);
      if (draw_contours_) {
        DrawContour(contour, {255, 0, 0});
      }
      ::std::vector<::Eigen::Vector2f> unwarped_contour =
          target_finder_.UnWarpContour(contour);
      if (draw_contours_) {
        DrawContour(unwarped_contour, {0, 0, 255});
      }

      // Process to polygons.
      const Polygon polygon = target_finder_.FindPolygon(
          ::std::move(unwarped_contour), draw_raw_poly_);
      if (polygon.segments.empty()) {
        if (!draw_contours_) {
          DrawBlob(blob, {255, 0, 0});
        }
      } else {
        raw_polys.push_back(polygon);
        if (draw_select_blob_) {
          DrawBlob(blob, {0, 0, 255});
        }
        if (draw_raw_poly_) {
          std::vector<PixelRef> colors = GetNColors(polygon.segments.size());
          std::vector<Vector<2>> corners;
          for (size_t i = 0; i < polygon.segments.size(); ++i) {
            corners.push_back(polygon.segments[i].Intersect(
                polygon.segments[(i + 1) % polygon.segments.size()]));
          }

          for (size_t i = 0; i < polygon.segments.size(); ++i) {
            overlay_.AddLine(corners[i],
                             corners[(i + 1) % polygon.segments.size()],
                             colors[i]);
          }
        }
      }
    }

    // Calculate each component side of a possible target.
    std::vector<TargetComponent> target_component_list =
        target_finder_.FillTargetComponentList(raw_polys, draw_components_);
    if (draw_components_) {
      for (const TargetComponent &component : target_component_list) {
        DrawComponent(component, {0, 255, 255}, {0, 255, 255}, {255, 0, 0},
                      {0, 0, 255});
        overlay_.DrawCross(component.bottom_point, 4, {128, 0, 255});
      }
    }

    // Put the compenents together into targets.
    std::vector<Target> target_list = target_finder_.FindTargetsFromComponents(
        target_component_list, draw_raw_target_);
    if (draw_raw_target_) {
      for (const Target &target : target_list) {
        DrawTarget(target);
      }
    }

    // Use the solver to generate an intermediate version of our results.
    std::vector<IntermediateResult> results;
    for (const Target &target : target_list) {
      results.emplace_back(
          target_finder_.ProcessTargetToResult(target, draw_raw_IR_));
      if (draw_raw_IR_) {
        IntermediateResult updatable_result = results.back();
        target_finder_.MaybePickAndUpdateResult(&updatable_result,
                                                draw_raw_IR_);
        DrawResult(updatable_result, {255, 128, 0});
      }
    }

    // Check that our current results match possible solutions.
    results = target_finder_.FilterResults(results, 0, draw_results_);
    if (draw_results_) {
      for (const IntermediateResult &result : results) {
        ::std::cout << "Found target x: "
                    << camera_geometry.location[0] +
                           ::std::cos(camera_geometry.heading +
                                      result.extrinsics.r2) *
                               result.extrinsics.z
                    << ::std::endl;
        ::std::cout << "Found target y: "
                    << camera_geometry.location[1] +
                           ::std::sin(camera_geometry.heading +
                                      result.extrinsics.r2) *
                               result.extrinsics.z
                    << ::std::endl;
        ::std::cout << "Found target z: "
                    << camera_geometry.location[2] + result.extrinsics.y
                    << ::std::endl;
        DrawTarget(result, {0, 255, 0});
      }
    }

    int desired_exposure;
    if (target_finder_.TestExposure(results, num_pixels, &desired_exposure)) {
      printf("Switching exposure to %d.\n", desired_exposure);
      SetExposure(desired_exposure);
    }

    // If the target list is not empty then we found a target.
    return !results.empty();
  }

  std::function<void(uint32_t)> RegisterKeyPress() override {
    return [this](uint32_t key) {
      (void)key;
      if (key == 'z') {
        draw_results_ = !draw_results_;
      } else if (key == 'x') {
        draw_raw_IR_ = !draw_raw_IR_;
      } else if (key == 'c') {
        draw_raw_target_ = !draw_raw_target_;
      } else if (key == 'v') {
        draw_components_ = !draw_components_;
      } else if (key == 'b') {
        draw_raw_poly_ = !draw_raw_poly_;
      } else if (key == 'n') {
        draw_contours_ = !draw_contours_;
      } else if (key == 'm') {
        draw_select_blob_ = !draw_select_blob_;
      } else if (key == 'h') {
        printf("Key Mappings:\n");
        printf(" z: Toggle drawing final target pose.\n");
        printf(" x: Toggle drawing re-projected targets and print solver results.\n");
        printf(" c: Toggle drawing proposed target groupings.\n");
        printf(" v: Toggle drawing ordered target components.\n");
        printf(" b: Toggle drawing proposed target components.\n");
        printf(" n: Toggle drawing countours before and after warping.\n");
        printf(" m: Toggle drawing raw blob data (may need to change image to toggle a redraw).\n");
        printf(" h: Print this message.\n");
        printf(" a: May log camera image to /tmp/debug_viewer_jpeg_<#>.yuyv\n");
        printf(" q: Exit the application.\n");
      } else if (key == 'q') {
        printf("User requested shutdown.\n");
        exit(0);
      }
      HandleBlobs(imgs_last_, fmt_last_);
      viewer_->Redraw();
    };
  }

  void DrawContour(ContourNode *contour, PixelRef color) {
    if (viewer_) {
      for (ContourNode *node = contour; node->next != contour;) {
        Vector<2> a(node->pt.x, node->pt.y);
        Vector<2> b(node->next->pt.x, node->next->pt.y);
        overlay_.AddLine(a, b, color);
        node = node->next;
      }
    }
  }

  void DrawContour(const ::std::vector<::Eigen::Vector2f> &contour,
                   PixelRef color) {
    if (viewer_) {
      for (size_t i = 0; i < contour.size(); ++i) {
        Vector<2> a(contour[i].x(), contour[i].y());
        Vector<2> b(contour[(i + 1) % contour.size()].x(),
                    contour[(i + 1) % contour.size()].y());
        overlay_.AddLine(a, b, color);
      }
    }
  }

  void DrawComponent(const TargetComponent &comp, PixelRef top_color,
                     PixelRef bot_color, PixelRef in_color,
                     PixelRef out_color) {
    overlay_.AddLine(comp.top, comp.inside, top_color);
    overlay_.AddLine(comp.bottom, comp.outside, bot_color);

    overlay_.AddLine(comp.bottom, comp.inside, in_color);
    overlay_.AddLine(comp.top, comp.outside, out_color);
  }

  void DrawTarget(const Target &target) {
    Vector<2> leftTop = (target.left.top + target.left.inside) * 0.5;
    Vector<2> rightTop = (target.right.top + target.right.inside) * 0.5;
    overlay_.AddLine(leftTop, rightTop, {255, 215, 0});

    Vector<2> leftBot = (target.left.bottom + target.left.outside) * 0.5;
    Vector<2> rightBot = (target.right.bottom + target.right.outside) * 0.5;
    overlay_.AddLine(leftBot, rightBot, {255, 215, 0});

    overlay_.AddLine(leftTop, leftBot, {255, 215, 0});
    overlay_.AddLine(rightTop, rightBot, {255, 215, 0});
  }

  void DrawResult(const IntermediateResult &result, PixelRef color) {
    Target target = Project(target_finder_.GetTemplateTarget(), intrinsics(),
                            result.extrinsics);
    DrawComponent(target.left, color, color, color, color);
    DrawComponent(target.right, color, color, color, color);
  }

  void DrawTarget(const IntermediateResult &result, PixelRef color) {
    Target target = Project(target_finder_.GetTemplateTarget(), intrinsics(),
                            result.extrinsics);
    Segment<2> leftAx((target.left.top + target.left.inside) * 0.5,
                      (target.left.bottom + target.left.outside) * 0.5);
    leftAx.Set(leftAx.A() * 0.9 + leftAx.B() * 0.1,
               leftAx.B() * 0.9 + leftAx.A() * 0.1);
    overlay_.AddLine(leftAx, color);

    Segment<2> rightAx((target.right.top + target.right.inside) * 0.5,
                       (target.right.bottom + target.right.outside) * 0.5);
    rightAx.Set(rightAx.A() * 0.9 + rightAx.B() * 0.1,
                rightAx.B() * 0.9 + rightAx.A() * 0.1);
    overlay_.AddLine(rightAx, color);

    overlay_.AddLine(leftAx.A(), rightAx.A(), color);
    overlay_.AddLine(leftAx.B(), rightAx.B(), color);
    Vector<3> p1(0.0, 0.0, 100.0);

    Vector<3> p2 =
        Rotate(intrinsics().mount_angle, result.extrinsics.r1, 0.0, p1);
    Vector<2> p3(p2.x(), p2.y());
    overlay_.AddLine(leftAx.A(), p3 + leftAx.A(), {0, 255, 0});
    overlay_.AddLine(leftAx.B(), p3 + leftAx.B(), {0, 255, 0});
    overlay_.AddLine(rightAx.A(), p3 + rightAx.A(), {0, 255, 0});
    overlay_.AddLine(rightAx.B(), p3 + rightAx.B(), {0, 255, 0});

    overlay_.AddLine(p3 + leftAx.A(), p3 + leftAx.B(), {0, 255, 0});
    overlay_.AddLine(p3 + leftAx.A(), p3 + rightAx.A(), {0, 255, 0});
    overlay_.AddLine(p3 + rightAx.A(), p3 + rightAx.B(), {0, 255, 0});
    overlay_.AddLine(p3 + leftAx.B(), p3 + rightAx.B(), {0, 255, 0});
  }

  const IntrinsicParams &intrinsics() const {
    return target_finder_.intrinsics();
  }

 private:
  // implementation of the filter pipeline.
  TargetFinder target_finder_;
  aos::vision::BlobStreamViewer *viewer_ = nullptr;
  aos::vision::PixelLinesOverlay overlay_;
  std::vector<aos::vision::OverlayBase *> overlays_;
  BlobList imgs_last_;
  ImageFormat fmt_last_;
  bool draw_select_blob_ = false;
  bool draw_contours_ = true;
  bool draw_raw_poly_ = true;
  bool draw_components_ = false;
  bool draw_raw_target_ = false;
  bool draw_raw_IR_ = true;
  bool draw_results_ = true;
};

}  // namespace vision
}  // namespace y2017

int main(int argc, char **argv) {
  ::gflags::ParseCommandLineFlags(&argc, &argv, true);

  y2019::vision::FilterHarness filter_harness;
  aos::vision::DebugFrameworkMain(argc, argv, &filter_harness,
                                  aos::vision::CameraParams());
}


// Copyright (c) 2017-2018 The Rhombus Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <test/setup_common.h>

#include <crypto/sha256.h>

#include <secp256k1.h>
#include <secp256k1_rangeproof.h>
#include <secp256k1_bulletproofs.h>
#include <stdint.h>
#include <util/strencodings.h>

#include <boost/test/unit_test.hpp>

#include <blind.h>

BOOST_FIXTURE_TEST_SUITE(ct_tests, BasicTestingSetup)

class CTxOutValueTest
{
public:
    secp256k1_pedersen_commitment commitment;
    std::vector<uint8_t> vchRangeproof;
    std::vector<uint8_t> vchNonceCommitment;
};

BOOST_AUTO_TEST_CASE(ct_test)
{
    SeedInsecureRand();
    secp256k1_context *ctx = secp256k1_context_create(SECP256K1_CONTEXT_SIGN | SECP256K1_CONTEXT_VERIFY);

    std::vector<CTxOutValueTest> txins(1);

    std::vector<const uint8_t*> blindptrs;
    uint8_t blindsin[1][32];
    //GetStrongRandBytes(&blindsin[0][0], 32);
    InsecureRandBytes(&blindsin[0][0], 32);
    blindptrs.push_back(&blindsin[0][0]);

    CAmount nValueIn = 45.69 * COIN;
    BOOST_CHECK(secp256k1_pedersen_commit(ctx, &txins[0].commitment, &blindsin[0][0], nValueIn, &secp256k1_generator_const_h, &secp256k1_generator_const_g));

    const int nTxOut = 2;
    std::vector<CTxOutValueTest> txouts(nTxOut);

    std::vector<CAmount> amount_outs(2);
    amount_outs[0] = 5.69 * COIN;
    amount_outs[1] = 40 * COIN;

    std::vector<CKey> kto_outs(2);
    InsecureNewKey(kto_outs[0], true);
    InsecureNewKey(kto_outs[1], true);

    std::vector<CPubKey> pkto_outs(2);
    pkto_outs[0] = kto_outs[0].GetPubKey();
    pkto_outs[1] = kto_outs[1].GetPubKey();

    uint8_t blind[nTxOut][32];

    size_t nBlinded = 0;
    for (size_t k = 0; k < txouts.size(); ++k) {
        CTxOutValueTest &txout = txouts[k];

        if (nBlinded + 1 == txouts.size()) {
            // Last to-be-blinded value: compute from all other blinding factors.
            // sum of output blinding values must equal sum of input blinding values
            BOOST_CHECK(secp256k1_pedersen_blind_sum(ctx, &blind[nBlinded][0], &blindptrs[0], 2, 1));
            blindptrs.push_back(&blind[nBlinded++][0]);
        } else {
            //GetStrongRandBytes(&blind[nBlinded][0], 32);
            InsecureRandBytes(&blind[nBlinded][0], 32);
            blindptrs.push_back(&blind[nBlinded++][0]);
        }

        BOOST_CHECK(secp256k1_pedersen_commit(ctx, &txout.commitment, (uint8_t*)blindptrs.back(), amount_outs[k], &secp256k1_generator_const_h, &secp256k1_generator_const_g));

        // Generate ephemeral key for ECDH nonce generation
        CKey ephemeral_key;
        InsecureNewKey(ephemeral_key, true);
        CPubKey ephemeral_pubkey = ephemeral_key.GetPubKey();
        txout.vchNonceCommitment.resize(33);
        memcpy(&txout.vchNonceCommitment[0], &ephemeral_pubkey[0], 33);

        // Generate nonce
        uint256 nonce = ephemeral_key.ECDH(pkto_outs[k]);
        CSHA256().Write(nonce.begin(), 32).Finalize(nonce.begin());

        // Create range proof
        size_t nRangeProofLen = 5134;
        // TODO: smarter min_value selection

        txout.vchRangeproof.resize(nRangeProofLen);

        uint64_t min_value = 0;
        int ct_exponent = 2;
        int ct_bits = 32;

        const char *message = "narration";
        size_t mlen = strlen(message);

        BOOST_CHECK(secp256k1_rangeproof_sign(ctx,
            &txout.vchRangeproof[0], &nRangeProofLen,
            min_value, &txout.commitment,
            blindptrs.back(), nonce.begin(),
            ct_exponent, ct_bits,
            amount_outs[k],
            (const unsigned char*) message, mlen,
            nullptr, 0,
            secp256k1_generator_h));

        txout.vchRangeproof.resize(nRangeProofLen);
    }

    std::vector<secp256k1_pedersen_commitment*> vpCommitsIn, vpCommitsOut;
    vpCommitsIn.push_back(&txins[0].commitment);

    vpCommitsOut.push_back(&txouts[0].commitment);
    vpCommitsOut.push_back(&txouts[1].commitment);

    BOOST_CHECK(secp256k1_pedersen_verify_tally(ctx, vpCommitsIn.data(), vpCommitsIn.size(), vpCommitsOut.data(), vpCommitsOut.size()));

    for (size_t k = 0; k < txouts.size(); ++k) {
        CTxOutValueTest &txout = txouts[k];

        int rexp;
        int rmantissa;
        uint64_t min_value, max_value;

        BOOST_CHECK(secp256k1_rangeproof_info(ctx,
            &rexp, &rmantissa,
            &min_value, &max_value,
            &txout.vchRangeproof[0], txout.vchRangeproof.size()) == 1);

        min_value = 0;
        max_value = 0;
        BOOST_CHECK(1 == secp256k1_rangeproof_verify(ctx, &min_value, &max_value,
            &txout.commitment, txout.vchRangeproof.data(), txout.vchRangeproof.size(),
            nullptr, 0,
            secp256k1_generator_h));

        CPubKey ephemeral_key(txout.vchNonceCommitment);
        BOOST_CHECK(ephemeral_key.IsValid());
        uint256 nonce = kto_outs[k].ECDH(ephemeral_key);
        CSHA256().Write(nonce.begin(), 32).Finalize(nonce.begin());

        uint8_t blindOut[32];
        unsigned char msg[4096];
        size_t msg_size = sizeof(msg);
        uint64_t amountOut;
        BOOST_CHECK(secp256k1_rangeproof_rewind(ctx,
            blindOut, &amountOut, msg, &msg_size, nonce.begin(),
            &min_value, &max_value,
            &txout.commitment, txout.vchRangeproof.data(), txout.vchRangeproof.size(),
            nullptr, 0,
            secp256k1_generator_h));

        msg[9] = '\0';
        BOOST_CHECK(memcmp(msg, "narration", 9) == 0);
    }

    secp256k1_context_destroy(ctx);
}

BOOST_AUTO_TEST_CASE(ct_test_bulletproofs)
{
    SeedInsecureRand();
    secp256k1_context *ctx = secp256k1_context_create(SECP256K1_CONTEXT_SIGN | SECP256K1_CONTEXT_VERIFY);
    secp256k1_scratch_space *scratch = secp256k1_scratch_space_create(ctx, 1024 * 1024);
    secp256k1_bulletproof_generators *gens;

    gens = secp256k1_bulletproof_generators_create(ctx, &secp256k1_generator_const_g, 256);
    BOOST_CHECK(gens != nullptr);

    std::vector<CTxOutValueTest> txins(1);

    std::vector<const uint8_t*> blindptrs;
    uint8_t blindsin[1][32];
    //GetStrongRandBytes(&blindsin[0][0], 32);
    InsecureRandBytes(&blindsin[0][0], 32);
    blindptrs.push_back(&blindsin[0][0]);

    CAmount nValueIn = 45.69 * COIN;
    BOOST_CHECK(secp256k1_pedersen_commit(ctx, &txins[0].commitment, &blindsin[0][0], nValueIn, &secp256k1_generator_const_h, &secp256k1_generator_const_g));

    const int nTxOut = 2;
    std::vector<CTxOutValueTest> txouts(nTxOut);

    std::vector<CAmount> amount_outs(2);
    amount_outs[0] = 5.69 * COIN;
    amount_outs[1] = 40 * COIN;

    std::vector<CKey> kto_outs(2);
    InsecureNewKey(kto_outs[0], true);
    InsecureNewKey(kto_outs[1], true);

    std::vector<CPubKey> pkto_outs(2);
    pkto_outs[0] = kto_outs[0].GetPubKey();
    pkto_outs[1] = kto_outs[1].GetPubKey();

    uint8_t blind[nTxOut][32];

    size_t nBlinded = 0;
    for (size_t k = 0; k < txouts.size(); ++k) {
        CTxOutValueTest &txout = txouts[k];

        if (nBlinded + 1 == txouts.size()) {
            // Last to-be-blinded value: compute from all other blinding factors.
            // sum of output blinding values must equal sum of input blinding values
            BOOST_CHECK(secp256k1_pedersen_blind_sum(ctx, &blind[nBlinded][0], &blindptrs[0], 2, 1));
            blindptrs.push_back(&blind[nBlinded++][0]);
        } else {
            //GetStrongRandBytes(&blind[nBlinded][0], 32);
            InsecureRandBytes(&blind[nBlinded][0], 32);
            blindptrs.push_back(&blind[nBlinded++][0]);
        }

        BOOST_CHECK(secp256k1_pedersen_commit(ctx, &txout.commitment, (uint8_t*)blindptrs.back(), amount_outs[k], &secp256k1_generator_const_h, &secp256k1_generator_const_g));

        // Generate ephemeral key for ECDH nonce generation
        CKey ephemeral_key;
        InsecureNewKey(ephemeral_key, true);
        CPubKey ephemeral_pubkey = ephemeral_key.GetPubKey();
        txout.vchNonceCommitment.resize(33);
        memcpy(&txout.vchNonceCommitment[0], &ephemeral_pubkey[0], 33);

        // Generate nonce
        uint256 nonce = ephemeral_key.ECDH(pkto_outs[k]);
        CSHA256().Write(nonce.begin(), 32).Finalize(nonce.begin());

        // Create range proof
        size_t nRangeProofLen = 5134;
        txout.vchRangeproof.resize(nRangeProofLen);

        uint8_t *proof = &txout.vchRangeproof[0];
        const uint8_t *blindptrs_[] = {blindptrs.back()};
        BOOST_CHECK(secp256k1_bulletproof_rangeproof_prove(ctx, scratch, gens, proof, &nRangeProofLen, (const uint64_t*)&amount_outs[k], NULL, blindptrs_, 1, &secp256k1_generator_const_h, 64, nonce.begin(), NULL, 0) == 1);

        txout.vchRangeproof.resize(nRangeProofLen);
    }

    std::vector<secp256k1_pedersen_commitment*> vpCommitsIn, vpCommitsOut;
    vpCommitsIn.push_back(&txins[0].commitment);

    vpCommitsOut.push_back(&txouts[0].commitment);
    vpCommitsOut.push_back(&txouts[1].commitment);

    BOOST_CHECK(secp256k1_pedersen_verify_tally(ctx, vpCommitsIn.data(), vpCommitsIn.size(), vpCommitsOut.data(), vpCommitsOut.size()));

    for (size_t k = 0; k < txouts.size(); ++k) {
        CTxOutValueTest &txout = txouts[k];

        uint64_t value_out;

        uint8_t *proof = &txout.vchRangeproof[0];
        size_t nRangeProofLen = txout.vchRangeproof.size();
        BOOST_CHECK(secp256k1_bulletproof_rangeproof_verify(ctx, scratch, gens, proof, nRangeProofLen, NULL, &txout.commitment, 1, 64, &secp256k1_generator_const_h, NULL, 0) == 1);

        CPubKey ephemeral_key(txout.vchNonceCommitment);
        BOOST_CHECK(ephemeral_key.IsValid());
        uint256 nonce = kto_outs[k].ECDH(ephemeral_key);
        CSHA256().Write(nonce.begin(), 32).Finalize(nonce.begin());
        uint8_t blind_out[32];
        BOOST_CHECK(secp256k1_bulletproof_rangeproof_rewind(ctx, gens, &value_out, blind_out, proof, nRangeProofLen, 0, &txout.commitment, &secp256k1_generator_const_h, nonce.begin(), NULL, 0));
        BOOST_CHECK((int64_t)value_out == amount_outs[k]);
    }

    secp256k1_bulletproof_generators_destroy(ctx, gens);
    secp256k1_scratch_space_destroy(scratch);
    secp256k1_context_destroy(ctx);
}

BOOST_AUTO_TEST_CASE(ct_parameters_test)
{
    //for (size_t k = 0; k < 10000; ++k)
    for (size_t k = 0; k < 100; ++k)
    {
        CAmount nValue = (GetRand((MAX_MONEY / (k+1))) / COIN) * COIN;
        uint64_t min_value = 0;
        int ct_exponent = 0;
        int ct_bits = 32;

        SelectRangeProofParameters(nValue, min_value, ct_exponent, ct_bits);
    };
}

BOOST_AUTO_TEST_CASE(ct_commitment_test)
{
    secp256k1_context *ctx = secp256k1_context_create(SECP256K1_CONTEXT_SIGN | SECP256K1_CONTEXT_VERIFY);

    secp256k1_pedersen_commitment commitment1, commitment2, commitment3;
    uint8_t blind[32];
    memset(blind, 0, 32);

    BOOST_CHECK(secp256k1_pedersen_commit(ctx, &commitment1, blind, 10, &secp256k1_generator_const_h, &secp256k1_generator_const_g));
    BOOST_CHECK(HexStr(commitment1.data, commitment1.data+33) == "093806b3e479859dc6dd508eca22257d796bba3e32a6616cc97b51723b50a5f429");

    memset(blind, 1, 32);
    BOOST_CHECK(secp256k1_pedersen_commit(ctx, &commitment2, blind, 10, &secp256k1_generator_const_h, &secp256k1_generator_const_g));
    BOOST_CHECK(HexStr(commitment2.data, commitment2.data+33) == "09badd85325926c329aa62f5a7d37d0a015aabfb52608052d277530bd025ddc971");

    secp256k1_pedersen_commitment *pc[2];
    pc[0] = &commitment1;
    pc[1] = &commitment2;
    BOOST_CHECK(secp256k1_pedersen_commitment_sum(ctx, &commitment3, pc, 2));
    BOOST_CHECK(HexStr(commitment3.data, commitment3.data+33) == "09e922a6c61aecd734d79ce41dbf09f71779bfcca6d3f30e4495923eb9801fb9a2");

    secp256k1_context_destroy(ctx);
}

BOOST_AUTO_TEST_SUITE_END()


/*! @file pitts_tensortrain_from_dense_twosided.hpp
* @brief conversion of a dense tensor to the tensor-train format (based on a hopefully faster TSQR algorithm, two-sided variant)
* @author Melven Roehrig-Zoellner <Melven.Roehrig-Zoellner@DLR.de>
* @date 2020-08-08
* @copyright Deutsches Zentrum fuer Luft- und Raumfahrt e. V. (DLR), German Aerospace Center
*
**/

// include guard
#ifndef PITTS_TENSORTRAIN_FROM_DENSE_TWOSIDED_HPP
#define PITTS_TENSORTRAIN_FROM_DENSE_TWOSIDED_HPP

// includes
#include <limits>
#include <numeric>
#pragma GCC push_options
#pragma GCC optimize("no-unsafe-math-optimizations")
#include <Eigen/Dense>
#pragma GCC pop_options
#include "pitts_tensortrain.hpp"
#include "pitts_multivector.hpp"
#include "pitts_multivector_tsqr.hpp"
#include "pitts_multivector_transform.hpp"
#include "pitts_multivector_transpose.hpp"
#include "pitts_multivector_eigen_adaptor.hpp"
#include "pitts_tensor2.hpp"
#include "pitts_tensor2_eigen_adaptor.hpp"
#include "pitts_timer.hpp"

//! namespace for the library PITTS (parallel iterative tensor train solvers)
namespace PITTS
{
  //! namespace for helper functionality
  namespace internal
  {
    //! helper namespace for high-order SVD functionality (e.g. TensorTrain_fromDense)
    namespace HOSVD
    {
      template<typename T>
      void split(const MultiVector<T>& X, MultiVector<T>& Y, Tensor2<T>& M, int nextDim, T rankTolerance, int maxRank)
      {
        using EigenMatrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
        Eigen::JacobiSVD<EigenMatrix> svd;
std::cout << "HOSVD::split  matrix dimensions: " << X.rows() << " x " << X.cols() << "\n";
        if( X.rows() > 10*X.cols() )
        {
          // calculate QR decomposition (QR-trick: X=QR,SVD(R))
          block_TSQR(X, M, 0, false);
          svd.compute(ConstEigenMap(M), Eigen::ComputeThinU | Eigen::ComputeThinV);
        }
        else
        {
          svd.compute(ConstEigenMap(X), Eigen::ComputeThinU | Eigen::ComputeThinV);
        }

std::cout << "singular values: " << svd.singularValues().transpose() << "\n";

        // truncate svd
        svd.setThreshold(rankTolerance);
        int rank = svd.rank();
        if( maxRank > 0 )
          rank = std::min(maxRank, rank);

        // copy right singular vectors
        M.resize(X.cols(), rank);
        EigenMap(M) = svd.matrixV().leftCols(rank);

        // transform input, s.t. Y \approx X M
        transform(X, M, Y, {X.rows()/nextDim, rank*nextDim});
      }
    }
  }

  //! calculate tensor-train decomposition of a tensor stored in fully dense format
  //!
  //! Passing a large enough buffer in work helps to avoid costly reallocations + later page-faults for large data.
  //!
  //! @warning To reduce memory overhead, this function will overwrite the input arguments with temporary data.
  //!          Please pass a copy of the data if you still need it!
  //!
  //! @tparam T         underlying data type (double, complex, ...)
  //!
  //! @param X              input tensor, overwritten and modified output, dimension must be (size/lastDim, lastDim) where lastDim = dimensions.back()
  //! @param dimensions     tensor dimensions, input is interpreted in Fortran storage order (first index changes the fastest)
  //! @param work           buffer for temporary data, will be resized and modified
  //! @param rankTolerance  approximation accuracy, used to reduce the TTranks of the resulting tensor train
  //! @param maxRank        maximal TTrank (bond dimension), unbounded by default
  //! @return               resulting tensor train
  //!
  template<typename T>
  TensorTrain<T> fromDense_twoSided(MultiVector<T>& X, MultiVector<T>& work, const std::vector<int>& dimensions, T rankTolerance = std::sqrt(std::numeric_limits<T>::epsilon()), int maxRank = -1)
  {
    // timer
    const auto timer = PITTS::timing::createScopedTimer<TensorTrain<T>>();

    // abort early for zero dimensions
    if( dimensions.size() == 0 )
    {
      if( X.rows()*X.cols() !=  0 )
        throw std::out_of_range("Mismatching dimensions in TensorTrain<T>::fromDense");
      return TensorTrain<T>{dimensions};
    }

    const auto totalSize = std::accumulate(begin(dimensions), end(dimensions), (std::ptrdiff_t)1, std::multiplies<std::ptrdiff_t>());
    const auto nDims = dimensions.size();
    if( X.rows() != totalSize/dimensions[nDims-1] || X.cols() != dimensions[nDims-1] )
      throw std::out_of_range("Mismatching dimensions in TensorTrain<T>::fromDense");

    TensorTrain<T> result(dimensions);

    // actually convert to tensor train format
    Tensor2<T> M;
    for(int ii = 0; ii < nDims; ii++)
    {
      if( ii % 2 == 0 )
      {
        // right part
        const auto iDim = nDims - 1 - ii/2;
        if( ii > 0 )
        {
          const auto r1 = result.subTensors()[iDim+1].r1();
          const auto n = dimensions[iDim];
          transpose(work, X, {(work.rows()*work.cols())/(n*r1), n*r1}, true);
        }
        if( ii != nDims-1 )
        {
          internal::HOSVD::split(X, work, M, 1, rankTolerance, maxRank);
        }
        else
        {
          M.resize(X.cols(), X.rows());
          EigenMap(M) = ConstEigenMap(X).transpose();
        }

        auto& subT = result.editableSubTensors()[iDim];
        const int rank = M.r2();
        subT.resize(rank, dimensions[iDim], X.cols()/dimensions[iDim]);
        for(int i = 0; i < rank; i++)
          for(int j = 0; j < subT.n(); j++)
            for(int k = 0; k < subT.r2(); k++)
              subT(i,j,k) = M(j+subT.n()*k, i);
      }
      else
      {
        // left part
        const auto iDim = ii / 2;
        {
          const auto r2 = (iDim == 0 ) ? 1 : result.subTensors()[iDim-1].r2();
          const auto n = dimensions[iDim];
          transpose(work, X, {(work.cols()*work.rows())/(n*r2), n*r2}, false);
        }
        if( ii != nDims - 1 )
        {
          internal::HOSVD::split(X, work, M, 1, rankTolerance, maxRank);
        }
        else
        {
          M.resize(X.cols(), X.rows());
          EigenMap(M) = ConstEigenMap(X).transpose();
        }

        auto& subT = result.editableSubTensors()[iDim];
        const int rank = M.r2();
        subT.resize(X.cols()/dimensions[iDim], dimensions[iDim], rank);
        for(int i = 0; i < rank; i++)
          for(int j = 0; j < subT.n(); j++)
            for(int k = 0; k < subT.r1(); k++)
              subT(k,j,i) = M(k+j*subT.r1(), i);
      }
    }

    return result;
  }

}

#endif // PITTS_TENSORTRAIN_FROM_DENSE_TWOSIDED_HPP