// [[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;
}


#define BOOST_TEST_MODULE "test_matrix"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <boost/mpl/list.hpp>
#include <mjolnir/math/Matrix.hpp>
#include <random>
#include <cstdint>

constexpr std::uint32_t seed = 123456789;
constexpr std::size_t   N    = 10000;
typedef boost::mpl::list<double, float> test_targets;

namespace test
{
template<typename T>
decltype(boost::test_tools::tolerance(std::declval<T>())) tolerance();

template<>
decltype(boost::test_tools::tolerance(std::declval<float>()))
tolerance<float>()
{return boost::test_tools::tolerance(3.0f / static_cast<float>(std::pow(2, 8)));}

template<>
decltype(boost::test_tools::tolerance(std::declval<double>()))
tolerance<double>()
{return boost::test_tools::tolerance(2.0 / std::pow(2, 14));}
} // test

BOOST_AUTO_TEST_CASE_TEMPLATE(add_matrix_3x3, Real, test_targets)
{
    using namespace mjolnir;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-1.0, 1.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Matrix<Real, 3, 3> lhs(
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt)
            ), rhs(
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt)
            );
        const auto add = lhs + rhs;

        BOOST_TEST(add(0, 0) == lhs(0, 0) + rhs(0, 0), test::tolerance<Real>());
        BOOST_TEST(add(0, 1) == lhs(0, 1) + rhs(0, 1), test::tolerance<Real>());
        BOOST_TEST(add(0, 2) == lhs(0, 2) + rhs(0, 2), test::tolerance<Real>());
        BOOST_TEST(add(1, 0) == lhs(1, 0) + rhs(1, 0), test::tolerance<Real>());
        BOOST_TEST(add(1, 1) == lhs(1, 1) + rhs(1, 1), test::tolerance<Real>());
        BOOST_TEST(add(1, 2) == lhs(1, 2) + rhs(1, 2), test::tolerance<Real>());
        BOOST_TEST(add(2, 0) == lhs(2, 0) + rhs(2, 0), test::tolerance<Real>());
        BOOST_TEST(add(2, 1) == lhs(2, 1) + rhs(2, 1), test::tolerance<Real>());
        BOOST_TEST(add(2, 2) == lhs(2, 2) + rhs(2, 2), test::tolerance<Real>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(sub_matrix_3x3, Real, test_targets)
{
    using namespace mjolnir;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-1.0, 1.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Matrix<Real, 3, 3> lhs(
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt)
            ), rhs(
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt)
            );
        const auto sub = lhs - rhs;

        BOOST_TEST(sub(0, 0) == lhs(0, 0) - rhs(0, 0), test::tolerance<Real>());
        BOOST_TEST(sub(0, 1) == lhs(0, 1) - rhs(0, 1), test::tolerance<Real>());
        BOOST_TEST(sub(0, 2) == lhs(0, 2) - rhs(0, 2), test::tolerance<Real>());
        BOOST_TEST(sub(1, 0) == lhs(1, 0) - rhs(1, 0), test::tolerance<Real>());
        BOOST_TEST(sub(1, 1) == lhs(1, 1) - rhs(1, 1), test::tolerance<Real>());
        BOOST_TEST(sub(1, 2) == lhs(1, 2) - rhs(1, 2), test::tolerance<Real>());
        BOOST_TEST(sub(2, 0) == lhs(2, 0) - rhs(2, 0), test::tolerance<Real>());
        BOOST_TEST(sub(2, 1) == lhs(2, 1) - rhs(2, 1), test::tolerance<Real>());
        BOOST_TEST(sub(2, 2) == lhs(2, 2) - rhs(2, 2), test::tolerance<Real>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(scalar_mul_matrix_3x3, Real, test_targets)
{
    using namespace mjolnir;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-1.0, 1.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const Real lhs = uni(mt);
        const math::Matrix<Real, 3, 3> rhs(
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt)
            );
        const auto mul = lhs * rhs;

        BOOST_TEST(mul(0, 0) == lhs * rhs(0, 0), test::tolerance<Real>());
        BOOST_TEST(mul(0, 1) == lhs * rhs(0, 1), test::tolerance<Real>());
        BOOST_TEST(mul(0, 2) == lhs * rhs(0, 2), test::tolerance<Real>());
        BOOST_TEST(mul(1, 0) == lhs * rhs(1, 0), test::tolerance<Real>());
        BOOST_TEST(mul(1, 1) == lhs * rhs(1, 1), test::tolerance<Real>());
        BOOST_TEST(mul(1, 2) == lhs * rhs(1, 2), test::tolerance<Real>());
        BOOST_TEST(mul(2, 0) == lhs * rhs(2, 0), test::tolerance<Real>());
        BOOST_TEST(mul(2, 1) == lhs * rhs(2, 1), test::tolerance<Real>());
        BOOST_TEST(mul(2, 2) == lhs * rhs(2, 2), test::tolerance<Real>());
    }

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Matrix<Real, 3, 3> lhs(
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt)
            );
        const Real rhs = uni(mt);
        const auto mul = lhs * rhs;

        BOOST_TEST(mul(0, 0) == lhs(0, 0) * rhs, test::tolerance<Real>());
        BOOST_TEST(mul(0, 1) == lhs(0, 1) * rhs, test::tolerance<Real>());
        BOOST_TEST(mul(0, 2) == lhs(0, 2) * rhs, test::tolerance<Real>());
        BOOST_TEST(mul(1, 0) == lhs(1, 0) * rhs, test::tolerance<Real>());
        BOOST_TEST(mul(1, 1) == lhs(1, 1) * rhs, test::tolerance<Real>());
        BOOST_TEST(mul(1, 2) == lhs(1, 2) * rhs, test::tolerance<Real>());
        BOOST_TEST(mul(2, 0) == lhs(2, 0) * rhs, test::tolerance<Real>());
        BOOST_TEST(mul(2, 1) == lhs(2, 1) * rhs, test::tolerance<Real>());
        BOOST_TEST(mul(2, 2) == lhs(2, 2) * rhs, test::tolerance<Real>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(scalar_div_matrix_3x3, Real, test_targets)
{
    using namespace mjolnir;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-1.0, 1.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Matrix<Real, 3, 3> lhs(
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt)
            );
        const Real rhs = uni(mt);
        const auto mul = lhs / rhs;

        BOOST_TEST(mul(0, 0) == lhs(0, 0) / rhs, test::tolerance<Real>());
        BOOST_TEST(mul(0, 1) == lhs(0, 1) / rhs, test::tolerance<Real>());
        BOOST_TEST(mul(0, 2) == lhs(0, 2) / rhs, test::tolerance<Real>());
        BOOST_TEST(mul(1, 0) == lhs(1, 0) / rhs, test::tolerance<Real>());
        BOOST_TEST(mul(1, 1) == lhs(1, 1) / rhs, test::tolerance<Real>());
        BOOST_TEST(mul(1, 2) == lhs(1, 2) / rhs, test::tolerance<Real>());
        BOOST_TEST(mul(2, 0) == lhs(2, 0) / rhs, test::tolerance<Real>());
        BOOST_TEST(mul(2, 1) == lhs(2, 1) / rhs, test::tolerance<Real>());
        BOOST_TEST(mul(2, 2) == lhs(2, 2) / rhs, test::tolerance<Real>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(mul_matrix_3x3, Real, test_targets)
{
    using namespace mjolnir;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-1.0, 1.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Matrix<Real, 3, 3> lhs(
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt)
            ), rhs(
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt)
            );
        const auto mul = lhs * rhs;

        for(std::size_t i=0; i<3; ++i)
        {
            for(std::size_t j=0; j<3; ++j)
            {
                Real sum = 0.;
                for(std::size_t k=0; k<3; ++k)
                {
                    sum += lhs(i, k) * rhs(k, j);
                }
                BOOST_TEST(mul(i, j) == sum, test::tolerance<Real>());
            }
        }
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(matrix_3x3_inverse, Real, test_targets)
{
    using namespace mjolnir;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-1.0, 1.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Matrix<Real, 3, 3> lhs(
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt),
                uni(mt), uni(mt), uni(mt)
            );
        if(determinant(lhs) == 0.0)
        {
            continue;
        }

        const auto inv = inverse(lhs);
        const auto unit1 = lhs * inv;
        const auto unit2 = inv * lhs;

        BOOST_TEST(unit1(0, 0) == static_cast<Real>(1.0), test::tolerance<Real>());
        BOOST_TEST(unit1(1, 1) == static_cast<Real>(1.0), test::tolerance<Real>());
        BOOST_TEST(unit1(2, 2) == static_cast<Real>(1.0), test::tolerance<Real>());
        BOOST_TEST(unit1(0, 1) == static_cast<Real>(0.0), test::tolerance<Real>());
        BOOST_TEST(unit1(0, 2) == static_cast<Real>(0.0), test::tolerance<Real>());
        BOOST_TEST(unit1(1, 0) == static_cast<Real>(0.0), test::tolerance<Real>());
        BOOST_TEST(unit1(1, 2) == static_cast<Real>(0.0), test::tolerance<Real>());
        BOOST_TEST(unit1(2, 0) == static_cast<Real>(0.0), test::tolerance<Real>());
        BOOST_TEST(unit1(2, 1) == static_cast<Real>(0.0), test::tolerance<Real>());

        BOOST_TEST(unit2(0, 0) == static_cast<Real>(1.0), test::tolerance<Real>());
        BOOST_TEST(unit2(1, 1) == static_cast<Real>(1.0), test::tolerance<Real>());
        BOOST_TEST(unit2(2, 2) == static_cast<Real>(1.0), test::tolerance<Real>());
        BOOST_TEST(unit2(0, 1) == static_cast<Real>(0.0), test::tolerance<Real>());
        BOOST_TEST(unit2(0, 2) == static_cast<Real>(0.0), test::tolerance<Real>());
        BOOST_TEST(unit2(1, 0) == static_cast<Real>(0.0), test::tolerance<Real>());
        BOOST_TEST(unit2(1, 2) == static_cast<Real>(0.0), test::tolerance<Real>());
        BOOST_TEST(unit2(2, 0) == static_cast<Real>(0.0), test::tolerance<Real>());
        BOOST_TEST(unit2(2, 1) == static_cast<Real>(0.0), test::tolerance<Real>());
    }
}


