#include <bio/defs.h>
USING_BIO_NS

#include <boost/test/unit_test.hpp>
#include <boost/test/parameterized_test.hpp>
#include <boost/assign/list_of.hpp>
#undef max
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
using namespace boost;
using namespace boost::assign;
using boost::unit_test::test_suite;

#include <iostream>
using namespace std;

#include <math.h>

#include "blas1c.h"
#include "lapackc.h"
#include "arlsmat.h"
//#include "arcomp.h"
#include "arlsmat.h"
#include "arlnsmat.h"
#include "arlssym.h"
#include "arlgsym.h"
#include "arlsnsym.h"
#include "arlgnsym.h"
#include "arlscomp.h"
#include "arlgcomp.h"

#define VERBOSE_CHECKING

template <class FLOAT>
int AREig(arcomplex<FLOAT> EigVal[], int n, int nnz, arcomplex<FLOAT> A[],
          int irow[], int pcol[], int nev, char* which = "LM", int ncv = 0,
          FLOAT tol = 0.0, int maxit = 0, arcomplex<FLOAT>* resid = 0,
          bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluNonSymMatrix<arcomplex<FLOAT> > matrix(n, nnz, A, irow, pcol);

  // Defining the eigenvalue problem.

  ARluCompStdEig<FLOAT> prob(nev, matrix, which, ncv, tol,
                             maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigVal);

} // complex standard problem, only eigenvalues, regular mode.

template <class FLOAT>
int AREig(arcomplex<FLOAT> EigVal[], arcomplex<FLOAT> EigVec[], int n,
          int nnz, arcomplex<FLOAT> A[], int irow[], int pcol[],
          int nev, char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, arcomplex<FLOAT>* resid = 0, bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluNonSymMatrix<arcomplex<FLOAT> > matrix(n, nnz, A, irow, pcol);

  // Defining the eigenvalue problem.

  ARluCompStdEig<FLOAT> prob(nev, matrix, which, ncv, tol,
                             maxit, resid, AutoShift);

  // Finding eigenvalues and eigenvectors.

  return prob.EigenValVectors(EigVec, EigVal);

} // complex standard problem, values and vectors, regular mode.

template <class FLOAT>
int AREig(arcomplex<FLOAT> EigVal[], int n, int nnz, arcomplex<FLOAT> A[],
          int irow[], int pcol[], arcomplex<FLOAT> sigma, int nev,
          char* which = "LM", int ncv = 0, FLOAT tol = 0.0, int maxit = 0,
          arcomplex<FLOAT>* resid = 0, bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluNonSymMatrix<arcomplex<FLOAT> > matrix(n, nnz, A, irow, pcol);

  // Defining the eigenvalue problem.

  ARluCompStdEig<FLOAT> prob(nev, matrix, sigma, which, ncv,
                             tol, maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigVal);

} // complex standard problem, only eigenvalues, shift-and-invert.

template <class FLOAT>
int AREig(arcomplex<FLOAT> EigVal[], arcomplex<FLOAT> EigVec[], int n,
          int nnz, arcomplex<FLOAT> A[], int irow[], int pcol[],
          arcomplex<FLOAT> sigma, int nev, char* which = "LM",
          int ncv = 0, FLOAT tol = 0.0, int maxit = 0,
          arcomplex<FLOAT>* resid = 0, bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluNonSymMatrix<arcomplex<FLOAT> > matrix(n, nnz, A, irow, pcol);

  // Defining the eigenvalue problem.

  ARluCompStdEig<FLOAT> prob(nev, matrix, sigma, which, ncv,
                             tol, maxit, resid, AutoShift);

  // Finding eigenvalues and eigenvectors.

  return prob.EigenValVectors(EigVec, EigVal);

} // complex standard problem, values and vectors, shift-and-invert.

template <class FLOAT>
int AREig(arcomplex<FLOAT> EigVal[], int n, int nnzA,
          arcomplex<FLOAT> A[], int irowA[], int pcolA[], int nnzB,
          arcomplex<FLOAT> B[], int irowB[], int pcolB[], int nev,
          char* which = "LM", int ncv = 0, FLOAT tol = 0.0, int maxit = 0,
          arcomplex<FLOAT>* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluNonSymMatrix<arcomplex<FLOAT> > matrixA(n, nnzA, A, irowA, pcolA);
  ARluNonSymMatrix<arcomplex<FLOAT> > matrixB(n, nnzB, B, irowB, pcolB);

  // Defining the eigenvalue problem.

  ARluCompGenEig<FLOAT> prob(nev, matrixA, matrixB, which,
                             ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigVal);

} // complex generalized problem, only eigenvalues, regular mode.

