#include "runtime.h"

#include <cmath>
#include <time.h>
#include <chrono>
#include <vector>

#include "timers.h"
#include "stdio.h"

#ifdef EIGEN
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
using namespace Eigen;
#endif

extern "C" {
int loc(int v0, int v1, int *neighbors_start, int *neighbors) {
  int l = neighbors_start[v0];
  while(neighbors[l] != v1) l++;
  return l;
}

double atan2_f64(double y, double x) {
  return atan2(y, x);
}

float atan2_f32(float y, float x) {
  double d_y = y;
  double d_x = x;
  return (float)atan2(d_y, d_x);
}

double tan_f64(double x) {
  return tan(x);
}

float tan_f32(float x) {
  double d_x = x;
  return (float)tan(d_x);
}

double asin_f64(double x) {
  return asin(x);
}

float asin_f32(float x) {
  double d_x = x;
  return (float)asin(d_x);
}

double acos_f64(double x) {
  return acos(x);
}

float acos_f32(float x) {
  double d_x = x;
  return (float)acos(d_x);
}

double max_f64(double a,double b) {
  return max(a,b);
}

float max_f32(float a, float b) {
  double d_a = a;
  double d_b = b;
  return (float)max(d_a,d_b);
}

double min_f64(double a,double b) {
  return min(a,b);
}

float min_f32(float a, float b) {
  double d_a = a;
  double d_b = b;
  return (float)min(d_a,d_b);
}

double cbrt_f64(double x) {
  return cbrt(x);
}

float cbrt_f32(float x) {
  double d_x = x;
  return (float)cbrt(d_x);
}

double det3_f64(double * a){
  return a[0] * (a[4]*a[8]-a[5]*a[7])
       - a[1] * (a[3]*a[8]-a[5]*a[6])
       + a[2] * (a[3]*a[7]-a[4]*a[6]);
}

float det3_f32(float * a){
  return a[0] * (a[4]*a[8]-a[5]*a[7])
       - a[1] * (a[3]*a[8]-a[5]*a[6])
       + a[2] * (a[3]*a[7]-a[4]*a[6]);
}

double det2_f64(double * a){
  return a[0] * a[3] - a[1] * a[2];
}

float det2_f32(float * a){
  return a[0] * a[3] - a[1] * a[2];
}

double det4_f64(double * a){
  double det0 = a[5] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[9]*a[15]-a[11]*a[13])
	   + a[7] * (a[9]*a[14]-a[10]*a[13]);
  double det1 = a[4] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[14]-a[10]*a[12]);
  double det2 = a[4] * (a[9]*a[15]-a[11]*a[13])
	   - a[5] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[13]-a[9]*a[12]);
  double det3 = a[4] * (a[9]*a[14]-a[10]*a[13])
	   - a[5] * (a[8]*a[14]-a[10]*a[12])
	   + a[6] * (a[8]*a[13]-a[9]*a[12]);
  return a[0]*det0 - a[1]*det1 + a[2]*det2 - a[3]*det3;
}

float det4_f32(float * a){
  float det0 = a[5] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[9]*a[15]-a[11]*a[13])
	   + a[7] * (a[9]*a[14]-a[10]*a[13]);
  float det1 = a[4] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[14]-a[10]*a[12]);
  float det2 = a[4] * (a[9]*a[15]-a[11]*a[13])
	   - a[5] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[13]-a[9]*a[12]);
  float det3 = a[4] * (a[9]*a[14]-a[10]*a[13])
	   - a[5] * (a[8]*a[14]-a[10]*a[12])
	   + a[6] * (a[8]*a[13]-a[9]*a[12]);
  return a[0]*det0 - a[1]*det1 + a[2]*det2 - a[3]*det3;
}

void inv3_f64(double * a, double * inv){
  double cof00 = a[4]*a[8]-a[5]*a[7];
  double cof01 =-a[3]*a[8]+a[5]*a[6];
  double cof02 = a[3]*a[7]-a[4]*a[6];

  double cof10 =-a[1]*a[8]+a[2]*a[7];
  double cof11 = a[0]*a[8]-a[2]*a[6];
  double cof12 =-a[0]*a[7]+a[1]*a[6];

  double cof20 = a[1]*a[5]-a[2]*a[4];
  double cof21 =-a[0]*a[5]+a[2]*a[3];
  double cof22 = a[0]*a[4]-a[1]*a[3];

  double determ = a[0] * cof00 + a[1] * cof01 + a[2]*cof02;

  determ = 1.0/determ;
  inv[0] = cof00 * determ;
  inv[1] = cof10 * determ;
  inv[2] = cof20 * determ;

  inv[3] = cof01 * determ;
  inv[4] = cof11 * determ;
  inv[5] = cof21 * determ;

  inv[6] = cof02 * determ;
  inv[7] = cof12 * determ;
  inv[8] = cof22 * determ;
}

void inv3_f32(float * a, float * inv){
  float cof00 = a[4]*a[8]-a[5]*a[7];
  float cof01 =-a[3]*a[8]+a[5]*a[6];
  float cof02 = a[3]*a[7]-a[4]*a[6];

  float cof10 =-a[1]*a[8]+a[2]*a[7];
  float cof11 = a[0]*a[8]-a[2]*a[6];
  float cof12 =-a[0]*a[7]+a[1]*a[6];

  float cof20 = a[1]*a[5]-a[2]*a[4];
  float cof21 =-a[0]*a[5]+a[2]*a[3];
  float cof22 = a[0]*a[4]-a[1]*a[3];

  float determ = a[0] * cof00 + a[1] * cof01 + a[2]*cof02;

  determ = 1.0/determ;
  inv[0] = cof00 * determ;
  inv[1] = cof10 * determ;
  inv[2] = cof20 * determ;

  inv[3] = cof01 * determ;
  inv[4] = cof11 * determ;
  inv[5] = cof21 * determ;

  inv[6] = cof02 * determ;
  inv[7] = cof12 * determ;
  inv[8] = cof22 * determ;
}

void inv2_f64(double * a, double * inv){
  double determ = a[0] * a[3] - a[1] * a[2];

  determ = 1.0/determ;
  inv[0] = a[3] * determ;
  inv[1] = -a[1] * determ;
  inv[2] = -a[2] * determ;
  inv[3] = a[0] * determ;
}

void inv2_f32(float * a, float * inv){
  float determ = a[0] * a[3] - a[1] * a[2];

  determ = 1.0/determ;
  inv[0] = a[3] * determ;
  inv[1] = -a[1] * determ;
  inv[2] = -a[2] * determ;
  inv[3] = a[0] * determ;
}

void inv4_f64(double * a, double * inv){
  double det0 = a[5] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[9]*a[15]-a[11]*a[13])
	   + a[7] * (a[9]*a[14]-a[10]*a[13]);
  double det1 = a[4] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[14]-a[10]*a[12]);
  double det2 = a[4] * (a[9]*a[15]-a[11]*a[13])
	   - a[5] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[13]-a[9]*a[12]);
  double det3 = a[4] * (a[9]*a[14]-a[10]*a[13])
	   - a[5] * (a[8]*a[14]-a[10]*a[12])
	   + a[6] * (a[8]*a[13]-a[9]*a[12]);
  double determ = a[0]*det0 - a[1]*det1 + a[2]*det2 - a[3]*det3;

  determ = 1.0/determ;
  inv[0] = (a[5]*a[10]*a[15] + a[6]*a[11]*a[13] + a[7]*a[9]*a[14] - a[5]*a[11]*a[14] - a[6]*a[9]*a[15] - a[7]*a[10]*a[13]) * determ;
  inv[1] = (a[1]*a[11]*a[14] + a[2]*a[9]*a[15] + a[3]*a[10]*a[13] - a[1]*a[10]*a[15] - a[2]*a[11]*a[13] - a[3]*a[9]*a[14]) * determ;
  inv[2] = (a[1]*a[6]*a[15] + a[2]*a[7]*a[13] + a[3]*a[5]*a[14] - a[1]*a[7]*a[14] - a[2]*a[5]*a[15] - a[3]*a[6]*a[13]) * determ;
  inv[3] = (a[1]*a[7]*a[10] + a[2]*a[5]*a[11] + a[3]*a[6]*a[9] - a[1]*a[6]*a[11] - a[2]*a[7]*a[9] - a[3]*a[5]*a[10]) * determ;
  inv[4] = (a[4]*a[11]*a[14] + a[6]*a[8]*a[15] + a[7]*a[10]*a[12] - a[4]*a[10]*a[15] - a[6]*a[11]*a[12] - a[7]*a[8]*a[14]) * determ;
  inv[5] = (a[0]*a[10]*a[15] + a[2]*a[11]*a[12] + a[3]*a[8]*a[14] - a[0]*a[11]*a[14] - a[2]*a[8]*a[15] - a[3]*a[10]*a[12]) * determ;
  inv[6] = (a[0]*a[7]*a[14] + a[2]*a[4]*a[15] + a[3]*a[6]*a[12] - a[0]*a[6]*a[15] - a[2]*a[7]*a[12] - a[3]*a[4]*a[14]) * determ;
  inv[7] = (a[0]*a[6]*a[11] + a[2]*a[7]*a[8] + a[3]*a[4]*a[10] - a[0]*a[7]*a[10] - a[2]*a[4]*a[11] - a[3]*a[6]*a[8]) * determ;
  inv[8] = (a[4]*a[9]*a[15] + a[5]*a[11]*a[12] + a[7]*a[8]*a[13] - a[4]*a[11]*a[13] - a[5]*a[8]*a[15] - a[7]*a[9]*a[12]) * determ;
  inv[9] = (a[0]*a[11]*a[13] + a[1]*a[8]*a[15] + a[3]*a[9]*a[12] - a[0]*a[9]*a[15] - a[1]*a[11]*a[12] - a[3]*a[8]*a[13]) * determ;
  inv[10] = (a[0]*a[5]*a[15] + a[1]*a[7]*a[12] + a[3]*a[4]*a[13] - a[0]*a[7]*a[13] - a[1]*a[4]*a[15] - a[3]*a[5]*a[12]) *  determ;
  inv[11] = (a[0]*a[7]*a[9] + a[1]*a[4]*a[11] + a[3]*a[5]*a[8] - a[0]*a[5]*a[11] - a[1]*a[7]*a[8] - a[3]*a[4]*a[9]) * determ;
  inv[12] = (a[4]*a[10]*a[13] + a[5]*a[8]*a[14] + a[6]*a[9]*a[12] - a[4]*a[9]*a[14] - a[5]*a[10]*a[12] - a[6]*a[8]*a[13]) * determ;
  inv[13] = (a[0]*a[9]*a[14] + a[1]*a[10]*a[12] + a[2]*a[8]*a[13] - a[0]*a[10]*a[13] - a[1]*a[8]*a[14] - a[2]*a[9]*a[12]) * determ;
  inv[14] = (a[0]*a[6]*a[13] + a[1]*a[4]*a[14] + a[2]*a[5]*a[12] - a[0]*a[5]*a[14] - a[1]*a[6]*a[12] - a[2]*a[4]*a[13]) * determ;
  inv[15] = (a[0]*a[5]*a[10] + a[1]*a[6]*a[8] + a[2]*a[4]*a[9] - a[0]*a[6]*a[9] - a[1]*a[4]*a[10] - a[2]*a[5]*a[8]) * determ;
}