/*    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


///////////////////////////////////////////////////////////////////////////////////////////
// distribution::survival::example::data::random.cpp                                     //
//                                                                                //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                            //
//  Software License, Version 1.0. (See accompanying file                                //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)                     //
///////////////////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <ostream>
#include <fstream>
#include <stdexcept>
#include <string> //needed?
#include <algorithm>
#include <iterator>
#include <vector>

#include <boost/typeof/typeof.hpp>
#include <boost/range.hpp>
#include <boost/assert.hpp>
#include <boost/foreach.hpp>
#include <boost/accumulators/accumulators.hpp>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

#include <boost/fusion/container/generation/make_map.hpp>
#include <boost/fusion/include/make_map.hpp>

#include <boost/statistics/detail/non_parametric/kolmogorov_smirnov/check_convergence.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/exponential/include.hpp>

#include <boost/statistics/detail/distribution/survival/record/key/include.hpp>
#include <boost/statistics/detail/distribution/survival/response/types/right_truncated/include.hpp>
#include <boost/statistics/detail/distribution/survival/failure_time/meta/include.hpp>
#include <boost/statistics/detail/distribution/survival/models/exponential/scalar/include.hpp>

#include <libs/statistics/detail/distribution/survival/example/data/random.h>

void example_data_random(std::ostream& os)
{
    os << "-> example_data_random : " << std::endl;

    // Generate failure-time data randomly
    // Computes kolmogorov-smirnov statistics at various iterations to verify
    // convergence to the desired distribution

    using namespace boost;
    namespace stat = boost::statistics::detail;
    namespace ds = stat::distribution::survival;
    namespace dt = stat::distribution::toolkit;
    namespace dm = stat::distribution::model;
    namespace ks = stat::kolmogorov_smirnov;

    typedef ds::record::tag::entry_time     tag_et_;
    typedef ds::record::tag::failure_time   tag_ft_;
    typedef dm::tag::covariate          	tag_x_;
    typedef boost::mpl::int_<1>         	tag_cdf_;
    typedef mt19937                     	urng_;
    typedef double                      	val_;
    typedef ds::exponential_model<val_> 	m_;
    typedef ds::failure_time::distribution<m_>::type           d_;
    typedef ds::failure_time::random_distribution<m_>::type    r_;
    typedef boost::variate_generator<urng_&,r_>                g_;
	typedef ks::check_convergence<val_> check_;

    const int n_loops = 1.5e1;
    const int n_init = 2;
    const int n_factor = 2;

    urng_ urng;
    m_ m;
    val_ x = -1.0;
    val_ p = 1.0;
    m.set_parameter(p);
    d_ d = ds::failure_time::make_distribution( x, m );
    g_ g(
        urng,
        ds::failure_time::make_random_distribution( x, m )
    );

	check_ check;

    check(n_loops,n_init,n_factor,d,g,os);

}


//
// Copyright 2019 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef BOOST_GIL_IMAGE_PROCESSING_DETAIL_MATH_HPP
#define BOOST_GIL_IMAGE_PROCESSING_DETAIL_MATH_HPP

#include <array>
#include <boost/gil/extension/numeric/kernel.hpp>

namespace boost {
namespace gil {
namespace detail {

static constexpr double pi = 3.14159265358979323846;

static constexpr std::array<float, 9> dx_sobel = {
    {-1, 0, 1, -2, 0, 2, -1, 0, 1}};
static constexpr std::array<float, 9> dx_scharr = {
    {-1, 0, 1, -1, 0, 1, -1, 0, 1}};
static constexpr std::array<float, 9> dy_sobel = {
    {1, 2, 1, 0, 0, 0, -1, -2, -1}};
static constexpr std::array<float, 9> dy_scharr = {
    {1, 1, 1, 0, 0, 0, -1, -1, -1}};

template <typename T, typename Allocator>
inline detail::kernel_2d<T, Allocator> get_identity_kernel() {
  detail::kernel_2d<T, Allocator> kernel(1, 0, 0);
  kernel[0] = 1;
  return kernel;
}

} // namespace detail
} // namespace gil
} // namespace boost

#endif


#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) 2018, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019, Amit Singh, amitsingh19975@gmail.com
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <iostream>
#include <random>
#include <boost/numeric/ublas/tensor.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/test/unit_test.hpp>

#include "utility.hpp"

BOOST_AUTO_TEST_SUITE ( test_tensor_static_rank_matrix_interoperability )

using test_types = zip<int,float>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

BOOST_AUTO_TEST_CASE_TEMPLATE( test_tensor_matrix_copy_ctor, value,  test_types)
{
  namespace ublas  = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout = typename value::second_type;
    using tensor = ublas::tensor_static_rank<value_type, 2,layout>;
    using matrix = typename tensor::matrix_type;

    auto a2 = tensor( matrix(1,1) );
    BOOST_CHECK_EQUAL(  a2.size() , 1 );
    BOOST_CHECK( !a2.empty() );
    BOOST_CHECK_NE(  a2.data() , nullptr);

    auto a3 = tensor( matrix(2,1) );
    BOOST_CHECK_EQUAL(  a3.size() , 2 );
    BOOST_CHECK( !a3.empty() );
    BOOST_CHECK_NE(  a3.data() , nullptr);

    auto a4 = tensor( matrix(1,2) );
    BOOST_CHECK_EQUAL(  a4.size() , 2 );
    BOOST_CHECK( !a4.empty() );
    BOOST_CHECK_NE(  a4.data() , nullptr);

    auto a5 = tensor( matrix(2,3) );
    BOOST_CHECK_EQUAL(  a5.size() , 6 );
    BOOST_CHECK( !a5.empty() );
    BOOST_CHECK_NE(  a5.data() , nullptr);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_tensor_vector_copy_ctor, value,  test_types)
{
  namespace ublas  = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;
    using tensor_type  = ublas::tensor_static_rank<value_type, 2,layout_type>;
    using vector_type = typename tensor_type::vector_type;

    auto a2 = tensor_type( vector_type(1) );
    BOOST_CHECK_EQUAL(  a2.size() , 1 );
    BOOST_CHECK( !a2.empty() );
    BOOST_CHECK_NE(  a2.data() , nullptr);

    auto a3 = tensor_type( vector_type(2) );
    BOOST_CHECK_EQUAL(  a3.size() , 2 );
    BOOST_CHECK( !a3.empty() );
    BOOST_CHECK_NE(  a3.data() , nullptr);

    auto a4 = tensor_type( vector_type(2) );
    BOOST_CHECK_EQUAL(  a4.size() , 2 );
    BOOST_CHECK( !a4.empty() );
    BOOST_CHECK_NE(  a4.data() , nullptr);

    auto a5 = tensor_type( vector_type(3) );
    BOOST_CHECK_EQUAL(  a5.size() , 3 );
    BOOST_CHECK( !a5.empty() );
    BOOST_CHECK_NE(  a5.data() , nullptr);
}

struct fixture
{
    template<size_t N>
    using extents_type = boost::numeric::ublas::extents<N>;

    std::tuple<
        extents_type<2>, // 0
        extents_type<2>, // 1
        extents_type<2>, // 2
        extents_type<2>, // 3
        extents_type<2>  // 4
    > extents = {
        {1,1},
        {1,2},
        {2,1},
        {6,6},
        {9,7},
    };
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_matrix_copy_ctor_extents, value,  test_types, fixture )
{
  namespace ublas  = boost::numeric::ublas;
  using value_type  = typename value::first_type;
  using layout_type = typename value::second_type;

  auto check = [](auto const& /*unused*/, auto& e) {
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor = ublas::tensor_static_rank<value_type, size,layout_type>;
    using matrix = typename tensor::matrix_type;

    assert(ublas::size(e)==2);
    tensor t = matrix{e[0],e[1]};
    BOOST_CHECK_EQUAL (  t.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  t.rank() , ublas::size   (e) );
    BOOST_CHECK       ( !t.empty()    );
    BOOST_CHECK_NE    (  t.data() , nullptr);
  };

  for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_vector_copy_ctor_extents, value,  test_types, fixture )
{
  namespace ublas  = boost::numeric::ublas;
  using value_type  = typename value::first_type;
  using layout_type = typename value::second_type;

  auto check = [](auto const& /*unused*/, auto& e) {
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor = ublas::tensor_static_rank<value_type, size,layout_type>;
    using vector = typename tensor::vector_type;

    assert(ublas::size(e)==2);
    if(ublas::empty(e))
      return;

    tensor t = vector (product(e));
    BOOST_CHECK_EQUAL (  t.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  t.rank() , ublas::size   (e) );
    BOOST_CHECK       ( !t.empty()    );
    BOOST_CHECK_NE    (  t.data() , nullptr);
  };

  for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_matrix_copy_assignment, value,  test_types, fixture )
{
  namespace ublas  = boost::numeric::ublas;
  using value_type  = typename value::first_type;
  using layout_type = typename value::second_type;

  for_each_in_tuple(extents, [](auto const& /*unused*/, auto& e) {
    using etype  = std::decay_t<decltype(e)>;
    constexpr auto size = std::tuple_size_v<etype>;
    using tensor = ublas::tensor_static_rank<value_type,size,layout_type>;
    using matrix = typename tensor::matrix_type;

    assert(ublas::size(e) == 2);
    auto t = tensor{e[1],e[0]};
    auto r = matrix(e[0],e[1]);
    std::iota(r.data().begin(),r.data().end(), 1);
    t = r;

    BOOST_CHECK_EQUAL (  t.extents().at(0) , e.at(0) );
    BOOST_CHECK_EQUAL (  t.extents().at(1) , e.at(1) );
    BOOST_CHECK_EQUAL (  t.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  t.rank() , ublas::size   (e) );
    BOOST_CHECK       ( !t.empty()    );
    BOOST_CHECK_NE    (  t.data() , nullptr);

    for(auto j = 0ul; j < t.size(1); ++j){
      for(auto i = 0ul; i < t.size(0); ++i){
        BOOST_CHECK_EQUAL( t.at(i,j), r(i,j)  );
      }
    }
  });

  //for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_vector_copy_assignment, value,  test_types, fixture )
{
  namespace ublas  = boost::numeric::ublas;
  using value_type  = typename value::first_type;
  using layout_type = typename value::second_type;

  auto check = [](auto const& /*unused*/, auto& e) {
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_type = ublas::tensor_static_rank<value_type, size,layout_type>;
    using vector_type = typename tensor_type::vector_type;

    assert(ublas::size(e) == 2);
    auto t = tensor_type{e[1],e[0]};
    auto r = vector_type(e[0]*e[1]);
    std::iota(r.data().begin(),r.data().end(), 1);
    t = r;

    BOOST_CHECK_EQUAL (  t.extents().at(0) , e.at(0)*e.at(1) );
    BOOST_CHECK_EQUAL (  t.extents().at(1) , 1);
    BOOST_CHECK_EQUAL (  t.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  t.rank() , ublas::size   (e) );
    BOOST_CHECK       ( !t.empty()    );
    BOOST_CHECK_NE    (  t.data() , nullptr);

    for(auto i = 0ul; i < t.size(); ++i){
      BOOST_CHECK_EQUAL( t[i], r(i)  );
    }
  };

  for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_matrix_move_assignment, value,  test_types, fixture )
{
  namespace ublas  = boost::numeric::ublas;
  using value_type  = typename value::first_type;
  using layout_type = typename value::second_type;

  auto check = [](auto const& /*unused*/, auto& e) {
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_type = ublas::tensor_static_rank<value_type, size,layout_type>;
    using matrix_type = typename tensor_type::matrix_type;

    assert(ublas::size(e) == 2);
    auto t = tensor_type{e[1],e[0]};
    auto r = matrix_type(e[0],e[1]);
    std::iota(r.data().begin(),r.data().end(), 1);
    auto q = r;
    t = std::move(r);

    BOOST_CHECK_EQUAL (  t.extents().at(0) , e.at(0) );
    BOOST_CHECK_EQUAL (  t.extents().at(1) , e.at(1) );
    BOOST_CHECK_EQUAL (  t.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  t.rank() , ublas::size   (e) );
    BOOST_CHECK       ( !t.empty()    );
    BOOST_CHECK_NE    (  t.data() , nullptr);

    for(auto j = 0ul; j < t.size(1); ++j){
      for(auto i = 0ul; i < t.size(0); ++i){
        BOOST_CHECK_EQUAL( t.at(i,j), q(i,j)  );
      }
    }
  };

  for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_vector_move_assignment, value,  test_types, fixture )
{
  namespace ublas  = boost::numeric::ublas;
  using value_type  = typename value::first_type;
  using layout_type = typename value::second_type;

  auto check = [](auto const& /*unused*/, auto& e) {
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_type = ublas::tensor_static_rank<value_type, size,layout_type>;
    using vector_type = typename tensor_type::vector_type;

    assert(ublas::size(e) == 2);
    auto t = tensor_type{e[1],e[0]};
    auto r = vector_type(e[0]*e[1]);
    std::iota(r.data().begin(),r.data().end(), 1);
    auto q = r;
    t = std::move(r);

    BOOST_CHECK_EQUAL (  t.extents().at(0) , e.at(0) * e.at(1));
    BOOST_CHECK_EQUAL (  t.extents().at(1) , 1);
    BOOST_CHECK_EQUAL (  t.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  t.rank() , ublas::size   (e) );
    BOOST_CHECK       ( !t.empty()    );
    BOOST_CHECK_NE    (  t.data() , nullptr);

    for(auto i = 0ul; i < t.size(); ++i){
      BOOST_CHECK_EQUAL( t[i], q(i)  );
    }
  };

  for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_matrix_expressions, value,  test_types, fixture )
{
  namespace ublas  = boost::numeric::ublas;
  using value_type  = typename value::first_type;
  using layout_type = typename value::second_type;

  auto check = [](auto const& /*unused*/, auto& e) {
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_type = ublas::tensor_static_rank<value_type, size,layout_type>;
    using matrix_type = typename tensor_type::matrix_type;

    assert(ublas::size(e) == 2);
    auto t = tensor_type{e[1],e[0]};
    auto r = matrix_type(e[0],e[1]);
    std::iota(r.data().begin(),r.data().end(), 1);
    t = r + 3*r;
    tensor_type s = r + 3*r;
    tensor_type q = s + r + 3*r + s; // + 3*r

    BOOST_CHECK_EQUAL (  t.extents().at(0) , e.at(0) );
    BOOST_CHECK_EQUAL (  t.extents().at(1) , e.at(1) );
    BOOST_CHECK_EQUAL (  t.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  t.rank() , ublas::size   (e) );
    BOOST_CHECK       ( !t.empty()    );
    BOOST_CHECK_NE    (  t.data() , nullptr);

    BOOST_CHECK_EQUAL (  s.extents().at(0) , e.at(0) );
    BOOST_CHECK_EQUAL (  s.extents().at(1) , e.at(1) );
    BOOST_CHECK_EQUAL (  s.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  s.rank() , ublas::size   (e) );
    BOOST_CHECK       ( !s.empty()    );
    BOOST_CHECK_NE    (  s.data() , nullptr);

    BOOST_CHECK_EQUAL (  q.extents().at(0) , e.at(0) );
    BOOST_CHECK_EQUAL (  q.extents().at(1) , e.at(1) );
    BOOST_CHECK_EQUAL (  q.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  q.rank() , ublas::size   (e) );
    BOOST_CHECK       ( !q.empty()    );
    BOOST_CHECK_NE    (  q.data() , nullptr);

    for(auto j = 0ul; j < t.size(1); ++j){
      for(auto i = 0ul; i < t.size(0); ++i){
        BOOST_CHECK_EQUAL( t.at(i,j), 4*r(i,j)  );
        BOOST_CHECK_EQUAL( s.at(i,j), t.at(i,j)  );
        BOOST_CHECK_EQUAL( q.at(i,j), 3*s.at(i,j)  );
      }
    }
  };

  for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_vector_expressions, value,  test_types, fixture )
{
  namespace ublas  = boost::numeric::ublas;
  using value_type  = typename value::first_type;
  using layout_type = typename value::second_type;

  auto check = [](auto const& /*unused*/, auto& e) {
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_type = ublas::tensor_static_rank<value_type, size,layout_type>;
    using vector_type = typename tensor_type::vector_type;

    assert(ublas::size(e) == 2);
    auto t = tensor_type{e[1],e[0]};
    auto r = vector_type(e[0]*e[1]);
    std::iota(r.data().begin(),r.data().end(), 1);
    t = r + 3*r;
    tensor_type s = r + 3*r;
    tensor_type q = s + r + 3*r + s; // + 3*r

    BOOST_CHECK_EQUAL (  t.extents().at(0) , e.at(0)*e.at(1) );
    BOOST_CHECK_EQUAL (  t.extents().at(1) , 1);
    BOOST_CHECK_EQUAL (  t.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  t.rank() , ublas::size   (e) );
    BOOST_CHECK       ( !t.empty()    );
    BOOST_CHECK_NE    (  t.data() , nullptr);

    BOOST_CHECK_EQUAL (  s.extents().at(0) , e.at(0)*e.at(1) );
    BOOST_CHECK_EQUAL (  s.extents().at(1) , 1);
    BOOST_CHECK_EQUAL (  s.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  s.rank() , ublas::size   (e) );
    BOOST_CHECK       ( !s.empty()    );
    BOOST_CHECK_NE    (  s.data() , nullptr);

    BOOST_CHECK_EQUAL (  q.extents().at(0) , e.at(0)*e.at(1) );
    BOOST_CHECK_EQUAL (  q.extents().at(1) , 1);
    BOOST_CHECK_EQUAL (  q.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  q.rank() , ublas::size   (e) );
    BOOST_CHECK       ( !q.empty()    );
    BOOST_CHECK_NE    (  q.data() , nullptr);

    for(auto i = 0ul; i < t.size(); ++i){
      BOOST_CHECK_EQUAL( t.at(i), 4*r(i)  );
      BOOST_CHECK_EQUAL( s.at(i), t.at(i)  );
      BOOST_CHECK_EQUAL( q.at(i), 3*s.at(i)  );
    }
  };

  for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_matrix_vector_expressions, pair,  test_types, fixture )
{
  namespace ublas  = boost::numeric::ublas;
  using value  = typename pair::first_type;
  using layout = typename pair::second_type;

  auto check = [](auto const& /*unused*/, auto& e) {
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor = ublas::tensor_static_rank<value, size, layout>;
    using matrix = typename tensor::matrix_type;
    using vector = typename tensor::vector_type;

    if(product(e) <= 2)
      return;
    assert(ublas::size(e) == 2);
    auto Q = tensor{e[0],1};
    auto A = matrix(e[0],e[1]);
    auto b = vector(e[1]);
    auto c = vector(e[0]);
    std::iota(b.data().begin(),b.data().end(), 1);
    std::fill(A.data().begin(),A.data().end(), 1);
    std::fill(c.data().begin(),c.data().end(), 2);
    std::fill(Q.begin(),Q.end(), 2);

    tensor T = Q + (ublas::prod(A , b) + 2*c) + 3*Q;

    BOOST_CHECK_EQUAL (  T.extents().at(0) , Q.extents().at(0) );
    BOOST_CHECK_EQUAL (  T.extents().at(1) , Q.extents().at(1));
    BOOST_CHECK_EQUAL (  T.size() , Q.size() );
    BOOST_CHECK_EQUAL (  T.size() , c.size() );
    BOOST_CHECK_EQUAL (  T.rank() , Q.rank() );
    BOOST_CHECK       ( !T.empty()    );
    BOOST_CHECK_NE    (  T.data() , nullptr);

    const auto n   = e[1];
    const auto ab  = value(std::div(n*(n+1),2).quot);
    const auto ref = ab+4*Q(0)+2*c(0);
    BOOST_CHECK( std::all_of(T.begin(),T.end(), [ref](auto cc){ return ref == cc; }) );

//    for(auto i = 0ul; i < T.size(); ++i){
//      auto n = e[1];
//      auto ab = n * (n+1) / 2;
//      BOOST_CHECK_EQUAL( T(i), ab+4*Q(0)+2*c(0)  );
//    }

  };
  for_each_in_tuple(extents,check);
}

BOOST_AUTO_TEST_SUITE_END()


/* =========================================================================
   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;
}