template <class FLOAT>
int AREig(arcomplex<FLOAT> EigVal[], arcomplex<FLOAT> EigVec[], int n,
          int nnzA, arcomplex<FLOAT> A[], int irowA[], int pcolA[],
          int nnzB, arcomplex<FLOAT> B[], int irowB[], int pcolB[],
          int nev, char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, arcomplex<FLOAT>* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluNonSymMatrix<arcomplex<FLOAT> > matrixA(n, nnzA, A, irowA, pcolA);
  ARluNonSymMatrix<arcomplex<FLOAT> > matrixB(n, nnzB, B, irowB, pcolB);

  // Defining the eigenvalue problem.

  ARluCompGenEig<FLOAT> prob(nev, matrixA, matrixB, which,
                             ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues and eigenvectors.

  return prob.EigenValVectors(EigVec, EigVal);

} // complex generalized problem, values and vectors, regular mode.

template <class FLOAT>
int AREig(arcomplex<FLOAT> EigVal[], int n, int nnzA, arcomplex<FLOAT> A[],
          int irowA[], int pcolA[], int nnzB, arcomplex<FLOAT> B[],
          int irowB[], int pcolB[], arcomplex<FLOAT> sigma, int nev,
          char* which = "LM", int ncv = 0, FLOAT tol = 0.0, int maxit = 0,
          arcomplex<FLOAT>* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluNonSymMatrix<arcomplex<FLOAT> > matrixA(n, nnzA, A, irowA, pcolA);
  ARluNonSymMatrix<arcomplex<FLOAT> > matrixB(n, nnzB, B, irowB, pcolB);

  // Defining the eigenvalue problem.

  ARluCompGenEig<FLOAT> prob(nev, matrixA, matrixB, sigma, which,
                             ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigVal);

} // complex generalized problem, only eigenvalues, shift-and-invert mode.

template <class FLOAT>
int AREig(arcomplex<FLOAT> EigVal[], arcomplex<FLOAT> EigVec[], int n,
          int nnzA, arcomplex<FLOAT> A[], int irowA[], int pcolA[],
          int nnzB, arcomplex<FLOAT> B[], int irowB[], int pcolB[],
          arcomplex<FLOAT> sigma, int nev, char* which = "LM",
          int ncv = 0, FLOAT tol = 0.0, int maxit = 0,
          arcomplex<FLOAT>* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluNonSymMatrix<arcomplex<FLOAT> > matrixA(n, nnzA, A, irowA, pcolA);
  ARluNonSymMatrix<arcomplex<FLOAT> > matrixB(n, nnzB, B, irowB, pcolB);

  // Defining the eigenvalue problem.

  ARluCompGenEig<FLOAT> prob(nev, matrixA, matrixB, sigma, which,
                             ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues and eigenvectors.

  return prob.EigenValVectors(EigVec, EigVal);

} // complex generalized problem, values and vectors, shift-and-invert mode.

template <class FLOAT>
int AREig(double EigValR[], FLOAT EigValI[], int n, int nnz,
          FLOAT A[], int irow[], int pcol[], int nev, char* which = "LM",
          int ncv = 0, FLOAT tol = 0.0, int maxit = 0, FLOAT* resid = 0,
          bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrix(n, nnz, A, irow, pcol);

  // Defining the eigenvalue problem.

  ARluNonSymStdEig<FLOAT> prob(nev, matrix, which, ncv, tol,
                               maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigValR, EigValI);

} // real nonsymmetric standard problem, only eigenvalues, regular mode.

template <class FLOAT>
int AREig(float EigValR[], FLOAT EigValI[], int n, int nnz,
          FLOAT A[], int irow[], int pcol[], int nev, char* which = "LM",
          int ncv = 0, FLOAT tol = 0.0, int maxit = 0, FLOAT* resid = 0,
          bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrix(n, nnz, A, irow, pcol);

  // Defining the eigenvalue problem.

  ARluNonSymStdEig<FLOAT> prob(nev, matrix, which, ncv, tol,
                               maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigValR, EigValI);

} // real nonsymmetric standard problem, only eigenvalues, regular mode.

template <class FLOAT>
int AREig(FLOAT EigValR[], FLOAT EigValI[], FLOAT EigVec[], int n, int nnz,
          FLOAT A[], int irow[], int pcol[], int nev, char* which = "LM",
          int ncv = 0, FLOAT tol = 0.0, int maxit = 0, FLOAT* resid = 0,
          bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrix(n, nnz, A, irow, pcol);

  // Defining the eigenvalue problem.

  ARluNonSymStdEig<FLOAT> prob(nev, matrix, which, ncv, tol,
                               maxit, resid, AutoShift);

  // Finding eigenvalues and eigenvectors.

  return prob.EigenValVectors(EigVec, EigValR, EigValI);

} // real nonsymmetric standard problem, values and vectors, regular mode.

template <class FLOAT>
int AREig(double EigValR[], FLOAT EigValI[], int n, int nnz,
          FLOAT A[], int irow[], int pcol[], FLOAT sigma, int nev,
          char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrix(n, nnz, A, irow, pcol);

  // Defining the eigenvalue problem.

  ARluNonSymStdEig<FLOAT> prob(nev, matrix, sigma, which, ncv,
                               tol, maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigValR, EigValI);

} // real nonsymmetric standard problem, only eigenvalues, shift-and-invert.

template <class FLOAT>
int AREig(float EigValR[], FLOAT EigValI[], int n, int nnz,
          FLOAT A[], int irow[], int pcol[], FLOAT sigma, int nev,
          char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrix(n, nnz, A, irow, pcol);

  // Defining the eigenvalue problem.

  ARluNonSymStdEig<FLOAT> prob(nev, matrix, sigma, which, ncv,
                               tol, maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigValR, EigValI);

} // real nonsymmetric standard problem, only eigenvalues, shift-and-invert.

template <class FLOAT>
int AREig(FLOAT EigValR[], FLOAT EigValI[], FLOAT EigVec[], int n, int nnz,
          FLOAT A[], int irow[], int pcol[], FLOAT sigma, int nev,
          char* which = "LM", int ncv = 0, FLOAT tol = 0.0, int maxit = 0,
          FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrix(n, nnz, A, irow, pcol);

  // Defining the eigenvalue problem.

  ARluNonSymStdEig<FLOAT> prob(nev, matrix, sigma, which, ncv,
                               tol, maxit, resid, AutoShift);

  // Finding eigenvalues and eigenvectors.

  return prob.EigenValVectors(EigVec, EigValR, EigValI);

} // real nonsymmetric standard problem, values and vectors, shift-and-invert.

template <class FLOAT>
int AREig(double EigValR[], FLOAT EigValI[], int n, int nnzA,
          FLOAT A[], int irowA[], int pcolA[], int nnzB,
          FLOAT B[], int irowB[], int pcolB[], int nev,
          char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrixA(n, nnzA, A, irowA, pcolA);
  ARluNonSymMatrix<FLOAT> matrixB(n, nnzB, B, irowB, pcolB);

  // Defining the eigenvalue problem.

  ARluNonSymGenEig<FLOAT> prob(nev, matrixA, matrixB, which,
                               ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigValR, EigValI);

} // real nonsymmetric generalized problem, only eigenvalues, regular mode.

template <class FLOAT>
int AREig(float EigValR[], FLOAT EigValI[], int n, int nnzA,
          FLOAT A[], int irowA[], int pcolA[], int nnzB,
          FLOAT B[], int irowB[], int pcolB[], int nev,
          char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrixA(n, nnzA, A, irowA, pcolA);
  ARluNonSymMatrix<FLOAT> matrixB(n, nnzB, B, irowB, pcolB);

  // Defining the eigenvalue problem.

  ARluNonSymGenEig<FLOAT> prob(nev, matrixA, matrixB, which,
                               ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigValR, EigValI);

} // real nonsymmetric generalized problem, only eigenvalues, regular mode.

template <class FLOAT>
int AREig(FLOAT EigValR[], FLOAT EigValI[], FLOAT EigVec[], int n,
          int nnzA, FLOAT A[], int irowA[], int pcolA[],
          int nnzB, FLOAT B[], int irowB[], int pcolB[],
          int nev, char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrixA(n, nnzA, A, irowA, pcolA);
  ARluNonSymMatrix<FLOAT> matrixB(n, nnzB, B, irowB, pcolB);

  // Defining the eigenvalue problem.

  ARluNonSymGenEig<FLOAT> prob(nev, matrixA, matrixB, which,
                               ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues and eigenvectors.

  return prob.EigenValVectors(EigVec, EigValR, EigValI);

} // real nonsymmetric generalized problem, values and vectors, regular mode.

template <class FLOAT>
int AREig(double EigValR[], FLOAT EigValI[], int n, int nnzA,
          FLOAT A[], int irowA[], int pcolA[], int nnzB,
          FLOAT B[], int irowB[], int pcolB[], FLOAT sigma,
          int nev, char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrixA(n, nnzA, A, irowA, pcolA);
  ARluNonSymMatrix<FLOAT> matrixB(n, nnzB, B, irowB, pcolB);

  // Defining the eigenvalue problem.

  ARluNonSymGenEig<FLOAT> prob(nev, matrixA, matrixB, sigma, which,
                               ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigValR, EigValI);

} // real nonsymmetric generalized problem, only eigenvalues,
  // real shift-and-invert mode.

template <class FLOAT>
int AREig(float EigValR[], FLOAT EigValI[], int n, int nnzA,
          FLOAT A[], int irowA[], int pcolA[], int nnzB,
          FLOAT B[], int irowB[], int pcolB[], FLOAT sigma,
          int nev, char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrixA(n, nnzA, A, irowA, pcolA);
  ARluNonSymMatrix<FLOAT> matrixB(n, nnzB, B, irowB, pcolB);

  // Defining the eigenvalue problem.

  ARluNonSymGenEig<FLOAT> prob(nev, matrixA, matrixB, sigma, which,
                               ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigValR, EigValI);

} // real nonsymmetric generalized problem, only eigenvalues,
  // real shift-and-invert mode.

template <class FLOAT>
int AREig(FLOAT EigValR[], FLOAT EigValI[], FLOAT EigVec[], int n,
          int nnzA, FLOAT A[], int irowA[], int pcolA[], int nnzB,
          FLOAT B[], int irowB[], int pcolB[], FLOAT sigma, int nev,
          char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrixA(n, nnzA, A, irowA, pcolA);
  ARluNonSymMatrix<FLOAT> matrixB(n, nnzB, B, irowB, pcolB);

  // Defining the eigenvalue problem.

  ARluNonSymGenEig<FLOAT> prob(nev, matrixA, matrixB, sigma, which,
                               ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues and eigenvectors.

  return prob.EigenValVectors(EigVec, EigValR, EigValI);

} // real nonsymmetric generalized problem, values and vectors,
  // real shift-and-invert mode.

template <class FLOAT>
int AREig(FLOAT EigValR[], FLOAT EigValI[], int n, int nnzA, FLOAT A[],
          int irowA[], int pcolA[], int nnzB, FLOAT B[], int irowB[],
          int pcolB[], char part, FLOAT sigmaR, FLOAT sigmaI,
          int nev, char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrixA(n, nnzA, A, irowA, pcolA);
  ARluNonSymMatrix<FLOAT> matrixB(n, nnzB, B, irowB, pcolB);

  // Defining the eigenvalue problem.

  ARluNonSymGenEig<FLOAT> prob(nev, matrixA, matrixB, part,
                               sigmaR, sigmaI, which, ncv,
                               tol, maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigValR, EigValI);

} // real nonsymmetric generalized problem, only eigenvalues,
  // complex shift-and-invert mode.

template <class FLOAT>
int AREig(FLOAT EigValR[], FLOAT EigValI[], FLOAT EigVec[], int n, int nnzA,
          FLOAT A[], int irowA[], int pcolA[], int nnzB, FLOAT B[],
          int irowB[], int pcolB[], char part, FLOAT sigmaR, FLOAT sigmaI,
          int nev, char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluNonSymMatrix<FLOAT> matrixA(n, nnzA, A, irowA, pcolA);
  ARluNonSymMatrix<FLOAT> matrixB(n, nnzB, B, irowB, pcolB);

  // Defining the eigenvalue problem.

  ARluNonSymGenEig<FLOAT> prob(nev, matrixA, matrixB, part,
                               sigmaR, sigmaI, which, ncv,
                               tol, maxit, resid, AutoShift);

  // Finding eigenvalues and eigenvectors.

  return prob.EigenValVectors(EigVec, EigValR, EigValI);

} // real nonsymmetric generalized problem, values and vectors,
  // complex shift-and-invert mode.

template <class FLOAT>
int AREig(FLOAT EigVal[], int n, int nnz, FLOAT A[], int irow[],
          int pcol[], char uplo, int nev, char* which = "LM",
          int ncv = 0, FLOAT tol = 0.0, int maxit = 0,
          FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluSymMatrix<FLOAT> matrix(n, nnz, A, irow, pcol, uplo);

  // Defining the eigenvalue problem.

  ARluSymStdEig<FLOAT> prob(nev, matrix, which, ncv, tol,
                            maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigVal);

} // real symmetric standard problem, only eigenvalues, regular mode.

template <class FLOAT>
int AREig(FLOAT EigVal[], FLOAT EigVec[], int n, int nnz, FLOAT A[],
          int irow[], int pcol[], char uplo, int nev,
          char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluSymMatrix<FLOAT> matrix(n, nnz, A, irow, pcol, uplo);

  // Defining the eigenvalue problem.

  ARluSymStdEig<FLOAT> prob(nev, matrix, which, ncv, tol,
                            maxit, resid, AutoShift);

  // Finding eigenvalues and eigenvectors.

  return prob.EigenValVectors(EigVec, EigVal);

} // real symmetric standard problem, values and vectors, regular mode.

template <class FLOAT>
int AREig(FLOAT EigVal[], int n, int nnz, FLOAT A[], int irow[],
          int pcol[], char uplo, FLOAT sigma, int nev,
          char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluSymMatrix<FLOAT> matrix(n, nnz, A, irow, pcol, uplo);

  // Defining the eigenvalue problem.

  ARluSymStdEig<FLOAT> prob(nev, matrix, sigma, which, ncv,
                            tol, maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigVal);

} // real symmetric standard problem, only eigenvalues, shift-and-invert.

template <class FLOAT>
int AREig(FLOAT EigVal[], FLOAT EigVec[], int n, int nnz, FLOAT A[],
          int irow[], int pcol[], char uplo, FLOAT sigma,
          int nev, char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating a matrix in ARPACK++ format.

  ARluSymMatrix<FLOAT> matrix(n, nnz, A, irow, pcol, uplo);

  // Defining the eigenvalue problem.

  ARluSymStdEig<FLOAT> prob(nev, matrix, sigma, which, ncv,
                            tol, maxit, resid, AutoShift);

  // Finding eigenvalues and eigenvectors.

  return prob.EigenValVectors(EigVec, EigVal);

} // real symmetric standard problem, values and vectors, shift-and-invert.

template <class FLOAT>
int AREig(FLOAT EigVal[], int n, int nnzA, FLOAT A[], int irowA[],
          int pcolA[], int nnzB, FLOAT B[], int irowB[], int pcolB[],
          char uplo, int nev, char* which = "LM", int ncv = 0,
          FLOAT tol = 0.0, int maxit = 0, FLOAT* resid = 0,
          bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluSymMatrix<FLOAT> matrixA(n, nnzA, A, irowA, pcolA, uplo);
  ARluSymMatrix<FLOAT> matrixB(n, nnzB, B, irowB, pcolB, uplo);

  // Defining the eigenvalue problem.

  ARluSymGenEig<FLOAT> prob(nev, matrixA, matrixB, which,
                            ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigVal);

} // real symmetric generalized problem, only eigenvalues, regular mode.

template <class FLOAT>
int AREig(FLOAT EigVal[], FLOAT EigVec[], int n, int nnzA, FLOAT A[],
          int irowA[], int pcolA[], int nnzB, FLOAT B[], int irowB[],
          int pcolB[], char uplo, int nev, char* which = "LM",
          int ncv = 0, FLOAT tol = 0.0, int maxit = 0,
          FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluSymMatrix<FLOAT> matrixA(n, nnzA, A, irowA, pcolA, uplo);
  ARluSymMatrix<FLOAT> matrixB(n, nnzB, B, irowB, pcolB, uplo);

  // Defining the eigenvalue problem.

  ARluSymGenEig<FLOAT> prob(nev, matrixA, matrixB, which,
                            ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues and eigenvectors.

  return prob.EigenValVectors(EigVec, EigVal);

} // real symmetric generalized problem, values and vectors, regular mode.

template <class FLOAT>
int AREig(FLOAT EigVal[], int n, int nnzA, FLOAT A[], int irowA[],
          int pcolA[], int nnzB, FLOAT B[], int irowB[], int pcolB[],
          char uplo, char InvertMode, FLOAT sigma, int nev,
          char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluSymMatrix<FLOAT> matrixA(n, nnzA, A, irowA, pcolA, uplo);
  ARluSymMatrix<FLOAT> matrixB(n, nnzB, B, irowB, pcolB, uplo);

  // Defining the eigenvalue problem.

  ARluSymGenEig<FLOAT> prob(InvertMode, nev, matrixA, matrixB, sigma,
                            which, ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues.

  return prob.Eigenvalues(EigVal);

} // real symmetric generalized problem, only eigenvalues,
  // shift-and-invert, buckling and Cayley modes.

template <class FLOAT>
int AREig(FLOAT EigVal[], FLOAT EigVec[], int n, int nnzA, FLOAT A[],
          int irowA[], int pcolA[], int nnzB, FLOAT B[], int irowB[],
          int pcolB[], char uplo, char InvertMode, FLOAT sigma,
          int nev, char* which = "LM", int ncv = 0, FLOAT tol = 0.0,
          int maxit = 0, FLOAT* resid = 0, bool AutoShift = true)
{

  // Creating two matrices in ARPACK++ format.

  ARluSymMatrix<FLOAT> matrixA(n, nnzA, A, irowA, pcolA, uplo);
  ARluSymMatrix<FLOAT> matrixB(n, nnzB, B, irowB, pcolB, uplo);

  // Defining the eigenvalue problem.

  ARluSymGenEig<FLOAT> prob(InvertMode, nev, matrixA, matrixB, sigma,
                            which, ncv, tol, maxit, resid, AutoShift);

  // Finding eigenvalues and eigenvectors.

  return prob.EigenValVectors(EigVec, EigVal);

} // real symmetric generalized problem, values and vectors,
  // shift-and-invert, buckling and Cayley modes.

template<class FLOAT, class INT>
void SymmetricMatrixA(INT nx, INT& n, INT& nnz, FLOAT* &A,
                      INT* &irow, INT* &pcol, char uplo = 'L')

{

  // Defining internal variables.

  INT    i, j;
  FLOAT  h2, df, dd;

  // Defining constants.

  h2  = 1.0/(FLOAT(nx+1)*FLOAT(nx+1));
  dd  = 4.0/h2;
  df  = -1.0/h2;

  // Defining the number of columns and nonzero elements of matrix.

  n   = nx*nx;
  nnz = 3*n-2*nx;

  // Creating output vectors.

  A    = new FLOAT[nnz];
  irow = new INT[nnz];
  pcol = new INT[n+1];

  // Defining  matrix A.

  pcol[0] = 0;
  i       = 0;

  if (uplo == 'U') {

    for (j = 0; j < n; j++) {
      if (j >= nx) {
        A[i] = df;   irow[i++] = j-nx;
      }
      if ((j%nx) != 0) {
        A[i] = df;   irow[i++] = j-1;
      }
      A[i] = dd;     irow[i++] = j;
      pcol[j+1] = i;
    }

  }
  else {

    for (j = 0; j < n; j++) {
      A[i] = dd;     irow[i++] = j;
      if (((j+1)%nx) != 0) {
        A[i] = df;   irow[i++] = j+1;
      }
      if (j < n-nx) {
        A[i] = df;   irow[i++] = j+nx;
      }
      pcol[j+1] = i;
    }

  }

} // SymmetricMatrixA.

template<class FLOAT, class INT>
void Solution(INT nconv, INT n, INT nnz, FLOAT A[], INT irow[], INT pcol[],
              char uplo, FLOAT EigVal[], FLOAT* EigVec = 0)
/*
  Prints eigenvalues and eigenvectors of symmetric eigen-problems
  on standard "cout" stream.
*/

{

  INT                  i;
  FLOAT*               Ax;
  FLOAT*               ResNorm;
  ARluSymMatrix<FLOAT> matrix(n, nnz, A, irow, pcol, uplo);

  cout << endl << endl << "Testing ARPACK++ function AREig" << endl;
  cout << "Real symmetric eigenvalue problem: A*x - lambda*x \n \n";

  cout << "Dimension of the system            : " << n     << endl;
  cout << "Number of 'converged' eigenvalues  : " << nconv << endl << endl;

  // Printing eigenvalues.

  cout << "Eigenvalues:" << endl;

  for (i=0; i<nconv; i++) {
    cout << "  lambda[" << (i+1) << "]: " << EigVal[i] << endl;
  }
  cout << endl;

  // Printing eigenvectors.

  if (EigVec != 0) {

    // Finding the residual norm || A*x - lambda*x ||
    // for the nconv accurately computed eigenvectors.

    Ax      = new FLOAT[n];
    ResNorm = new FLOAT[nconv+1];

    for (i=0; i<nconv; i++) {
      matrix.MultMv(&EigVec[i*n], Ax);
      axpy(n, -EigVal[i], &EigVec[i*n], 1, Ax, 1);
      ResNorm[i] = nrm2(n, Ax, 1)/fabs(EigVal[i]);
    }

    for (i=0; i<nconv; i++) {
      cout << "||A*x(" << (i+1) << ") - lambda(" << (i+1);
      cout << ")*x(" << (i+1) << ")||: " << ResNorm[i] << endl;
    }
    cout << endl;

    delete[] Ax;
    delete[] ResNorm;

  }

} // Solution.

template<class FLOAT, class INT>
void Solution(INT nconv, INT n, INT nnzA, FLOAT A[], INT irowA[],
              INT pcolA[], INT nnzB, FLOAT B[], INT irowB[], INT pcolB[],
              char uplo, FLOAT EigVal[], FLOAT* EigVec = 0)
/*
  Prints eigenvalues and eigenvectors of symmetric generalized
  eigen-problem on standard "cout" stream.
*/

{

  INT                  i;
  FLOAT                *Ax, *Bx;
  FLOAT                *ResNorm;
  ARluSymMatrix<FLOAT> matrixA(n, nnzA, A, irowA, pcolA, uplo);
  ARluSymMatrix<FLOAT> matrixB(n, nnzB, B, irowB, pcolB, uplo);

  cout << endl << endl << "Testing ARPACK++ function AREig" << endl;
  cout << "Real symmetric generalized eigenvalue problem: A*x - lambda*B*x";
  cout << endl << endl;

  cout << "Dimension of the system            : " << n     << endl;
  cout << "Number of 'converged' eigenvalues  : " << nconv << endl << endl;

  // Printing eigenvalues.

  cout << "Eigenvalues:" << endl;

  for (i=0; i<nconv; i++) {
    cout << "  lambda[" << (i+1) << "]: " << EigVal[i] << endl;
  }
  cout << endl;

  // Printing eigenvectors.

  if (EigVec != 0) {

    // Printing the residual norm || A*x - lambda*B*x ||
    // for the nconv accurately computed eigenvectors.

    Ax      = new FLOAT[n];
    Bx      = new FLOAT[n];
    ResNorm = new FLOAT[nconv+1];

    for (i=0; i<nconv; i++) {
      matrixA.MultMv(&EigVec[i*n], Ax);
      matrixB.MultMv(&EigVec[i*n], Bx);
      axpy(n, -EigVal[i], Bx, 1, Ax, 1);
      ResNorm[i] = nrm2(n, Ax, 1)/fabs(EigVal[i]);
    }

    for (i=0; i<nconv; i++) {
      cout << "||A*x(" << i << ") - lambda(" << i;
      cout << ")*B*x(" << i << ")||: " << ResNorm[i] << endl;
    }
    cout << endl;

    delete[] Ax;
    delete[] Bx;
    delete[] ResNorm;

  }

} // Solution.

void
check_eigen_solve()
{
	cout << "******* check_eigen_solve()" << endl;

	int     nx;
	int     n;           // Dimension of the problem.
	int     nconv;       // Number of "converged" eigenvalues.
	int     nnz;         // Number of nonzero elements in A.
	int*    irow;        // pointer to an array that stores the row
	// indices of the nonzeros in A.
	int*    pcol;        // pointer to an array of pointers to the
	// beginning of each column of A in vector A.
	double* A;           // pointer to an array that stores the
	// nonzero elements of A.
	double EigVal[101];  // Eigenvalues.
	double EigVec[1001]; // Eigenvectors stored sequentially.
	char    uplo;        // Variable that indicates whether the upper
	// (uplo='U') ot the lower (uplo='L') part of
	// A will be stored in A, irow and pcol.

	// Creating a 100x100 matrix.

	nx = 10;
	uplo = 'U';
	SymmetricMatrixA(nx, n, nnz, A, irow, pcol, uplo);

	// Finding the four eigenvalues with smallest magnitude and
	// the related eigenvectors.

	nconv = AREig(EigVal, EigVec, n, nnz, A, irow, pcol, uplo, 4, "SM");

	// Printing solution.

	Solution(nconv, n, nnz, A, irow, pcol, uplo, EigVal, EigVec);
}

void
register_eigen_solve_tests( boost::unit_test::test_suite * test )
{
	test->add( BOOST_TEST_CASE( &check_eigen_solve ), 0);
}


#pragma once

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

int calculateInt()
{
    int number = 22;
    for (int i=1; i<20; ++i)
    {
        number = i * number / 4;
    }
    return number;
}

Eigen::Vector3f calculateVec()
{
    auto x = Eigen::Vector3f{1, 0, 0};
    auto y = Eigen::Vector3f{0, 1, 0};
    return x.cross(y);
}


#include <iostream>
#include <chrono>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/blas/blas.h>
#include <algorithm>

  int cc_find_new_j(double val [],
          int vct,
          double as [],
          int nas,
          double tol,
          double res []);

  void cc_solve_stomp(
          double * a,
          double * y,
          int * asize,
          int * ressize,
	  double OptTol,
	  int maxIters,
          int *niter,
          int *naSet,
          double *asol,
	  double *activeSet,
	  double *aresult);

  double cmeana_ms(double * vals, int size,double * res);


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_GENERIC_POW2_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_GENERIC_POW2_HPP_INCLUDED

#include <nt2/exponential/functions/pow2.hpp>
#include <boost/assert.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/functions/simd/fast_ldexp.hpp>
#include <nt2/include/functions/simd/toint.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/operator/functions/details/assert_utils.hpp>
#include <nt2/include/functions/simd/is_finite.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( pow2_, tag::cpu_
                            , (A0)(A1)
                            , (generic_< floating_<A0> >)
                              (generic_< integer_<A1> >)
                            )
  {
    typedef A0 result_type;

    NT2_FUNCTOR_CALL(2)
    {
      return nt2::fast_ldexp(a0, a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( pow2_, tag::cpu_
                            , (A0)
                            , (generic_< floating_<A0> >)
                              (generic_< floating_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      #ifndef BOOST_SIMD_NO_INVALIDS
      BOOST_ASSERT_MSG(boost::simd::assert_all(is_finite(a1)),
                       "pow2 is not defined for an invalid second parameter");
      #endif
      return nt2::fast_ldexp(a0, nt2::toint(a1));
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( pow2_, tag::cpu_
                            , (A0)(A1)
                            , (generic_< integer_<A0> >)
                              (generic_< integer_<A1> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      return nt2::fast_ldexp(a0, a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( pow2_, tag::cpu_
                            , (A0)
                            , (generic_< integer_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL_REPEAT(1)
    {
      return nt2::fast_ldexp(One<A0>(), a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( pow2_, tag::cpu_
                            , (A0)
                            , (generic_< floating_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL_REPEAT(1)
    {
      #ifndef BOOST_SIMD_NO_INVALIDS
      BOOST_ASSERT_MSG(boost::simd::assert_all(is_finite(a0)),
                       "pow2 with one parameter is not defined for an invalid entry");
      #endif
      return nt2::fast_ldexp(One<A0>(), toint(a0));
    }
  };
} }

#endif


/*
 * Copyright 2020 Adobe. All rights reserved.
 * This file is licensed to you 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 REPRESENTATIONS
 * OF ANY KIND, either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
 */
#include <cmath>
#include <iostream>

#include <lagrange/testing/common.h>

#include <Eigen/Geometry>

#include <lagrange/common.h>
#include <lagrange/compute_mesh_covariance.h>
#include <lagrange/create_mesh.h>
#include <lagrange/utils/safe_cast.h>

TEST_CASE("ComputeMeshCovariance", "[mesh][covariance]")
{
    using namespace lagrange;

    Vertices3D ref_vertices(6 + 3, 3);
    Triangles facets(4 + 1, 3);

    const double a = 0.5;
    const double b = 2.0;
    const double large_number = 1000;

    ref_vertices.row(0) << -a / 2., -b / 2., 0;
    ref_vertices.row(1) << +a / 2., -b / 2., 0;
    ref_vertices.row(2) << +a / 2., 0, 0;
    ref_vertices.row(3) << +a / 2., b / 4., 0;
    ref_vertices.row(4) << +a / 2., b / 2., 0;
    ref_vertices.row(5) << -a / 2., b / 2., 0;
    // Don't include in computation
    ref_vertices.row(6) << large_number, large_number, large_number;
    ref_vertices.row(7) << -large_number, -large_number, -large_number;
    ref_vertices.row(8) << 2 * large_number, 2 * large_number, 2 * large_number;

    facets.row(0) << 0, 1, 2;
    // Don't include in computation
    facets.row(1) << 6, 7, 8;
    //
    facets.row(2) << 0, 2, 3;
    facets.row(3) << 0, 3, 4;
    facets.row(4) << 0, 4, 5;

    // Reference values without transformations
    const double ref_area = a * b;
    Eigen::Vector3d ref_center(0, 0, 0);
    Eigen::Matrix3d ref_covariance = Eigen::Matrix3d::Zero();
    ref_covariance(0, 0) = b * a * a * a / 12.;
    ref_covariance(1, 1) = a * b * b * b / 12.;

    // Translate
    // Eigen::Vector3d tr(0,0,0);
    Eigen::Vector3d tr(-1, 3, 4);

    // Rotate
    // Eigen::Matrix3d rot = Eigen::Matrix3d::Identity();
    Eigen::Matrix3d rot =
        Eigen::AngleAxisd(1.2365, Eigen::Vector3d(-1, 2, 5.1).normalized()).toRotationMatrix();
    Eigen::Matrix3d rot_covariance = rot * ref_covariance * rot.transpose();
    Eigen::Matrix3d rot_covariance_tr = rot_covariance + ref_area * tr * tr.transpose();

    // Create the vertices
    Vertices3D vertices = (ref_vertices * rot.transpose()).rowwise() + tr.transpose();

    auto mesh_unique = lagrange::create_mesh(vertices, facets);
    auto out_covariance_at_zero =
        compute_mesh_covariance(*mesh_unique, Eigen::RowVector3d::Zero(), {0, 2, 3, 4});
    auto out_covariance_at_centroid =
        compute_mesh_covariance(*mesh_unique, tr.transpose(), {0, 2, 3, 4});

    CHECK((out_covariance_at_zero - rot_covariance_tr).norm() == Approx(0.).margin(1e-10));
    CHECK((out_covariance_at_centroid - rot_covariance).norm() == Approx(0.).margin(1e-10));

    // std::cout << out_covariance_at_zero.covariance << std::endl;
    // std::cout << rot_covariance_tr << std::endl;

} // end of TEST


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto is_permutation_of = curry<2>([](auto xs, auto perm) {
        return elem(permutations(xs), perm);
    });

    BOOST_HANA_CONSTEXPR_ASSERT(
        all(
            list(
                list('1', 2, 3.0),
                list('1', 3.0, 2),
                list(2, '1', 3.0),
                list(2, 3.0, '1'),
                list(3.0, '1', 2),
                list(3.0, 2, '1')
            ),
            is_permutation_of(list('1', 2, 3.0))
        )
    );
    //! [main]
}


#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <unsupported/Eigen/FFT>
#include <gravitysolvers.hpp>

// ************************************************************************* //
// ******************************** DATA IO ******************************** //
// ************************************************************************* //

Gravitysolver::DataIO::DataIO()
{
  epsilon = 0.0;
}

bool Gravitysolver::DataIO::readDataOld(const std::string &filename)
{
  std::ifstream infile(filename);
  int numParticles, numGasParticles, numStarParticles;

  if(infile.is_open()) {
    infile >> numParticles >> numGasParticles >> numStarParticles;

    particles = Eigen::MatrixXf::Zero(MATRIX_DATA_ROWS, numParticles);

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(0, i); // m
    }

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(1, i); // x
    }

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(2, i); // y
    }

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(3, i); // z
    }

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(4, i); // vx
    }

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(5, i); // vy
    }

    for(int i = 0; i < numParticles; i++) {
      infile >> particles(6, i); // vz
    }

    return true;
  }

  std::cout << "Gravitysolver::DataIO::readDataOld(\"" << filename << "\") " << "could not read file" << std::endl;
  return false;
}

bool Gravitysolver::DataIO::readData(const std::string &filename)
{
  std::ifstream infile(filename);
  int N;

  if(infile.is_open()) {
    infile >> N >> epsilon;
    particles = Eigen::MatrixXf::Zero(MATRIX_DATA_ROWS, N);

    float m, x, y, z, vx, vy, vz, fx, fy, fz;

    for(int j = 0; j < N; j++) {
      infile >> m >> x >> y >> z >> vx >> vy >> vz >> fx >> fy >> fz;
      particles(0, j) = m;
      particles(1, j) = x;
      particles(2, j) = y;
      particles(3, j) = z;
      particles(4, j) = vx;
      particles(5, j) = vy;
      particles(6, j) = vz;
      particles(7, j) = fx;
      particles(8, j) = fy;
      particles(9, j) = fz;
    }

    return true;
  }

  std::cout << "Gravitysolver::DataIO::readData(\"" << filename << "\") " << "could not read file" << std::endl;
  return false;
}

bool Gravitysolver::DataIO::writeData(const std::string &filename)
{
  std::ofstream outfile(filename);
  int N = particles.cols();

  if(outfile.is_open()) {
    outfile << N << " " << epsilon << "\n";

    for(int j = 0; j < N; j++) {
      outfile << particles(0, j) << "\n"; // m
      outfile << particles(1, j) << "\n"; // x
      outfile << particles(2, j) << "\n"; // y
      outfile << particles(3, j) << "\n"; // z
      outfile << particles(4, j) << "\n"; // vx
      outfile << particles(5, j) << "\n"; // vy
      outfile << particles(6, j) << "\n"; // vz
      outfile << particles(7, j) << "\n"; // fx
      outfile << particles(8, j) << "\n"; // fy
      outfile << particles(9, j) << "\n"; // fz
    }

    return true;
  }

  std::cout << "Gravitysolver::DataIO::writeData(\"" << filename << "\") " << "could not write file" << std::endl;
  return false;
}

// ************************************************************************* //
// ***************************** DIRECT SOLVER ***************************** //
// ************************************************************************* //

Gravitysolver::Direct::Direct()
{

}

float Gravitysolver::Direct::softening()
{
  return epsilon;
}

void Gravitysolver::Direct::setSoftening(float eps)
{
  epsilon = eps;
}

void Gravitysolver::Direct::solve()
{
  for(int i = 0; i < particles.cols(); i++) {
    particles(7, i) = .0;
    particles(8, i) = .0;
    particles(9, i) = .0;
  }

  float mi, mj, dx, dy, dz, sqrtInvDist, sqrtInvDist3, fx, fy, fz;

  for(int i = 0; i < particles.cols(); i++) {
    for(int j = i + 1; j < particles.cols(); j++) {
      mi = particles(0, i);
      mj = particles(0, j);
      dx = particles(1, i) - particles(1, j);
      dy = particles(2, i) - particles(2, j);
      dz = particles(3, i) - particles(3, j);

      sqrtInvDist = 1.0 / std::sqrt(dx * dx + dy * dy + dz * dz + epsilon * epsilon);
      sqrtInvDist3 = sqrtInvDist * sqrtInvDist * sqrtInvDist;

      // Assumption: G = 1
      fx = -mi * mj * dx * sqrtInvDist3;
      fy = -mi * mj * dy * sqrtInvDist3;
      fz = -mi * mj * dz * sqrtInvDist3;

      particles(7, i) += fx;
      particles(8, i) += fy;
      particles(9, i) += fz;
      particles(7, j) -= fx;
      particles(8, j) -= fy;
      particles(9, j) -= fz;
    }
  }
}

const MatrixData &Gravitysolver::Direct::data()
{
  return particles;
}

// ************************************************************************* //
// ******************************* PM SOLVER ******************************* //
// ************************************************************************* //

Gravitysolver::PM::PM(int numGridCells)
{
  // The Tensor in Eigen experiences a strange case of std::bad_alloc()
  // if it has more than 776 = 2*388 complex valued elements per dimension
  if(numGridCells > 0 && numGridCells <= 388) {
    Ng = numGridCells;
  }
  else {
    std::cout << "Gravitysolver::PM::PM(" << numGridCells << ") numGridCells must be > 0" << std::endl;
    Ng = 1;
  }

  h = .0;
}

/**
* Returns indices for a mesh cell for the galaxy given world coordinates.
* World coordinates have the center of the galaxy at (0, 0, 0).
* The mapping to grid coordinates is an affine mapping where the galaxy center
* (0, 0, 0) is in the center of the mesh as well.
*/
Vector3i Gravitysolver::PM::worldToGrid(float x, float y, float z)
{
  Vector3i gridCoor;

  gridCoor(0) = std::floor((x + (worldLen / 2)) / worldLen * Ng);
  gridCoor(1) = std::floor((y + (worldLen / 2)) / worldLen * Ng);
  gridCoor(2) = std::floor((z + (worldLen / 2)) / worldLen * Ng);

  return gridCoor;
}

/**
* Returns position at the center of a mesh cell in world coordinates given cell indices
* The position is mapped in a fashin such that (0, 0, 0) is the center of the galaxy
* in world coordinates.
*/
Vector3f Gravitysolver::PM::gridToWorld(int i, int j, int k)
{
  Vector3f worldCoor;

  worldCoor(0) = (i * worldLen / Ng) - (worldLen / 2) + (h / 2);
  worldCoor(1) = (j * worldLen / Ng) - (worldLen / 2) + (h / 2);
  worldCoor(2) = (k * worldLen / Ng) - (worldLen / 2) + (h / 2);

  return worldCoor;
}

void Gravitysolver::PM::fft3d(FieldTensorCF &t)
{
  const int x = t.dimension(0);
  const int y = t.dimension(1);
  const int z = t.dimension(2);

  Eigen::FFT<float> fft;

  for(int k = 0; k < z; k++) { // for each 2d sheet make a 2d fft
    for(int j = 0; j < y; j++) { // fft in x-dir
      VectorXcf tv(x);
      for(int i = 0; i < x; i++)
          tv(i) = t(i, j, k);

      VectorXcf fv = fft.fwd(tv);
      for(int i = 0; i < x; i++)
          t(i, j, k) = fv(i);
    }

    for(int i = 0; i < x; i++) { // fft in y-dir
      VectorXcf tv(y);
      for(int j = 0; j < y; j++)
          tv(j) = t(i, j, k);

      VectorXcf fv = fft.fwd(tv);
      for(int j = 0; j < y; j++)
          t(i, j, k) = fv(j);
    }
  }

  for(int i = 0; i < x; i++) { // and for each of the x*y spikes pointing upwards in z-dir do a 1D fft
    for(int j = 0; j < y; j++) {

      VectorXcf tv(z);
      for(int k = 0; k < z; k++)
        tv(k) = t(i, j, k);

      VectorXcf fv = fft.fwd(tv);
      for(int k = 0; k < z; k++)
        t(i, j, k) = fv(k);
    }
  }
}

void Gravitysolver::PM::ifft3d(FieldTensorCF &t)
{
  const int x = t.dimension(0);
  const int y = t.dimension(1);
  const int z = t.dimension(2);

  const float invXYZ = 1.0 / (x*y*z);

  for(int i = 0; i < x; i++) {
    for(int j = 0; j < y; j++) {
      for(int k = 0; k < z; k++) {
        t(i,j,k) = std::conj(t(i,j,k));
      }
    }
  }

  fft3d(t);

  for(int i = 0; i < x; i++) {
    for(int j = 0; j < y; j++) {
      for(int k = 0; k < z; k++) {
        t(i,j,k) = std::conj(t(i,j,k)) * invXYZ;
      }
    }
  }
}

void Gravitysolver::PM::conv3d(FieldTensorCF &out, FieldTensorCF &in, FieldTensorCF &kernel)
{
  int x = in.dimension(0);
  int y = in.dimension(1);
  int z = in.dimension(2);

  fft3d(in);
  fft3d(kernel);

  for(int i = 0; i < x; i++) {
    for(int j = 0; j < y; j++) {
      for(int k = 0; k < z; k++) {
        out(i,j,k) = in(i,j,k) * kernel(i,j,k);
      }
    }
  }

  ifft3d(out);
}

void Gravitysolver::PM::solve()
{
  const int N = particles.cols();
  Vector3f maxPos = particles.block(1, 0, 3, N).rowwise().maxCoeff();
  Vector3f minPosAbs = particles.block(1, 0, 3, N).rowwise().minCoeff().cwiseAbs();

  worldLen = std::max(maxPos.maxCoeff(), minPosAbs.maxCoeff()) * 2;

  // adds one layer of cells in each dimension so particles at the edge will
  // contribute with their entire mass to the density field
  worldLen += (worldLen / Ng);
  h = worldLen / Ng;

  std::cout << "PM solver global parameters" << std::endl;
  std::cout << "---------------------------" << std::endl;
  std::cout << "worldLen:   " << worldLen << std::endl;
  std::cout << "Ng:         " << Ng << std::endl;
  std::cout << "h:          " << h << std::endl;
  std::cout << "---------------------------" << std::endl;

  density.resize(2*Ng, 2*Ng, 2*Ng);
  greenFunction.resize(2*Ng, 2*Ng, 2*Ng);
  potential.resize(2*Ng, 2*Ng, 2*Ng);

  ax.resize(Ng, Ng, Ng);
  ay.resize(Ng, Ng, Ng);
  az.resize(Ng, Ng, Ng);

  // density field construction
  for(int col = 0; col < particles.cols(); col++) {
    float m = particles(0, col);
    float px = particles(1, col);
    float py = particles(2, col);
    float pz = particles(3, col);

    // (NGP)
    Vector3i ti = worldToGrid(px, py, pz);
    density(ti(0), ti(1), ti(2)) += (m / (h*h*h));
  }

  // green's function construction
  for(int i = 0; i < Ng; i++) {
    for(int j = 0; j < Ng; j++) {
      for(int k = 0; k < Ng; k++) {
        Vector3f worldPos = gridToWorld(i, j, k);

        // Assumption: G = 1
        float res = -1.0 / worldPos.norm();

        // make the function symmetric accross all axes
        greenFunction(i,               j,            k) = res;
        greenFunction(i,               j,     2*Ng-k-1) = res;
        greenFunction(i,        2*Ng-j-1,            k) = res;
        greenFunction(i,        2*Ng-j-1,     2*Ng-k-1) = res;
        greenFunction(2*Ng-i-1,        j,            k) = res;
        greenFunction(2*Ng-i-1,        j,     2*Ng-k-1) = res;
        greenFunction(2*Ng-i-1, 2*Ng-j-1,            k) = res;
        greenFunction(2*Ng-i-1, 2*Ng-j-1,     2*Ng-k-1) = res;
      }
    }
  }

  // solve the poisson equation to get the potential
  conv3d(potential, density, greenFunction);

  // calculate acceleration field from potential
  for(int i = 1; i < Ng - 1; i++) {
    for(int j = 1; j < Ng - 1; j++) {
      for(int k = 1; k < Ng - 1; k++) {
        ax(i, j, k) = -(potential(i+1,j,k).real() - potential(i-1,j,k).real()) / (2*h);
        ay(i, j, k) = -(potential(i,j+1,k).real() - potential(i,j-1,k).real()) / (2*h);
        az(i, j, k) = -(potential(i,j,k+1).real() - potential(i,j,k-1).real()) / (2*h);
      }
    }
  }

  // interpolate acceleration field back from mesh to particles
  for(int col = 0; col < particles.cols(); col++) {
    float m = particles(0, col);
    float px = particles(1, col);
    float py = particles(2, col);
    float pz = particles(3, col);

    // (NGP)
    Vector3i ti = worldToGrid(px, py, pz);

    // F = m*a
    particles(7, col) = m * ax(ti(0), ti(1), ti(2));
    particles(8, col) = m * ay(ti(0), ti(1), ti(2));
    particles(9, col) = m * az(ti(0), ti(1), ti(2));
  }
}

const MatrixData &Gravitysolver::PM::data()
{
  return particles;
}


#pragma once
#include <memory>
#include <Eigen/Dense>
#include "Bound3Intersect.hpp"
#include "../Ray.hpp"

class Bound3
{
public:
    Bound3(Eigen::Vector3f min, Eigen::Vector3f max) : _min(min), _max(max){};

    Bound3Intersect intersect_ray(const Ray &ray) const
    {
        const auto &origin = ray.get_origin();
        const auto &dir = ray.get_direction();

        auto tIn = (_min - origin).cwiseQuotient(dir).minCoeff();
        auto tOut = (_max - origin).cwiseQuotient(dir).maxCoeff();

        if (tIn < std::numeric_limits<float>::epsilon())
            return Bound3Intersect(0.0f, tOut);

        if (tOut - tIn > -std::numeric_limits<float>::epsilon())
            return Bound3Intersect(tIn, tOut);

        return Bound3Intersect();
    };

    static std::unique_ptr<Bound3> union_bound3(const std::unique_ptr<Bound3> &box1, const std::unique_ptr<Bound3> &box2)
    {
        auto min = box1->_min.cwiseMin(box2->_min);
        auto max = box1->_max.cwiseMax(box2->_max);
        return std::make_unique<Bound3>(min, max);
    };

    const Eigen::Vector3f &min() { return _min; };

    const Eigen::Vector3f &max() { return _max; };

private:
    Eigen::Vector3f _min;

    Eigen::Vector3f _max;
};


/*
 * SWE_Plane_Normal_Modes.hpp
 *
 *  Created on: 17 Nov 2019
 *      Author: Pedro Peixoto <pedrosp@ime.usp.br>
 *
 *      based on previous implementation by Martin Schreiber in swe_plane.cpp
 *
 */

#ifndef SRC_PROGRAMS_SWE_PLANE_NORMAL_MODES_HPP_
#define SRC_PROGRAMS_SWE_PLANE_NORMAL_MODES_HPP_

#include <rexi/EXPFunctions.hpp>
#include <sweet/plane/PlaneData_Physical.hpp>
#include <sweet/plane/PlaneData_Spectral.hpp>
#include <sweet/plane/PlaneData_SpectralComplex.hpp>
#include <sweet/SimulationVariables.hpp>
#include <sweet/plane/PlaneOperators.hpp>
#include <functional>
#if SWEET_EIGEN
#include <Eigen/Eigenvalues>
#endif
/**
 * SWE Plane normal mode
 */
class SWE_Plane_Normal_Modes
{
public:

	template <typename TCallbackClass>
	static
	void normal_mode_analysis(
			PlaneData_Spectral &io_prog_h_pert, // h: surface height (perturbation)
			PlaneData_Spectral &io_prog_u, // u: velocity in x-direction
			PlaneData_Spectral &io_prog_v, // v: velocity in y-direction
			int number_of_prognostic_variables,
			SimulationVariables &i_simVars, // Simulation variables
			TCallbackClass *i_class,
			void(TCallbackClass::* const i_run_timestep_method)(void)
	)
	{

		const PlaneDataConfig *planeDataConfig = io_prog_h_pert.planeDataConfig;

		// dummy time step to get time step size
		if (i_simVars.timecontrol.current_timestep_size <= 0)
			SWEETError("Normal mode analysis requires setting fixed time step size");

		/*
		 *
		 * Mode-wise normal mode analysis
		 *
		 *
		 */

		if (i_simVars.misc.normal_mode_analysis_generation == 4)
		{
#if SWEET_EIGEN
#if SWEET_USE_PLANE_SPECTRAL_DEALIASING
			SWEETError("SWE_Plane_Normal_Modes: This test was build for linear or linearized models, so please compile without dealising --plane-spectral-dealiasing=disable.");
#endif

			/*
			 * Setup all output files
			 */
			const char* filename; //general filename
			char buffer_real[1024];

			if (i_simVars.iodata.output_file_name == "")
				filename = "output_%s_t%020.8f.csv";
			else
				filename = i_simVars.iodata.output_file_name.c_str();

			sprintf(buffer_real, filename, "normal_modes_plane", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file(buffer_real, std::ios_base::trunc);
			std::cout << "Writing normal mode analysis to files of the form '" << buffer_real << "'" << std::endl;

			//Positive inertia-gravity modes
			sprintf(buffer_real, filename, "normal_modes_plane_igpos", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file_igpos(buffer_real, std::ios_base::trunc);

			//Negative inertia-gravity modes
			sprintf(buffer_real, filename, "normal_modes_plane_igneg", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file_igneg(buffer_real, std::ios_base::trunc);

			//Geostrophic modes
			sprintf(buffer_real, filename, "normal_modes_plane_geo", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file_geo(buffer_real, std::ios_base::trunc);

			//std::cout << "WARNING: OUTPUT IS TRANSPOSED!" << std::endl;

			// use very high precision
			file << std::setprecision(20);
			file_igpos << std::setprecision(20);
			file_igneg << std::setprecision(20);
			file_geo << std::setprecision(20);

			file << "# dt " << i_simVars.timecontrol.current_timestep_size << std::endl;
			file << "# g " << i_simVars.sim.gravitation << std::endl;
			file << "# h " << i_simVars.sim.h0 << std::endl;
			file << "# r " << i_simVars.sim.sphere_radius << std::endl;
			file << "# f " << i_simVars.sim.plane_rotating_f0 << std::endl;

#if SWEET_USE_PLANE_SPECTRAL_SPACE
			int specmodes = planeDataConfig->get_spectral_iteration_range_area(0)+planeDataConfig->get_spectral_iteration_range_area(1);
			file << "# specnummodes " << specmodes << std::endl;
			file << "# specrealresx " << planeDataConfig->spectral_real_modes[0] << std::endl;
			file << "# specrealresy " << planeDataConfig->spectral_real_modes[1] << std::endl;
#endif

			file << "# physresx " << planeDataConfig->physical_res[0] << std::endl;
			file << "# physresy " << planeDataConfig->physical_res[1] << std::endl;
			file << "# normalmodegeneration " << i_simVars.misc.normal_mode_analysis_generation << std::endl;
			file << "# antialiasing ";
#if SWEET_USE_PLANE_SPECTRAL_DEALIASING
			file << 1;
#else
			file << 0;
#endif
			file << std::endl;

			PlaneData_Spectral* prog[3] = {&io_prog_h_pert, &io_prog_u, &io_prog_v};

			int number_of_prognostic_variables = 3;
			//The basic state is with zero in all variables
			// The only non zero variable in the basic state is the total height
			//    for which the constant is added within run_timestep()
			io_prog_h_pert.spectral_set_zero();
			io_prog_u.spectral_set_zero();
			io_prog_v.spectral_set_zero();

			//int num_timesteps = 1;

			// Timestep and perturbation
			double dt = i_simVars.timecontrol.current_timestep_size;
			double eps = dt;

			//Matrix representing discrete linear operator in spectral space
			Eigen::MatrixXcf A(3,3) ;
			//Eigen solver
			Eigen::ComplexEigenSolver<Eigen::MatrixXcf> ces;
			//Final eigenvalues
			std::complex<double> eval[3];

			//For each spectral mode
			//for (int r = 0; r < 2; r++) //only required to get the symmetric half of the spectrum
			//{
			int r = 0;

			for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
			{
				std::cout << "." << std::flush;
				for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
				{
					//This is the mode to be analysed
					//std::cout << "Mode (i,j)= (" << i << " , " << j <<")"<< std::endl;

					for (int outer_prog_id = 0; outer_prog_id < number_of_prognostic_variables; outer_prog_id++)
					{

						// reset time control
						i_simVars.timecontrol.current_timestep_nr = 0;
						i_simVars.timecontrol.current_simulation_time = 0;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							prog[inner_prog_id]->spectral_set_zero();

						// activate mode via real coefficient
						prog[outer_prog_id]->spectral_set(j, i, 1.0);
						//Activate the symetric couterpart of the mode (only needed if j>0 )
						if (j > 0)
							prog[outer_prog_id]->spectral_set(planeDataConfig->spectral_data_size[1]-j, i, 1.0);

						/*
						 * RUN timestep
						 */
						////prog[outer_prog_id]->request_data_physical();
						(i_class->*i_run_timestep_method)();

						/*
						 * compute
						 * 1/dt * (U(t+1) - U(t))
						 */
						///////prog[outer_prog_id]->request_data_spectral();

						std::complex<double> val = prog[outer_prog_id]->spectral_get(j, i);
						val = val - 1.0; //subtract U(0) from mode
						prog[outer_prog_id]->spectral_set(j, i, val);

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							(*prog[inner_prog_id]) /= eps;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							A(inner_prog_id,outer_prog_id)=prog[inner_prog_id]->spectral_get(j, i);;
						}

					}

					//std::cout << "Lik matrix" << std::endl;
					//std::cout << A << std::endl;

					//std::cout<<"Normal modes" << std::endl;
					ces.compute(A);
					for(int i=0; i<3; i++)
					{
						eval[i]=ces.eigenvalues()[i];
						//std::cout << "Eigenvalue "<< i << " : " << eval[i].real() <<" "<<eval[i].imag() << std::endl;

					}
					/* We will try to separate the modes in 3 types:
					 * -positive inertia-gravity (imag>f) - we will adopt coriolis f to test as if > zero, since the exact freq is sqrt(f^2+cK*K)
					 * -negative inertia-gravity (imag<-f)
					 * -negative inertia-gravity (imag aprox 0) - we will fit all other modes here
					 */
					int count_igpos=0;
					int count_igneg=0;
					int count_geo=0;
					for(int i=0; i<3; i++)
					{
						if(eval[i].imag() > 0.5 * i_simVars.sim.plane_rotating_f0)
						{
							//std::cout<< "IG pos mode: " << eval[i].imag() << std::endl;
							//file_igpos << eval[i].imag();
							file_igpos << eval[i].real()<< "\t" << eval[i].imag();
							file_igpos << "\t";
							count_igpos++;
						}
						if(eval[i].imag() < - 0.5 * i_simVars.sim.plane_rotating_f0)
						{
							//std::cout<< "IG neg mode: " << eval[i].imag() << std::endl;
							//file_igneg << eval[i].imag();
							file_igneg << eval[i].real()<< "\t" << eval[i].imag();
							file_igneg << "\t";
							count_igneg++;
						}
						if(eval[i].imag() >= - 0.5 * i_simVars.sim.plane_rotating_f0 && eval[i].imag() <=  0.5 * i_simVars.sim.plane_rotating_f0 )
						{
							//std::cout<< "IG geo mode: " << eval[i].imag() << std::endl;
							//file_geo << eval[i].imag();
							file_geo << eval[i].real()<< "\t" << eval[i].imag();
							file_geo << "\t";
							count_geo++;
						}
					}
					//Check if we got the correct modes
					if ( count_igpos * count_igneg * count_geo > 0 )
					{
						count_igpos=0;
						count_igneg=0;
						count_geo=0;
					}
					else
					{
						SWEETError("SWE_Plane_Normal_Modes: Could not separate modes!!");
					}

					//std::cout<<"-------------------------" << std::endl;
				}
				file_igpos << std::endl;
				file_igneg << std::endl;
				file_geo << std::endl;
			}

			//}
			//std::cout<<"-------------------------" << std::endl;
			//SWEETError("still needs work...");
#else
			SWEETError("SWE_Plane_Normal_Modes: Cannot test this without Eigen library. Please compile with --eigen=enable");
#endif
		}
		/*
		 * Do a normal mode analysis using perturbation, see
		 * Hillary Weller, John Thuburn, Collin J. Cotter,
		 * "Computational Modes and Grid Imprinting on Five Quasi-Uniform Spherical C Grids"
		 */
		else
		{

			//run_timestep();
			const char* filename;
			char buffer_real[1024];

			if (i_simVars.iodata.output_file_name == "")
				filename = "output_%s_normalmodes.csv";
			else
				filename = i_simVars.iodata.output_file_name.c_str();

			sprintf(buffer_real, filename, "normal_modes_physical", i_simVars.timecontrol.current_timestep_size*i_simVars.iodata.output_time_scale);
			std::ofstream file(buffer_real, std::ios_base::trunc);
			std::cout << "Writing normal mode analysis to file '" << buffer_real << "'" << std::endl;

			std::cout << "WARNING: OUTPUT IS TRANSPOSED!" << std::endl;

			// use very high precision
			file << std::setprecision(20);

			PlaneData_Spectral* prog[3] = {&io_prog_h_pert, &io_prog_u, &io_prog_v};

			/*
			 * Maximum number of prognostic variables
			 *
			 * Advection e.g. has only one
			 */
			if (number_of_prognostic_variables <= 0)
				SWEETError("simVars.pde.number_of_prognostic_variables must be set!");

			if (number_of_prognostic_variables == 3)
			{
				io_prog_h_pert.spectral_set_zero();
				io_prog_u.spectral_set_zero();
				io_prog_v.spectral_set_zero();
			}
			else if (number_of_prognostic_variables == 1)
			{
				io_prog_h_pert.spectral_set_zero();
			}
			else
			{
				SWEETError("Not yet supported");
			}

#if 0
			if (i_simVars.disc.timestepping_method == SimulationVariables::Discretization::LEAPFROG_EXPLICIT)
			{
				SWEETError("Not yet tested and supported");
				std::cout << "WARNING: Leapfrog time stepping doesn't make real sense since 1st step is based on RK-like method" << std::endl;
				std::cout << "We'll do two Leapfrog time steps here to take the LF errors into account!" << std::endl;
				std::cout << "Therefore, we also halve the time step size here" << std::endl;

				i_simVars.timecontrol.current_timestep_size = 0.5*i_simVars.sim.CFL;
				i_simVars.sim.CFL = -i_simVars.timecontrol.current_timestep_size;
			}
#endif

			int num_timesteps = 1;
			if (i_simVars.misc.normal_mode_analysis_generation >= 10)
			{
				if (i_simVars.timecontrol.max_timesteps_nr > 0)
					num_timesteps = i_simVars.timecontrol.max_timesteps_nr;
			}

			if (i_simVars.timecontrol.max_simulation_time > 0)
				file << "# t " << i_simVars.timecontrol.max_simulation_time << std::endl;
			else
				file << "# t " << (num_timesteps*(-i_simVars.timecontrol.current_timestep_size)) << std::endl;

			file << "# g " << i_simVars.sim.gravitation << std::endl;
			file << "# h " << i_simVars.sim.h0 << std::endl;
//			file << "# r " << i_simVars.sim.sphere_radius << std::endl;
			file << "# f " << i_simVars.sim.plane_rotating_f0 << std::endl;

#if SWEET_USE_PLANE_SPECTRAL_SPACE
			int specmodes = planeDataConfig->get_spectral_iteration_range_area(0)+planeDataConfig->get_spectral_iteration_range_area(1);
			file << "# specnummodes " << specmodes << std::endl;
			file << "# specrealresx " << planeDataConfig->spectral_real_modes[0] << std::endl;
			file << "# specrealresy " << planeDataConfig->spectral_real_modes[1] << std::endl;
#endif

			file << "# physresx " << planeDataConfig->physical_res[0] << std::endl;
			file << "# physresy " << planeDataConfig->physical_res[1] << std::endl;
			file << "# normalmodegeneration " << i_simVars.misc.normal_mode_analysis_generation << std::endl;
			file << "# antialiasing ";

#if SWEET_USE_PLANE_SPECTRAL_DEALIASING
			file << 1;
#else
			file << 0;
#endif

			file << std::endl;

			// iterate over all prognostic variables
			for (int outer_prog_id = 0; outer_prog_id < number_of_prognostic_variables; outer_prog_id++)
			{
				if (i_simVars.misc.normal_mode_analysis_generation == 1 || i_simVars.misc.normal_mode_analysis_generation == 11)
				{
					// iterate over physical space
					for (std::size_t outer_i = 0; outer_i < planeDataConfig->physical_array_data_number_of_elements; outer_i++)
					{
						// reset time control
						i_simVars.timecontrol.current_timestep_nr = 0;
						i_simVars.timecontrol.current_simulation_time = 0;

						std::cout << "." << std::flush;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							prog[inner_prog_id]->spectral_set_zero();

						// activate mode
						///prog[outer_prog_id]->request_data_physical();
						///prog[outer_prog_id]->physical_space_data[outer_i] = 1;
						PlaneData_Physical tmp = prog[outer_prog_id]->toPhys();
						tmp.physical_space_data[outer_i] = 1;
						prog[outer_prog_id]->loadPlaneDataPhysical(tmp);

						/*
						 * RUN timestep
						 */

						(i_class->*i_run_timestep_method)();

						if (i_simVars.misc.normal_mode_analysis_generation == 1)
						{
							/*
							 * compute
							 * 1/dt * (U(t+1) - U(t))
							 */
							///////prog[outer_prog_id]->request_data_physical();
							///////prog[outer_prog_id]->physical_space_data[outer_i] -= 1.0;
							PlaneData_Physical tmp2 = prog[outer_prog_id]->toPhys();
							tmp2.physical_space_data[outer_i] -= 1.0;

							for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								tmp2 /= i_simVars.timecontrol.current_timestep_size;
								//(*prog[inner_prog_id]) /= i_simVars.timecontrol.current_timestep_size;

							prog[outer_prog_id]->loadPlaneDataPhysical(tmp2);
						}

						tmp = prog[outer_prog_id]->toPhys();
						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							///prog[inner_prog_id]->request_data_physical();
							for (std::size_t k = 0; k < planeDataConfig->physical_array_data_number_of_elements; k++)
							{
								///file << prog[inner_prog_id]->physical_space_data[k];
								file << tmp.physical_space_data[k];
								if (inner_prog_id != number_of_prognostic_variables-1 || k != planeDataConfig->physical_array_data_number_of_elements-1)
									file << "\t";
								else
									file << std::endl;
							}
						}
					}
				}
#if 1
				else if (i_simVars.misc.normal_mode_analysis_generation == 3 || i_simVars.misc.normal_mode_analysis_generation == 13)
				{
#if !SWEET_USE_PLANE_SPECTRAL_SPACE
					SWEETError("Only available with if plane spectral space is activated during compile time!");
#else

					// iterate over spectral space
					for (int r = 0; r < 2; r++)
					{

						for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
						{
							for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
							{
								// reset time control
								i_simVars.timecontrol.current_timestep_nr = 0;
								i_simVars.timecontrol.current_simulation_time = 0;

								std::cout << "." << std::flush;

								for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
									prog[inner_prog_id]->spectral_set_zero();

								// activate mode via real coefficient
								prog[outer_prog_id]->spectral_set(j, i, 1.0);

								/*
								 * RUN timestep
								 */
								(i_class->*i_run_timestep_method)();

								if (i_simVars.misc.normal_mode_analysis_generation == 3)
								{
									/*
									 * compute
									 * 1/dt * (U(t+1) - U(t))
									 */
									///prog[outer_prog_id]->request_data_spectral();

									std::complex<double> val = prog[outer_prog_id]->spectral_get(j, i);
									val = val - 1.0;
									prog[outer_prog_id]->spectral_set(j, i, val);

									for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
										(*prog[inner_prog_id]) /= i_simVars.timecontrol.current_timestep_size;
								}

								for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								{
									///prog[inner_prog_id]->request_data_spectral();

									/*
									 * REAL
									 */

									for (int r = 0; r < 2; r++)
									{
										for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
										{
											for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
											{
												file << prog[inner_prog_id]->spectral_get(j, i).real();
												file << "\t";
											}
										}
									}
								}

								for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								{
									/*
									 * IMAG
									 */
									int c = 0;
									for (int r = 0; r < 2; r++)
									{
										for (std::size_t j = planeDataConfig->spectral_data_iteration_ranges[r][1][0]; j < planeDataConfig->spectral_data_iteration_ranges[r][1][1]; j++)
										{
											for (std::size_t i = planeDataConfig->spectral_data_iteration_ranges[r][0][0]; i < planeDataConfig->spectral_data_iteration_ranges[r][0][1]; i++)
											{
												file << prog[inner_prog_id]->spectral_get(j, i).imag();

												if (inner_prog_id != number_of_prognostic_variables-1 || c != specmodes-1)
													file << "\t";
												else
													file << std::endl;

												c++;
											}
										}
									}
								}
							}
						}
					}
#endif
				}
#else
				else if (i_simVars.misc.normal_mode_analysis_generation == 3 || i_simVars.misc.normal_mode_analysis_generation == 13)
				{
					PlaneData_SpectralComplex t1(planeDataConfig);
					PlaneData_SpectralComplex t2(planeDataConfig);
					PlaneData_SpectralComplex t3(planeDataConfig);
					PlaneDataComplex* prog_cplx[3] = {&t1, &t2, &t3};

					// iterate over spectral space
					for (std::size_t outer_i = 0; outer_i < planeDataConfig->spectral_complex_array_data_number_of_elements; outer_i++)
					{
						// reset time control
						i_simVars.timecontrol.current_timestep_nr = 0;
						i_simVars.timecontrol.current_simulation_time = 0;

						std::cout << "." << std::flush;

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
							prog_cplx[inner_prog_id]->spectral_set_zero();

						// activate mode via real coefficient
						prog_cplx[outer_prog_id]->request_data_spectral();
						prog_cplx[outer_prog_id]->spectral_space_data[outer_i].real(1);

						// convert PlaneData_SpectralComplex to PlaneData
						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							*prog[inner_prog_id] = Convert_PlaneDataSpectralComplex_To_PlaneDataSpectral::physical_convert(*prog_cplx[inner_prog_id]);
							prog[inner_prog_id]->spectral_zeroAliasingModes();
						}

						/*
						 * RUN timestep
						 */
						(i_class->*i_run_timestep_method)();

						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							prog[inner_prog_id]->spectral_zeroAliasingModes();
#warning "update this physical_convert maybe to spectral_convert"

							*prog_cplx[inner_prog_id] = Convert_PlaneDataSpectral_To_PlaneDataSpectralComplex::physical_convert(*prog[inner_prog_id]);

							prog_cplx[inner_prog_id]->request_data_spectral();
						}

						if (i_simVars.misc.normal_mode_analysis_generation == 3)
						{
							/*
							 * compute
							 * 1/dt * (U(t+1) - U(t))
							 */
							prog_cplx[outer_prog_id]->request_data_spectral();
							prog_cplx[outer_prog_id]->spectral_space_data[outer_i] -= 1.0;

							for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
								prog_cplx[inner_prog_id]->operator*=(1.0/i_simVars.timecontrol.current_timestep_size);
						}

						// convert PlaneData_SpectralComplex to PlaneData
						for (int inner_prog_id = 0; inner_prog_id < number_of_prognostic_variables; inner_prog_id++)
						{
							prog_cplx[inner_prog_id]->request_data_spectral();

							/*
							 * REAL
							 */
							for (std::size_t k = 0; k < planeDataConfig->spectral_complex_array_data_number_of_elements; k++)
							{
								file << prog_cplx[inner_prog_id]->spectral_space_data[k].real();
								file << "\t";
							}

							/*
							 * IMAG
							 */
							for (std::size_t k = 0; k < planeDataConfig->spectral_complex_array_data_number_of_elements; k++)
							{
								file << prog_cplx[inner_prog_id]->spectral_space_data[k].imag();

								if (inner_prog_id != number_of_prognostic_variables-1 || k != planeDataConfig->spectral_complex_array_data_number_of_elements-1)
									file << "\t";
								else
									file << std::endl;
							}
						}
					}
				}
#endif
			}
		}
	}

	~SWE_Plane_Normal_Modes()
	{

	}
};

#endif /* SRC_PROGRAMS_SWE_PLANE_NORMAL_MODES_HPP_ */


#include <iostream>
#include <fstream>
#include <cstring>
#include <cstdlib>
#include <vector>
#include <limits>       // std::numeric_limits
#include <boost/crc.hpp>
#include <boost/program_options.hpp>
#include <boost/system/error_code.hpp>

using namespace boost ;

const int CRC16_CCITT_R=0x8408;  //https://en.wikipedia.org/wiki/Cyclic_redundancy_check
const int DEFAULT_SIZE_OF_PACK=1;
const int DEFAULT_PACK_COUNT=10;

unsigned int packSize = DEFAULT_SIZE_OF_PACK;
unsigned int packCount = DEFAULT_PACK_COUNT;

std::vector<unsigned int> data;
std::vector<unsigned int> remote;

std::string sOutputFile = "file.binary";
std::string sInpuFile = "";

unsigned int sawSize = std::numeric_limits<int>::max();
unsigned int rndSize = 0;
unsigned int mulSize = 1;
unsigned int crcPoly = CRC16_CCITT_R;

int main(int argc, char* argv[])
{

    namespace po = boost::program_options;
    po::options_description desc("Allowed options");
    desc.add_options()
    ("help,h", "show options")
    ("addcrc,c", po::value<unsigned int> (&packSize), "count crc after this")
    ("addsum,u", po::value<unsigned int> (&packSize), "count simple sum after this")
    ("datacount,d", po::value<unsigned int> (&packCount), "count of genrated packs")
    ("outfile,f", po::value<std::string> (&sOutputFile), "outputfilename, default:file.binary")
    ("inputfile,k", po::value<std::string> (&sInpuFile), "get data from inputfile instead of algo" )
    ("saw,s", po::value<unsigned int> (&sawSize), "algorithm: saw modulo (default:maxint)")
    ("rnd,r", po::value<unsigned int> (&rndSize), "algorithm: random modulo (default:0)")
    ("mul,m", po::value<unsigned int> (&mulSize), "algorithm: multiplication (default:1)")
    ("crcpoly,e", po::value<unsigned int> (&crcPoly), "crc polynominal (default:0x8408)")
    ("print,p", "show data")
    ;
    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).
              options(desc).run(), vm);
    po::notify(vm);

    if (vm.count("help"))
    {
        std::cerr << argv[0] << " - data generator" << std::endl;
        std::cerr << "The idea of this program is to generate data with or without crc."<< std::endl;
        std::cerr << "For testing purposes."<< std::endl;
        std::cerr << "Algorithm: ((rnd + (j*CRC_packSize + i)) * mul) % saw" << std::endl;
        std::cerr << "Examples:"<< std::endl;
        std::cerr << "\t./xgenerator -d 8 -s 30 -p -m 10:"<< std::endl;
        std::cerr << "\t\t>0 10 20 0 10 20 0 10"<< std::endl;
        std::cerr << "\t./xgenerator -d 8 -p"<< std::endl;
        std::cerr << "\t\t>0 1 2 3 4 5 6 7"<< std::endl;
        std::cerr << "\t./xgenerator -d 8 -p -r 20"<< std::endl;
        std::cerr << "\t\t>3 7 19 18 17 20 12 19"<< std::endl;
        std::cerr << "\t./xgenerator -d 2 -p -c 4"<< std::endl;
        std::cerr << "\t\t0 1 2 3 37368"<< std::endl;
        std::cerr << "\t\t4 5 6 7 24536"<< std::endl;
        std::cerr << desc << std::endl ;
        return system::errc::success;
    }

    auto myfile = std::fstream(sOutputFile, std::ios::out | std::ios::binary);

    if (sInpuFile != "" && vm.count("addcrc"))
    {

        auto remotefile = std::fstream(sInpuFile, std::ios::in | std::ios::binary);
        unsigned int val ;
        while (remotefile.read((char*)&val,sizeof(unsigned int)))
        {
            remote.push_back(val);
        }
        remotefile.close();

        assert( remote.size() >= packCount * packSize);
    }

    int crcqCnt=0; //Count of crc that landend in output file
    int sumqCnt=0; //Count of crc that landend in output file
    int cnt=0; //Count of data read from remote file
    for(auto j = 0; j < packCount; j++)
    {

        std::vector<unsigned int> crcq;
        unsigned int sumq = 0;

        for(auto i = 0; i < packSize; ++i)
        {

            auto rndVector = 0;
            unsigned int val ;
            if (sInpuFile != "" && vm.count("addcrc"))
            {
                val = remote[cnt++];
            }
            else
            {
                if (rndSize>0)
                {
                    rndVector = rand()%rndSize;
                }
                val = ((rndVector + (j*packSize + i)) * mulSize)%sawSize ;
            }
            data.push_back(val);
            if (vm.count("addcrc"))
            {
                crcq.push_back(val);
            }
            if (vm.count("print"))
            {
                std::cout << val << " ";
            }

            if (vm.count("addsum"))
            {
                sumq+=val;
            }
        }

        if (vm.count("addcrc"))
        {

            boost::crc_basic<16> crcfn(crcPoly, 0x0, 0x0, false, false);
            crcfn.process_bytes(crcq.data(), sizeof(unsigned int) * crcq.size());
            if (vm.count("print"))
            {
                std::cout << crcfn.checksum() << std::endl ;
            }
            data.push_back(crcfn.checksum());
            crcqCnt++;
        }

        if (vm.count("addsum"))
        {
            if (vm.count("print"))
            {
                std::cout << sumq << std::endl ;
            }
            data.push_back(sumq);
            sumqCnt++;
        }
    }

    if (vm.count("print"))
    {
        std::cout << std::endl ;
    }

    myfile.write((char*)data.data(), data.size()*sizeof(unsigned int));
    myfile.close();

    std::cout << "count:"<< packCount * packSize << std::endl;
    if (vm.count("addcrc"))
    {
        std::cout << "crc cnt:"<< crcqCnt << std::endl;
    }
    if (vm.count("addsum"))
    {
        std::cout << "sum cnt:"<< sumqCnt << std::endl;
    }
    std::cout << "output:" << sOutputFile << std::endl;
    std::cout << "done." << std::endl;
    return system::errc::success;
}


// File: morton_dense.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    // Z-order matrix
    morton_dense<double, recursion::morton_z_mask>  A(10, 10);

    A= 0;
    A(2, 3)= 7.0;
    A[2][4]= 3.0;
    std::cout << "A is \n" << A << "\n";

    // B is an N-order matrix with column-major 4x4 blocks, see paper
    morton_dense<float, recursion::doppled_4_col_mask> B(10, 10);

    // Assign the identity matrix times 3 to B
    B= 3;
    std::cout << "B is \n" << B << "\n";

    return 0;
}


#include "refill/system_models/linearized_system_model.h"

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include "refill/distributions/gaussian_distribution.h"

namespace refill {

class LinearizedSystemModelClass : public LinearizedSystemModel {
 public:
  LinearizedSystemModelClass(const size_t& state_dim,
                             const DistributionInterface& system_noise)
      : LinearizedSystemModel(state_dim, system_noise) {}
  LinearizedSystemModelClass(const size_t& state_dim,
                             const DistributionInterface& system_noise,
                             const size_t& input_dim)
      : LinearizedSystemModel(state_dim, system_noise, input_dim) {}
  Eigen::VectorXd propagate(const Eigen::VectorXd& state,
                            const Eigen::VectorXd& input,
                            const Eigen::VectorXd& noise) const {
    return state + noise;
  }
};

TEST(LinearizedSystemModelTest, NoInputTest) {
  GaussianDistribution system_noise(Eigen::Vector2d::Zero(),
                                    Eigen::Matrix2d::Identity());

  LinearizedSystemModelClass system_model(2, system_noise);

  Eigen::MatrixXd state_jacobian = system_model.getStateJacobian(
      Eigen::Vector2d::Zero(), Eigen::VectorXd::Zero(0));

  ASSERT_EQ(state_jacobian.rows(), state_jacobian.cols());
  ASSERT_EQ(system_model.getStateDim(), state_jacobian.rows());
  ASSERT_EQ(Eigen::Matrix2d::Identity(), state_jacobian);

  Eigen::MatrixXd noise_jacobian = system_model.getNoiseJacobian(
      Eigen::Vector2d::Zero(), Eigen::VectorXd::Zero(0));

  ASSERT_EQ(system_model.getStateDim(), noise_jacobian.rows());
  ASSERT_EQ(system_model.getNoiseDim(), noise_jacobian.cols());
  ASSERT_EQ(Eigen::Matrix2d::Identity(), noise_jacobian);
}

TEST(LinearizedSystemModelTest, WithInputTest) {
  GaussianDistribution system_noise(Eigen::Vector2d::Zero(),
                                    Eigen::Matrix2d::Identity());

  LinearizedSystemModelClass system_model(2, system_noise, 2);

  Eigen::MatrixXd state_jacobian = system_model.getStateJacobian(
      Eigen::Vector2d::Zero(), Eigen::Vector2d::Zero());

  ASSERT_EQ(state_jacobian.rows(), state_jacobian.cols());
  ASSERT_EQ(system_model.getStateDim(), state_jacobian.rows());
  ASSERT_EQ(Eigen::Matrix2d::Identity(), state_jacobian);

  Eigen::MatrixXd noise_jacobian = system_model.getNoiseJacobian(
      Eigen::Vector2d::Zero(), Eigen::Vector2d::Zero());

  ASSERT_EQ(system_model.getStateDim(), noise_jacobian.rows());
  ASSERT_EQ(system_model.getNoiseDim(), noise_jacobian.cols());
  ASSERT_EQ(Eigen::Matrix2d::Identity(), noise_jacobian);
}

}  // namespace refill


#include <boost/program_options.hpp>
#include <string>
#include <math.h>
#include <proj_api.h>
#include "elevation.hxx"
#include "ObjWriter.hxx"
#include "Delaunay.h"

using namespace std;
using namespace osmwave;

static projPJ wgs84 = pj_init_plus("+proj=longlat +ellps=WGS84 +datum=WGS84 +no_defs");

// Note: in place addition
static void vecAdd(XYZ& a, const XYZ& b) {
    a.x += b.x;
    a.y += b.y;
    a.z += b.z;
}

static void vecSub(const XYZ& a, const XYZ& b, XYZ& result) {
    result.x = a.x - b.x;
    result.y = a.y - b.y;
    result.z = a.z - b.z;
}

static void triangleNormal(const XYZ* coords, const ITRIANGLE& tri, XYZ& normal) {
    XYZ u;
    XYZ v;

    vecSub(coords[tri.p2], coords[tri.p1], u);
    vecSub(coords[tri.p3], coords[tri.p1], v);

    normal.x = u.y*v.z - u.z*v.y;
    normal.y = u.z*v.x - u.x*v.z;
    normal.z = u.x*v.y - u.y*v.x;

    double l = sqrt(normal.x*normal.x+normal.y*normal.y+normal.z*normal.z);

    normal.x = normal.x / l;
    normal.y = normal.y / l;
    normal.z = normal.z / l;
}

static double findNearHeight(int rows, int cols, int index, int dx, int dy, XYZ* verts) {
    int s = index;
    do {
        s += dx * rows + dy;
    } while (std::isnan(verts[s].z));

    return verts[s].z;
}

static int thin(int rows, int cols, XYZ* verts, double tolerance) {
    int index = 0;
    int nonEmpty = 0;

    for (int i = 1; i < cols - 1; i++) {
        for (int j = 1; j < rows - 1; j++) {
            if (!std::isnan(verts[index].z)) {
                double e1 = verts[index].z;
                double e2 = findNearHeight(rows, cols, index, 1, -1, verts);
                double e3 = findNearHeight(rows, cols, index, 1, 0, verts);
                double e4 = findNearHeight(rows, cols, index, 1, 1, verts);
                double d2 = abs(e1 - e2);
                double d3 = abs(e1 - e3);
                double d4 = abs(e1 - e4);

                if (d2 <= tolerance &&
                    d3 <= tolerance &&
                    d4 <= tolerance) {
                    verts[index].z = NAN;
                } else {
                    nonEmpty++;
                }
            }

            index++;
        }
    }

    return nonEmpty;
}

void terrain_to_obj(const std::string& elevationPath, const std::string& projDef, double x1, double y1, double x2, double y2) {
    Elevation elevation(floor(y1), floor(x1), ceil(y2), ceil(x2), elevationPath);
    ObjWriter writer(cout);
    projPJ proj = pj_init_plus(projDef.c_str());
    double step = 1.0 / 3600;
    int rows = (int)floor((y2 - y1) / step + 1);
    int cols = (int)floor((x2 - x1) / step + 1);
    double bounds[] = {x1*DEG_TO_RAD, y1*DEG_TO_RAD, x2*DEG_TO_RAD, y2*DEG_TO_RAD};

    pj_transform(wgs84, proj, 2, 2, (double*)&bounds, (double*)&bounds + 1, nullptr);

    cerr << "rows: " << rows << ", cols: " << cols << endl;
    cerr << "bounds: " << bounds[0] << ", " << bounds[1] << " - " << bounds[2] << ", " << bounds[3] << endl;

    cerr << "Calculating vertices..." << endl;
    XYZ* coords = new XYZ[rows * cols + 3];
    XYZ* normals = new XYZ[rows * cols];

    int i = 0;
    // Having columns as outer loop ensures x will be growing,
    // which is a requirement for the triangulation algorithm,
    // as long as projection is west to east.
    for (int c = 0; c < cols; c++) {
        double x = bounds[0] + (bounds[2] - bounds[0]) * c / cols;
        for (int r = 0; r < rows; r++) {
            double y = bounds[1] + (bounds[3] - bounds[1]) * r / rows;
            double ll[2] = {x, y};
            pj_transform(proj, wgs84, 1, 2, (double*)&ll, (double*)&ll + 1, nullptr);

            XYZ& coord = coords[i++];
            coord.x = x;
            coord.y = y;
            coord.z = elevation.elevation(ll[1]*RAD_TO_DEG, ll[0]*RAD_TO_DEG);

            //cerr << (ll[0] * RAD_TO_DEG) << ", " << (ll[1] * RAD_TO_DEG) << " (" << coord.x << ", " << coord.y << "): " << coord.z << endl;
        }
    }

    int startCount = rows * cols,
        lastCount = 0,
        count = -1;
    cerr << "Thinning " << startCount << " vertices..." << endl;
    while (lastCount != count) {
        lastCount = count;
        count = thin(rows, cols, coords, 2);
    }

    int j = 0;
    for (int i = 0; i < rows * cols; i++) {
        if (!std::isnan(coords[i].z)) {
            coords[j++] = coords[i];
        }
    }
    cerr << "Thinned to " << j << " vertices" << endl;

    cerr << "Triangulating..." << endl;
    ITRIANGLE *tris = new ITRIANGLE[3 * rows * cols];
    int numTriangles;
    Triangulate(j, coords, tris, numTriangles);
    cerr << numTriangles << " triangles" << endl;

    memset((void*)normals, 0, sizeof(XYZ) * j);
    for (int i = 0; i < numTriangles; i++) {
        XYZ normal;
        triangleNormal(coords, tris[i], normal);
        vecAdd(normals[tris[i].p1], normal);
        vecAdd(normals[tris[i].p2], normal);
        vecAdd(normals[tris[i].p3], normal);
    }

    writer.checkpoint();
    for (int i = 0; i < j; i++) {
        writer.vertex(coords[i].y, coords[i].z, coords[i].x, normals[i].y, normals[i].z, normals[i].x);
    }

    for (int i = 0; i < numTriangles; i++) {
        writer.beginFace();
        writer << tris[i].p1 << tris[i].p2 << tris[i].p3;
        writer.endFace();
    }

    delete coords;
    delete normals;
    delete tris;
}

int main(int argc, char* argv[]) {
    namespace po = boost::program_options;
    po::options_description desc("Options");
    desc.add_options()
        ("elevation_dir,e", po::value<string>()->required(), "Set directory containing elevation data")
        ("proj,p", po::value<string>(), "Projection definition")
        ("x1", po::value<double>()->required(), "X1")
        ("y1", po::value<double>()->required(), "Y1")
        ("x2", po::value<double>()->required(), "X2")
        ("y2", po::value<double>()->required(), "Y2");
    po::positional_options_description positionOptions;
    positionOptions.add("x1", 1);
    positionOptions.add("y1", 1);
    positionOptions.add("x2", 1);
    positionOptions.add("y2", 1);

    po::variables_map vm;
    try {
        po::store(po::command_line_parser(argc, argv)
            .options(desc)
            .positional(positionOptions)
            .run(), vm);

        po::notify(vm);
    } catch (po::error& e) {
        cerr << "Error " << e.what() << endl << endl;
        cerr << desc << endl;
        return 1;
    }

    const string& elevPath(vm["elevation_dir"].as<string>());
    const string* projDef = nullptr;
    const double x1 = vm["x1"].as<double>();
    const double y1 = vm["y1"].as<double>();
    const double x2 = vm["x2"].as<double>();
    const double y2 = vm["y2"].as<double>();

    if (vm.count("proj")) {
        projDef = &vm["proj"].as<string>();
    } else {
        ostringstream stream;
        stream << "+proj=tmerc +lat_0=" << ((y1 + y2) / 2) << " +lon_0=" << ((x1 + x2) / 2) << " +k=1.000000 +x_0=0 +y_0=0 +ellps=WGS84 +datum=WGS84 +units=m +no_defs";
        projDef = new string(stream.str());
    }

    terrain_to_obj(elevPath, *projDef, x1, y1, x2, y2);

    return 0;
}


#ifndef MLT_MODELS_TRANSFORMERS_SPARSE_TIED_AUTOENCODER_HPP
#define MLT_MODELS_TRANSFORMERS_SPARSE_TIED_AUTOENCODER_HPP

#include <cmath>
#include <tuple>
#include <type_traits>

#include <Eigen/Core>

#include "transformer.hpp"
#include "../implementations/autoencoder.hpp"
#include "../../utils/eigen.hpp"

namespace mlt {
namespace models {
namespace transformers {
	using namespace utils::eigen;

	template <class HiddenActivation, class ReconstructionActivation, class Optimizer>
	class SparseTiedAutoencoder : public Transformer<SparseTiedAutoencoder<HiddenActivation, ReconstructionActivation, Optimizer>> {
	public:
		template <typename H, typename R, typename O,
			class = enable_if<is_convertible<decay_t<H>, HiddenActivation>::value
			&& is_convertible<decay_t<R>, ReconstructionActivation>::value
			&& is_convertible<decay_t<O>, Optimizer>::value>>
		explicit SparseTiedAutoencoder(int hidden_units, H&& hidden_activation, R&& reconstruction_activation, O&& optimizer, double regularization,
		double sparsity, double sparsity_weight) : _hidden_units(hidden_units), _hidden_activation(forward<H>(hidden_activation)),
			_reconstruction_activation(forward<R>(reconstruction_activation)), _optimizer(forward<O>(optimizer)), _regularization(regularization),
			_sparsity(sparsity), _sparsity_weight(sparsity_weight) {}

		Result transform(Features input) const {
			assert(_fitted);
			return _hidden_activation.compute((_weights * input).colwise() + _hidden_intercepts);
		}

		Self& fit(Features input, bool cold_start = true) {
			VectorXd init(_hidden_units * input.rows() + _hidden_units + input.rows());

			if (_fitted && !cold_start) {
				init.block(0, 0, _weights.size(), 1) = ravel(_weights);
				init.block(_weights.size(), 0, _hidden_intercepts.size(), 1) = _hidden_intercepts;

				init.block(_weights.size() + _hidden_intercepts.size(),
					0, _reconstruction_intercepts.size(), 1) = _reconstruction_intercepts;
			} else {
				init = (init.setRandom() * 4 / sqrt(6.0 / (_hidden_units + input.rows())));
			}

			VectorXd coeffs = _optimizer(*this, input, input, init, cold_start);

			_weights = unravel(coeffs.block(0, 0, _hidden_units * input.rows(), 1), _hidden_units, input.rows());
			_hidden_intercepts = coeffs.block(_hidden_units * input.rows(), 0, _hidden_units, 1);
			_reconstruction_intercepts = coeffs.block(_hidden_units * input.rows() + _hidden_units, 0, input.rows(), 1);

			_fitted = true;

			return _self();
		}

		using Transformer<Self>::fit;

		auto loss(VectorXdRef coeffs, Features input, Features target) const {
			auto weights = unravel(coeffs.block(0, 0, _hidden_units * input.rows(), 1), _hidden_units, input.rows());
			auto hidden_intercepts = coeffs.block(_hidden_units * input.rows(), 0, _hidden_units, 1);
			auto reconstruction_intercepts = coeffs.block(_hidden_units * input.rows() + _hidden_units, 0, input.rows(), 1);

			return implementations::autoencoder::sparse_loss(_hidden_activation, _reconstruction_activation, weights, hidden_intercepts,
				weights.transpose(), reconstruction_intercepts, _regularization, _sparsity, _sparsity_weight, input, target);
		}

		auto gradient(VectorXdRef coeffs, Features input, Features target) const {
			auto weights = unravel(coeffs.block(0, 0, _hidden_units * input.rows(), 1), _hidden_units, input.rows());
			auto hidden_intercepts = coeffs.block(_hidden_units * input.rows(), 0, _hidden_units, 1);
			auto reconstruction_intercepts = coeffs.block(_hidden_units * input.rows() + _hidden_units, 0, input.rows(), 1);

			MatrixXd weights_grad, weights_transp_grad;
			VectorXd hid_inter_grad, rec_inter_grad;
			tie(weights_grad, hid_inter_grad, weights_transp_grad, rec_inter_grad) = implementations::autoencoder::sparse_gradient(_hidden_activation,
				_reconstruction_activation, weights, hidden_intercepts, weights.transpose(), reconstruction_intercepts, _regularization,
				_sparsity, _sparsity_weight, input, target);

			VectorXd gradient(coeffs.rows());

			gradient.block(0, 0, weights_grad.size(), 1) = ravel(weights_grad + weights_transp_grad.transpose());
			gradient.block(weights_grad.size(), 0, hid_inter_grad.size(), 1) = hid_inter_grad;

			gradient.block(weights_grad.size() + hid_inter_grad.size(),
				0, rec_inter_grad.size(), 1) = rec_inter_grad;

			return gradient;
		}

		auto loss_and_gradient(VectorXdRef coeffs, Features input, Features target) const {
			auto weights = unravel(coeffs.block(0, 0, _hidden_units * input.rows(), 1), _hidden_units, input.rows());
			auto hidden_intercepts = coeffs.block(_hidden_units * input.rows(), 0, _hidden_units, 1);
			auto reconstruction_intercepts = coeffs.block(_hidden_units * input.rows() + _hidden_units, 0, input.rows(), 1);

			double loss;
			MatrixXd weights_grad, weights_transp_grad;
			VectorXd hid_inter_grad, rec_inter_grad;
			tie(loss, weights_grad, hid_inter_grad, weights_transp_grad, rec_inter_grad) = implementations::autoencoder::sparse_loss_and_gradient(
				_hidden_activation, _reconstruction_activation, weights, hidden_intercepts, weights.transpose(), reconstruction_intercepts,
				_regularization, _sparsity, _sparsity_weight, input, target);

			VectorXd gradient(coeffs.rows());

			gradient.block(0, 0, weights_grad.size(), 1) = ravel(weights_grad + weights_transp_grad.transpose());
			gradient.block(weights_grad.size(), 0, hid_inter_grad.size(), 1) = hid_inter_grad;

			gradient.block(weights_grad.size() + hid_inter_grad.size(),
				0, rec_inter_grad.size(), 1) = rec_inter_grad;

			return make_tuple(loss, gradient);
		}

	protected:
		int _hidden_units;
		HiddenActivation _hidden_activation;
		ReconstructionActivation _reconstruction_activation;
		Optimizer _optimizer;
		double _regularization;
		double _sparsity;
		double _sparsity_weight;

		MatrixXd _weights;
		VectorXd _hidden_intercepts;
		VectorXd _reconstruction_intercepts;
	};

	template <class HiddenActivation, class ReconstructionActivation, class Optimizer>
	auto create_sparse_tied_autoencoder(int hidden_units, HiddenActivation&& hidden_activation,
	ReconstructionActivation&& reconstruction_activation, Optimizer&& optimizer,
	double regularization, double sparsity, double sparsity_weight) {
		return SparseTiedAutoencoder<HiddenActivation, ReconstructionActivation, Optimizer>(
			hidden_units,
			forward<HiddenActivation>(hidden_activation),
			forward<ReconstructionActivation>(reconstruction_activation),
			forward<Optimizer>(optimizer),
			regularization, sparsity, sparsity_weight);
	}
}
}
}
#endif