// [[Rcpp::depends(RcppArmadillo)]]
#define ARMA_DONT_PRINT_ERRORS
#include <iostream>
#include <fstream>
#include <cmath>
#include <armadillo>
#include <errno.h>
#include <RcppArmadillo.h>
//' Get observation location in 2D space
//'
//' @param  time time to return observer position
//' @param  strip_size size of strip in (x, y) dimensions
//' @param  buffer buffer size
//' @param  delta (dx, dt) vector
//' @param  transect_type 0 = line, 1 = point
//' @param  observer_speed speed of observer
//'
//' @return  (x, y) location of observer at time t
// [[Rcpp::export]]
arma::vec GetObserverPosition(const double time,
                        const arma::vec strip_size,
                        const double buffer,
                        const arma::vec delta,
                        const int transect_type,
                        const double observer_speed) {
  arma::vec pos(2);
  pos(0) = 0.5 * strip_size(0);
  if (transect_type == 1) {
    // point transect assumed to be center of grid
	  pos(1) = 0.5 * strip_size(1);
  } else {
    pos(1) = observer_speed * time + buffer;
  }
  return pos;
}
//' Calculates the sparse transition rate matrix
//'
//' @param  num_cells vector with number of cells in (total space, x-direction,
//'     y-direction)
//' @param  sd vector of diffusive standard deviation for each behavioural state
//' @param  dx grid cell size in the space (c.f. delta(0))
//'
//' @return sparse transition rate matrix
// [[Rcpp::export]]
arma::sp_mat CalcTrm(const arma::vec num_cells, const double sd, const double dx) {
  arma::sp_mat tpr = arma::zeros<arma::sp_mat>(num_cells(0), num_cells(0));
  double rate = sd * sd / (2 * dx * dx);
  int s;
  for (int i = 0; i < num_cells(1); ++i) {
    for (int j = 0; j < num_cells(2); ++j) {
      s = i + num_cells(1) * j;
      if (i < num_cells(1) - 1) {
        tpr(s, s + 1) = rate;
      }
      if (i > 0) {
        tpr(s, s - 1) = rate;
      }
      if (j < num_cells(2) - 1) {
        tpr(s, s + num_cells(1)) = rate;
      }
      if (j > 0) {
        tpr(s, s - num_cells(1)) = rate;
      }
      tpr(s, s) = -4 * rate;
    }
  }
  return tpr.t();
}
//' Diffuse probability distribution over space
//'
//' @description Calculate product of v with matrix exponential of a using
//' the Arnoldi process. Thereby diffusing the probability distribution
//' according to Brownian motion. Code is transcribed from Expokit package.
//'
//' @note NOTICE
//' Permission to use, copy, modify, and distribute EXPOKIT and its
//'   supporting documentation for non-commercial purposes, is hereby
//'     granted without fee, provided that this permission message and
//'     copyright notice appear in all copies. Approval must be sought for
//'       commercial purposes as testimony of its usage in applications.
//'
//'     Neither the Institution (University of Queensland) nor the Author
//'       make any representations about the suitability of this software for
//'         any purpose.  This software is provided ``as is'' without express or
//'        implied warranty.
//'
//'       The work resulting from EXPOKIT has been published in ACM-Transactions
//'         on Mathematical Software, 24(1):130-156, 1998.
//'
//'       The bibtex record of the citation:
//'
//'         ARTICLE{EXPOKIT,
//'                  AUTHOR  = {Sidje, R. B.},
//'                  TITLE   = {{Expokit.} {A} Software Package for
//'                    Computing Matrix Exponentials},
//'                    JOURNAL = {ACM Trans. Math. Softw.},
//'                    VOLUME  = {24},
//'                    NUMBER  = {1},
//'                    PAGES   = {130-156}
//'           YEAR    = {1998}
//'         }
//'
//'       Certain elements of the current software may include inadequacies
//'         that may be corrected at any time, as they are discovered. The Web
//'         always contains the latest updates.
//'
//'       Original Author:
//'         Roger B. Sidje <rbs@maths.uq.edu.au>
//'         Department of Mathematics, University of Queensland
//'         Brisbane, QLD-4072, Australia, (c) 1996-2006 All Rights Reserved
//'
//' @param a transition rate matrix
//' @param  v vector to be multiplied
//' @param  t time to diffuse over
//' @param  num_cells vector with number of cells in (total space, x-direction,
//'     y-direction)
//' @param  krylov_dim dimension of the approximating Krylov space
//' @param  tol tolerance in error
//'
//' @return  diffused probability distribution
// [[Rcpp::export]]
arma::rowvec Diffuse(const arma::sp_mat a,
                     const arma::rowvec v,
                     const double t,
                     const arma::vec num_cells,
                     const int& krylov_dim = 30,
                     const double& tol = 1e-10) {
  double m = fmin(a.n_rows, krylov_dim);
  double anorm = norm(a, "Inf");
  double mxrej = 10;
  double mx;
  double btol = 1e-7;
  double gamma = 0.9;
  double mb = m;
  int nstep = 0;
  double t_now = 0;
  double t_step;
  double delta = 1.2;
  double t_out = fabs(t);
  double s_error = 0;
  double rndoff = anorm * 1e-16;
  int k1 = 1;
  double xm = 1 / m;
  double normv = norm(v);
  double avnorm;
  double beta = normv;
  double fact = std::pow((m + 1) / std::exp(1), m + 1) * std::sqrt(2 * M_PI * (m + 1));
  double t_new = (1.0 / anorm) * std::pow((fact * tol) / (4 * beta * anorm), xm);
  double s = std::pow(10, std::floor(std::log10(t_new)) - 1);
  t_new = std::ceil(t_new / s) * s;
  double sgn = t > 0 ? 1 : -1;
  int ireject;
  double err_loc;
  double phi1;
  double phi2;
  arma::vec w = v.t();
  double hump = normv;
  arma::mat vmat = arma::zeros<arma::mat>(a.n_rows, m + 1);
  arma::mat hmat = arma::zeros<arma::mat>(m + 2, m + 2);
  arma::mat fmat;
  arma::vec p;
  while (t_now < t_out) {
    Rcpp::checkUserInterrupt();
    ++nstep;
    t_step = fmin(t_out - t_now, t_new);
    vmat.zeros();
    hmat.zeros();
    vmat.col(0) = (1 / beta) * w;
    for (int j = 0; j < m; ++j) {
      p = a * vmat.col(j);
      for (int i = 0; i <= j; ++i) {
        hmat(i, j) = dot(vmat.col(i), p);
        p -= hmat(i, j) * vmat.col(i);
      }
      s = norm(p);
      if (s < btol) {
        k1 = 0;
        mb = j;
        t_step = t_out - t_now;
        break;
      }
      hmat(j + 1, j) = s;
      vmat.col(j + 1) = (1 / s) * p;
    }
    if (k1 != 0) {
      hmat(m + 1, m) = 1;
      avnorm = norm(a * vmat.col(m));
    }
    ireject = 0;
    while (ireject <= mxrej) {
      mx = mb + k1;
      fmat = expmat(sgn * t_step * hmat.submat(0, 0, mx, mx));

      if (k1 == 0) {
        err_loc = btol;
        break;
      }
      else {
        phi1 = fabs(beta * fmat(m, 0));
        phi2 = fabs(beta * fmat(m + 1, 0) * avnorm);
        if (phi1 > 10 * phi2) {
          err_loc = phi2;
          xm = 1 / m;
        }
        else if (phi1 > phi2) {
          err_loc = (phi1 * phi2) / (phi1 - phi2);
          xm = 1 / m;
        }
        else {
          err_loc = phi1;
          xm = 1 / (m - 1);
        }
      }
      if (err_loc <= delta * t_step * tol) break;
      else {
        t_step = gamma * t_step * std::pow(t_step * tol / err_loc, xm);
        s = std::pow(10, std::floor(std::log10(t_step)) - 1);
        t_step = std::ceil(t_step / s) * s;
        if (ireject == mxrej) {
          Rcpp::Rcout << "error: requested tolerance too high for Krylov approximation" << std::endl;
        }
        ++ireject;
      }
    }
    mx = mb + fmax(0, k1 - 1);
    w = vmat.cols(0, mx) * beta * fmat.col(0).rows(0, mx);
    beta = norm(w);
    hump = fmax(hump, beta);

    t_now = t_now + t_step;
    t_new = gamma * t_step * std::pow(t_step * tol / err_loc, xm);
    s = std::pow(10, std::floor(std::log10(t_new) - 1));
    t_new = std::ceil(t_new / s) * s;

    err_loc = fmax(err_loc, rndoff);
    s_error += err_loc;
  }
  double err = s_error;
  hump = hump / normv;
  return w.t();
}
//' Calculates the initial distribution of animal locations.
//' Assumes uniform distribution relative to transect.
//'
//' @param  num_cells vector with number of cells in (total space, x-direction,
//'     y-direction)
//' @param  delta spatial and temporal increments (dx, dt)
//' @param  region_size size of survey region in (x,y) extents
//'
//' @return Row vector with i^th entry probability animal in i^th grid cell initially
// [[Rcpp::export]]
arma::rowvec CalcInitialDistribution(const arma::vec num_cells,
                                     const arma::vec delta,
                                     const arma::vec region_size) {
  arma::rowvec initial_phi = arma::ones<arma::rowvec>(num_cells(0));
  initial_phi *= delta(0) * delta(0);
  initial_phi /=  prod(region_size);
  return(initial_phi);
}
//' Transform working parameters (for the optimiser) to natural parameters
//' @param working_parameter working parameters
//' @param hzfn hazard function type
//' @return natural parameters
// [[Rcpp::export]]
arma::vec Working2Natural(arma::vec working_parameter, int hzfn = 1) {
  arma::vec parameter = arma::exp(working_parameter);
  //parameter(1) += 2;
  return parameter;
}
//' Transform natural parameters to unconstrained working parameters
//' @param parameter natural parameters
//' @param hzfn hazard function type
//' @return working parameters
// [[Rcpp::export]]
arma::vec Natural2Working(arma::vec parameter, int hzfn = 1) {
  arma::vec working_parameter(parameter);
  //working_parameter(1) -= 2;
  working_parameter = arma::log(working_parameter);
  return working_parameter;
}