void inv4_f32(float * a, float * inv){
  float det0 = a[5] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[9]*a[15]-a[11]*a[13])
	   + a[7] * (a[9]*a[14]-a[10]*a[13]);
  float det1 = a[4] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[14]-a[10]*a[12]);
  float det2 = a[4] * (a[9]*a[15]-a[11]*a[13])
	   - a[5] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[13]-a[9]*a[12]);
  float det3 = a[4] * (a[9]*a[14]-a[10]*a[13])
	   - a[5] * (a[8]*a[14]-a[10]*a[12])
	   + a[6] * (a[8]*a[13]-a[9]*a[12]);
  float determ = a[0]*det0 - a[1]*det1 + a[2]*det2 - a[3]*det3;

  determ = 1.0/determ;
  inv[0] = (a[5]*a[10]*a[15] + a[6]*a[11]*a[13] + a[7]*a[9]*a[14] - a[5]*a[11]*a[14] - a[6]*a[9]*a[15] - a[7]*a[10]*a[13]) * determ;
  inv[1] = (a[1]*a[11]*a[14] + a[2]*a[9]*a[15] + a[3]*a[10]*a[13] - a[1]*a[10]*a[15] - a[2]*a[11]*a[13] - a[3]*a[9]*a[14]) * determ;
  inv[2] = (a[1]*a[6]*a[15] + a[2]*a[7]*a[13] + a[3]*a[5]*a[14] - a[1]*a[7]*a[14] - a[2]*a[5]*a[15] - a[3]*a[6]*a[13]) * determ;
  inv[3] = (a[1]*a[7]*a[10] + a[2]*a[5]*a[11] + a[3]*a[6]*a[9] - a[1]*a[6]*a[11] - a[2]*a[7]*a[9] - a[3]*a[5]*a[10]) * determ;
  inv[4] = (a[4]*a[11]*a[14] + a[6]*a[8]*a[15] + a[7]*a[10]*a[12] - a[4]*a[10]*a[15] - a[6]*a[11]*a[12] - a[7]*a[8]*a[14]) * determ;
  inv[5] = (a[0]*a[10]*a[15] + a[2]*a[11]*a[12] + a[3]*a[8]*a[14] - a[0]*a[11]*a[14] - a[2]*a[8]*a[15] - a[3]*a[10]*a[12]) * determ;
  inv[6] = (a[0]*a[7]*a[14] + a[2]*a[4]*a[15] + a[3]*a[6]*a[12] - a[0]*a[6]*a[15] - a[2]*a[7]*a[12] - a[3]*a[4]*a[14]) * determ;
  inv[7] = (a[0]*a[6]*a[11] + a[2]*a[7]*a[8] + a[3]*a[4]*a[10] - a[0]*a[7]*a[10] - a[2]*a[4]*a[11] - a[3]*a[6]*a[8]) * determ;
  inv[8] = (a[4]*a[9]*a[15] + a[5]*a[11]*a[12] + a[7]*a[8]*a[13] - a[4]*a[11]*a[13] - a[5]*a[8]*a[15] - a[7]*a[9]*a[12]) * determ;
  inv[9] = (a[0]*a[11]*a[13] + a[1]*a[8]*a[15] + a[3]*a[9]*a[12] - a[0]*a[9]*a[15] - a[1]*a[11]*a[12] - a[3]*a[8]*a[13]) * determ;
  inv[10] = (a[0]*a[5]*a[15] + a[1]*a[7]*a[12] + a[3]*a[4]*a[13] - a[0]*a[7]*a[13] - a[1]*a[4]*a[15] - a[3]*a[5]*a[12]) *  determ;
  inv[11] = (a[0]*a[7]*a[9] + a[1]*a[4]*a[11] + a[3]*a[5]*a[8] - a[0]*a[5]*a[11] - a[1]*a[7]*a[8] - a[3]*a[4]*a[9]) * determ;
  inv[12] = (a[4]*a[10]*a[13] + a[5]*a[8]*a[14] + a[6]*a[9]*a[12] - a[4]*a[9]*a[14] - a[5]*a[10]*a[12] - a[6]*a[8]*a[13]) * determ;
  inv[13] = (a[0]*a[9]*a[14] + a[1]*a[10]*a[12] + a[2]*a[8]*a[13] - a[0]*a[10]*a[13] - a[1]*a[8]*a[14] - a[2]*a[9]*a[12]) * determ;
  inv[14] = (a[0]*a[6]*a[13] + a[1]*a[4]*a[14] + a[2]*a[5]*a[12] - a[0]*a[5]*a[14] - a[1]*a[6]*a[12] - a[2]*a[4]*a[13]) * determ;
  inv[15] = (a[0]*a[5]*a[10] + a[1]*a[6]*a[8] + a[2]*a[4]*a[9] - a[0]*a[6]*a[9] - a[1]*a[4]*a[10] - a[2]*a[5]*a[8]) * determ;
}

double complexNorm_f64(double r, double i) {
  return sqrt(r*r+i*i);
}

float complexNorm_f32(float r, float i) {
  return sqrt(r*r+i*i);
}

void storeTime(int i, double value) {
  simit::ir::TimerStorage::getInstance().storeTime(i, value);
}

double simitClock() {
  using namespace std::chrono;
  auto t = high_resolution_clock::now();
  time_point<high_resolution_clock,microseconds> usec = time_point_cast<microseconds>(t);
  return (double)(usec.time_since_epoch().count());
}
} // extern "C"

/// Temporary external spmm implementation until Simit supports assembling
/// matrix indices during computation.
template <typename Float>
int spmm(int Bn,  int Bm,  int* Browptr, int* Bcolidx,
         int Bnn, int Bmm, Float* Bvals,
         int Cn,  int Cm,  int* Crowptr, int* Ccolidx,
         int Cnn, int Cmm, Float* Cvals,
         int An,  int Am,  int** Arowptr, int** Acolidx,
         int Ann, int Amm, Float** Avals) {
#ifdef EIGEN
  auto B = csr2eigen<Float,RowMajor>(Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals);
  auto C = csr2eigen<Float,RowMajor>(Cn, Cm, Crowptr, Ccolidx, Cnn, Cmm, Cvals);

  SparseMatrix<Float,RowMajor> A(An, Am);
  A = B*C;
  eigen2csr(A, An, Am, Arowptr, Acolidx, Ann, Amm, Avals);
#else
  simit_ierror << "extern spmm requires Eigen";
#endif
  return 0;
}
extern "C" int sspmm(int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                     int Bnn, int Bmm, float* Bvals,
                     int Cn,  int Cm,  int* Crowptr, int* Ccolidx,
                     int Cnn, int Cmm, float* Cvals,
                     int An,  int Am,  int** Arowptr, int** Acolidx,
                     int Ann, int Amm, float** Avals) {
  return spmm(Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals,
              Cn, Cm, Crowptr, Ccolidx, Cnn, Cmm, Cvals,
              An, Am, Arowptr, Acolidx, Ann, Amm, Avals);
}
extern "C" int dspmm(int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                     int Bnn, int Bmm, double* Bvals,
                     int Cn,  int Cm,  int* Crowptr, int* Ccolidx,
                     int Cnn, int Cmm, double* Cvals,
                     int An,  int Am,  int** Arowptr, int** Acolidx,
                     int Ann, int Amm, double** Avals) {
  return spmm(Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals,
              Cn, Cm, Crowptr, Ccolidx, Cnn, Cmm, Cvals,
              An, Am, Arowptr, Acolidx, Ann, Amm, Avals);
}

// Solvers
#define SOLVER_ERROR                                            \
do {                                                            \
  simit_ierror << "Solvers require that Simit was built with Eigen."; \
} while (false)

template <typename Float>
void solve(int n,  int m,  int* rowptr, int* colidx,
           int nn, int mm, Float* Avals, Float* bvals, Float* xvals) {
#ifdef EIGEN
  auto A = csr2eigen<Float,ColMajor>(n, m, rowptr, colidx, nn, mm, Avals);
  auto x = new Map<Matrix<Float,Dynamic,1>>(xvals, m);
  auto b = new Map<Matrix<Float,Dynamic,1>>(bvals, n);

  SparseLU<SparseMatrix<Float, ColMajor>> solver;
  solver.compute(A);
  *x = solver.solve(*b);
#else
  SOLVER_ERROR;
#endif
}
extern "C" void cMatSolve_f64(int n,  int m,  int* rowptr, int* colidx,
                              int nn, int mm, double* A, double* x, double* b) {
  return solve(n, m, rowptr, colidx, nn, mm, A, x, b);
}
extern "C" void cMatSolve_f32(int n,  int m,  int* rowptr, int* colidx,
                              int nn, int mm, float* A, float* x, float* b) {
  return solve(n, m, rowptr, colidx, nn, mm, A, x, b);
}