//' Calculates hazard of detection
//'
//' @param  x relative x coordinate
//' @param  y relative y coordinate
//' @param  dt time increment
//' @param observer_speed speed of the observer
//' @param parameter vector of (detection shape, detection scale)
//' @param  type transect type (0 = line, 1 = point)
//' @param hzfn hazard function code (see ?hazardfns)
//'
//' @return  hazard of detection
// [[Rcpp::export]]
double CalcHazard(const double x,
                  const double y,
                  const double dt,
                  const double observer_speed,
                  const arma::vec parameter,
                  const int type,
                  const int hzfn) {
  double hazard = 0;
  double r0, r1, abeta, abeta2, y1;
  double s, sx, sy, d, c, k;
  switch(hzfn) {
  case 0:
    // Hayes and Buckland isotropic h(r) = (r/s)^(-2)
    // parameter = (s, d)
    s = parameter(0);
    d = 1;
    c = pow(s, d);
    r0 = x * x + y * y;
    if (type == 1) {
      hazard = dt * c / pow(r0, 0.5 * d);
    }
    else {
      // assume cannot detect behind observer
      if (y < 0) return 0;
      abeta = 0.5 * (d - 1.0);
      y1 = y - observer_speed * dt;
      if (y1 < 0) y1 = 0;
      r1 = x * x  + y1 * y1;
      if (r1 < 1e-10) return arma::datum::inf;
      if (fabs(x) < 1e-10) {
        if (fabs(d - 1) < 1e-10) {
          hazard = log(sqrt(r1)) - log(sqrt(r0));
          hazard *= c;
        } else {
          hazard = 1.0 / pow(r1, abeta) - 1.0 / pow(r0, abeta);
          hazard *= c  / (d - 1.0);
        }
      } else {
        hazard = R::pbeta(x * x / r1, abeta, 0.5, 1, 0) - R::pbeta(x * x / r0,
                          abeta, 0.5, 1, 0);
        hazard *= R::beta(abeta, 0.5) * c / (2.0 * pow(fabs(x),
                                             d - 1.0));
      }
    }
    return hazard;
    break;

  case 1:
    // Hayes and Buckland isotropic h(r) = (r/s)^(-d)
    // parameter = (s, d)
    s = parameter(0);
    d = 1 + parameter(1);
    c = pow(s, d);
    r0 = x * x + y * y;
    if (type == 1) {
      hazard = dt * c / pow(r0, 0.5 * d);
    }
    else {
      // assume cannot detect behind observer
      if (y < 0) return 0;
      abeta = 0.5 * (d - 1.0);
      y1 = y - observer_speed * dt;
      if (y1 < 0) y1 = 0;
      r1 = x * x  + y1 * y1;
      if (r1 < 1e-10) return arma::datum::inf;
      if (fabs(x) < 1e-10) {
        if (fabs(d - 1) < 1e-10) {
          hazard = log(sqrt(r0)) - log(sqrt(r1));
          hazard *= c;
        } else {
          hazard = 1.0 / pow(r1, abeta) - 1.0 / pow(r0, abeta);
          hazard *= c / (d - 1.0);
        }
      } else {
        hazard = R::pbeta(x * x / r1, abeta, 0.5, 1, 0) - R::pbeta(x * x / r0,
                          abeta, 0.5, 1, 0);
        hazard *= R::beta(abeta, 0.5) * c / (2.0 * pow(fabs(x),
                                                 d - 1.0));
      }
    }
    return hazard;
    break;

  case 2:
    // Hayes and Buckland anisotropic h(r) = (x^2/sx^2 + y^2/sy^2)^(-d/2)
    // parameter = (sx, sy, d)
    sx = parameter(0);
    sy = parameter(1);
    d = 1 + parameter(2);
    r0 = (x * x) / (sx * sx) + (y * y) / (sy * sy);
    if (type == 1) {
      hazard = dt * pow(r0, -0.5 * d);
    }
    else {
      // assume cannot detect behind observer
      if (y < 0) return 0;
      abeta = 0.5 * (d - 1.0);
      y1 = y - observer_speed * dt;
      if (y1 < 0) y1 = 0;
      r1 = (x * x) / (sx * sx)  + (y1 * y1) / (sy * sy);
      if (r1 < 1e-10) return arma::datum::inf;
      if (fabs(x) < 1e-10) {
        if (fabs(d - 1) < 1e-10) {
          hazard = log(sqrt(y)) - log(sqrt(y1));
          hazard *= pow(sy, d);
        } else {
          hazard = 1.0 / pow(y1, abeta) - 1.0 / pow(y, abeta);
          hazard *= pow(sy, d) / (d - 1.0);
        }
      } else {
        hazard = R::pbeta(x * x / (sx * sx * r1), abeta, 0.5, 1, 0) - R::pbeta(x * x / (sx * sx * r0),
                          abeta, 0.5, 1, 0);
        hazard *= R::beta(abeta, 0.5) * pow(sx, d - 1) * sy / (2.0 * pow(fabs(x),
                                             d - 1.0));
      }
    }
    return hazard;
    break;

  case 3:
    // Hayes and Buckland shape-anisotropic h(r) = (x^2+(y+k)^2)^(-d/2)
    // parameter = (s, d, k)
    s = parameter(0);
    d = 1 + parameter(1);
    k = parameter(2);
    c = pow(s, d);
    r0 = x * x / (s * s) + (y / s + k) * (y / s + k);
    if (type == 1) {
      hazard = dt * c * (1 + k * y / sqrt(r0)) / pow(r0, 0.5 * d);
    }
    else {
      // assume cannot detect behind observer
      if (y < 0) return 0;
      abeta = 0.5 * (d - 1.0);
      y1 = y - observer_speed * dt;
      if (y1 < 0) y1 = 0;
      r1 = x * x / (s * s) + (y1 / s + k) * (y1 / s + k);
      if (r1 < 1e-10) return arma::datum::inf;
      if (fabs(x) < 1e-10) {
        if (fabs(d - 1) < 1e-10) {
          hazard = log(sqrt(y + s * k)) - log(sqrt(y1 + s * k));
          hazard *= c;
        } else {
          hazard = 1.0 / pow(y1 + s * k, abeta) - 1.0 / pow(y + s * k, abeta);
          hazard *= c / (d - 1.0);
        }
      } else {
        hazard = R::pbeta(x * x / (s * s * r1), abeta, 0.5, 1, 0) - R::pbeta(x * x / (s * s * r0),
                          abeta, 0.5, 1, 0);
        hazard *= R::beta(abeta, 0.5) * c / (2.0 * pow(fabs(x), d - 1.0));
      }
    }
    return hazard;
    break;

  case 4:
    // Hayes and Buckland anisotropic h(r) = (x^2/sx^2 + (y/sy + k)^2)^(-d/2)
    // parameter = (sx, sy, d, k)
    sx = parameter(0);
    sy = parameter(1);
    d = 1 + parameter(2);
    k = parameter(3);
    r0 = (x * x) / (sx * sx) + pow(y / sy + k, 2.0);
    if (type == 1) {
      hazard = dt * pow(r0, -0.5 * d);
    }
    else {
      // assume cannot detect behind observer
      if (y < 0) return 0;
      abeta = 0.5 * (d - 1.0);
      y1 = y - observer_speed * dt;
      if (y1 < 0) y1 = 0;
      r1 = (x * x) / (sx * sx)  + pow(y1 / sy + k, 2.0);
      if (r1 < 1e-10) return arma::datum::inf;
      if (fabs(x) < 1e-10) {
        if (fabs(d - 1) < 1e-10) {
          hazard = log(sqrt(y)) - log(sqrt(y1));
          hazard *= pow(sy, d);
        } else {
          hazard = 1.0 / pow(y1, abeta) - 1.0 / pow(y, abeta);
          hazard *= pow(sy, d) / (d - 1.0);
        }
      } else {
        hazard = R::pbeta(x * x / (sx * sx * r1), abeta, 0.5, 1, 0) - R::pbeta(x * x / (sx * sx * r0),
                          abeta, 0.5, 1, 0);
        hazard *= R::beta(abeta, 0.5) * pow(sx, d - 1) * sy / (2.0 * pow(fabs(x),
                                            d - 1.0));
      }
    }
    return hazard;
    break;

  default:
    Rcpp::Rcout << "error: no hazard specified." << std::endl;
  return -arma::datum::inf;
  }
}
//' Computes the probability of survival for each spatial location
//'
//' @param t time step
//' @param parameter (scale, shape, diffusion) parameter
//' @param num_cells number of cells in (x, y, all) dimensions
//' @param delta (dx, dt) vector
//' @param strip_size size of strip in (x, y) dimensions
//' @param buffer buffer size
//' @param observer_speed speed of the observer
//' @param type transect type
//' @param hzfn hazard function code
//' @param nint not used
//'
//'  @return row vector of survival probabilities over space
// [[Rcpp::export]]
arma::rowvec CalcSurvivalPr(const int t,
                      const arma::vec parameter,
                      const arma::vec num_cells,
                      const arma::vec delta,
                      const arma::vec strip_size,
                      const double buffer,
                      const double observer_speed,
                      const int type,
                      const int hzfn,
                      const int nint = 4) {

  arma::rowvec pr_survive = arma::ones<arma::rowvec>(num_cells(0));
  arma::vec observer_position(GetObserverPosition(t * delta(1), strip_size, buffer, delta, type,
                                            observer_speed));
  double x, ix;
  double y, iy;
  int s;
  int ymin = 0;
  if (type == 0) ymin = floor(observer_position(1) / delta(0));
  for (int x_cell = 0; x_cell < num_cells(1); ++x_cell) {
    for (int y_cell = ymin; y_cell < num_cells(2); ++y_cell) {
      s = x_cell + num_cells(1) * y_cell;
      pr_survive(s) = 0;
      x = x_cell * delta(0) - observer_position(0);
      y = y_cell * delta(0) - observer_position(1);
      for (int i = 0; i < nint; ++i) {
        //for (int j = 0; j < nint; ++j) {
          ix = x + (i * delta(0)) / nint;
          //iy = y + (j * delta(0)) / nint;
          pr_survive(s) += CalcHazard(ix, y, delta(1), observer_speed, parameter, type, hzfn);
       // }
      }
      //pr_survive(s) /= 1.0*nint*nint;
      pr_survive(s) /= 1.0*nint;
      pr_survive(s) = exp(-pr_survive(s));
    }
  }
  return pr_survive;
}

//' Thins probability distribution by the proportion detected in each grid cell
//'
//' @param t time step
//' @param pr probability distribution over finite grid
//' @param parameter (scale, shape, diffusion) parameter
//' @param num_cells number of cells in (x, y, all) dimensions
//' @param delta (dx, dt) vector
//' @param strip_size size of strip in (x, y) dimensions
//' @param buffer buffer width
//' @param observer_speed speed of the observer
//' @param type transect type
//' @param hzfn hazard function code
//'
//'  @return  thinned probability distribution
// [[Rcpp::export]]
arma::rowvec Detect(const int t,
                    const arma::rowvec pr,
                    const arma::vec parameter,
                    const arma::vec num_cells,
                    const arma::vec delta,
                    const arma::vec strip_size,
                    const double buffer,
                    const double observer_speed,
                    const int type,
                    const int hzfn) {
  arma::rowvec pr_survive = CalcSurvivalPr(t, parameter, num_cells, delta,
		  strip_size, buffer, observer_speed, type, hzfn);
  pr_survive %= pr;
  return(pr_survive);
}

//' Compute hazard of each detection within time-step
//'
//' @param data (x, y, t) data matrix
//' @param dt time step
//' @param transdat transect data matrix
//' @param parameter (scale, shape, diffusion) parameters
//' @param observer_speed speed of observer
//' @param type 1 = point, 0 = line transect
//' @param hzfn hazard function code (see ?hazardfns)
//' @return PDF for within-timestep detection
// [[Rcpp::export]]
double CalcHazardDetected(const arma::mat data,
                          double dt,
                          arma::mat transdat,
                          arma::vec parameter,
                          double observer_speed,
                          int type,
                          const int hzfn) {
  arma::vec r2, cosang;
  arma::vec t_remaining = data.col(4) - floor((data.col(4)) / dt) * dt;
  double log_hazard = 0;
  for (int i = 0; i < data.n_rows; ++i) {
    log_hazard -= CalcHazard(data(i, 2), data(i, 3) + t_remaining(i) *
		    observer_speed, t_remaining(i), observer_speed, parameter, type, hzfn);
  }

  double s, sx, sy, d, c, k;
  switch(hzfn) {
  case 0:
    s = parameter(0);
    d = 1;
    c = pow(s, d);
    r2 = sum(data.cols(2, 3) % data.cols(2, 3), 1);
    log_hazard += arma::accu(log(c) - 0.5 * d * log(r2));
    break;

  case 1:
    s = parameter(0);
    d = 1 + parameter(1);
    c = pow(s, d);
    r2 = sum(data.cols(2, 3) % data.cols(2, 3), 1);
    log_hazard += arma::accu(log(c) - 0.5 * d * log(r2));
   break;

  case 2:
    sx = parameter(0);
    sy = parameter(1);
    d = 1 + parameter(2);
    log_hazard += -0.5 * d * arma::accu(log(data.col(2) % data.col(2) / (sx * sx) + data.col(3) % data.col(3) / (sy * sy)));
    break;

  case 3:
    s = parameter(0);
    d = 1 + parameter(1);
    k = parameter(2);
    r2 = data.col(2) % data.col(2) / (s * s) + pow(data.col(3) / s + k, 2.0);
    log_hazard += arma::accu(- 0.5 * d * log(r2));
    break;

  case 4:
    sx = parameter(0);
    sy = parameter(1);
    d = 1 + parameter(2);
    k = parameter(3);
    r2 = data.col(2) % data.col(2) / (sx * sx) + pow(data.col(3) / sy + k, 2.0);
    log_hazard += arma::accu(- 0.5 * d * log(r2));
    break;

  default:
    Rcpp::Rcout << "error: no hazard specified." << std::endl;
    return -arma::datum::inf;
  }
  return log_hazard;
}
//' Calculates movement model log-likelihood
//'
//' @param  sd diffusion
//' @param  data Rcpp List where each component represent an individual path
//' and continas a matrix where each row is an observed location (x,y,t)
//'
//' @return log-likelihood
// [[Rcpp::export]]
double CalcMovementLogLikelihood(const double sd, const Rcpp::List data) {
  double log_likelihood = 0;
  arma::vec xdiff, ydiff;
  int ntags = data.size();
  arma::mat tag;
  for (int i = 0; i < ntags; ++i) {
    tag = Rcpp::as<arma::mat>(data(i));
    arma::vec xdiff = arma::diff(tag.col(0));
    arma::vec ydiff = arma::diff(tag.col(1));
    arma::vec tdiff = arma::diff(tag.col(2));
    log_likelihood -= tag.n_rows * log(2 * M_PI * sd * sd);
    log_likelihood -= accu(log(tdiff) + (xdiff % xdiff + ydiff % ydiff) / (2 * sd * sd * tdiff));
  }
  return log_likelihood;
}
//' Computes what grid cells are inside and outside transect
//'
//' @param  num_cells number of cells in (total, x, y) direction
//' @param strip_size size of strip in (x,y) directions
//' @param dx grid cell size
//' @param w for lines, half-width, for points radius
//' @param ymax maximum forward distance for lines
//' @param buffer distance
//' @param type =0 for lines, =1 for points
//'
//' @return vector with 1 for each grid cell inside and 0 otherwise
// [[Rcpp::export]]
arma::rowvec InTransect(const arma::vec num_cells,
                        const arma::vec strip_size,
                        const double dx,
                        const double w,
                        const double ymax,
                        const double buffer,
                        const int type) {
  arma::rowvec intrans = arma::zeros<arma::rowvec>(num_cells(0));
  double x, y, r;
  double top, bot, lenx, leny;
  if (type == 0) {
    for (int i = 0; i < num_cells(1); ++i) {
      x = i * dx - strip_size(0) * 0.5;
      top = fmin(x + dx, w);
      bot = fmax(x, -w);
      lenx = top - bot;
      if (lenx < 0) lenx = 0;
      for (int j = 0; j < num_cells(2); ++j) {
        y = j * dx - buffer;
        top = fmin(y + dx, ymax);
        bot = fmax(y, 0);
        leny = top - bot;
        if (leny < 0) leny = 0;
        intrans(i + j * num_cells(1)) = lenx * leny / (dx * dx);
      }
    }
  } else {
    for (int i = 0; i < num_cells(1); ++i) {
      x = i * dx - strip_size(0) * 0.5;
      for (int j = 0; j < num_cells(2); ++j) {
        y = j * dx - strip_size(1) * 0.5;
        r = x * x + y * y;
        if (r <= w*w) intrans(i + j * num_cells(1)) = 1;
      }
    }
  }
  return intrans;
}

//' Computes negative log-likelihood of moveDs model
//'
//' @param  working_parameter unconstrained version of parameter vector containing
//'     (detection shape, detection scale, diffusion sd)
//' @param start start value for parameters on natural scale
//' @param  data matrix with (trans id, grid cell,t) distance sampling survey data (assumed to be ordered by transect and time)
//' @param  transdat matrix with (stripsize(1), numcells in y, totaltimestep, number of observations)
//' @param  auxiliary_data vector containing (area x extent, area y extent, strip width, transect_type)
//' @param  delta vector of (dx, dt) spacetime increments
//' @param  num_cells number of cells in (total space, x-direction, y-direction)
//' @param  T total time of survey for longest transect
//' @param  ymax maximum length of a transect
//' @param  buffer buffer distance
//' @param  movement_data field object where each component represents an individual
//'   path and contains a matrix where each row is an observed location (x,y,t)
//' @param fixed_sd if move_method = 2
//' @param hzfn hazard function code (see ?hazardfns)
//' @param  move_method 0 = 2d CDS model, 1 = 2d MDS model (movement estimated),
//'    2 = 2d MDS model (movement fixed)
//' @param  print if TRUE then print likelihood and parmeters after evaluation
//' @param con parameters are constrained to be between 1/con * start value and
//' con * start value
//'
//' @return  negative log-likelihood
// [[Rcpp::export]]
double NegativeLogLikelihood(const arma::vec working_parameter,
                             const arma::vec start,
                             const arma::mat data,
                             const arma::mat transdat,
                             const arma::vec auxiliary_data,
                             const arma::vec delta,
                             const arma::vec num_cells,
                             const int T,
                             const double ymax,
                             const double buffer,
                             const Rcpp::List movement_data,
                             const double fixed_sd = 0,
                             const int hzfn = 1,
                             const int move_method = 1,
                             const bool print = false,
                             const double con = 100) {
  // unpack auxiliary data
  arma::vec region_size(auxiliary_data.rows(0, 1));
  arma::vec strip_size(2);
  strip_size(0) = 2 * auxiliary_data(2) + 2 * buffer;
  strip_size(1) = ymax + 2 * buffer;
  double observer_speed = auxiliary_data(3);
  int num_transects = transdat.n_rows;
  int transect_type = auxiliary_data(4);
  double dx = delta(0);
  double dt = delta(1);
  // unpack parameters
  arma::vec parameter = Working2Natural(working_parameter, hzfn);
  int npar = parameter.n_elem;
  // constraints
  for (int p = 0; p < npar; ++p) {
    if (parameter(p) > con * start(p)) return arma::datum::inf;
    if (parameter(p) < start(p) / con) return arma::datum::inf;
  }
  double sd = 0;
  if (move_method == 1) sd = parameter(npar - 1);
  if (move_method == 2) sd = fixed_sd;
  // setup variables
  int curtrans = 0;
  int curobs = 0;
  double pr_survived;
  double pr_outside;
  double accu_hazard;
  double pdet;
  double llk = 0;
  // calculate initial probability in each grid cell
  arma::rowvec pr_t = CalcInitialDistribution(num_cells, delta, region_size);
  arma::rowvec old_pr_t(pr_t);
  // probability outside buffer region at t = 0
  pr_outside = 1.0 - arma::prod(strip_size) / arma::prod(region_size);
  // compute movement matrices for survey
  arma::sp_mat trm;
  arma::rowvec flux = arma::ones<arma::rowvec>(num_cells(0));
  if (move_method > 0) {
    trm = CalcTrm(num_cells, sd, dx);
    flux = Diffuse(trm.t(), flux, dt, num_cells);
    flux = 1.0 - flux;
  }
  double num_boundary_states = floor(prod(region_size) / (dx * dx)) - num_cells(0);
  // intialise variables
  double curt = floor((data(curobs, 4)) / dt);
  if (curt < 0) curt = 0;
  if ((curt > T - 1) & (curt < T + 1)) curt = T - 1;
  int endtime = transdat(curtrans, 2);
  arma::rowvec intrans = InTransect(num_cells, strip_size, dx, auxiliary_data(2), ymax, buffer, transect_type);
  accu_hazard = 0;
  pdet = 0;
  double diff = 0;
  // compute HMM approximation
  for (int t = 0; t < T; ++t) {
    Rcpp::checkUserInterrupt();
    // add to pr_obs, the observations that occur during time interval t
    while (t == curt) {
      if (data(curobs, 1) > num_cells(0)) {
        Rcpp::Rcout << "Warning: buffer region too small to include all detections." << std::endl;
      } else {
        llk += log(pr_t(data(curobs, 1))) + accu_hazard - log(dx * dx);
      }
      ++curobs;
      if (curobs > data.n_rows - 1) {
        curt = T + 1;
      } else {
        curt = floor((data(curobs, 4)) / dt);
        if (curt < 0) curt = 0;
        if ((curt > T - 1) & (curt < T + 1)) curt = T - 1;
      }
    }
    // thin pr_t by those that are detected
    diff = 0;
    diff += arma::accu(pr_t % intrans);
    pr_t = Detect(t, pr_t, parameter, num_cells, delta, strip_size, buffer, observer_speed, transect_type, hzfn);
    diff -= arma::accu(pr_t % intrans);
    pdet += diff * exp(accu_hazard);
    if (arma::accu(pr_t) < 1e-10) return(arma::datum::inf);
    // move animals
    if (move_method > 0) {
      old_pr_t = pr_t;
      //move animals that are inside strip
      try {
        pr_t = Diffuse(trm, pr_t, dt, num_cells);
      } catch(...) {
        return arma::datum::inf;
      }
      //move animals outside strip that come into strip
      pr_t += flux * pr_outside / num_boundary_states;
      //account for transversal of boundary
      pr_outside += accu(old_pr_t % flux) - accu(flux) * pr_outside / num_boundary_states;
    }
    // add contribution to accu_hazard
    pr_survived = accu(pr_t) + pr_outside;
    accu_hazard += log(pr_survived);
    // scale to avoid underflow
    pr_t /= pr_survived;
    pr_outside /=  pr_survived;
    // if transect ends divide by conditional probability
    while (endtime == t) {
     llk -= transdat(curtrans, 1) * log(pdet);
     ++curtrans;
     if (curtrans > transdat.n_rows - 1) {
       endtime = T + 1;
     } else {
       endtime = transdat(curtrans, 2);
     }
    }
 }
 // add hazard of detections
 llk += CalcHazardDetected(data, dt, transdat, parameter, observer_speed, transect_type, hzfn);
 double movement_log_likelihood = 0;
 if (move_method == 1) movement_log_likelihood = CalcMovementLogLikelihood(sd, movement_data);
 double negative_log_likelihood = -llk - movement_log_likelihood;
 if (print) {
   int old_precision = Rcpp::Rcout.precision();
   Rcpp::Rcout.precision(4);
   Rcpp::Rcout << -negative_log_likelihood << "   ";
   for (int par = 0; par < npar; ++par) Rcpp::Rcout << parameter(par) << "   ";
   Rcpp::Rcout << std::endl;
 }
 return negative_log_likelihood;
}
//' Computes covered area for entire survey
//'
//' @param  working_parameter unconstrained version of parameter vector containing
//'     (detection shape, detection scale, diffusion sd)
//' @param  transdat matrix with (stripsize(1), numcells in y, totaltimestep, number of observations)
//' @param  auxiliary_data vector containing (area x extent, area y extent, strip width, transect_type)
//' @param  delta vector of (dx, dt) spacetime increments
//' @param  num_cells number of cells in (total space, x-direction, y-direction)
//' @param  T total time of survey for longest transect
//' @param  ymax maximum length of a transect
//' @param  buffer buffer distance
//' @param fixed_sd if move_method = 2
//' @param hzfn hazard function code (see ?hazardfns)
//' @param  move_method 0 = 2d CDS model, 1 = 2d MDS model (movement estimated),
//'    2 = 2d MDS model (movement fixed)
//'
//' @return  negative log-likelihood
//' @return covered area
//' unpack auxiliary data
// [[Rcpp::export]]
double GetPenc(const arma::vec working_parameter,
               const arma::mat transdat,
               const arma::vec auxiliary_data,
               const arma::vec delta,
               const arma::vec num_cells,
               const int T,
               const double ymax,
               const double buffer,
               const double fixed_sd,
               const int hzfn,
               int move_method) {

  arma::vec region_size(auxiliary_data.rows(0, 1));
  arma::vec strip_size(2);
  strip_size(0) = 2 * auxiliary_data(2) + 2 * buffer;
  strip_size(1) = ymax + 2 * buffer;
  double observer_speed = auxiliary_data(3);
  int num_transects = transdat.n_rows;
  int transect_type = auxiliary_data(4);
  double dx = delta(0);
  double dt = delta(1);
  // unpack parameters
  arma::vec parameter = Working2Natural(working_parameter, hzfn);
  int npar = parameter.n_elem;
  double sd = 0;
  if (move_method == 1) sd = parameter(npar - 1);
  if (move_method == 2) sd = fixed_sd;
  // setup variables
  int curtrans = 0;
  int curobs = 0;
  double pr_survived;
  double pr_outside;
  double accu_hazard;
  double pdet;
  arma::vec penc(num_transects); penc.zeros();
  // calculate initial probability in each grid cell
  arma::rowvec pr_t = CalcInitialDistribution(num_cells, delta, region_size);
  arma::rowvec old_pr_t(pr_t);
  // probability outside buffer region at t = 0
  pr_outside = 1.0 - arma::prod(strip_size) / arma::prod(region_size);
  // compute movement matrices for survey
  arma::sp_mat trm;
  arma::rowvec flux = arma::ones<arma::rowvec>(num_cells(0));
  if (move_method > 0) {
    trm = CalcTrm(num_cells, sd, dx);
    flux = Diffuse(trm.t(), flux, dt, num_cells);
    flux = 1.0 - flux;
  }
  double num_boundary_states = floor(prod(region_size) / (dx * dx)) - num_cells(0);
  // intialise variables
  int endtime = transdat(curtrans, 2);
  arma::rowvec intrans = InTransect(num_cells, strip_size, dx, auxiliary_data(2), ymax, buffer, transect_type);
  accu_hazard = 0;
  pdet = 0;
  double diff;
  // compute HMM approximation
  for (int t = 0; t < T; ++t) {
    Rcpp::checkUserInterrupt();
    // thin pr_t by those that are detected
    diff = 0;
    diff += arma::accu(pr_t % intrans);
    pr_t = Detect(t, pr_t, parameter, num_cells, delta, strip_size, buffer, observer_speed, transect_type, hzfn);
    diff -= arma::accu(pr_t % intrans);
    pdet += diff * exp(accu_hazard);
    // move animals
    if (move_method > 0) {
      old_pr_t = pr_t;
      // move animals that are inside strip
      pr_t = Diffuse(trm, pr_t, dt, num_cells);
      // move animals outside strip that come into strip
      pr_t += flux * pr_outside / num_boundary_states;
      // account for transversal of boundary
      pr_outside += accu(old_pr_t % flux) - accu(flux) * pr_outside / num_boundary_states;
    }
    // add contribution to accu_hazard
    pr_survived = accu(pr_t) + pr_outside;
    accu_hazard += log(pr_survived);
    // scale to avoid underflow
    pr_t /= pr_survived;
    pr_outside /=  pr_survived;
    // if transect ends divide by conditional probability
    while (endtime == t) {
      penc(curtrans) = pdet;
      ++curtrans;
      if (curtrans > transdat.n_rows - 1) {
        endtime = T + 1;
      } else {
        endtime = transdat(curtrans, 2);
      }
    }
  }
  return arma::accu(penc);
}
//' Computes PDF of observed detections for each (x,y) cell around the observer.
//'
//' @param  working_parameter unconstrained version of parameter vector containing
//'     (detection shape, detection scale, diffusion sd)
//' @param range to compute out to in x and y directions
//' @param  transdat matrix with (stripsize(1), numcells in y, totaltimestep, number of observations)
//' @param  auxiliary_data vector containing (area x extent, area y extent, strip width, transect_type)
//' @param  delta vector of (dx, dt) spacetime increments
//' @param  num_cells number of cells in (total space, x-direction, y-direction)
//' @param  T total time of survey for longest transect
//' @param  ymax maximum length of a transect
//' @param  buffer buffer distance
//' @param fixed_sd if move_method = 2
//' @param hzfn hazard function code (see ?hazardfns)
//' @param  move_method 0 = 2d CDS model, 1 = 2d MDS model (movement estimated),
//'    2 = 2d MDS model (movement fixed)
//'
//' @return  matrix where (i,j) entry is cell i*dx perpendicular and j*dx forward of
//'   observer
//' unpack auxiliary data
// [[Rcpp::export]]
arma::mat GetHist(const arma::vec working_parameter,
               const arma::vec range,
               const arma::mat transdat,
               const arma::vec auxiliary_data,
               const arma::vec delta,
               const arma::vec num_cells,
               const int T,
               const double ymax,
               const double buffer,
               const double fixed_sd = 0,
               const int hzfn = 1,
               int move_method = 1) {

  arma::vec region_size(auxiliary_data.rows(0, 1));
  arma::vec strip_size(2);
  strip_size(0) = 2 * auxiliary_data(2) + 2 * buffer;
  strip_size(1) = ymax + 2 * buffer;
  double observer_speed = auxiliary_data(3);
  int num_transects = transdat.n_rows;
  int transect_type = auxiliary_data(4);
  double dx = delta(0);
  double dt = delta(1);
  // unpack parameters
  arma::vec parameter = Working2Natural(working_parameter, hzfn);
  int npar = parameter.n_elem;
  double sd = 0;
  if (move_method == 1) sd = parameter(npar - 1);
  if (move_method == 2) sd = fixed_sd;
  // setup variables
  int curtrans = 0;
  int curobs = 0;
  double pr_survived;
  double pr_outside;
  double accu_hazard;
  arma::vec obspos;
  int sobs;
  int smax;
  int Nperp = floor(2 * range(0) / dx);
  int Nforw = floor(range(1) / dx);
  arma::rowvec count(num_cells(1) * Nforw); count.zeros();
  arma::rowvec accum(num_cells(1) * Nforw); accum.zeros();
  // nalive is the number of transect still being surveyed in the meta-transect
  int nalive = num_transects;
  // calculate initial probability in each grid cell
  arma::rowvec pr_t = CalcInitialDistribution(num_cells, delta, region_size);
  arma::rowvec old_pr_t(pr_t);
  // probability outside buffer region at t = 0
  pr_outside = 1.0 - arma::prod(strip_size) / arma::prod(region_size);
  // compute movement matrices for survey
  arma::sp_mat trm;
  arma::rowvec flux = arma::ones<arma::rowvec>(num_cells(0));
  if (move_method > 0) {
    trm = CalcTrm(num_cells, sd, dx);
    flux = Diffuse(trm.t(), flux, dt, num_cells);
    flux = 1.0 - flux;
  }
  double num_boundary_states = floor(prod(region_size) / (dx * dx)) - num_cells(0);
  arma::rowvec intrans = InTransect(num_cells, strip_size, dx, auxiliary_data(2), ymax, buffer, transect_type);
  // intialise variables
  int endtime = transdat(curtrans, 2);
  accu_hazard = 0;
  // compute HMM approximation
  for (int t = 0; t < T; ++t) {
    Rcpp::checkUserInterrupt();
    obspos = GetObserverPosition(t * dt, strip_size, buffer, delta, transect_type, observer_speed);
    // observer grid cell
    sobs = num_cells(1) * floor(obspos(1) / dx);
    // sum from that point to 2 * Nperp * Nforw state or max size
    smax = sobs + num_cells(1) * Nforw - 1;
    if (smax >= num_cells(0)) smax = num_cells(0) - 1;
    if (sobs >= num_cells(0)) sobs = num_cells(0) - 1;
    // add in all animals present in cell
    count.cols(0, smax - sobs) += exp(log(pr_t.cols(sobs, smax) % intrans.cols(sobs, smax)) + accu_hazard);
    // thin pr_t by those that are detected
    pr_t = Detect(t, pr_t, parameter, num_cells, delta, strip_size, buffer, observer_speed, transect_type, hzfn);
    // subtract those animals still present (failed to be detected)
    count.cols(0, smax - sobs) -= exp(log(pr_t.cols(sobs, smax) % intrans.cols(sobs, smax)) + accu_hazard);
    if (move_method > 0) {
      old_pr_t = pr_t;
      // move animals that are inside strip
      pr_t = Diffuse(trm, pr_t, dt, num_cells);
      // move animals outside strip that come into strip
      pr_t += flux * pr_outside / num_boundary_states;
      // account for transversal of boundary
      pr_outside += accu(old_pr_t % flux) - accu(flux) * pr_outside / num_boundary_states;
    }
    // add contribution to accu_hazard
    pr_survived = accu(pr_t) + pr_outside;
    accu_hazard += log(pr_survived);
    // scale to avoid underflow
    pr_t /= pr_survived;
    pr_outside /=  pr_survived;
    // if transect ends divide by conditional probability
    while (endtime == t) {
      accum += count;
      --nalive;
      ++curtrans;
      if (curtrans > transdat.n_rows - 1) {
        endtime = T + 1;
      } else {
        endtime = transdat(curtrans, 2);
      }
    }
  }
  return accum;
}