/// LU factorization. Returns a solver object that can be used with
/// `lusolve` and `lumatsolve`. The solver object must be freed using
/// `lufree`.
template <typename Float>
int lu(int An,  int Am,  int* Arowptr, int* Acolidx,
       int Ann, int Amm, Float* Avals,
       void** solverPtr) {
#ifdef EIGEN
  auto A = csr2eigen<Float,Eigen::ColMajor>(An, Am, Arowptr, Acolidx,
                                            Ann, Amm, Avals);
  auto solver = new SparseLU<SparseMatrix<Float,ColMajor>>();
  solver->compute(A);
  *solverPtr = static_cast<void*>(solver);
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int slu(int An,  int Am,  int* Arowptr, int* Acolidx,
                   int Ann, int Amm, float* Avals,
                   void** solver) {
  return lu(An, Am, Arowptr, Acolidx, Ann, Amm, Avals, solver);
}
extern "C" int dlu(int An,  int Am,  int* Arowptr, int* Acolidx,
                   int Ann, int Amm, double* Avals,
                   void** solver) {
  return lu(An, Am, Arowptr, Acolidx, Ann, Amm, Avals, solver);
}

/// Free an LU solver.
template <typename Float>
int lufree(void** solverPtr) {
#ifdef EIGEN
  auto solver=static_cast<SparseLU<SparseMatrix<Float,ColMajor>>*>(*solverPtr);
  delete solver;
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int slufree(void** solverPtr) {
  return lufree<float>(solverPtr);
}
extern "C" int dlufree(void** solverPtr){
  return lufree<double>(solverPtr);
}

/// Solve `t=L^{-1}*b` and `x=L'^{-1}*t`, where `A=LL'` is the matrix that was
/// factorized with the provided solver using `chol`.
template <typename Float>
int lusolve(void** solverPtr, int nb, Float *bvals, int nx, Float *xvals) {
#ifdef EIGEN
  auto solver=static_cast<SparseLU<SparseMatrix<Float,ColMajor>>*>(*solverPtr);
  auto b = dense2eigen(nb, bvals);
  auto x = Eigen::Matrix<Float,Eigen::Dynamic,1>(nx);
  x = solver->solve(b);
  for (int i=0; i<nx; ++i) {
    xvals[i] = x(i);
  }
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" {
  int slusolve(void** solverPtr, int bn, float *bvals, int xn, float *xvals) {
    return lusolve(solverPtr, bn, bvals, xn, xvals);
  }
  int dlusolve(void** solverPtr, int bn, double *bvals, int xn, double *xvals){
    return lusolve(solverPtr, bn, bvals, xn, xvals);
  }
}

/// Solve `T=L^{-1}*B` and `X=L'^{-1}*T`, where `A=LL'` is the matrix that was
/// factorized with the provided solver using `chol`.
template <typename Float>
int lumatsolve(void** solverPtr,
                int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                int Bnn, int Bmm, Float* Bvals,
                int Xn,  int Xm,  int** Xrowptr, int** Xcolidx,
                int Xnn, int Xmm, Float** Xvals){
#ifdef EIGEN
  auto solver=static_cast<SparseLU<SparseMatrix<Float,ColMajor>>*>(*solverPtr);
  auto B = csr2eigen<Float,ColMajor>(Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals);
  SparseMatrix<Float> X(Xn, Xm);
  X = solver->solve(B);
  X = X.transpose();
  eigen2csr<Float>(X, Xn, Xm, Xrowptr, Xcolidx, Xnn, Xmm, Xvals);
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int slumatsolve(void** solverPtr,
                            int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                            int Bnn, int Bmm, float* Bvals,
                            int Xn,  int Xm,  int** Xrowptr, int** Xcolidx,
                            int Xnn, int Xmm, float** Xvals) {
  return lumatsolve(solverPtr,
                     Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals,
                     Xn, Xm, Xrowptr, Xcolidx, Xnn, Xmm, Xvals);
}
extern "C" int dlumatsolve(void** solverPtr,
                            int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                            int Bnn, int Bmm, double* Bvals,
                            int Xn,  int Xm,  int** Xrowptr, int** Xcolidx,
                            int Xnn, int Xmm, double** Xvals) {
  return lumatsolve(solverPtr,
                     Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals,
                     Xn, Xm, Xrowptr, Xcolidx, Xnn, Xmm, Xvals);
}

/// Cholesky factorization. Returns a solver object that can be used with
/// `lltsolve` and `lltmatsolve`. The solver object must be freed using
/// `cholfree`.
template <typename Float>
int chol(int An,  int Am,  int* Arowptr, int* Acolidx,
         int Ann, int Amm, Float* Avals,
         void** solverPtr) {
#ifdef EIGEN
  auto A = csr2eigen<Float,Eigen::ColMajor>(An, Am, Arowptr, Acolidx,
                                            Ann, Amm, Avals);
  auto solver = new SimplicialCholesky<SparseMatrix<Float>>();
  solver->compute(A);
  *solverPtr = static_cast<void*>(solver);
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int schol(int An,  int Am,  int* Arowptr, int* Acolidx,
                     int Ann, int Amm, float* Avals,
                     void** solver) {
  return chol(An, Am, Arowptr, Acolidx, Ann, Amm, Avals, solver);
}
extern "C" int dchol(int An,  int Am,  int* Arowptr, int* Acolidx,
                     int Ann, int Amm, double* Avals,
                     void** solver) {
  return chol(An, Am, Arowptr, Acolidx, Ann, Amm, Avals, solver);
}

/// Free a Cholesky solver.
template <typename Float>
int cholfree(void** solverPtr) {
#ifdef EIGEN
  auto solver=static_cast<SimplicialCholesky<SparseMatrix<Float>>*>(*solverPtr);
  delete solver;
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int scholfree(void** solverPtr) {
  return cholfree<float>(solverPtr);
}
extern "C" int dcholfree(void** solverPtr){
  return cholfree<double>(solverPtr);
}

/// Solve `t=L^{-1}*b` and `x=L'^{-1}*t`, where `A=LL'` is the matrix that was
/// factorized with the provided solver using `chol`.
template <typename Float>
int lltsolve(void** solverPtr, int nb, Float *bvals, int nx, Float *xvals) {
#ifdef EIGEN
  auto solver=static_cast<SimplicialCholesky<SparseMatrix<Float>>*>(*solverPtr);
  auto b = dense2eigen(nb, bvals);
  auto x = Eigen::Matrix<Float,Eigen::Dynamic,1>(nx);
  x = solver->solve(b);
  for (int i=0; i<nx; ++i) {
    xvals[i] = x(i);
  }
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" {
int slltsolve(void** solverPtr, int bn, float *bvals, int xn, float *xvals) {
  return lltsolve(solverPtr, bn, bvals, xn, xvals);
}
int dlltsolve(void** solverPtr, int bn, double *bvals, int xn, double *xvals){
  return lltsolve(solverPtr, bn, bvals, xn, xvals);
}
}

/// Solve `T=L^{-1}*B` and `X=L'^{-1}*T`, where `A=LL'` is the matrix that was
/// factorized with the provided solver using `chol`.
template <typename Float>
int lltmatsolve(void** solverPtr,
                 int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                 int Bnn, int Bmm, Float* Bvals,
                 int Xn,  int Xm,  int** Xrowptr, int** Xcolidx,
                 int Xnn, int Xmm, Float** Xvals){
#ifdef EIGEN
  auto solver=static_cast<SimplicialCholesky<SparseMatrix<Float,ColMajor>>*>(*solverPtr);
  auto B = csr2eigen<Float,ColMajor>(Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals);
  SparseMatrix<Float> X(Xn, Xm);
  X = solver->solve(B);
  X = X.transpose();
  eigen2csr<Float>(X, Xn, Xm, Xrowptr, Xcolidx, Xnn, Xmm, Xvals);
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int slltmatsolve(void** solverPtr,
                            int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                            int Bnn, int Bmm, float* Bvals,
                            int Xn,  int Xm,  int** Xrowptr, int** Xcolidx,
                            int Xnn, int Xmm, float** Xvals) {
  return lltmatsolve(solverPtr,
                      Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals,
                      Xn, Xm, Xrowptr, Xcolidx, Xnn, Xmm, Xvals);
}
extern "C" int dlltmatsolve(void** solverPtr,
                            int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                            int Bnn, int Bmm, double* Bvals,
                            int Xn,  int Xm,  int** Xrowptr, int** Xcolidx,
                            int Xnn, int Xmm, double** Xvals) {
  return lltmatsolve(solverPtr,
                      Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals,
                      Xn, Xm, Xrowptr, Xcolidx, Xnn, Xmm, Xvals);
}

/// cross product between 2 vectors3D
template <typename Float>
void cross(int an, Float* a, int bn, Float* b, int cn, Float* c){
  assert(an==3 && bn==3);
  c[0] = a[1]*b[2]-a[2]*b[1];
  c[1] = a[2]*b[0]-a[0]*b[2];
  c[2] = a[0]*b[1]-a[1]*b[0];
}
extern "C" void scross(int an, float* a, int bn, float* b, int cn, float* c) {
  return cross(an, a, bn, b, cn, c);
}
extern "C" void dcross(int an, double* a, int bn, double* b, int cn, double* c) {
  return cross(an, a, bn, b, cn, c);
}

template <typename Float>
int triangularSolve(int An,  int Am,  int* Arowptr, int* Acolidx,
			     int Ann, int Amm, Float* Avals,
				 int nb, Float *bvals, int nx, Float *xvals) {
#ifdef EIGEN
  auto A = csr2eigen<Float,Eigen::ColMajor>(An, Am, Arowptr, Acolidx,
                                            Ann, Amm, Avals);
  auto b = dense2eigen(nb, bvals);
  auto x = Eigen::Matrix<Float,Eigen::Dynamic,1>(nx);
  x = TriangularView<SparseMatrix<Float,ColMajor>,Lower>(A).solve(b);
  for (int i=0; i<nx; ++i) {
    xvals[i] = x(i);
  }
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int striangularSolve(int An,  int Am,  int* Arowptr, int* Acolidx,
                   int Ann, int Amm, float* Avals,
				   int nb, float *bvals, int nx, float *xvals) {
  return triangularSolve(An, Am, Arowptr, Acolidx, Ann, Amm,
                         Avals, nb, bvals, nx, xvals);
}
extern "C" int dtriangularSolve(int An,  int Am,  int* Arowptr, int* Acolidx,
                   int Ann, int Amm, double* Avals,
				   int nb, double *bvals, int nx, double *xvals) {
  return triangularSolve(An, Am, Arowptr, Acolidx, Ann, Amm,
                         Avals, nb, bvals, nx, xvals);
}


#include "exception.hh"
#include "network.hh"
#include "timer.hh"

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

#include <sys/resource.h>
#include <sys/time.h>

using namespace std;
using namespace Eigen;

constexpr size_t batch_size = 1;
constexpr size_t input_size = 64;
// epsilon for computation of numerical gradients
constexpr double grad_epsilon = 1e-5;
// max allowable absolute difference in numerical and backprop gradients
constexpr double compare_epsilon = 1e-5;
// max allowable percentage difference in numerical and backprop gradients
constexpr double percentage_error_epsilon = 1e-3;

void program_body()
{
  /* remove limit on stack size */
  const rlimit limits { RLIM_INFINITY, RLIM_INFINITY };
  CheckSystemCall( "setrlimit", setrlimit( RLIMIT_STACK, &limits ) );

  /* seed C RNG for Eigen random weight initialization */
  // srand( Timer::timestamp_ns() );
  srand( 0 );

  /* construct neural network on heap */
  auto nn = make_unique<Network<double, batch_size, input_size, 64, 64, 64, 64, 64, 64, 64, 64, 1>>();
  nn->initializeWeightsRandomly();

  srand( 10 );
  /* initialize inputs */
  Matrix<double, batch_size, input_size> input = Matrix<double, batch_size, input_size>::Random();

  /* forward prop */
  nn->apply( input );

  /* back prop */
  nn->computeDeltas();
  nn->evaluateGradients( input );

  /* compare back prop results with numerical gradients */
  bool errorsOverThreshold = false;
  double maxDiff = 0.0;
  double maxPercentageError = 0.0;
  unsigned int numLayers = nn->getNumLayers();
  for ( unsigned int layerNum = 0; layerNum < numLayers; layerNum++ ) {
    unsigned int numParams = nn->getNumParams( layerNum );
    for ( unsigned int paramNum = 0; paramNum < numParams; paramNum++ ) {
      double formulaGradient = nn->getEvaluatedGradient( layerNum, paramNum );
      double numericalGradient = nn->calculateNumericalGradient( input, layerNum, paramNum, grad_epsilon );
      double diff = abs( formulaGradient - numericalGradient );
      double percentageError = diff / max( abs( formulaGradient ), abs( numericalGradient ) );
      maxDiff = max( diff, maxDiff );
      maxPercentageError = max( percentageError, maxPercentageError );
      if ( ( diff > compare_epsilon ) and ( percentageError > percentage_error_epsilon ) ) {
        errorsOverThreshold = true;
        cout << "Error in Layer " << layerNum << ", Param " << paramNum << endl;
        cout << formulaGradient << " " << numericalGradient << endl;
        cout << "diff: " << diff << ", %diff: " << percentageError << endl;
        cout << endl;
      }
    }
  }
  cout << endl;
  cout << "Params: grad_epsilon " << grad_epsilon << ", compare_epsilon " << compare_epsilon
       << ", percentage_error_epsilon " << percentage_error_epsilon << endl;
  cout << "maxDiff: " << maxDiff << endl << "maxPercentageError: " << maxPercentageError << endl << endl;
  if ( errorsOverThreshold ) {
    throw runtime_error( "test failure" );
  }
}

int main()
{
  try {
    program_body();
    return EXIT_SUCCESS;
  } catch ( const exception& e ) {
    cerr << e.what() << "\n";
    return EXIT_FAILURE;
  }
}


/*
 * This file is part of MXE. See LICENSE.md for licensing information.
 *
 * This code was originally found on:
 *   https://eigen.tuxfamily.org/dox/GettingStarted.html
 */

#include <Eigen/Dense>

using Eigen::MatrixXd;

int main()
{
    MatrixXd m(2,2);
    m(0,0) = 3;
    m(1,0) = 2.5;
    m(0,1) = -1;
    m(1,1) = m(1,0) + m(0,1);
    return 0;
}


//
// Copyright (c) 2016-2019 CNRS, INRIA
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/algorithm/finite-differences.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

using namespace pinocchio;
using namespace Eigen;

template<bool local>
Data::Matrix6x finiteDiffJacobian(const Model & model, Data & data, const Eigen::VectorXd & q, const Model::JointIndex joint_id)
{
  Data::Matrix6x res(6,model.nv); res.setZero();
  VectorXd q_integrate (model.nq);
  VectorXd v_integrate (model.nv); v_integrate.setZero();

  forwardKinematics(model,data,q);
  const SE3 oMi_ref = data.oMi[joint_id];

  const VectorXd fd_increment = finiteDifferenceIncrement(model);

  double eps = 1e-8;
  for(int k=0; k<model.nv; ++k)
  {
    // Integrate along kth direction
    eps = fd_increment[k];
    v_integrate[k] = eps;
    q_integrate = integrate(model,q,v_integrate);

    forwardKinematics(model,data,q_integrate);
    const SE3 & oMi = data.oMi[joint_id];

    if (local)
      res.col(k) = log6(oMi_ref.inverse()*oMi).toVector();
    else
      res.col(k) = oMi_ref.act(log6(oMi_ref.inverse()*oMi)).toVector();

    res.col(k) /= eps;

    v_integrate[k] = 0.;
  }

  return res;
}

template<typename Matrix>
void filterValue(MatrixBase<Matrix> & mat, typename Matrix::Scalar value)
{
  for(int k = 0; k < mat.size(); ++k)
    mat.derived().data()[k] =  math::fabs(mat.derived().data()[k]) <= value?0:mat.derived().data()[k];
}

struct FiniteDiffJoint
{
  template<typename JointModel>
  static void init (JointModelBase<JointModel> & /*jmodel*/) {}

  template<typename JointModel>
  void operator()(JointModelBase<JointModel> & jmodel) const
  {
    typedef typename JointModel::ConfigVector_t CV;
    typedef typename JointModel::TangentVector_t TV;
    typedef typename LieGroup<JointModel>::type LieGroupType;

    init(jmodel); jmodel.setIndexes(0,0,0);
    typename JointModel::JointDataDerived jdata = jmodel.createData();
    CV q; LieGroupType().random(q);
    jmodel.calc(jdata,q);
    SE3 M_ref(jdata.M);

    CV q_int(jmodel.nq());
    TV v(jmodel.nv()); v.setZero();
    double eps = 1e-4;

    Eigen::Matrix<double,6,JointModel::NV> S(6,jmodel.nv()), S_ref(jdata.S.matrix());

    eps = jmodel.finiteDifferenceIncrement();
    for(int k=0;k<jmodel.nv();++k)
    {
      v[k] = eps;
      q_int = LieGroupType().integrate(q,v);
      jmodel.calc(jdata,q_int);
      SE3 M_int = jdata.M;

      S.col(k) = log6(M_ref.inverse()*M_int).toVector();
      S.col(k) /= eps;

      v[k] = 0.;
    }

    BOOST_CHECK(S.isApprox(S_ref,eps*1e1));
    std::cout << "name: " << jmodel.classname() << std::endl;
    std::cout << "S_ref:\n" << S_ref << std::endl;
    std::cout << "S:\n" << S << std::endl;
  }
};

template<>
void FiniteDiffJoint::init<JointModelRevoluteUnaligned>(JointModelBase<JointModelRevoluteUnaligned> & jmodel)
{
  jmodel.derived().axis.setRandom(); jmodel.derived().axis.normalize();
}

template<>
void FiniteDiffJoint::init<JointModelPrismaticUnaligned>(JointModelBase<JointModelPrismaticUnaligned> & jmodel)
{
  jmodel.derived().axis.setRandom(); jmodel.derived().axis.normalize();
}

template<>
void FiniteDiffJoint::init<JointModelComposite>(JointModelBase<JointModelComposite> & jmodel)
{
  jmodel.derived().addJoint(JointModelRX());
  jmodel.derived().addJoint(JointModelRZ());
}

template<>
void FiniteDiffJoint::operator()< JointModelComposite > (JointModelBase<JointModelComposite> & ) const
{
  // DO NOT CHECK BECAUSE IT IS NOT WORKINK YET - TODO
//  typedef typename JointModel::ConfigVector_t CV;
//  typedef typename JointModel::TangentVector_t TV;
//
//  pinocchio::JointModelComposite jmodel((pinocchio::JointModelRX())/*, (pinocchio::JointModelRY())*/);
//  jmodel.setIndexes(0,0,0);
//
//  pinocchio::JointModelComposite::JointDataDerived jdata = jmodel.createData();
//
//  CV q = jmodel.random();
//  jmodel.calc(jdata,q);
//  SE3 M_ref(jdata.M);
//
//  CV q_int;
//  TV v(Eigen::VectorXd::Random(jmodel.nv())); v.setZero();
//  double eps = 1e-4;
//
//  assert(q.size() == jmodel.nq()&& "nq false");
//  assert(v.size() == jmodel.nv()&& "nv false");
//  Eigen::MatrixXd S(6,jmodel.nv()), S_ref(ConstraintXd(jdata.S).matrix());
//
//  eps = jmodel.finiteDifferenceIncrement();
//  for(int k=0;k<jmodel.nv();++k)
//  {
//    v[k] = eps;
//    q_int = jmodel.integrate(q,v);
//    jmodel.calc(jdata,q_int);
//    SE3 M_int = jdata.M;
//
//    S.col(k) = log6(M_ref.inverse()*M_int).toVector();
//    S.col(k) /= eps;
//
//    v[k] = 0.;
//  }
//
//  std::cout << "S\n" << S << std::endl;
//  std::cout << "S_ref\n" << S_ref << std::endl;
  // BOOST_CHECK(S.isApprox(S_ref,eps*1e1)); //@TODO Uncomment to test once JointComposite maths are ok
}

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE(increment)
{
  typedef double Scalar;

  Model model;
  buildModels::humanoidRandom(model);

  VectorXd fd_increment(model.nv);
  fd_increment = finiteDifferenceIncrement(model);

  for(int k=0; k<model.nv; ++k)
  {
    BOOST_CHECK(fd_increment[k] > Eigen::NumTraits<Scalar>::epsilon());
    BOOST_CHECK(fd_increment[k] < 1e-3);
  }
}

BOOST_AUTO_TEST_CASE (test_S_finit_diff)
{
  boost::mpl::for_each<JointModelVariant::types>(FiniteDiffJoint());
}

BOOST_AUTO_TEST_CASE (test_jacobian_vs_finit_diff)
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model);

  const VectorXd fd_increment = finiteDifferenceIncrement(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment<4>(3).normalize();
  computeJointJacobians(model,data,q);

  Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
  Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);

  getJointJacobian(model,data,idx,WORLD,Jrh);
  Data::Matrix6x Jrh_finite_diff = finiteDiffJacobian<false>(model,data,q,idx);
  BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,fd_increment.maxCoeff()*1e1));

  getJointJacobian(model,data,idx,LOCAL,Jrh);
  Jrh_finite_diff = finiteDiffJacobian<true>(model,data,q,idx);
  BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,fd_increment.maxCoeff()*1e1));
}

BOOST_AUTO_TEST_SUITE_END()


// Some sort of comment up top describing the code
// This is a 1D Euler equation solver using the finite volume method
// Uses MUSCL scheme
// Think about what I want to use for time integration.

// This is the command needed to compile the code
// g++ -I ../../../eigen-3.4.0/ main.cpp fvm_1d_functions.cpp -o runSim.out && ./runSim.out

#include <iostream>
#include <Eigen/Dense>
#include "fvm_1D_functions.h"

using namespace std;
using namespace Eigen;

int main() {

    // Set physical constants
    const double gasGamma = 5./3.;

    // Set important constants for code
    const int maxIter = 100;            // The maximum number of iterations
    const double CFL = 0.9;             // Set CFL number

    // Set grid parameters
    const int nCells = 4;      // Number of cells in the domain. Note these set based on their cell centered values
    const int numGhost = 2;      // Number of ghost cells on either side of the domain

    // Set domain parameters
    const double xLeft = 0;   // Left boundary
    const double xRight = 1;   // Right boundary

    // Calculate grid parameters
    const int nNodes = nCells + 1;  // There is one more node than there are cells
    const int totalNCells = nCells + 2*numGhost;   // Total number of cells, including ghost cells
    const int totalNNodes = totalNCells + 1; // Total number of nodes is one plus total number of cells

    // Calculate domain length
    const double L = xRight - xLeft; // Get total domain length

    // Calculate dx
    const double dx = L / nCells;

    // Make the cell centered grid
    ArrayXd x = makeGrid(totalNCells, numGhost, xLeft, dx);

    // Set initial conditions based on Sod paper on shock tube simulations
    // See https://doi.org/10.1016/0021-9991(78)90023-2
    // The simulation is done in normalized units
    // The initial states are presented in primitive variables (rho, u, p)
    Array3d leftStates;
    leftStates << 1.0, 0.0, 1.0;
    Array3d rightStates;
    rightStates << 0.125, 0.0, 0.1;

    // Set initial shock location
    double x0 = 0.5;

    // Initialize an array of primitive variables
    Array<ArrayXd, 3, 1> V;

    // There is an interface at some x0
    // To the left is one set of states, to the right is another set of states
    // Loop through the variables
    for (int var = 0; var < 3; var++) {
        // Initialize an array of zeros
        V(var) = ArrayXd::Zero(totalNCells);

        // Iterate through x
        for (int i = 0; i < totalNCells; i++) {
            // If we are less than x0, then the IC is the left state.
            // Note that this is a simplified IC in that it will move the initial shock location based on the resolution. This should have a minimal impact on sufficiently resolved sims
            if (x(i) <= x0) {
                V(var)(i) = leftStates(var);
            }
            else {
                V(var)(i) = rightStates(var);
            }
        }
    }

    // Maybe think about a way to specify what BCs to use here.

    // Initialize the conserved variables
    Array<ArrayXd, 3, 1> Q;
    for (int var = 0; var < 3; var++) {
        // Initialize an array of zeros
        Q(var) = ArrayXd::Zero(totalNCells);
    }

    // Calculate the conserved variables based on the primitive variables
    prim2cons(gasGamma, V, Q);

    // Next, start populating the run sim function
    // Choose which ode solver to use and figure out a nice way to do the time integration as a loop
    // Look into SSPRK methods. maybe 4 stage 3rd order? Gets us a nice CFL condition

    // Think about how I want to setup my variables. Do I use rho, u, p or one big variable? Maybe the big variable is the way to go?
    // Each conservation law is the same general idea, just the values internally will be different

    // Maybe put everything below this in some sort of "runSim" function?
    //runSimulation_FVM1D(gasGamma, maxIter, nCells, numGhost, xLeft, xRight);
    // std::cout << "Hello World!" << endl;

    Array<ArrayXd, 3, 1> test;

    test(0) = ArrayXd::Zero(5) + 5.;
    test(1) = ArrayXd::Zero(5) + 1.;
    test(2) = ArrayXd::Zero(5) + 2.;

    //test.setConstant(1.1);

    cout << CFL * dx / (abs(V(1)) + pow(gasGamma*V(2)/V(0),0.5)).minCoeff() << endl;

}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/FFT.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../util/PeakDetection.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>

namespace fluid {
namespace algorithm {

class YINFFT
{

public:
  void processFrame(const RealVectorView& input, RealVectorView output,
                    double minFreq, double maxFreq, double sampleRate)
  {
    using namespace Eigen;
    PeakDetection pd;
    ArrayXd       mag = _impl::asEigen<Array>(input);
    ArrayXd       squareMag = mag.square();
    index         nBins = mag.size();
    FFT           fft(2 * (mag.size() - 1));
    double        squareMagSum = 2 * squareMag.sum();
    ArrayXd       squareMagSym(2 * (nBins - 1));
    squareMagSym << squareMag[0], squareMag.segment(1, nBins - 1),
        squareMag.segment(1, nBins - 2).reverse();
    ArrayXcd squareMagFFT = fft.process(squareMagSym);
    ArrayXd  yin = squareMagSum - squareMagFFT.real();
    if (maxFreq == 0) maxFreq = 1;
    if (minFreq == 0) minFreq = 1;
    yin(0) = 1;
    double tmpSum = 0;
    for (index i = 1; i < nBins; i++)
    {
      tmpSum += yin(i);
      yin(i) *= i / tmpSum;
    }
    double pitch = 0;
    double pitchConfidence = 0;
    if (tmpSum > 0)
    {
      ArrayXd yinFlip = -yin;
      // segment from max to min freq
      index minBin = std::lrint(sampleRate / maxFreq);
      index maxBin = std::lrint(sampleRate / minFreq);
      if (minBin > yinFlip.size() - 1) minBin = yinFlip.size() - 1;
      if (maxBin > yinFlip.size() - minBin - 1)
        maxBin = yinFlip.size() - minBin - 1;
      if (maxBin > minBin)
      {
        yinFlip = yinFlip.segment(minBin, maxBin - minBin);

        auto vec = pd.process(yinFlip, 1, yinFlip.minCoeff());
        if (vec.size() > 0)
        {
          pitch = sampleRate / (minBin + vec[0].first);
          pitchConfidence = std::max(1. + vec[0].second, 0.);
        }
      }
    }
    output(0) = pitch;
    output(1) = pitchConfidence;
  }
};
} // namespace algorithm
} // namespace fluid


#include <iostream>
#include <fstream>
#include <array>
#include <random>
#include <armadillo>

using namespace std;
using namespace arma;

void MonteCarlo(vec &players, int MCSteps, int N, int transactions, double lambda, double alpha, double gamma, ofstream &outFile, vec &binCounts, double binSize, double m0, ofstream &outFileErr);
void outPut(vec &players, int MCSteps, int N, int transactions, mat &expectVal);
double findVariance(vec &players, int transaction, int N, double m0);
void makeBins(vec &players, vec &binCount, double binSize);

int main(int argc, char *argv[])
{
    if (argc < 8){
        cout << "To few arguments given. Expected number of persons, Monte Carlo cycles, transactions, start money, lambda, alpha and gamma" << endl;
    }
    int N = stoi(argv[1]);
    int MCSteps = stoi(argv[2]);
    int transactions = stoi(argv[3]);

    double startMoney = stod(argv[4]);
    double lambda = stod(argv[5]);
    double alpha = stod(argv[6]);
    double gamma = stod(argv[7]);

    double binSize = 0.01*startMoney;

    ofstream outFileVar = ofstream("variance.txt");
    ofstream outFileErr = ofstream("distError.txt");

    //outFileVar.open("variance.txt");
    ofstream outFileParameter;
    outFileParameter.open("parameters.txt");

    ofstream binParameters = ofstream("binParameters.txt");

    outFileParameter << "N " << N << "\n";
    outFileParameter << "MCSteps " << MCSteps << "\n";
    outFileParameter << "Trasactions " << transactions << "\n";
    outFileParameter << "StartingMoney " << startMoney << "\n";
    outFileParameter << "Lambda " << lambda << "\n";
    outFileParameter << "Alpha " << alpha << "\n";
    outFileParameter << "Gamma " << gamma;

    double binEnd;
    if (alpha > 0 || gamma > 0){
        binEnd = 2*startMoney/(sqrt(lambda + 0.1)) + startMoney;
    }
    else{
        binEnd = 2*startMoney/(sqrt(lambda + 0.1)) + startMoney;
    }

    int binNum = int(binEnd/double(binSize));
    cout << binNum << endl;
    vec binCounts = zeros(binNum);
    vec players = ones(N)*startMoney;

    cout << alpha << " " << gamma << endl;

    binParameters << MCSteps << " " << N << " " << startMoney << " " << binSize << " " << binNum << " " << (binEnd) << " " << lambda << " " << alpha << " " << gamma << endl;
    binParameters.close();

    MonteCarlo(players, MCSteps,  N,transactions,lambda,alpha,gamma,outFileVar,binCounts,binSize,startMoney,outFileErr);
    //outPut(players, MCSteps, N, transactions, expectVal);

    binCounts.save("bins.bin",raw_binary);
    players.save("data.bin",raw_binary);

    outFileErr.close();
    outFileVar.close();

    //players.save("data.bin",raw_binary);

    cout << "Finished" << endl;

}

void MonteCarlo(vec &players, int MCSteps, int N, int transactions, double lambda,double alpha,double gamma, ofstream &outFile, vec &binCounts,double binSize,double m0,ofstream &outFileErr){

    random_device rd;
    mt19937_64 gen(rd());

    uniform_real_distribution<double> distribution(0.0,N);
    uniform_real_distribution<double> eps(0.0,1.0);

    int writingFreq = 100;
    double p = 0;

    mat c = zeros(N,N);
    double maxTransactions = 1;

    for (int i = 0; i < MCSteps; i++){

        players.fill(m0);

        for (int j = 0; j < transactions; j++){
            int index_i = distribution(gen);
            int index_j = distribution(gen);

            double epsFac = eps(gen);

            if (players(index_i) - players(index_j) == 0){
                p = 1.;
            }
            else{
                p = 2*pow(fabs((players(index_i) - players(index_j))/double(m0)),-alpha)*(pow((c(index_i,index_j)+1)/(maxTransactions+1),gamma));
            }

            if (eps(gen) < p && (index_i != index_j)){

                double m1 = lambda*players(index_i) + (1-lambda)*epsFac*    (players(index_i) + players(index_j));
                double m2 = lambda*players(index_j) + (1-lambda)*(1-epsFac)*(players(index_i) + players(index_j));

                //cout << "hei" << endl;

                players(index_i) = m1;
                players(index_j) = m2;

                c(index_j,index_i) += 1;
                c(index_i,index_j) += 1;

                if (c(index_j,index_i) > maxTransactions){
                    maxTransactions = c(index_j,index_i);
                }

                else if (c(index_i,index_j) > maxTransactions){
                    maxTransactions = c(index_i,index_j);
                }

            }

            if (MCSteps == 1){
                if (j%writingFreq == 0){

                    double mean = 0;
                    for (int i = 0; i < N; i++){
                        mean += players(i)/m0;

                    }

                    mean /= (N);

                    outFile << (j+1) << " " << findVariance(players,j+1,N,m0) << " " << mean << "\n";
                }

            }
        }

    vec tempCounts = binCounts;
    makeBins(players,binCounts,binSize);

    if (i > 1){
        outFileErr << i+1 << " " << norm(tempCounts/double(i-1) - binCounts/(double(i)) ) << endl;
    }

    }

}

//void outPut(vec &players, int MCSteps, int N, int transactions, mat &expectVal){

//    vec means = zeros(N);
//    for (int k = 0; k < N;k++){
//        means(k) = expectVal(0,k) / MCSteps;
//    }

//    means.save("data.bin",raw_binary);
//}

double findVariance(vec &players, int transaction, int N,double m0){

    double mean = 0;
    double secondMoment = 0;

    for (int i = 0; i < N; i++){
        mean += players(i)/m0;
        secondMoment += players(i)/m0*players(i)/m0;
    }

    mean /= (N);
    //cout << secondMoment << endl;
    secondMoment /= (N);

    return (secondMoment - mean*mean);

}

void makeBins(vec &players, vec &binCount, double binSize){

    for (int i = 0; i < players.size();i++){
        for (int j = 0; j < binCount.size();j++){
            if(players(i)> (j-1)*binSize && players(i)< (j)*binSize){
                binCount(j) += 1;
            }
        }
    }

    //binCount.print();

}


/* boost-supplement random/discrete_distribution.hpp header file
 *
 * Copyright (C) 2008 Kenta Murata.
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id: discrete_distribution.hpp 5906 2008-01-30 15:09:35Z mrkn $
 *
 */

#ifndef BOOST_SUPPLEMENT_ZIPF_DISTRIBUTION_HPP
#define BOOST_SUPPLEMENT_ZIPF_DISTRIBUTION_HPP 1

#include <boost/random/discrete_distribution.hpp>

#include <cmath>

namespace boost { namespace random {

// Zipf-Mandelbrot distribution
//
// Let N, q, and s be num, shift, and exp, respectively.  The
// probability distribution is P(k) = (k + q)^{-s} / H_{N,q,s} where k
// = 1, 2, ..., N, and H_{N,q,s} is generalized harmonic number, that
// is H_{N,q,s} = \sum_{i=1}^N (i+q)^{-s}.
//
// http://en.wikipedia.org/wiki/Zipf-Mandelbrot_law.
template<class IntType = long, class RealType = double>
class zipf_distribution
{
public:
  typedef RealType input_type;
  typedef IntType result_type;

#if !defined(BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS) && !(defined(BOOST_MSVC) && BOOST_MSVC <= 1300)
  BOOST_STATIC_ASSERT(std::numeric_limits<IntType>::is_integer);
  BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif

private:
  result_type num_;
  input_type shift_;
  input_type exp_;

  typedef discrete_distribution<IntType, RealType> dist_type;
  dist_type dist_;

  dist_type make_dist(result_type num, input_type shift, input_type exp)
    {
      std::vector<input_type> buffer(num);
      for (result_type k = 1; k <= num; ++k)
        buffer[k-1] = std::pow(k + shift, -exp);
      return dist_type(buffer.begin(), buffer.end());
    }

public:
  zipf_distribution(result_type num, input_type shift, input_type exp)
    : num_(num), shift_(shift), exp_(exp),
      dist_(make_dist(num, shift, exp))
    {}

  result_type num() const { return num_; }

  input_type shift() const { return shift_; }

  input_type exponent() const { return exp_; }

  template<class Engine>
  result_type operator()(Engine& eng) { return dist_(eng); }

  RealType pmf(IntType i) { return dist_.probabilities().at(i); }
};

} }

#endif // BOOST_SUPPLEMENT_ZIPF_DISTRIBUTION_HPP


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Izzo, D. and Vinko, T. ACT - Informatics - GTOP Database, ESA Advanced Concept Team, last
 *          accessed on 2012-01-12. http://www.esa.int/gsp/ACT/inf/op/globopt.htm.
 *      Musegaas, P. Gravity Assist calculation Verification.xlsx, last accessed: 3 December 2012,
 *          http://tudat.tudelft.nl/projects/tudat/wiki/Unit_tests, 2012.
 *
 *    Notes
 *      Three main functions are tested in these unit tests.
 *        Regarding the deltaV calculation gravity assist method:
 *          There is a complicated if-statement in this method. Hence many unit test are performed
 *          to test the functionality. Also various limit cases failed previously, hence many tests
 *          for this are also included:
 *              Case 1: required bending angle > maximum bending angle:
 *                  Two tests were written. In the first one no velocity effect is needed. This
 *                  test has a low accuracy, which should be replaced one day (it still relies on
 *                  hand calculator calculations done in 2011). In the second one a combination of
 *                  bending-effect deltaV and velocity-effect deltaV is calculated. This test has
 *                  been calculated using Tudat, and was verified using Excel.
 *                  Could definitely be improved.
 *              Case 2: no assist is required:
 *                  One test was written.
 *              Case 3: velocity effect deltaV only, using eccentricity iteration scheme:
 *                  Four tests were written. The first one calculates a case from Cassini-1 of GTOP
 *                  with high precision. The other three test limit cases: low incoming, high
 *                  outgoing velocity; high incoming, low outgoing velocity; low incoming, low
 *                  outgoing velocity. These tests were calculated using Tudat, but verified in
 *                  Excel to be exactly correct.
 *              Case 4: velocity effect deltaV only, using pericenter radius iteration scheme:
 *                  The same four tests as for case 3 were used.
 *        Regarding the unpowered gravity assist propagator:
 *          One test was written, based on GTOP. This should be a satisfactory test.
 *        Regarding the powered gravity assist propagator:
 *          Two tests were written. The first one is similar to the unpowered gravity assist
 *          propagator. The second one is reverse engineered from the Cassini-1 test, similar to
 *          the one in the deltaV calculation test.
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/MissionSegments/gravityAssist.h"

namespace tudat
{
namespace unit_tests
{

//! Test of gravity assist code.
BOOST_AUTO_TEST_SUITE( test_gravity_assist )

//! Test bending angle Delta-V computation.
BOOST_AUTO_TEST_CASE( testBendingAngleDeltaV )
{
    // Tolerance, determined primarily by the accuracy of the hand calculations for this test case.
    const double velocityTolerance = 0.0002;

    // In the first test case, the incoming and outgoing inertial velocities are defined such that
    // the hyperbolic excess velocities are equal. In that way, a delta-V is only needed to rotate
    // the velocity vector, which has been calculated by hand.
    // Expected delta-V for a powered swing-by around Mars.
    const double expectedDeltaV = 3.652e3;

    // Define swingby body gravitational parameter.
    const double marsGravitationalParameter = 4.2828018915e13;

    // Define Sun gravitational parameter.
    const double gravitationalParameterSun = 1.32712440018e20;

    // Define planet-Sun distance.
    const double distanceMarsToSun = unit_conversions::
            convertAstronomicalUnitsToMeters( 1.5 );

    // Define smallest periapsis distance factor.
    const double marsSmallestPeriapsisDistance = 3656248.0;

    // Define planet heliocentric velocity vector. The orbit is considered to be circular.
    const Eigen::Vector3d marsVelocity( 0.0,
                                        std::sqrt( gravitationalParameterSun / distanceMarsToSun ),
                                        0.0 );

    // Define satellite incoming vector.
    using mathematical_constants::PI;
    const Eigen::Vector3d incomingVelocity( -25.0e3 * std::sin( PI / 6.0 ),
                                            25.0e3 * std::cos( PI / 6.0 ),
                                            0.0 );

    // Define satellite outgoing vector.
    const Eigen::Vector3d outgoingVelocity( incomingVelocity( 0 ),
                                            2.0 * marsVelocity( 1 ) - incomingVelocity( 1 ),
                                            0.0 );

    // Perform the gravity assist.
    const double deltaV = mission_segments::gravityAssist( marsGravitationalParameter,
                                                           marsVelocity, incomingVelocity,
                                                           outgoingVelocity,
                                                           marsSmallestPeriapsisDistance );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( deltaV, expectedDeltaV, velocityTolerance );
}

//! Test case for both bending angle and velocity effect delta V.
BOOST_AUTO_TEST_CASE( testBendingAngleAndVelocityEffectDeltaVPericenter )
{
    // Tolerance.
    const double tolerance = 1e-12;

    // Expected deltaV cost, as obtained from this code, verified in Excel (Musegaas, 2012).
    const double expectedDeltaV = 183.8481861944;

    // Define swingby body gravitational parameter.
    const double venusGravitationalParameter = 3.24860e14;

    // Define smallest periapsis distance factor.
    const double venusSmallestPeriapsisDistance = 6351800.0;

    // Define heliocentric planet velocity vector.
    const Eigen::Vector3d venusVelocity( 35000.0, 0.0 , 0.0 );

    // Define heliocentric satellite incoming vector.
    const Eigen::Vector3d incomingVelocity( 36000.0 , 0.0 , 0.0 );

    // Define heliocentric satellite outgoing vector.
    const Eigen::Vector3d outgoingVelocity( 34500.0, 0.0, 0.0 );

    // Perform the gravity assist.
    const double deltaV = mission_segments::gravityAssist( venusGravitationalParameter,
                                                           venusVelocity,incomingVelocity,
                                                           outgoingVelocity,
                                                           venusSmallestPeriapsisDistance );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( deltaV, expectedDeltaV, tolerance );
}

//! Test case in which no assist is required.
BOOST_AUTO_TEST_CASE( testNoDeltaVRequired )
{
    // Tolerance.
    const double tolerance = std::numeric_limits< double >::epsilon( );

    // Expected deltaV cost, as obtained from this code, verified in Excel (Musegaas, 2012).
    const double expectedDeltaV = 0.0;

    // Define swingby body gravitational parameter.
    const double venusGravitationalParameter = 3.24860e14;

    // Define smallest periapsis distance factor.
    const double venusSmallestPeriapsisDistance = 6351800.0;

    // Define heliocentric planet velocity vector.
    const Eigen::Vector3d venusVelocity( 35000.0, 0.0 , 0.0 );

    // Define heliocentric satellite incoming vector.
    const Eigen::Vector3d incomingVelocity( 36000.0 , 0.0 , 0.0 );

    // Define heliocentric satellite outgoing vector.
    const Eigen::Vector3d outgoingVelocity( 35000.0, 1000.0, 0.0 );

    // Perform the gravity assist.
    const double deltaV = mission_segments::gravityAssist( venusGravitationalParameter,
                                                           venusVelocity,incomingVelocity,
                                                           outgoingVelocity,
                                                           venusSmallestPeriapsisDistance );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( deltaV, expectedDeltaV, tolerance );
}

//! Test velocity effect Delta-V computation using the eccentricity iteration scheme.
BOOST_AUTO_TEST_CASE( testVelocityEffectDeltaVEccentricity )
{
    // Tolerance. Benchmark obtained directly from the GTOP code, based on the first swing-by for
    // the ideal Cassini-1 trajectory. Values were obtained with a 15-digit accuracy from GTOP,
    // resulting in an accuracy of 6e-14 in the final results.
    const double tolerance = 1.0e-13;

    // Expected deltaV cost, as obtained from GTOP code.
    const double expectedDeltaV = 1090.64622870007;

    // Define swingby body gravitational parameter.
    const double venusGravitationalParameter = 3.24860e14;

    // Define smallest periapsis distance factor.
    const double venusSmallestPeriapsisDistance = 6351800.0;

    // Define heliocentric planet velocity vector.
    const Eigen::Vector3d venusVelocity( 32851.224953746, -11618.7310059974, -2055.04615890989 );

    // Define heliocentric satellite incoming vector.
    const Eigen::Vector3d incomingVelocity( 34216.4827530912, -15170.1440677825,
                                            395.792122152361 );

    // Define heliocentric satellite outgoing vector.
    const Eigen::Vector3d outgoingVelocity( 37954.2431376052, -14093.0467234774,
                                            -5753.53728279429 );

    // Set flag to use iteration scheme on eccentricity.
    const bool useEccentricity = true;

    // Perform the gravity assist.
    const double deltaV = mission_segments::gravityAssist( venusGravitationalParameter,
                                                           venusVelocity,incomingVelocity,
                                                           outgoingVelocity,
                                                           venusSmallestPeriapsisDistance,
                                                           useEccentricity );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( deltaV, expectedDeltaV, tolerance );
}

//! Test limit case for deltaV computation with low incoming velocity with eccentricity iteration.
BOOST_AUTO_TEST_CASE( testLimitCaseDeltaVLowIncomingVelocityEccentricity )
{
    // Tolerance.
    const double tolerance = 1.0e-11;

    // Expected deltaV cost, as obtained from this code, verified in Excel (Musegaas, 2012).
    const double expectedDeltaV = 966.37867363;

    // Define swingby body gravitational parameter.
    const double venusGravitationalParameter = 3.24860e14;

    // Define smallest periapsis distance factor.
    const double venusSmallestPeriapsisDistance = 6351800.0;

    // Define heliocentric planet velocity vector.
    const Eigen::Vector3d venusVelocity( 35000.0, 0.0 , 0.0 );

    // Define heliocentric satellite incoming vector.
    const Eigen::Vector3d incomingVelocity( 35000.01, 0.0, 0.0 );

    // Define heliocentric satellite outgoing vector.
    const Eigen::Vector3d outgoingVelocity( 35000.0 , 1000.0 , 0.0 );

    // Set flag to use iteration scheme on eccentricity.
    const bool useEccentricity = true;

    // Perform the gravity assist.
    const double deltaV = mission_segments::gravityAssist( venusGravitationalParameter,
                                                           venusVelocity,incomingVelocity,
                                                           outgoingVelocity,
                                                           venusSmallestPeriapsisDistance,
                                                           useEccentricity );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( deltaV, expectedDeltaV, tolerance );
}

//! Test limit case for deltaV computation with low outgoing velocity with eccentricity iteration.
BOOST_AUTO_TEST_CASE( testLimitCaseDeltaVLowOutgoingVelocityEccentricity )
{
    // Tolerance.
    const double tolerance = 1.0e-11;

    // Expected deltaV cost, as obtained from this code, verified in Excel (Musegaas, 2012).
    const double expectedDeltaV = 966.37867363;

    // Define swingby body gravitational parameter.
    const double venusGravitationalParameter = 3.24860e14;

    // Define smallest periapsis distance factor.
    const double venusSmallestPeriapsisDistance = 6351800.0;

    // Define heliocentric planet velocity vector.
    const Eigen::Vector3d venusVelocity( 35000.0, 0.0 , 0.0 );

    // Define heliocentric satellite incoming vector.
    const Eigen::Vector3d incomingVelocity( 35000.0 , 1000.0 , 0.0 );

    // Define heliocentric satellite outgoing vector.
    const Eigen::Vector3d outgoingVelocity( 35000.01, 0.0, 0.0 );

    // Set flag to use iteration scheme on eccentricity.
    const bool useEccentricity = true;

    // Perform the gravity assist.
    const double deltaV = mission_segments::gravityAssist( venusGravitationalParameter,
                                                           venusVelocity, incomingVelocity,
                                                           outgoingVelocity,
                                                           venusSmallestPeriapsisDistance,
                                                           useEccentricity );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( deltaV, expectedDeltaV, tolerance );
}

//! Test limit case for deltaV computation with low velocities with eccentricity iteration.
BOOST_AUTO_TEST_CASE( testLimitCaseDeltaVLowVelocitiesEccentricity )
{
    // Tolerance.
    const double tolerance = 1.0e-9;

    // Expected deltaV cost, as obtained from this code, verified in Excel (Musegaas, 2012).
    const double expectedDeltaV = 0.004260780473;

    // Define swingby body gravitational parameter.
    const double venusGravitationalParameter = 3.24860e14;

    // Define smallest periapsis distance factor.
    const double venusSmallestPeriapsisDistance = 6351800.0;

    // Define heliocentric planet velocity vector.
    const Eigen::Vector3d venusVelocity( 35000.0, 0.0 , 0.0 );

    // Define heliocentric satellite incoming vector.
    const Eigen::Vector3d incomingVelocity( 35000.0 , 0.02 , 0.0 );

    // Define heliocentric satellite outgoing vector.
    const Eigen::Vector3d outgoingVelocity( 35000.01, 0.0, 0.0 );

    // Set flag to use iteration scheme on eccentricity.
    const bool useEccentricity = true;

    // Perform the gravity assist.
    const double deltaV = mission_segments::gravityAssist( venusGravitationalParameter,
                                                           venusVelocity,incomingVelocity,
                                                           outgoingVelocity,
                                                           venusSmallestPeriapsisDistance,
                                                           useEccentricity );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( deltaV, expectedDeltaV, tolerance );
}

//! Test velocity effect Delta-V computation using the pericenter iteration scheme.
BOOST_AUTO_TEST_CASE( testVelocityEffectDeltaVPericenter )
{
    // Tolerance. Benchmark obtained directly from the GTOP code, based on the first swing-by for
    // the ideal Cassini-1 trajectory. Values were obtained with a 15-digit accuracy from GTOP,
    // resulting in an accuracy of 6e-14 in the final results.
    const double tolerance = 1.0e-13;

    // Expected deltaV cost, as obtained from GTOP code.
    const double expectedDeltaV = 1090.64622870007;

    // Define swingby body gravitational parameter.
    const double venusGravitationalParameter = 3.24860e14;

    // Define smallest periapsis distance factor.
    const double venusSmallestPeriapsisDistance = 6351800.0;

    // Define heliocentric planet velocity vector.
    const Eigen::Vector3d venusVelocity( 32851.224953746, -11618.7310059974, -2055.04615890989 );

    // Define heliocentric satellite incoming vector.
    const Eigen::Vector3d incomingVelocity( 34216.4827530912, -15170.1440677825,
                                            395.792122152361 );

    // Define heliocentric satellite outgoing vector.
    const Eigen::Vector3d outgoingVelocity( 37954.2431376052, -14093.0467234774,
                                            -5753.53728279429 );

    // Set flag to use iteration scheme on pericenter.
    const bool useEccentricity = false;

    // Perform the gravity assist.
    const double deltaV = mission_segments::gravityAssist( venusGravitationalParameter,
                                                           venusVelocity,incomingVelocity,
                                                           outgoingVelocity,
                                                           venusSmallestPeriapsisDistance,
                                                           useEccentricity );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( deltaV, expectedDeltaV, tolerance );
}

//! Test limit case for deltaV computation with low incoming velocity with pericenter iteration.
BOOST_AUTO_TEST_CASE( testLimitCaseDeltaVLowIncomingVelocityPericenter )
{
    // Tolerance.
    const double tolerance = 1.0e-11;

    // Expected deltaV cost, as obtained from this code, verified in Excel (Musegaas, 2012).
    const double expectedDeltaV = 966.37867363;

    // Define swingby body gravitational parameter.
    const double venusGravitationalParameter = 3.24860e14;

    // Define smallest periapsis distance factor.
    const double venusSmallestPeriapsisDistance = 6351800.0;

    // Define heliocentric planet velocity vector.
    const Eigen::Vector3d venusVelocity( 35000.0, 0.0 , 0.0 );

    // Define heliocentric satellite incoming vector.
    const Eigen::Vector3d incomingVelocity( 35000.01, 0.0, 0.0 );

    // Define heliocentric satellite outgoing vector.
    const Eigen::Vector3d outgoingVelocity( 35000.0 , 1000.0 , 0.0 );

    // Set flag to use iteration scheme on eccentricity.
    const bool useEccentricity = false;

    // Perform the gravity assist.
    const double deltaV = mission_segments::gravityAssist( venusGravitationalParameter,
                                                           venusVelocity,incomingVelocity,
                                                           outgoingVelocity,
                                                           venusSmallestPeriapsisDistance,
                                                           useEccentricity );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( deltaV, expectedDeltaV, tolerance );
}

//! Test limit case for deltaV computation with low outgoing velocity with pericenter iteration.
BOOST_AUTO_TEST_CASE( testLimitCaseDeltaVLowOutgoingVelocityPericenter )
{
    // Tolerance.
    const double tolerance = 1.0e-11;

    // Expected deltaV cost, as obtained from this code, verified in Excel (Musegaas, 2012).
    const double expectedDeltaV = 966.37867363;

    // Define swingby body gravitational parameter.
    const double venusGravitationalParameter = 3.24860e14;

    // Define smallest periapsis distance factor.
    const double venusSmallestPeriapsisDistance = 6351800.0;

    // Define heliocentric planet velocity vector.
    const Eigen::Vector3d venusVelocity( 35000.0, 0.0 , 0.0 );

    // Define heliocentric satellite incoming vector.
    const Eigen::Vector3d incomingVelocity( 35000.0 , 1000.0 , 0.0 );

    // Define heliocentric satellite outgoing vector.
    const Eigen::Vector3d outgoingVelocity( 35000.01, 0.0, 0.0 );

    // Set flag to use iteration scheme on pericenter.
    const bool useEccentricity = false;

    // Perform the gravity assist.
    const double deltaV = mission_segments::gravityAssist( venusGravitationalParameter,
                                                           venusVelocity,incomingVelocity,
                                                           outgoingVelocity,
                                                           venusSmallestPeriapsisDistance,
                                                           useEccentricity );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( deltaV, expectedDeltaV, tolerance );
}

//! Test limit case for deltaV computation with low velocities with pericenter iteration.
BOOST_AUTO_TEST_CASE( testLimitCaseDeltaVLowVelocitiesPericenter )
{
    // Tolerance.
    const double tolerance = 1.0e-9;

    // Expected deltaV cost, as obtained from this code, verified in Excel (Musegaas, 2012).
    const double expectedDeltaV = 0.004260780473;

    // Define swingby body gravitational parameter.
    const double venusGravitationalParameter = 3.24860e14;

    // Define smallest periapsis distance factor.
    const double venusSmallestPeriapsisDistance = 6351800.0;

    // Define heliocentric planet velocity vector.
    const Eigen::Vector3d venusVelocity( 35000.0, 0.0 , 0.0 );

    // Define heliocentric satellite incoming vector.
    const Eigen::Vector3d incomingVelocity( 35000.0 , 0.02 , 0.0 );

    // Define heliocentric satellite outgoing vector.
    const Eigen::Vector3d outgoingVelocity( 35000.01, 0.0, 0.0 );

    // Set flag to use iteration scheme on pericenter.
    const bool useEccentricity = false;

    // Perform the gravity assist.
    const double deltaV = mission_segments::gravityAssist( venusGravitationalParameter,
                                                           venusVelocity,incomingVelocity,
                                                           outgoingVelocity,
                                                           venusSmallestPeriapsisDistance,
                                                           useEccentricity );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( deltaV, expectedDeltaV, tolerance );
}

//! Test unpowered gravity assist propagation.
BOOST_AUTO_TEST_CASE( testUnpoweredGravityAssistPropagation )
{
    // Tolerance. Benchmark obtained directly from the GTOP code, based on the first swing-by for
    // the ideal Messenger trajectory. Values were obtained with a 15-digit accuracy from GTOP,
    // resulting in an accuracy of 2e-14 in the final results.
    const double tolerance = 1.0e-13;

    // Expected deltaV cost, as obtained from GTOP code.
    const Eigen::Vector3d expectedOutgoingVelocity( 12868.5248737923, -22821.444560174,
                                                    -775.698475033994 );

    // Define swingby body gravitational parameter.
    const double earthGravitationalParameter = 3.9860119e14;

    // Define heliocentric planet velocity vector.
    const Eigen::Vector3d earthVelocity( 15025.522196446, -25544.3782752036, 0.0 );

    // Define heliocentric satellite incoming vector.
    const Eigen::Vector3d incomingVelocity( 17969.3166254716, -23543.691593914, 6.38384671663496 );

    // Define rotation angle.
    const double rotationAngle = 1.35077257078;

    // Define pericenter radius.
    const double pericenterRadius = 1.80629232251 * 6378000.0;

    // Perform the gravity assist.
    const Eigen::Vector3d outgoingVelocity = mission_segments::gravityAssist(
                                                    earthGravitationalParameter, earthVelocity,
                                                    incomingVelocity, rotationAngle,
                                                    pericenterRadius );

    // Test if the computed outgoing velocity corresponds to the expected velocity within the
    // specified tolerance.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedOutgoingVelocity, outgoingVelocity, tolerance );
}

//! Test powered gravity assist propagation function for unpowered gravity assist.
BOOST_AUTO_TEST_CASE( testPoweredGravityAssistPropagationForUnpoweredGravityAssist )
{
    // Tolerance. Benchmark obtained directly from the GTOP code, based on the first swing-by for
    // the ideal Messenger trajectory. Values were obtained with a 15-digit accuracy from GTOP,
    // resulting in an accuracy of 2e-14 in the final results.
    const double tolerance = 1.0e-13;

    // Expected deltaV cost, as obtained from GTOP code.
    const Eigen::Vector3d expectedOutgoingVelocity( 12868.5248737923, -22821.444560174,
                                                    -775.698475033994 );

    // Define swingby body gravitational parameter.
    const double earthGravitationalParameter = 3.9860119e14;

    // Define heliocentric planet velocity vector.
    const Eigen::Vector3d earthVelocity( 15025.522196446, -25544.3782752036, 0.0 );

    // Define heliocentric satellite incoming vector.
    const Eigen::Vector3d incomingVelocity( 17969.3166254716, -23543.691593914, 6.38384671663496 );

    // Define rotation angle.
    const double rotationAngle = 1.35077257078;

    // Define pericenter radius.
    const double pericenterRadius = 1.80629232251 * 6378000.0;

    // Define deltaV.
    const double deltaV = 0.0;

    // Perform the gravity assist.
    const Eigen::Vector3d outgoingVelocity = mission_segments::gravityAssist(
                                                    earthGravitationalParameter, earthVelocity,
                                                    incomingVelocity, rotationAngle,
                                                    pericenterRadius, deltaV );

    // Test if the computed outgoing velocity corresponds to the expected velocity within the
    // specified tolerance.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedOutgoingVelocity, outgoingVelocity, tolerance );
}

//! Test powered gravity assist propagation function for reverse engineered test case.
BOOST_AUTO_TEST_CASE( testPoweredGravityAssistPropagationReverseEngineered )
{
    // Tolerance. Benchmark obtained by reverse engineering the GTOP code, based on the first
    // swing-by for the ideal Cassini-1 trajectory. Values were obtained with a 15-digit accuracy
    // from GTOP, resulting in an accuracy of 1e-14 in the final results.
    const double tolerance = 1.0e-14;

    // Expected deltaV cost, as obtained from GTOP code.
    const Eigen::Vector3d expectedOutgoingVelocity( 37954.2431376052, -14093.0467234774,
                                                    -5753.53728279429 );

    // Define swingby body gravitational parameter.
    const double venusGravitationalParameter = 3.24860e14;

    // Define heliocentric planet velocity vector.
    const Eigen::Vector3d venusVelocity( 32851.224953746, -11618.7310059974, -2055.04615890989 );

    // Define heliocentric satellite incoming vector.
    const Eigen::Vector3d incomingVelocity( 34216.4827530912, -15170.1440677825,
                                            395.792122152361 );

    // Define rotation angle.
    const double rotationAngle = -2.0291949514117;

    // Define pericenter radius.
    const double pericenterRadius = 6351801.04541467;

    // Define deltaV.
    const double deltaV = 1090.64622870007;

    // Perform the gravity assist.
    const Eigen::Vector3d outgoingVelocity = mission_segments::gravityAssist(
                                                    venusGravitationalParameter, venusVelocity,
                                                    incomingVelocity, rotationAngle,
                                                    pericenterRadius, deltaV );

    // Test if the computed outgoing velocity corresponds to the expected velocity within the
    // specified tolerance.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedOutgoingVelocity, outgoingVelocity, tolerance );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// test CPU direct R solver

#include <minisam/3rdparty/Catch2/catch.hpp>
#include <minisam/utils/testAssertions.h>

#include <minisam/linear/Covariance.h>
#include <minisam/linear/SquareRootCholesky.h>

#include <Eigen/LU> // rank

using namespace minisam;

// exmaple systems
Eigen::SparseMatrix<double> H1(2,2), H2(2,2), H3(2,2), H;
Eigen::MatrixXd Hinv1(2,2), Hinv2(2,2), Hinv3(2,2), Hinv;
const int A_row = 40, A_col = 20;

/* ************************************************************************** */
TEST_CASE("Covariance_prep_static_values", "[linear]") {

  Eigen::SparseMatrix<double> A1(2,2), A2(2,2), A3(3,2);

  // sys 1, identity
  A1.insert(0,0) = 1.0;
  A1.insert(1,1) = 1.0;
  A1.makeCompressed();
  H1 = A1.transpose() * A1;
  Hinv1 = Eigen::MatrixXd::Identity(2, 2);

  // sys 2, well-cond, use matlab get ground truth
  A2.insert(0,0) = 3.2;
  A2.insert(0,1) = 4.5;
  A2.insert(1,0) = -1.9;
  A2.insert(1,1) = 7.6;
  A2.makeCompressed();
  H2 = A2.transpose() * A2;
  Hinv2 <<  7.220227298789961e-02,  3.702206024247995e-05,
            3.702206024247995e-05,  1.281888835895923e-02;

  // sys 3, over-cond, use matlab get ground truth
  A3.insert(0,0) = 3.2;
  A3.insert(0,1) = 4.5;
  A3.insert(1,0) = -1.9;
  A3.insert(1,1) = 7.6;
  A3.insert(2,0) = 5.5;
  A3.insert(2,1) = 3.4;
  A3.makeCompressed();
  H3 = A3.transpose() * A3;
  Hinv3 <<  2.486783565761668e-02,  -5.180683413767191e-03,
            -5.180683413767191e-03, 1.224373732835655e-02;

  // use random A to generate test, to test whether R'R - ordering.permute(A'A) = 0
  Eigen::MatrixXd A_dense;
  int A_rank;
  do {
    A_dense = Eigen::MatrixXd::Random(A_row, A_col);
    Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(A_dense);
    A_rank = lu_decomp.rank();
  } while (A_rank < A_col); // make sure full rank

  Eigen::MatrixXd H_dense = A_dense.transpose() * A_dense;
  H = H_dense.sparseView();
  Hinv = H_dense.inverse();
}

/* ************************************************************************** */
TEST_CASE("Covariance_ground_truth", "[linear]") {
  // get ground truth from matlab
  SquareRootSolverCholesky sqrsolver(OrderingMethod::NONE);
  Eigen::SparseMatrix<double> L;
  Eigen::MatrixXd Hinv_act;
  std::vector<int> full_indices = {0, 1};

  sqrsolver.initialize(H1);
  sqrsolver.solveL(H1, L);
  Covariance c1(L);
  Hinv_act = c1.marginalCovariance(full_indices);
  CHECK(assert_equal(Hinv1, Hinv_act));

  sqrsolver.initialize(H2);
  sqrsolver.solveL(H2, L);
  Covariance c2(L);
  Hinv_act = c2.marginalCovariance(full_indices);
  CHECK(assert_equal(Hinv2, Hinv_act));

  sqrsolver.initialize(H3);
  sqrsolver.solveL(H3, L);
  Covariance c3(L);
  Hinv_act = c3.marginalCovariance(full_indices);
  CHECK(assert_equal(Hinv3, Hinv_act));
}

/* ************************************************************************** */
TEST_CASE("Covariance_random", "[linear]") {
  // get ground truth from matlab
  SquareRootSolverCholesky sqrsolver(OrderingMethod::NONE);
  Eigen::SparseMatrix<double> L;
  Eigen::MatrixXd Hinv_act, Hinv_partial;
  std::vector<int> full_indices;

  sqrsolver.initialize(H);
  sqrsolver.solveL(H, L);
  Covariance c(L);

  // block index
  full_indices = {5, 6, 7, 8};
  Hinv_act = c.marginalCovariance(full_indices);
  Hinv_partial = Hinv.block(5, 5, 4, 4);
  CHECK(assert_equal(Hinv_partial, Hinv_act));

  // full index
  full_indices.clear();
  for (int i = 0; i < A_col; i++) full_indices.push_back(i);
  Hinv_act = c.marginalCovariance(full_indices);
  CHECK(assert_equal(Hinv, Hinv_act));
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_TWO_PROD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_TWO_PROD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing two_prod capabilities

    For any two reals @c x and @c y two_prod computes two reals (in an std::pair)
    @c r0 and @c r1 so that:

    @code
    r0 = x * y
    r1 = r0 -(x * y)
    @endcode

    using perfect arithmetic.

    Its main usage is to be able to compute
    sum of reals and the residual error using IEEE 754 arithmetic.

  **/
  std::pair<Value, Value> two_prod(Value const& x, Value const& y);

} }
#endif

#include <boost/simd/function/scalar/two_prod.hpp>
#include <boost/simd/function/simd/two_prod.hpp>

#endif


#include "matrixIO.hpp"
#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <vector>

using namespace std;
using namespace Eigen;

namespace matrixIO {

void saveData(const string &fileName, const MatrixXd &matrix)
{
  // see https://eigen.tuxfamily.org/dox/structEigen_1_1IOFormat.html
  const static IOFormat CSVFormat(FullPrecision, DontAlignCols, ", ", "\n");

  ofstream file(fileName);
  if (file.is_open()) {
    file << matrix.format(CSVFormat);
    file.close();
  }
}

MatrixXd openData(const string &fileToOpen, const int matrixSize)
{
  ifstream matrixDataFile(fileToOpen);

  // one row of the matrix as a string with comma-separated values
  string matrixRowString;

  // a single matrix entry as a string
  string matrixEntry;

  // matrix entries row by row
  vector<double> matrixEntries;

  int matrixRowNumber = 0;
  int matrixColNumber = 0;

  // read matrixDataFile row by row
  while (getline(matrixDataFile, matrixRowString)) {
    // convert matrixRowString to a stream
    stringstream matrixRowStringStream(matrixRowString);

    // read matrixRowStringStream entry by entry
    while (getline(matrixRowStringStream, matrixEntry, ',')) {
      matrixEntries.push_back(stod(matrixEntry));
      ++matrixColNumber;
    }
    if (matrixColNumber != matrixSize) {
      throw std::runtime_error("The matrix stored in \"" + fileToOpen +
                               "\" has " + std::to_string(matrixColNumber) +
                               " columns, while the specified matrix size is " +
                               std::to_string(matrixSize));
    }
    matrixColNumber = 0;
    ++matrixRowNumber;
  }

  if (matrixRowNumber != matrixSize) {
    throw std::runtime_error("The matrix stored in \"" + fileToOpen +
                             "\" has " + std::to_string(matrixRowNumber) +
                             " rows, while the specified matrix size is " +
                             std::to_string(matrixSize));
  }

  // convert std vector into Eigen matrix and return the matrix
  return Map<Matrix<double, Dynamic, Dynamic, RowMajor>>(
      matrixEntries.data(), matrixRowNumber,
      matrixEntries.size() / matrixRowNumber);
}

} // namespace matrixIO


// Copyright (C) 2019 David Harmon and Artificial Necessity
// This code distributed under zlib, see LICENSE.txt for terms.

#pragma once

#include <Eigen/Core>

template<typename Scalar, int K>
class kDOP {
public:
    using VectorK  = Eigen::Matrix<Scalar, K/2, 1>;
    using MatrixK3 = Eigen::Matrix<Scalar, K/2, 3>;

    kDOP() { setEmpty(); }

    void setEmpty() {
        min_ = VectorK::Constant(std::numeric_limits<Scalar>::max());
        max_ = VectorK::Constant(std::numeric_limits<Scalar>::min());
    }

    template<typename Derived>
    kDOP& extendK(const Eigen::MatrixBase<Derived>& vals) {
        min_ = min_.cwiseMin(vals);
        max_ = max_.cwiseMax(vals);
        return *this;
    }

    template<typename Derived>
    kDOP& extend(const Eigen::MatrixBase<Derived>& p) {
        VectorK p_proj = project(p);
        min_ = min_.cwiseMin(p_proj);
        max_ = max_.cwiseMax(p_proj);
        return *this;
    }

    kDOP& extend(const kDOP& b) {
        min_ = min_.cwiseMin(b.min_);
        max_ = max_.cwiseMax(b.max_);
        return *this;
    }

    template<typename Derived>
    bool contains(const Eigen::MatrixBase<Derived>& p) const {
        VectorK p_proj = project(p);
        return (min_.array() <= p_proj.array()).all() &&
               (p_proj.array() <= max_.array()).all();
    }

    bool intersects(const kDOP& b) const {
        return (min_.array() <= (b.max)().array()).all() &&
                ((b.min)().array() <= max_.array()).all();
    }

    template<typename Derived>
    VectorK project(const Eigen::MatrixBase<Derived>& p) const {
        return Eigen::Map<const MatrixK3>(dirs.data()) * p;
    }

    const VectorK& min() const { return min_; }
    const VectorK& max() const { return max_; }

protected:
    VectorK min_;
    VectorK max_;

    static constexpr std::array<Scalar, 39> dirs = {
            1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0,
            0, 1, 0, 1, -1, 1, -1, 1, 0, 1, -1, 0, 1,
            0, 0, 1, 1, 1, -1, -1, 0, 1, 1, 0, -1, -1 };
};

using kDOP26d = kDOP<double, 26>;


#ifndef _KERNEL_INDUCTION_HPP_
#define _KERNEL_INDUCTION_HPP_

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/SparseCore>
#include <cstdarg>
#include <mimkl/definitions.hpp>
#include <spdlog/spdlog.h>

using mimkl::definitions::Index;

namespace mimkl
{
namespace induction
{

auto logger = spdlog::stdout_color_mt("kernel_induction");

//! If only the diagonal entries of \f$XLX^T\f$ are needed
/*!  one full matrix multiplication cannot be avoided with matrix induction,
    but we only compute the diagonal of \f$(X*L)*X^T\f$
\param matrix NxM data-matrix.
\param inducer pathway specific (sparse) MxM matrix.
\param diagonal a  Nx1 Matrix.
\sa test/diagonal_from_square_induction
*/
template <typename MatrixDerived, typename InducerDerived, typename DiagonalDerived>
void get_diagonal_from_square_induction(
const Eigen::MatrixBase<MatrixDerived> &matrix,
const Eigen::SparseMatrixBase<InducerDerived> &inducer,
const Eigen::EigenBase<DiagonalDerived> &diagonal)
{
    const Eigen::Matrix<typename MatrixDerived::Scalar, MatrixDerived::RowsAtCompileTime,
                        MatrixDerived::ColsAtCompileTime>
    matrix_inducer =
    matrix * inducer.template selfadjointView<Eigen::Lower>(); // no .noalias()
    // possible here:
    // matrix_inducer =
    // ;

    Eigen::EigenBase<DiagonalDerived> &diagonal_ =
    const_cast<Eigen::EigenBase<DiagonalDerived> &>(diagonal);
    const Index n = matrix_inducer.rows(); // == matrix.rows()
    if (diagonal.rows() < n)
    {
        spdlog::get("kernel_induction")
        ->critical("error in get_diagonal_from_square_induction():\n rows of "
                   "diagonal {}, rows needed: {}",
                   diagonal.rows(), matrix.rows());
    }
    //  try {
    for (Index i = 0; i < n; i++)
    {
        diagonal_.derived()(i, 0) =
        matrix_inducer.row(i) *
        matrix.adjoint().col(i); // matrix.row(i) for RealScalars
    }
    //  } catch (...){ //
    //		spdlog::get("kernel_induction")->critical("error in
    // get_diagonal_from_square_induction():\n rows of diagonal {}, rows needed:
    //[]", diagonal.rows(), matrix.rows());
    //		throw;
    //  }
}

//! Matrix induction of a linear kernel with the extended kernel K = X*L*Y_t
/*!
\param lhs left hand side NxM data-matrix.
\param rhs untransposed (unconjugated) right hand side KxM data-matrix.
\param inducer pathway specific (sparse, symmetric) MxM matrix.
\returns kernel_matrix the similarity NxK matrix.
\sa test/linear_induction
*/
template <typename KDerived, typename LhsDerived, typename RhsDerived, typename LDerived>
KDerived induce_linear_kernel(LhsDerived &lhs, RhsDerived &rhs, LDerived inducer)
{
    return lhs * inducer.template selfadjointView<Eigen::Lower>() *
           rhs.adjoint(); // symmetry allows optimization Lower vs. Upper?
}

//! Matrix induction of a polynomial kernel with the extended kernel K =
//! (X*L*Y_t + c)^p
/*!
\param lhs left hand side NxM data-matrix.
\param rhs untransposed (unconjugated)  right hand side KxM data-matrix.
\param inducer pathway specific (sparse) MxM matrix.
\param degree polynomial degree.
\param offset "free parameter trading off the influence of higher-order versus
lower-order terms in the polynomial".
\returns kernel_matrix the similarity NxK matrix.
\sa TODO
*/
template <typename KDerived, typename LhsDerived, typename RhsDerived, typename LDerived>
KDerived induce_polynomial_kernel(LhsDerived &lhs,
                                  RhsDerived &rhs,
                                  LDerived inducer,
                                  const double degree,
                                  const double offset)
{
    return (induce_linear_kernel<KDerived>(lhs, rhs, inducer).array() + offset)
    .pow(degree)
    .matrix();
}

//! Matrix induction of a gaussian kernel with the extended kernel k = exp(-
//! (x-y)_t*L*(x-y) / ( 2*s^2 )).
/*!  This squared pairwise euclidean distance cannot be expressed in a concise
matrix multiplication.
The squared distance of xi to yj = \f$(x_i^T*L*x_i) -(x_i^T*L*y_j)
-(y_j^T*L*x_i) + (y_j^T*L*y_j) \f$
This means next to \f$XLY^T\f$ (the linear kernel) only the diagonal entries of
\f$XLX^T\f$ and \f$YLY^T\f$ are needed.
\param lhs left hand side NxM data-matrix.
\param rhs untransposed  (unconjugated) right hand side KxM data-matrix.
\param inducer pathway specific (sparse) MxM matrix.
\param sigma_square variance of the bell curve.
\returns kernel_matrix the similarity NxK matrix.
\sa gaussian_induction
*/
template <typename KDerived, typename LhsDerived, typename RhsDerived, typename LDerived>
KDerived induce_gaussian_kernel(LhsDerived &lhs,
                                RhsDerived &rhs,
                                LDerived inducer,
                                const double sigma_square)
{
    // we only need the diagonal entries of X*L*X_t, so only fill the diagonal
    // of
    // (X*L)*X_t
    Eigen::Matrix<typename LhsDerived::Scalar, LhsDerived::RowsAtCompileTime, 1>
    diag_lhs_inducer_lhs_t; // Length N
    if (LhsDerived::RowsAtCompileTime == Eigen::Dynamic)
        diag_lhs_inducer_lhs_t.resize(lhs.rows());
    get_diagonal_from_square_induction(lhs, inducer, diag_lhs_inducer_lhs_t);
    // same for (Y*L)*Y_t
    Eigen::Matrix<typename RhsDerived::Scalar, RhsDerived::RowsAtCompileTime, 1>
    diag_rhs_inducer_rhs_t; // Length K
    if (RhsDerived::RowsAtCompileTime == Eigen::Dynamic)
        diag_rhs_inducer_rhs_t.resize(rhs.rows());
    get_diagonal_from_square_induction(rhs, inducer, diag_rhs_inducer_rhs_t);

    // X*L*Y_t
    return (-(
            /*! -2* lhs_inducer_rhs only in case of scalar matrices.
             with complex numbers we need: -lhs_inducer_rhs
             -lhs_inducer_rhs.adjoint
             imaginary parts cancel out! */
            ((-2 * induce_linear_kernel<KDerived>(lhs, rhs, inducer).real()).colwise() +
             diag_lhs_inducer_lhs_t)
            .rowwise() +
            diag_rhs_inducer_rhs_t.transpose()) /
            (2 * sigma_square))
    .array()
    .exp()
    .matrix();
}

//! Matrix induction of a sigmoidal kernel with the extended kernel k = tanh(a*
//! (x_t*L*y) +b).
/*!
\param lhs left hand side NxM data-matrix.
\param rhs untransposed  (unconjugated) right hand side KxM data-matrix.
\param inducer pathway specific (sparse) MxM matrix.
\param a
\param b
\returns kernel_matrix the similarity NxK matrix.
\sa test/sigmoidal_induction
*/
template <typename KDerived, typename LhsDerived, typename RhsDerived, typename LDerived>
KDerived induce_sigmoidal_kernel(LhsDerived &lhs,
                                 RhsDerived &rhs,
                                 LDerived inducer,
                                 const double a,
                                 const double b)
{
    return (a * induce_linear_kernel<KDerived>(lhs, rhs, inducer).array() + b)
    .tanh()
    .matrix();
}

template <typename KDerived, typename LhsDerived, typename RhsDerived, typename LDerived>
std::vector<std::function<KDerived(LhsDerived &, RhsDerived &)>>
inducer_combination(std::function<KDerived(LhsDerived &, RhsDerived &, LDerived)>
                    kernel_function, // parameter specification done
                                     // previously
                    std::vector<LDerived> inducers)
{
    std::vector<std::function<KDerived(LhsDerived &, RhsDerived &)>> lambda_expressions;
    lambda_expressions.reserve(inducers.size());

    for (LDerived inducer : inducers)
    {
        lambda_expressions.push_back(
        [&, inducer](const LhsDerived &lhs, const RhsDerived &rhs) {
            return kernel_function(lhs, rhs, inducer);
        });
    }

    return lambda_expressions;
}

} // namespace induction
} // namespace mimkl

#endif /*_KERNEL_INDUCTION_HPP_*/