/*    Copyright (c) 2010-2018, 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
 *      PyKEP toolbox, Dario Izzo, ESA Advanced Concepts Team.
 *      Richard H. An Introduction to the Mathematics and Methods of Astrodynamics, Revised
 *          Edition.
 *      Battin, AIAA Education Series.
 *
 */

#include <cmath>

#include <boost/math/special_functions.hpp> // for asinh and acosh

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/MissionSegments/multiRevolutionLambertTargeterIzzo.h"
#include "Tudat/Mathematics/BasicMathematics/convergenceException.h"

namespace tudat
{
namespace mission_segments
{

//! Compute solution for N revolutions and branch.
void MultiRevolutionLambertTargeterIzzo::computeForRevolutionsAndBranch(
        const int aNumberOfRevolutions, const bool aIsRightBranch )
{
    // Adjust parameters for new solution
    numberOfRevolutions = aNumberOfRevolutions;
    isRightBranch = aIsRightBranch;

    // Check whether number of revolutions is possible
    sanityCheckNumberOfRevolutions( );

    // Execute problem solving for new solution
    execute( );
}

//! Get maximum number of revolutions calculated.
int MultiRevolutionLambertTargeterIzzo::getMaximumNumberOfRevolutions( )
{
    if ( !solved )
    {
        transformDimensions( );
        sanityCheckNumberOfRevolutions( );
    }

    return maximumNumberOfRevolutions;
}

//! Sanity check number of revolutions.
void MultiRevolutionLambertTargeterIzzo::sanityCheckNumberOfRevolutions( )
{
    // If not yet defined, calculate number of revolutions possible.
    if ( maximumNumberOfRevolutions == NO_MAXIMUM_REVOLUTIONS )
    {
        // Temporarily store specified number, as numberOfRevolutions is needed to calculate max
        // (this is a tricky way to work, but on the other hand this makes this approach decidedly
        // different from PyKEP routines and it also happens only once per object).
        int copyOfOriginalNumberOfRevolutions = numberOfRevolutions;

        // Calculate first guess of maximum, by dividing the time of flight of the minimum energy
        // ellipse by the normalized time of flight.
        numberOfRevolutions = static_cast< int >(
                    normalizedTimeOfFlight / (
                        mathematical_constants::PI / 2.0
                        * std::sqrt( 2.0 * normalizedSemiPerimeter
                                     * normalizedSemiPerimeter
                                     * normalizedSemiPerimeter ) ) );

        // If the current guess for the maximum is non-zero, then additional analysis is required to
        // determine the correct maximum.
        if( numberOfRevolutions != 0)
        {
            // The following try-block is meant to check whether the solution converges or not. If
            // the current guess for the maximum number of revolutions is correct, then the problem
            // will converge. If it does not, an exception will be thrown stating that it did not
            // converge. Catching this exception allows to decrease the guess only when the
            // exception occurs, and not under other circumstances.
            try
            {
                // Compute root (no further information is required)
                computeRootTimeOfFlight();
            }
            catch( basic_mathematics::ConvergenceException )
            {
                // If the rootfinder did not converge, then the current guess is wrong and needs to
                // be decreased
                numberOfRevolutions--;
            }
        }
        // No further analysis is needed of the current guess is equal to zero.

        // Maximum is now found.
        maximumNumberOfRevolutions = numberOfRevolutions;

        // Reinstating original number of revolutions specified.
        numberOfRevolutions = copyOfOriginalNumberOfRevolutions;
    }

    // Default: compare maximum with specified number of revolutions.
    // If specified is larger than maximum, no solution is possible.
    if ( numberOfRevolutions > maximumNumberOfRevolutions )
    {
        // Throw exception.
        throw std::runtime_error(  "Number of revolutions specified in Lambert problem is larger than possible. Specified number of revolutions is " +
                                   std::to_string( numberOfRevolutions )  + " while the maximum is " +
                                   std::to_string( maximumNumberOfRevolutions ) );
    }
    // Else, nothing wrong.
}

//! Execute solving procedure (for multiple revolutions).
void MultiRevolutionLambertTargeterIzzo::execute( )
{
    // Sanity checks.
    sanityCheckTimeOfFlight( );
    sanityCheckGravitationalParameter( );

    // Transform dimensions.
    transformDimensions( );

    /*// Sanity check for number of revolutions (must be after dimension removal).
    sanityCheckNumberOfRevolutions( );*/

    if ( numberOfRevolutions == 0 )
    {
        // call base class function that works on zero revolutions.
        ZeroRevolutionLambertTargeterIzzo::execute( );
    }
    else
    {
        // Solve multi-rev root.
        double multipleRevolutionXParameter = computeRootTimeOfFlight( );

        // Reconstruct velocities.
        computeVelocities( multipleRevolutionXParameter );
    }

    solved = true;
}

//! Compute time-of-flight using Lagrange's equation (for multiple revolutions).
double MultiRevolutionLambertTargeterIzzo::computeTimeOfFlight( const double xParameter )
{
    // Determine semi-major axis.
    const double semiMajorAxis = normalizedMinimumEnergySemiMajorAxis
            / ( 1.0 - xParameter * xParameter );

    // If x < 1, the solution is an ellipse.
    if ( xParameter < 1.0 )
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * std::acos( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter;

        // If long transfer arc.
        if ( isLongway )
        {
            betaParameter = -2.0 * std::asin(
                        std::sqrt( ( normalizedSemiPerimeter - normalizedChord )
                                   / ( 2.0 * semiMajorAxis ) ) );
        }
        // Otherwise short transfer arc.
        else
        {
            betaParameter = 2.0 * std::asin(
                        std::sqrt( ( normalizedSemiPerimeter - normalizedChord )
                                   / ( 2.0 * semiMajorAxis ) ) );
        }

        // Time-of-flight according to Lagrange including multiple revolutions.
        const double timeOfFlight = semiMajorAxis * std::sqrt( semiMajorAxis ) *
                ( ( alphaParameter - std::sin( alphaParameter ) )
                  - ( betaParameter - std::sin( betaParameter ) )
                  + 2.0 * mathematical_constants::PI
                  * numberOfRevolutions );

        return timeOfFlight;
    }
    // Otherwise it is a hyperbola.
    else
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * boost::math::acosh( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter;

        // If long transfer arc.
        if ( isLongway )
        {
            betaParameter = -2.0 * boost::math::asinh( std::sqrt( ( normalizedSemiPerimeter
                                                                    - normalizedChord )
                                                                  / ( -2.0 * semiMajorAxis ) ) );
        }
        // Otherwise short transfer arc
        else
        {
            betaParameter = 2.0 * boost::math::asinh( std::sqrt( ( normalizedSemiPerimeter
                                                                   - normalizedChord )
                                                                 / ( -2.0 * semiMajorAxis ) ) );
        }

        // Time-of-flight according to Lagrange.
        const double timeOfFlightLagrange = -semiMajorAxis * std::sqrt( -semiMajorAxis ) *
                ( ( std::sinh( alphaParameter ) - alphaParameter )
                  - ( std::sinh( betaParameter ) - betaParameter ) );

        return timeOfFlightLagrange;
    }
}

//! Solve the time of flight equation for x (for multiple revolutions).
double MultiRevolutionLambertTargeterIzzo::computeRootTimeOfFlight( )
{
    using mathematical_constants::PI;

    // Define initial guesses for abcissae (x) and ordinates (y).
    double x1, x2;

    if ( isRightBranch )
    { // right branch solution.
        x1 = std::tan( .7234 * PI / 2.0 );
        x2 = std::tan( .5234 * PI / 2.0 );
    }
    else
    { // left branch solution.
        x1 = std::tan( -.5234 * PI / 2.0 );
        x2 = std::tan( -.2234 * PI / 2.0 );
    }

    double y1 = computeTimeOfFlight( std::atan( x1 ) * 2.0 / PI ) - normalizedTimeOfFlight;

    double y2 = computeTimeOfFlight( std::atan( x2 ) * 2.0 / PI ) - normalizedTimeOfFlight;

    // Declare and initialize root-finding parameters.
    double rootFindingError = 1.0, xNew = 0.0, yNew = 0.0;
    int iterator = 0;

    // Root-finding loop.
    while ( ( rootFindingError > convergenceTolerance ) && ( y1 != y2 )
            && ( iterator < maximumNumberOfIterations ) )
    {
        // Update iterator.
        iterator++;

        // Compute new x-value.
        xNew = ( x1 * y2 - y1 * x2 ) / ( y2 - y1 );

        // Compute corresponding y-value.
        yNew = computeTimeOfFlight( std::atan( xNew ) * 2.0 / PI ) - normalizedTimeOfFlight;

        // Update abcissae and ordinates.
        x1 = x2;
        y1 = y2;
        x2 = xNew;
        y2 = yNew;

        // Compute root-finding error.
        rootFindingError = std::fabs( x1 - xNew );
    }

    // Verify that root-finder has converged.
    if ( iterator == maximumNumberOfIterations )
    {
        throw basic_mathematics::ConvergenceException(
                    "Multi-Revolution Lambert targeter failed to converge to a solution. Reached the maximum number of iterations: %d"
                    + std::to_string( maximumNumberOfIterations ) );;
    }

    // Revert to x parameter.
    double xParameter = std::atan( xNew ) * 2.0 / PI;
    return xParameter;
}

// Add compute maximum number of revolutions routine?

} // namespace mission_segments
} // namespace tudat


#include "MeshGeometry.h"
#include "MeshConnectivity.h"
#include <Eigen/Dense>

MeshGeometry::MeshGeometry()
{

}

MeshGeometry::MeshGeometry(const Eigen::MatrixXd &V, MeshConnectivity &mesh)
{
    // compute barycentric matrices and Js
    int nfaces = mesh.nFaces();
    Bs.resize(nfaces);
    Js.resize(2 * nfaces, 2);
    faceNormals.resize(nfaces, 3);

    averageEdgeLength = 0;
    for (int i = 0; i < nfaces; i++)
    {
        Eigen::Vector3d v0 = V.row(mesh.faces()(i, 0));
        Eigen::Vector3d v1 = V.row(mesh.faces()(i, 1));
        Eigen::Vector3d v2 = V.row(mesh.faces()(i, 2));
        Bs[i].col(0) = v1 - v0;
        Bs[i].col(1) = v2 - v0;

        averageEdgeLength += (v1 - v0).norm();
        averageEdgeLength += (v2 - v1).norm();
        averageEdgeLength += (v0 - v2).norm();

        Eigen::Vector3d n = (v1 - v0).cross(v2 - v0);
        n /= n.norm();
        faceNormals.row(i) = n.transpose();

        Eigen::Matrix2d BTB = Bs[i].transpose() * Bs[i];
        Eigen::Matrix<double, 3, 2> ncrossB;
        ncrossB.col(0) = n.cross(v1 - v0);
        ncrossB.col(1) = n.cross(v2 - v0);
        Js.block<2, 2>(2 * i, 0) = BTB.inverse() * Bs[i].transpose() * ncrossB;
    }

    averageEdgeLength /= 3.0 * nfaces;

    // compute cDiffs and transition matrices
    int nedges = mesh.nEdges();
    cDiffs.resize(2 * nedges, 2);
    Ts.resize(2 * nedges, 4);
    for (int edgeidx = 0; edgeidx < nedges; edgeidx++)
    {
        //collect neighboring face indices
        int face1 = mesh.edgeFace(edgeidx, 0);
        int face2 = mesh.edgeFace(edgeidx, 1);

        if(face1 == -1 || face2 == -1)
            continue;

        int v1 = mesh.edgeVertex(edgeidx, 0);
        int v2 = mesh.edgeVertex(edgeidx, 1);

        Eigen::Vector3d n1 = faceNormals.row(face1).transpose();
        Eigen::Vector3d n2 = faceNormals.row(face2).transpose();
        // collect: (1) the midpoint of the common edge, (2) unit vector in direction of common edge,
        // (3) the face normals, (4) the centroid of neighboring faces

        Eigen::Vector3d midpt = 0.5 * (V.row(v1).transpose() + V.row(v2).transpose());
        Eigen::Vector3d commone = V.row(v2).transpose() - V.row(v1).transpose();
        commone /= commone.norm();
        Eigen::Vector3d centroids[2];
        centroids[0].setZero();
        centroids[1].setZero();

        for (int i = 0; i < 3; i++)
        {
            centroids[0] += V.row(mesh.faces()(face1, i)).transpose();
            centroids[1] += V.row(mesh.faces()(face2, i)).transpose();
        }

        centroids[0] /= 3.0;
        centroids[1] /= 3.0;

        //rotate each centroid into the plane of the opposite triangle and compute ci minus c

        Eigen::Vector3d t1 = n1.cross(commone);
        Eigen::Vector3d t2 = n2.cross(commone);
        Eigen::Vector3d diff2 = centroids[1] - midpt;
        double alpha = commone.dot(diff2);
        double beta = t2.dot(diff2);

        Eigen::Matrix2d BTB1 = Bs[face1].transpose() * Bs[face1];
        Eigen::Matrix2d BTB2 = Bs[face2].transpose() * Bs[face2];

        cDiffs.row(2 * edgeidx) = BTB1.inverse() * Bs[face1].transpose() * (midpt + alpha * commone + beta * t1 - centroids[0]);
        Eigen::Vector3d diff1 = centroids[0] - midpt;
        alpha = commone.dot(diff1);
        beta = t1.dot(diff1);
        cDiffs.row(2 * edgeidx + 1) = BTB2.inverse() * Bs[face2].transpose() * (midpt + alpha*commone + beta * t2 - centroids[1]);

        Eigen::Vector3d e1 = V.row(mesh.faces()(face1, 1)).transpose() - V.row(mesh.faces()(face1, 0)).transpose();
        Eigen::Vector3d e2 = V.row(mesh.faces()(face1, 2)).transpose() - V.row(mesh.faces()(face1, 0)).transpose();

        double alpha1 = commone.dot(e1);
        double beta1 = t1.dot(e1);
        Eigen::Vector3d newe1 = alpha1*commone + beta1 * t2;
        Ts.block<2, 1>(2 * edgeidx, 0) = BTB2.inverse() * Bs[face2].transpose() * newe1;

        double alpha2 = commone.dot(e2);
        double beta2 = t1.dot(e2);
        Eigen::Vector3d newe2 = alpha2*commone + beta2*t2;
        Ts.block<2, 1>(2 * edgeidx, 1) = BTB2.inverse() * Bs[face2].transpose() * newe2;

        e1 = V.row(mesh.faces()(face2, 1)).transpose() - V.row(mesh.faces()(face2, 0)).transpose();
        e2 = V.row(mesh.faces()(face2, 2)).transpose() - V.row(mesh.faces()(face2, 0)).transpose();

        alpha1 = commone.dot(e1);
        beta1 = t2.dot(e1);
        newe1 = alpha1 * commone + beta1 * t1;
        Ts.block<2, 1>(2 * edgeidx, 2) = BTB1.inverse() * Bs[face1].transpose() * newe1;

        alpha2 = commone.dot(e2);
        beta2 = t2.dot(e2);
        newe2 = alpha2*commone + beta2*t1;
        Ts.block<2, 1>(2 * edgeidx, 3) = BTB1.inverse() * Bs[face1].transpose() * newe2;
    }
}


#ifndef PR_TREE_POINT_HPP
#define PR_TREE_POINT_HPP
#include <boost/type_index.hpp>
#include <type_traits>
#include <utility>
#include <array>
#include <cmath>

namespace perior
{

namespace meta
{
template<typename ... Ts>
struct and_{};
template<typename T>
struct and_<T>: std::integral_constant<bool, T::value>{};
template<typename T, typename ...Ts>
struct and_<T, Ts...>: std::integral_constant<bool,
    (T::value ? and_<Ts...>::value : false)>{};
} // meta

template<typename T, std::size_t N>
struct point
{
    typedef T value_type;
    typedef value_type scalar_type;
    constexpr static std::size_t dim = N;

    point() noexcept   = default;
    ~point() noexcept  = default;

    template<typename ...Ts, typename std::enable_if<(sizeof...(Ts) == dim) &&
        meta::and_<std::is_convertible<Ts, scalar_type>...>::value,
        std::nullptr_t>::type = nullptr>
    point(Ts&& ... xs) noexcept : v_{{std::forward<scalar_type>(xs)...}}{}

    point(const point& rhs) noexcept = default;
    point(point&&      rhs) noexcept = default;
    point& operator=(const point& rhs) noexcept = default;
    point& operator=(point&&      rhs) noexcept = default;

    point& operator+=(const point& rhs) noexcept
    {
        for(std::size_t i=0; i<N; ++i)
        {
            v_[i] += rhs[i];
        }
        return *this;
    }
    point& operator-=(const point& rhs)
    {
        for(std::size_t i=0; i<N; ++i)
        {
            v_[i] -= rhs[i];
        }
        return *this;
    }
    point& operator*=(const scalar_type rhs)
    {
        for(std::size_t i=0; i<N; ++i)
        {
            v_[i] *= rhs;
        }
        return *this;
    }
    point& operator/=(const scalar_type rhs)
    {
        for(std::size_t i=0; i<N; ++i)
        {
            v_[i] /= rhs;
        }
        return *this;
    }

    constexpr inline std::size_t size() const noexcept {return dim;}

    scalar_type& operator[](const std::size_t i)       noexcept {return v_[i];}
    scalar_type  operator[](const std::size_t i) const noexcept {return v_[i];}
    scalar_type& at(const std::size_t i)       {return v_.at(i);}
    scalar_type  at(const std::size_t i) const {return v_.at(i);}

  private:
    std::array<value_type, dim> v_;
};

template<typename T, std::size_t N>
inline bool
operator==(const point<T, N>& lhs, const point<T, N>& rhs) noexcept
{
    for(std::size_t i=0; i<N; ++i)
    {
        if(lhs[i] != rhs[i]){return false;}
    }
    return true;
}

template<typename T, std::size_t N>
inline bool
operator!=(const point<T, N>& lhs, const point<T, N>& rhs) noexcept
{
    for(std::size_t i=0; i<N; ++i)
    {
        if(lhs[i] != rhs[i]){return true;}
    }
    return false;
}

template<typename T, std::size_t N>
inline point<T, N>
operator+(const point<T, N>& lhs, const point<T, N>& rhs) noexcept
{
    point<T, N> p;
    for(std::size_t i=0; i<N; ++i)
    {
        p[i] = lhs[i] + rhs[i];
    }
    return p;
}
template<typename T, std::size_t N>
inline point<T, N>
operator-(const point<T, N>& lhs, const point<T, N>& rhs) noexcept
{
    point<T, N> p;
    for(std::size_t i=0; i<N; ++i)
    {
        p[i] = lhs[i] - rhs[i];
    }
    return p;
}
template<typename T, std::size_t N>
inline point<T, N>
operator*(const point<T, N>& lhs, const T rhs) noexcept
{
    point<T, N> p;
    for(std::size_t i=0; i<N; ++i)
    {
        p[i] = lhs[i] * rhs;
    }
    return p;
}
template<typename T, std::size_t N>
inline point<T, N>
operator*(const T lhs, const point<T, N>& rhs) noexcept
{
    point<T, N> p;
    for(std::size_t i=0; i<N; ++i)
    {
        p[i] = lhs * rhs[i];
    }
    return p;
}
template<typename T, std::size_t N>
inline point<T, N>
operator/(const point<T, N>& lhs, const T rhs) noexcept
{
    point<T, N> p;
    for(std::size_t i=0; i<N; ++i)
    {
        p[i] = lhs[i] / rhs;
    }
    return p;
}

template<typename charT, typename traits, typename realT, std::size_t N>
inline std::basic_ostream<charT, traits>&
operator<<(std::basic_ostream<charT, traits>& os,
           const point<realT, N>& pos)
{
    os << "point<" << boost::typeindex::type_id<realT>().pretty_name() << ", " << N << ">(";
    for(std::size_t i=0; i<N-1; ++i)
    {
        os << pos[i] << ", ";
    }
    os << pos[N-1] << ')';
    return os;
}

template<typename T, std::size_t N>
inline point<T, N>
abs(const point<T, N>& lhs) noexcept
{
    point<T, N> p;
    for(std::size_t i=0; i<N; ++i)
    {
        p[i] = std::abs(lhs[i]);
    }
    return p;
}

template<typename T, std::size_t N>
inline T
dot_product(const point<T, N>& lhs, const point<T, N>& rhs) noexcept
{
    T retval = 0;
    for(std::size_t i=0; i<N; ++i)
    {
        retval += lhs[i] * rhs[i];
    }
    return retval;
}

template<typename T, std::size_t N>
inline point<T, N>
cross_product(const point<T, 3>& lhs, const point<T, 3>& rhs) noexcept
{
    return point<T, N>(lhs[1] * rhs[2] - lhs[2] * rhs[1],
                    lhs[2] * rhs[0] - lhs[0] * rhs[2],
                    lhs[0] * rhs[1] - lhs[1] * rhs[0]);
}

template<typename T, std::size_t N>
inline T
length_sq(const point<T, N>& lhs) noexcept
{
    return dot_product(lhs, lhs);
}

template<typename T, std::size_t N>
inline T
length(const point<T, N>& lhs) noexcept
{
    return std::sqrt(length_sq(lhs));
}

}// perior
#endif//TEST_PERIOR_TREE_POINT_TRAITS_HPP


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial:  Shows how to exchange data between ViennaCL and MTL4 (http://www.mtl4.org/) objects.
*
*/

//
// include necessary system headers
//
#include <iostream>

//
// Include MTL4 headers
//
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

//
// Must be set prior to any ViennaCL includes if you want to use ViennaCL algorithms on MTL4 objects
//
#define VIENNACL_WITH_MTL4 1

//#define VIENNACL_BUILD_INFO
//#define VIENNACL_DEBUG_ALL

//
// ViennaCL includes
//
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/linalg/vector_operations.hpp"
#include "viennacl/compressed_matrix.hpp"

#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"

// Some helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"
#include "../benchmarks/benchmark-utils.hpp"

template<typename ScalarType>
void run_test()
{
  typedef mtl::dense2D<ScalarType>        MTL4DenseMatrix;
  typedef mtl::compressed2D<ScalarType>   MTL4SparseMatrix;

  //
  // Create and fill dense matrices from the MTL4 library:
  //
  mtl::dense2D<ScalarType>   mtl4_densemat(5, 5);
  mtl::dense2D<ScalarType>   mtl4_densemat2(5, 5);
  mtl4_densemat(0,0) = 2.0;   mtl4_densemat(0,1) = -1.0;
  mtl4_densemat(1,0) = -1.0;  mtl4_densemat(1,1) =  2.0;  mtl4_densemat(1,2) = -1.0;
  mtl4_densemat(2,1) = -1.0;  mtl4_densemat(2,2) = -1.0;  mtl4_densemat(2,3) = -1.0;
  mtl4_densemat(3,2) = -1.0;  mtl4_densemat(3,3) =  2.0;  mtl4_densemat(3,4) = -1.0;
                              mtl4_densemat(4,4) = -1.0;  mtl4_densemat(4,4) = -1.0;

  //
  // Create and fill sparse matrices from the MTL4 library:
  //
  MTL4SparseMatrix mtl4_sparsemat;
  set_to_zero(mtl4_sparsemat);
  mtl4_sparsemat.change_dim(5, 5);

  MTL4SparseMatrix mtl4_sparsemat2;
  set_to_zero(mtl4_sparsemat2);
  mtl4_sparsemat2.change_dim(5, 5);

  {
    mtl::matrix::inserter< MTL4SparseMatrix >  ins(mtl4_sparsemat);
    typedef typename mtl::Collection<MTL4SparseMatrix>::value_type  ValueType;
    ins(0,0) <<  ValueType(2.0);   ins(0,1) << ValueType(-1.0);
    ins(1,1) <<  ValueType(2.0);   ins(1,2) << ValueType(-1.0);
    ins(2,2) << ValueType(-1.0);   ins(2,3) << ValueType(-1.0);
    ins(3,3) <<  ValueType(2.0);   ins(3,4) << ValueType(-1.0);
    ins(4,4) << ValueType(-1.0);
  }

  //
  // Create and fill a few vectors from the MTL4 library:
  //
  mtl::dense_vector<ScalarType> mtl4_rhs(5, 0.0);
  mtl::dense_vector<ScalarType> mtl4_result(5, 0.0);
  mtl::dense_vector<ScalarType> mtl4_temp(5, 0.0);

  mtl4_rhs(0) = 10.0;
  mtl4_rhs(1) = 11.0;
  mtl4_rhs(2) = 12.0;
  mtl4_rhs(3) = 13.0;
  mtl4_rhs(4) = 14.0;

  //
  // Let us create the ViennaCL analogues:
  //
  viennacl::vector<ScalarType> vcl_rhs(5);
  viennacl::vector<ScalarType> vcl_result(5);
  viennacl::matrix<ScalarType> vcl_densemat(5, 5);
  viennacl::compressed_matrix<ScalarType> vcl_sparsemat(5, 5);

  //
  // Directly copy the MTL4 objects to ViennaCL objects
  //
  viennacl::copy(&(mtl4_rhs[0]), &(mtl4_rhs[0]) + 5, vcl_rhs.begin());  //method 1: via iterator interface (cf. std::copy())
  viennacl::copy(mtl4_rhs, vcl_rhs);  //method 2: via built-in wrappers (convenience layer)

  viennacl::copy(mtl4_densemat, vcl_densemat);
  viennacl::copy(mtl4_sparsemat, vcl_sparsemat);

  // For completeness: Copy matrices from ViennaCL back to Eigen:
  viennacl::copy(vcl_densemat, mtl4_densemat2);
  viennacl::copy(vcl_sparsemat, mtl4_sparsemat2);

  //
  // Run matrix-vector products and compare results:
  //
  mtl4_result = mtl4_densemat * mtl4_rhs;
  vcl_result = viennacl::linalg::prod(vcl_densemat, vcl_rhs);
  viennacl::copy(vcl_result, mtl4_temp);
  mtl4_result -= mtl4_temp;
  std::cout << "Difference for dense matrix-vector product: " << mtl::two_norm(mtl4_result) << std::endl;
  mtl4_result = mtl4_densemat2 * mtl4_rhs - mtl4_temp;
  std::cout << "Difference for dense matrix-vector product (MTL4->ViennaCL->MTL4): "
            << mtl::two_norm(mtl4_result) << std::endl;

  //
  // Same for sparse matrix:
  //
  mtl4_result = mtl4_sparsemat * mtl4_rhs;
  vcl_result = viennacl::linalg::prod(vcl_sparsemat, vcl_rhs);
  viennacl::copy(vcl_result, mtl4_temp);
  mtl4_result -= mtl4_temp;
  std::cout << "Difference for sparse matrix-vector product: " << mtl::two_norm(mtl4_result) << std::endl;
  mtl4_result = mtl4_sparsemat2 * mtl4_rhs - mtl4_temp;
  std::cout << "Difference for sparse matrix-vector product (MTL4->ViennaCL->MTL4): "
            << mtl::two_norm(mtl4_result) << std::endl;

  //
  // Please have a look at the other tutorials on how to use the ViennaCL types
  //
}

int main(int, char *[])
{
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "## Single precision" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  run_test<float>();

#ifdef VIENNACL_HAVE_OPENCL
  if ( viennacl::ocl::current_device().double_support() )
#endif
  {
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "## Double precision" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    run_test<double>();
  }

  std::cout << std::endl;
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;
  std::cout << std::endl;
}


#pragma once

#include "ArithmeticProgression.hpp"
#include "Misc.hpp"
#include "Sequences.hpp"
#include "VectorHelpers.hpp"
#include <boost/iterator/iterator_facade.hpp>

namespace discreture
{

////////////////////////////////////////////////////////////////////////////////////
/// \brief Class for iterating through all dyck (dyck) paths.
/// \param IntType must be a SIGNED integer type.
///
/// Dyck paths, also called Catalan Paths, are paths that go from \f$(0,0)\f$ to
/// \f$(0,2n)\f$, which never go below the \f$ y=0\f$ line, in which each step
/// is from \f$(x,y)\f$ to either \f$(x+1,y+1)\f$ or \f$(x+1,y-1)\f$ #Example
/// Usage:
///
///     dyck_paths X(3)
///     for (auto&& x : X)
///         cout << x << endl;
/// Prints out:
///     [ 1 1 1 -1 -1 -1 ]
///     [ 1 1 -1 1 -1 -1 ]
///     [ 1 -1 1 1 -1 -1 ]
///     [ 1 1 -1 -1 1 -1 ]
///     [ 1 -1 1 -1 1 -1 ]
///
/// # Example: Parenthesis
///
///     dyck_paths X(3)
///     for (auto&& x : X)
///         cout << dyck_paths::to_string(x, "()") << endl;
///
/// Prints out:
///     ((()))
///     (()())
///     ()(())
///     (())()
///     ()()()
///
/////////////////////////////////////////////////////////////////////////////////////
template <class IntType = int, class RAContainerInt = std::vector<IntType>>
class DyckPaths
{
public:
    static_assert(std::is_integral<IntType>::value,
                  "Template parameter IntType must be integral");
    static_assert(std::is_signed<IntType>::value,
                  "Template parameter IntType must be signed");
    using value_type = RAContainerInt;
    using dyck_path = value_type;
    using difference_type = std::ptrdiff_t;
    using size_type = difference_type;
    class iterator;
    using const_iterator = iterator;

    // **************** Begin static functions
    static void next_dyck_path(dyck_path& data)
    {
        size_t n_ = data.size()/2;

        if (n_ == 0)
            return;

        if (data[1] != -1)
        {
            size_t loc = 2;

            while (data[loc] != -1)
                ++loc;

            data[loc] = 1;
            data[loc - 1] = -1;

            return;
        }

        size_t verif = 0;
        size_t i = 1;

        while (i < n_ && verif == 0)
        {
            if (data[(2*i) + 1] == 1)
                verif = ((2*i) + 1);

            ++i;
        }

        //          for(nuint i=1; i<t; ++t)
        //          {
        //              if(data[(2*i)+1]==0)
        //                  verif=((2*i)+1);
        //          }

        if (verif == 0)
        {
            return;
        }

        size_t cont = 0;
        auto encontrar = verif + 1;

        for (size_t i = 0; i < verif; ++i)
        {
            if (data[i] == -1)
            {
                data[i] = 1;
                ++cont;
            }
        }

        while (data[encontrar] != -1)
            ++encontrar;

        data[encontrar] = 1;
        data[encontrar - 1] = -1;

        while (cont != 0)
        {
            data[encontrar - 1 - cont] = -1;
            --cont;
        }
    }

    static std::string to_string(const dyck_path& data,
                                 const std::string& delim = "()")
    {
        std::string toReturn;

        for (auto i : data)
        {
            auto j = 1 - (i + 1)/2;
            toReturn.push_back(delim[j]);
        }

        return toReturn;
    }

    // **************** End static functions

public:
    ////////////////////////////////////////////////////////////
    /// \brief Constructor
    ///
    /// \param n is an integer >= 0
    ///
    ////////////////////////////////////////////////////////////
    explicit DyckPaths(IntType n) : n_(n) {}

    ////////////////////////////////////////////////////////////
    /// \brief The total number of dyck_paths
    ///
    /// \return binomial(2n,n)/(n+1)
    ///
    ////////////////////////////////////////////////////////////
    size_type size() const { return catalan(n_); }

    IntType get_n() const { return n_; }

    ////////////////////////////////////////////////////////////
    /// \brief Forward iterator class.
    ////////////////////////////////////////////////////////////
    class iterator
        : public boost::iterator_facade<iterator, const dyck_path&, boost::forward_traversal_tag>
    {
    public:
        iterator() = default; // empty initializer
        explicit iterator(IntType n) : ID_(0), data_(2*n, 1)
        {
            for (size_t i = n; i < data_.size(); ++i)
                data_[i] = -1;
        }

        size_type ID() const { return ID_; }

        bool is_at_end(IntType n) const { return ID_ == catalan(n); }

        void reset(IntType n)
        {
            ID_ = 0;
            data_.resize(2*n);
            auto r = static_cast<size_t>(n);
            for (size_t i = 0; i < r; ++i)
                data_[i] = 1;

            for (size_t i = r; i < data_.size(); ++i)
                data_[i] = -1;
        }

        static const iterator make_invalid_with_id(size_type id)
        {
            iterator it;
            it.ID_ = id;
            return it;
        }

    private:
        void increment()
        {
            ++ID_;

            next_dyck_path(data_);
        }

        const dyck_path& dereference() const { return data_; }

        bool equal(const iterator& it) const { return it.ID() == ID(); }

    private:
        size_type ID_{0};
        dyck_path data_{};

        friend class boost::iterator_core_access;
    }; // end class iterator

    iterator begin() const { return iterator(n_); }

    const iterator end() const
    {
        return iterator::make_invalid_with_id(size());
    }

private:
    IntType n_;

}; // end class DyckPaths

using boost::container::static_vector;

using dyck_paths = DyckPaths<int>;
using dyck_paths_stack = DyckPaths<int, static_vector<int, 48>>;

} // namespace discreture


//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//  Observable functions for Kitaev Quantum Monte Carlo
//  written by: Tim Eschmann, June 2016
//  Modified version: February 2019
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////

#define _USE_MATH_DEFINES

#include <iostream>
#include <cmath>
#include <complex>
#include <armadillo>

using namespace arma;

///////////////////////////////////////////////////////////////
// Calculate free energy for a given Z2 configuration:
///////////////////////////////////////////////////////////////

double free_en(vec ev, double b)
{
    int p; // running index
    double fe = 0; // free energy

    // Calculating:
    for (p = 0; p < size(ev)[0]/2; p++)
    {
        fe -= (1/b)*logl(2*coshl(b*ev[p]/2));
    }

    return fe;

}

///////////////////////////////////////////////////////////////
// Calculate internal energy of the Majorana fermion system:
///////////////////////////////////////////////////////////////

double en(vec ev, double b)
{
    int q; // running index;
    double en = 0; // internal energy

    for (q = 0; q < size(ev)[0]/2; q++)
    {
        en += ev[q]/2 * tanhl(-b * ev[q]/2);
    }

    return en;
}

///////////////////////////////////////////////////////////////
// Calculate derivative of E w.r.t. beta:
///////////////////////////////////////////////////////////////

double diffE(vec ev, double b)
{
    int qq; // running index
    double drv = 0;
    for (qq = 0; qq < size(ev)[0]/2; qq++)
    {
        //drv += ev[qq]*ev[qq]/4. * (1 - tanh(-beta_ * ev[qq]/2)*tanh(-beta_ * ev[qq]/2));
        drv += powl(ev[qq], 2)/4. / powl((coshl(-b * ev[qq]/2)),2);
    }

    return drv;
}

///////////////////////////////////////////////////////////////
// Calculate average flux per elementary plaquette:
///////////////////////////////////////////////////////////////

std::complex <double> flux(cx_mat ham, Mat<int> plaq)
{
    std::complex <double> flux;
    std::complex <double> av_flux = std::complex<double>(0.0, 0.0);
    int N = size(ham)[0];
    int M1 = size(plaq)[0];
    int M2 = size(plaq)[1];
    int coord1, coord2;

    for (int i = 0; i < M1; i++)
    {
        flux = std::complex<double>(1.0, 0.0);
        for (int j = 0; j < M2; j++)
        {
            coord1 = plaq(i,j) / N;
            coord2 = plaq(i,j) % N;

            // Plaquette operator: W_p = prod_<i,j> (-i*u_ij)
            flux *= (-ham(coord1, coord2)) / std::abs(ham(coord1, coord2));
        }
        av_flux += flux;
    }

    av_flux /= double(M1);

    return av_flux;

}

///////////////////////////////////////////////////////////////
// Calculate flux disorder ratio p:
///////////////////////////////////////////////////////////////

double get_p(cx_mat ham, Mat<int> plaq)
{
    std::complex <double> flux;
    double p;
    int N = size(ham)[0];
    int M1 = size(plaq)[0];
    int M2 = size(plaq)[1];
    int coord1, coord2;

    for (int i = 0; i < M1; i++)
    {
        flux = std::complex<double>(1.0, 0.0);
        for (int j = 0; j < M2; j++)
        {
            coord1 = plaq(i,j) / N;
            coord2 = plaq(i,j) % N;

            // Plaquette operator: W_p = prod_<i,j> (-i*u_ij)
            flux *= (-ham(coord1, coord2)) / std::abs(ham(coord1, coord2));
        }
        if ((M2 % 4 == 0) && (flux == std::complex<double>(-1, 0)))
            p += 1;
        else if ((M2 % 4 == 2) && (flux == std::complex<double>(1, 0)))
            p += 1;
        else if ((M2 % 2 != 0) && (flux == std::complex<double>(0, 1)))
            p += 1;
    }

    p /= double(M1);

    return p;

}

///////////////////////////////////////////////////////////////
// Give flux configurations as output:
///////////////////////////////////////////////////////////////
cx_vec flux_confs(cx_mat ham, Mat<int> plaq)
{
    int M1 = size(plaq)[0];
    int M2 = size(plaq)[1];
    int N = size(ham)[0];
    int coord1, coord2;
    cx_vec confs(M1);
    std::complex <double> flux;

    for (int i = 0; i < M1; i++)
    {
        flux = std::complex<double>(1.0, 0.0);
        for (int j = 0; j < M2; j++)
        {
            coord1 = plaq(i,j) / N;
            coord2 = plaq(i,j) % N;

            // Plaquette operator: W_p = prod_<i,j> (-i*u_ij)
            flux *= (-ham(coord1, coord2)) / std::abs(ham(coord1, coord2));
        }

        confs[i] = flux;
    }

    return confs;

}

///////////////////////////////////////////////////////////////
// Calculate spin-spin correlation:
// Make sure only to include one subset of bonds
///////////////////////////////////////////////////////////////

double correlation(cx_mat ham, std::vector<int> v_, double b)
{
    double value = 0.;
    std::complex<double> av;
    vec eigval;
    cx_mat eigvec;
    int N = size(ham)[0];
    int coord1, coord2;

    eig_sym(eigval, eigvec, ham);
    //eigvec = normalise(eigvec);

    for (int j = 0; j < v_.size(); j++)
    {
        coord1 = v_[j]/N;
        coord2 = v_[j]%N;

        for (int i = size(eigval)[0]/2; i < size(eigval)[0]; i++)
        {
            av = conj(eigvec(coord1,i))*((-ham(coord1, coord2)) / std::abs(ham(coord1, coord2)))*eigvec(coord2, i);
            av += conj(eigvec(coord2,i))*((-ham(coord2, coord1)) / std::abs(ham(coord2, coord1)))*eigvec(coord1, i);
            value -= real(av)*tanhl(b * eigval[i]/2.);
        }
    }

    value *= 2/double(N);
    return value;

}

/////////////////////////////////////////////////////////
// Set up simulation temperatures (w. different options):
/////////////////////////////////////////////////////////

double calc_temp(double T_min, double T_max, int me, int np, std::string dist)
{
    int i;
    double T;

    // a) Read temperature distribution from file:
    if (dist == "external")
    {
        double temperatures[np];
        std::ifstream tempfile("temp.saved", std::ifstream::in);
        if(tempfile.good())
        {
            for (i = 0; i < np; i++)
            {
                tempfile >> temperatures[i];
            }
        }

        T = temperatures[me - 1];
    }

    // b) Linear temperature distribution:
    else if (dist == "lin")
    {
        T = T_min + (T_max - T_min)*(me - 1)/float(np);
    }
    // c) Logarithmic temperature distribution:*/
    else if (dist == "log")
    {
        T = pow(10,log10(T_min)+((log10(T_max) - log10(T_min))*(me - 1)/double(np)));
    }
    // d) Double-logarithmic temperature distribution:
    else if (dist == "double_log")
    {
        T = pow(10,log10(T_min)+((log10(T_max) - log10(T_min))*pow(10, -(np - (me - 1))/double(np))));
    }

    return T;

}

//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//  Ensemble optimization functions
//  -> check if MC replica has been at T_min or T_max latest and assign
// "+1" or "-1" accordingly
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////
// Record if replica has been at lowest or highest T latest ...      ////////
/////////////////////////////////////////////////////////////////////////////

int check_sign_i(int i, int np, int s_i, int s_i_plus_1)
{
    int sign;

    if (i != 1 && (i+1 != (np - 1)))
    {
        if (s_i == 1 && s_i_plus_1 == 0)
            sign = 0;
        else if (s_i == 0 && s_i_plus_1 == 1)
            sign = 1;
        else if (s_i == 1 && s_i_plus_1 == -1)
            sign = -1;
        else if (s_i == -1 && s_i_plus_1 == 1)
            sign = 1;
        else if (s_i == 0 && s_i_plus_1 == -1)
            sign = -1;
        else if (s_i == -1 && s_i_plus_1 == 0)
            sign = 0;
        else if (s_i == 0 && s_i_plus_1 == 0)
            sign = 0;
        else if (s_i == 1 && s_i_plus_1 == 1)
            sign = 1;
        else if (s_i == -1 && s_i_plus_1 == -1)
            sign = -1;
    }
    else if (i == 1)
        sign = 1;
    else if (i+1 == np - 1)
        sign = -1;
    else
        sign = 0;

    return sign;

}

int check_sign_i_plus_1(int i, int np, int s_i, int s_i_plus_1)
{
    int sign;

    if (i != 1 && (i+1 != (np - 1)))
    {
        if (s_i == 1 && s_i_plus_1 == 0)
            sign = 1;
        else if (s_i == 0 && s_i_plus_1 == 1)
            sign = 0;
        else if (s_i == 1 && s_i_plus_1 == -1)
            sign = 1;
        else if (s_i == -1 && s_i_plus_1 == 1)
            sign = -1;
        else if (s_i == 0 && s_i_plus_1 == -1)
            sign = 0;
        else if (s_i == -1 && s_i_plus_1 == 0)
            sign = -1;
        else if (s_i == 0 && s_i_plus_1 == 0)
            sign = 0;
        else if (s_i == 1 && s_i_plus_1 == 1)
            sign = 1;
        else if (s_i == -1 && s_i_plus_1 == -1)
            sign = -1;
    }
    else if (i == 1)
        sign = 1;
    else if (i+1 == np - 1)
        sign = -1;
    else
        sign = 0;

    return sign;
}


#include "reconstruction.h"
#include "simulation.h"
#include <Eigen/SVD>
#include <Eigen/StdVector>
#include <igl/PI.h>
#include <igl/copyleft/marching_cubes.h>
#include <iostream>
#include <tbb/parallel_for.h>

void reconstruct(Eigen::MatrixXd &V, Eigen::MatrixXi &F, const Eigen::MatrixXd &P, const Eigen::Vector3i &res,
                 const Eigen::VectorXd &mass, const Eigen::VectorXd &density, double h, double isovalue) {
    auto W_k = 10. / (7. * igl::PI);
    auto P1 = [&](double r) {
        auto q = r;
        auto res = 0.0;
        if (q <= 1.0)
            res = (1 - 1.5 * q * q + 0.75 * q * q * q);
        else if (q < 2.0) {
            auto q2 = 2 - q;
            res = 0.25 * q2 * q2 * q2;
        }
        return res;
    };

    printf("P.rows() = %d\n", P.rows());
    using igl::copyleft::marching_cubes;
    // Eigen::Vector3d lower = Eigen::Vector3d::Zero() - Eigen::Vector3d::Constant(2 * h); // = P.colwise().minCoeff();
    // Eigen::Vector3d upper = Eigen::Vector3d::Ones() + Eigen::Vector3d::Constant(2 * h); // P.colwise().maxCoeff();
    Eigen::Vector3d lower = Eigen::Vector3d(P.colwise().minCoeff()) - Eigen::Vector3d::Constant(2 * h);
    Eigen::Vector3d upper = Eigen::Vector3d(P.colwise().maxCoeff()) + Eigen::Vector3d::Constant(2 * h);
    Eigen::Vector3d extent = upper - lower;
    Eigen::VectorXd S;
    Eigen::MatrixXd GV;
    S.resize(res.prod());
    GV.resize(res.prod(), 3);
    Eigen::Vector3d nn_cell_size(2 * h, 2 * h, 2 * h);
    Eigen::Vector3i nn_grid_size;
    nn_grid_size << extent[0] / nn_cell_size[0], extent[1] / nn_cell_size[1], extent[2] / nn_cell_size[2];
    Eigen::Vector3d grid_cell_size = extent.array() / (res - Eigen::Vector3i::Ones()).cast<double>().array();
    for (int i = 0; i < res.prod(); i++) {
        int x = i % res[0];
        int y = (i % (res[0] * res[1])) / res[0];
        int z = i / (res[0] * res[1]);
        Eigen::Vector3d p = Eigen::Vector3d(Eigen::Array3d(x, y, z) * grid_cell_size.array()) + lower;
        GV.row(i) = p;
    }
    // std::cout << GV << std::endl;
    struct Cell {
        std::vector<int> particles;
    };
    std::vector<Cell> grid(nn_grid_size.prod());
    std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> G(P.rows());
    std::vector<std::vector<int>> neighbors(P.rows());
    Eigen::MatrixXd X;
    X.resizeLike(P);
    // auto get_cell_index = [&](const Eigen::Vector3d &x) -> Eigen::Vector3i {
    //     Eigen::Vector3i ip =
    //         Eigen::Vector3i(((x - lower).array() / extent.array() * res.array().cast<double>()).cast<int>());
    //     for (int i = 0; i < 3; i++) {
    //         ip[i] = std::min(res[i] - 1, std::max(ip[i], 0));
    //     }
    //     return ip;
    // };
    // auto get_linear_index = [&](const Eigen::Vector3i &ip) { return ip[0] + ip[1] * res[0] + ip[2] * res[0] * res[1];
    // };
    auto get_nn_cell_index = [&](const Eigen::Vector3d &x) -> Eigen::Vector3i {
        Eigen::Vector3i ip = Eigen::Vector3i(((x - lower).array() / nn_cell_size.array()).cast<int>());
        for (int i = 0; i < 3; i++) {
            ip[i] = std::min(nn_grid_size[i] - 1, std::max(ip[i], 0));
        }
        return ip;
    };
    auto get_nn_linear_index = [&](const Eigen::Vector3i &ip) {
        return ip[0] + ip[1] * nn_grid_size[0] + ip[2] * nn_grid_size[0] * nn_grid_size[1];
    };
    for (int i = 0; i < P.rows(); i++) {
        Eigen::Vector3i ip = get_nn_cell_index(P.row(i));
        auto idx = get_nn_linear_index(ip);
        CHECK(idx < grid.size());
        // printf("%d\n", idx);
        grid[idx].particles.push_back(i);
    }
    for (int i = 0; i < P.rows(); i++) {

        Eigen::Vector3d p = P.row(i);
        Eigen::Vector3i ip = get_nn_cell_index(p);
        for (int dx = -1; dx <= 1; dx++) {
            for (int dy = -1; dy <= 1; dy++) {
                for (int dz = -1; dz <= 1; dz++) {
                    Eigen::Vector3i cell_idx = ip + Eigen::Vector3i(dx, dy, dz);
                    if ((cell_idx.array() >= Eigen::Array3i::Zero()).all() &&
                        (cell_idx.array() < nn_grid_size.array()).all()) {
                        auto &cell = grid[get_nn_linear_index(cell_idx)];
                        for (auto &j : cell.particles) {
                            if (i == j)
                                continue;
                            Eigen::Vector3d q = P.row(j);
                            if ((p - q).norm() < 2 * h) {
                                neighbors[i].push_back(j);
                            }
                        }
                    }
                }
            }
        }
    }
    CHECK(neighbors.size() == P.rows());
    // for (int i = 0; i < P.rows(); i++) {
    tbb::parallel_for<int>(0, P.rows(), [&](int i) {
        Eigen::Vector3d xi = P.row(i);
        Eigen::Vector3d xiw(0, 0, 0);
        Eigen::Matrix3d C;
        Eigen::Vector3d x_bar(0.0, 0.0, 0.0);
        const auto lambda = 0.92;
        C.setZero();
        if (!neighbors.empty()) {
            double sum_w = 0.0;

            for (auto &j : neighbors[i]) {
                CHECK(j < P.rows());
                Eigen::Vector3d xj = P.row(j);
                const auto r = 2 * h;
                auto w = 0.0;
                if ((xi - xj).norm() <= r && (xi - xj).norm() > 0.0) {
                    w = 1.0 - std::pow((xi - xj).norm() / r, 3);
                }
                sum_w += w;
                xiw += w * xj;
            }
            xiw /= sum_w;
            x_bar = (1.0 - lambda) * xi + lambda * xiw;
            X.row(i) = x_bar;
        } else {
            X.row(i) = xi;
            xiw = xi;
        }
        {
            double sum_w = 0.0;
            for (auto &j : neighbors[i]) {
                Eigen::Vector3d xj = P.row(j);
                const auto r = 2 * h;
                auto w = 0.0;
                if ((xi - xj).norm() <= r && (xi - xj).norm() > 0.0) {
                    w = 1.0 - std::pow((xi - xj).norm() / r, 3);
                }
                sum_w += w;
                C += w * (xj - xiw) * (xj - xiw).transpose();
            }
            // std::cout << C << std::endl;
            C /= sum_w;
        }
        Eigen::JacobiSVD<Eigen::Matrix3d> svd(C, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::Matrix3d R = svd.matrixU();
        Eigen::Vector3d sigmas = svd.singularValues();
        const auto kr = 4.0;
        const auto ks = 1400.0;
        const auto kn = 0.5;
        const int Ne = 25;
        double sigma1 = sigmas[0];
        for (int i = 0; i < 3; i++) {
            sigmas[i] = std::fmax(sigmas[i], sigma1 / kr);
        }
        auto N = (int)neighbors[i].size();
        // std::cout << "N: " << N << std::endl;
        Eigen::DiagonalMatrix<double, 3> Sigma;
        if (N < Ne) {
            Sigma = kn * Eigen::Vector3d::Ones().asDiagonal();
        } else {
            Sigma = ks * sigmas.asDiagonal();
        }
        G[i] = 1.0 / h * R * Sigma.inverse() * R.transpose();
        // G[i] = 1.0 / h * Eigen::Matrix3d::Identity();
        // std::cout << G[i] << std::endl;
    });
    for (auto &cell : grid) {
        cell.particles.clear();
    }
    for (int i = 0; i < P.rows(); i++) {
        Eigen::Vector3i ip = get_nn_cell_index(P.row(i));
        auto idx = get_nn_linear_index(ip);
        CHECK(idx < grid.size());
        grid[idx].particles.push_back(i);
    }
    neighbors.clear();
    neighbors.resize(GV.rows());
    // for (int i = 0; i < GV.rows(); i++) {
    tbb::parallel_for<int>(0, GV.rows(), [&](int i) {
        // int x = i % res[0];
        // int y = (i % (res[0] * res[1])) / res[0];
        // int z = i / (res[0] * res[1]);
        // Eigen::Vector3i gv_p(x, y, z);
        Eigen::Vector3d p = GV.row(i);
        Eigen::Vector3i ip = get_nn_cell_index(p);
        for (int dx = -1; dx <= 1; dx++) {
            for (int dy = -1; dy <= 1; dy++) {
                for (int dz = -1; dz <= 1; dz++) {
                    Eigen::Vector3i cell_idx = ip + Eigen::Vector3i(dx, dy, dz);
                    if ((cell_idx.array() >= Eigen::Array3i::Zero()).all() &&
                        (cell_idx.array() < nn_grid_size.array()).all()) {
                        auto &cell = grid[get_nn_linear_index(cell_idx)];
                        for (auto &j : cell.particles) {
                            Eigen::Vector3d q = X.row(j);
                            if ((p - q).norm() < 2 * h) {
                                neighbors[i].push_back(j);
                            }
                        }
                    }
                }
            }
        }
    });

    // for (int i = 0; i < GV.rows(); i++) {
    tbb::parallel_for<int>(0, GV.rows(), [&](int i) {
        const double r = 2 * h;
        // Eigen::Vector3i ip;
        // {
        //     int x = i % res[0];
        //     int y = (i % (res[0] * res[1])) / res[0];
        //     int z = i / (res[0] * res[1]);
        //     ip = Eigen::Vector3i(x, y, z);
        // }
        Eigen::Vector3d x = GV.row(i);
        double s = 0.0;
        for (auto j : neighbors[i]) {
            Eigen::Vector3d xj = X.row(j);
            Eigen::Vector3d r = x - xj;
            auto W = W_k / (h * h) * G[j].norm() * P1((G[j] * r).norm());
            // std::cout << (G[j] * r).norm() << std::endl;
            // std::cout << (r.norm() / h) << std::endl;
            s += mass[j] / density[j] * W;
        }
        S[i] = s;

        // double s = 0.0;
        // for (auto j : neighbors[i]) {
        //     Eigen::Vector3d xj = P.row(j);
        //     Eigen::Vector3d r = x - xj;
        //     auto W = W_k / (h * h * h) * P1(r.norm() / h);
        //     s += mass[j] / density[j] * W;
        // }

        // S[i] = s;
    });
    // std::cout << S << std::endl;
    printf("S.mean() = %f\n", S.mean());

    marching_cubes(S, GV, res[0], res[1], res[2], isovalue, V, F);
}


#include <iostream>
#include <armadillo>
#include <vector>
#include <cstdlib>
#include <fstream>

#include "world_rep.h"

int main(int argc, const char **argv)
{
    // Values for single neuron.
    int snn_id = 2;
    int n_inputs = 2;
    int n_lateral = 1;
    std::vector<double> init_d({2, 4});
    std::vector<double> init_w({4});
    double tau_m = 2;
    double u_rest = 0;
    double init_v = 20; // when testing math, make this large
    unsigned char t_rest = 2;
    double kappa_naugh = 3;
    double round_zero = 0.1;
    double u_max = 10;

    SpikeResponseModelNeuron single_neuron = SpikeResponseModelNeuron(
        snn_id, n_inputs, init_d, tau_m, u_rest,
        init_v, t_rest, kappa_naugh, round_zero, u_max);
    std::cout << "Created individual neuron." << std::endl;

    std::vector<std::vector<bool>> presynaptic_train = {
        {0, 0, 1, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}
    };

    std::ofstream exp_res;
    exp_res.open("experiment_results.log");
    exp_res << "[";
    for(unsigned int epoch = 0; epoch < presynaptic_train.at(0).size(); epoch++)
    {
        exp_res << single_neuron.membrane_potential() << ", ";
        // load dendrites with current value
        single_neuron.dendrite.at(0) = DelayedSpike(0, presynaptic_train.at(0).at(epoch));
        single_neuron.dendrite.at(1) = DelayedSpike(0, presynaptic_train.at(1).at(epoch));

        // Move network forward
        single_neuron.t_pulse();

    }
    exp_res << "]" << std::endl;

    printf("Testing FSTNs\n");
    double alpha = 1;
    FirstSpikeTimeNeuron fstn = FirstSpikeTimeNeuron(0, alpha);
    fstn.dendrite = 2;
    fstn.encode();

    unsigned char total_pulses = 10;

    exp_res << "First Spike Time Neuron spike after " << total_pulses << "pulses." << std::endl << "[";
    exp_res << fstn.axon.signal << ", ";
    for(unsigned char pulse= 0; pulse < total_pulses; pulse++)
    {
        fstn.t_pulse();
        exp_res << fstn.axon.signal << ", ";
    }
    exp_res << "]" << std::endl;

    exp_res.close();

    // Testing training algorthm
    unsigned int n_data = 2;
    tau_m = 0.8;
    u_rest = 0;
    init_v = 5;
    unsigned int t_reset = 3;
    double k_nought = 3;
    round_zero = 0.05;
    alpha = 1;
    // note that n_x * n_y = h_layer_size
    unsigned int n_x = 8;
    unsigned int n_y = 8;
    double delay_distance = 0.2;
    unsigned int distance_unit = 1;
    double sigma_neighbor = 1;
    double eta_d = 1.7;
    unsigned int t_max = 25;
    u_max = 10;

    printf("Create SNN\n");
    SNN model(n_data, tau_m, u_rest, init_v,
    t_reset, k_nought, round_zero, alpha, n_x, n_y, delay_distance,
    distance_unit, sigma_neighbor, eta_d, t_max, u_max);

    std::vector<std::vector<double>> data = {
        {0, 0, 0, 1, 2, 2, 3, 3, 3, 3, 3, 2, 1, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 1, 2, 2, 2, 3, 3, 4, 5, 6, 7, 7, 7, 7, 6, 5, 4, 3, 2, 1, 0}
    };
    std::vector<std::vector<double>> data_2 = {
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 1, 1, 1, 1},
    };

    printf("Create output file\n");

    // save delays into file:
    std::ofstream delay_file;
    delay_file.open("training_delays.txt");

    // print out delays
    delay_file << "Delays before training:" << std::endl;
    for(unsigned int i = 0; i < model.snn->d_ji.size(); i++)
    {
        delay_file << "[ ";
        for(unsigned int j = 0; j < model.snn->d_ji.at(i).size(); j++)
        {
            delay_file << model.snn->d_ji.at(i).at(j) << ", ";
        }
        delay_file << "]" << std::endl;
    }

    for(unsigned int p = 0; p < 1; p++)
        model.train(data);

    delay_file << std::endl << "Delays after training:"<< std::endl;
     for(unsigned int i = 0; i < model.snn->d_ji.size(); i++)
    {
        delay_file << "[ ";
        for(unsigned int j = 0; j < model.snn->d_ji.at(i).size(); j++)
        {
            delay_file << model.snn->d_ji.at(i).at(j) << ", ";
        }
        delay_file << "]" << std::endl;
    }

    for(unsigned int p = 0; p < 8; p++, sigma_neighbor -= 0.1)
    {
        model = SNN(n_data, tau_m, u_rest, init_v,
        t_reset, k_nought, round_zero, alpha, n_x, n_y, delay_distance,
        distance_unit, sigma_neighbor, eta_d, t_max, u_max);

        model.train(data);

        delay_file << std::endl << "Delays after training with sigma_neighbor = " << sigma_neighbor << ": \n" << std::endl;
        for(unsigned int i = 0; i < model.snn->d_ji.size(); i++)
        {
            delay_file << "[ ";
            for(unsigned int j = 0; j < model.snn->d_ji.at(i).size(); j++)
            {
                delay_file << model.snn->d_ji.at(i).at(j) << ", ";
            }
            delay_file << "]" << std::endl;
        }
    }

    printf("test world representation and planner.\n");

    sigma_neighbor = 0.3;

    std::ofstream planner_file;
    planner_file.open("world_representation.txt");

    double prune_dist = 0.1;

    WorldRep planner = WorldRep(k_nought, tau_m, init_v, round_zero,
    n_x, n_y, delay_distance, sigma_neighbor, eta_d, t_max, u_max,
    prune_dist);

    planner.train(data);

    planner_file << std::endl << "Delays after training:"<< std::endl;
    for(unsigned int i = 0; i < planner.snn.snn->d_ji.size(); i++)
    {
        planner_file << "[ ";
        for(unsigned int j = 0; j < planner.snn.snn->d_ji.at(i).size(); j++)
        {
            planner_file << planner.snn.snn->d_ji.at(i).at(j) << ", ";
        }
        planner_file << "]" << std::endl;
    }

    std::vector<std::vector<double>> world_rep = planner.get_map();

    planner_file << std::endl << "Delays after training with sigma_neighbor = " << sigma_neighbor << " and prunning with value:" << prune_dist << std::endl;

    for(unsigned int i = 0; i < world_rep.size(); i++)
    {
        planner_file << "[ ";
        for(unsigned int j = 0; j < world_rep.at(i).size(); j++)
        {
            planner_file << world_rep.at(i).at(j) << ", ";
        }
        planner_file << "]" << std::endl;
    }

    planner.dijkstra(28, 409);
    std::vector<double> src_coor, goal_coor;
    src_coor = std::vector<double>({0, 0});
    goal_coor = std::vector<double>({0, 7});
    std::vector<std::vector<double>> path = planner.get_path(WorldRep::PathAlgorithm::dijkstras, src_coor, goal_coor);
    printf("[");
    for(unsigned int x = 0; x < path.at(0).size(); x++)
    {
        printf("[%f, %f], ", path.at(0).at(x), path.at(1).at(x));
    }
    printf("]\n");
    planner_file.close();
}