#include "gradient_psv.hpp"

#include <Eigen/Dense>
#include <cmath>
#include <complex>
#include <fmt/format.h>
#include <fmt/ostream.h>
#include <fstream>
#include <memory>
#include <utility>

using namespace Eigen;
using namespace std::complex_literals;

using std::exp;
using std::pow;
using std::sqrt;

using complex_d = std::complex<double>;
using Mat42cd = Matrix<complex_d, 4, 2>;
using Ary24cd = Array<complex_d, 2, 4>;

const double PI = 3.14159265358979323846;

namespace grad_psv {

GRTCoeff::GRTCoeff(const Ref<const ArrayXXd> model, const double freq,
                   const double c)
    : z_(model.col(1)), rho_(model.col(2)), beta_(model.col(3)),
      alpha_(model.col(4)), mu_(rho_ * beta_.pow(2)), nl_(model.rows()),
      angfreq_(2.0 * PI * freq), c_(c), gamma_(nl_), nv_(nl_),
      e11_(nl_, Matrix2cd::Zero()), e12_(nl_, Matrix2cd::Zero()),
      e21_(nl_, Matrix2cd::Zero()), e22_(nl_, Matrix2cd::Zero()),
      t_d_(nl_, Matrix2cd::Zero()), r_ud_(nl_, Matrix2cd::Zero()),
      r_du_(nl_, Matrix2cd::Zero()), t_u_(nl_, Matrix2cd::Zero()),
      gt_d_(nl_, Matrix2cd::Zero()), gr_ud_(nl_, Matrix2cd::Zero()),
      gr_du_(nl_ + 1, Matrix2cd::Zero()), gt_u_(nl_, Matrix2cd::Zero()),
      Cd_(nl_, Vector2cd::Zero()), Cu_(nl_, Vector2cd::Zero()) {

  initialize_gamma();
  initialize_nv();
  initialize_E();

  compute_rtc();
  compute_grtc();
  compute_CdCu();
}

void GRTCoeff::initialize_E() {
  complex_d k = angfreq_ / c_;
  for (auto i = 0; i < nl_; ++i) {
    complex_d gamma = gamma_(i);
    complex_d nv = nv_(i);
    complex_d xi = pow(k, 2) + pow(nv, 2);

    // E
    e11_[i] << alpha_(i) * k, beta_(i) * nv, alpha_(i) * gamma, beta_(i) * k;
    e11_[i] /= angfreq_;

    auto &tmp11 = e11_[i];
    e12_[i] << tmp11(0, 0), tmp11(0, 1), -tmp11(1, 0), -tmp11(1, 1);

    e21_[i] << -2.0 * alpha_(i) * mu_(i) * k * gamma, -beta_(i) * mu_(i) * xi,
        -alpha_(i) * mu_(i) * xi, -2.0 * beta_(i) * mu_(i) * k * nv;
    e21_[i] /= angfreq_;

    auto &tmp21 = e21_[i];
    e22_[i] << -tmp21(0, 0), -tmp21(0, 1), tmp21(1, 0), tmp21(1, 1);
  }
}

void GRTCoeff::initialize_gamma() {
  for (int i = 0; i < nl_; ++i) {
    complex_d val =
        std::sqrt(pow(angfreq_ / c_, 2) - pow(angfreq_ / alpha_(i), 2));
    if (val.real() < 0) {
      val = -val;
    }
    gamma_(i) = val;
  }
}

void GRTCoeff::initialize_nv() {
  for (int i = 0; i < nl_; ++i) {
    complex_d val =
        std::sqrt(pow(angfreq_ / c_, 2) - pow(angfreq_ / beta_(i), 2));
    if (val.real() < 0) {
      val = -val;
    }
    nv_(i) = val;
  }
}

Matrix2cd GRTCoeff::get_Ad(const double z, const int ind_layer) const {
  Matrix2cd Ad;
  Ad << exp(-gamma_(ind_layer) * (z - z_(ind_layer))), 0., 0.,
      exp(-nv_(ind_layer) * (z - z_(ind_layer)));
  return Ad;
}

Matrix2cd GRTCoeff::get_Au(const double z, const int ind_layer) const {
  Matrix2cd Au;
  if (ind_layer == nl_ - 1) {
    Au << 0., 0., 0., 0.;
  } else {
    Au << exp(-gamma_(ind_layer) * (z_(ind_layer + 1) - z)), 0., 0.,
        exp(-nv_(ind_layer) * (z_(ind_layer + 1) - z));
  }
  return Au;
}

Matrix2cd GRTCoeff::get_Ad_der(const double z, const int ind_layer) const {
  Matrix2cd Ad_der;
  Ad_der << -gamma_(ind_layer) * exp(-gamma_(ind_layer) * (z - z_(ind_layer))),
      0., 0., -nv_(ind_layer) * exp(-nv_(ind_layer) * (z - z_(ind_layer)));
  return Ad_der;
}

Matrix2cd GRTCoeff::get_Au_der(const double z, const int ind_layer) const {
  if (ind_layer == nl_ - 1) {
    return Matrix2cd::Zero();
  }
  Matrix2cd Au_der;
  Au_der << gamma_(ind_layer) *
                exp(-gamma_(ind_layer) * (z_(ind_layer + 1) - z)),
      0., 0., nv_(ind_layer) * exp(-nv_(ind_layer) * (z_(ind_layer + 1) - z));
  return Au_der;
}

void GRTCoeff::compute_rtc() {
  for (auto i = 1; i < nl_ - 1; ++i) {
    auto &e11_0 = e11_[i - 1];
    auto &e12_0 = e12_[i - 1];
    auto &e21_0 = e21_[i - 1];
    auto &e22_0 = e22_[i - 1];
    auto &e11_1 = e11_[i];
    auto &e12_1 = e12_[i];
    auto &e21_1 = e21_[i];
    auto &e22_1 = e22_[i];

    Matrix2cd exp_d00 = get_Ad(z_(i), i - 1);
    Matrix2cd exp_u10 = get_Au(z_(i), i);

    Matrix4cd mat1;
    mat1 << e11_1(0, 0), e11_1(0, 1), -e12_0(0, 0), -e12_0(0, 1), //
        e11_1(1, 0), e11_1(1, 1), -e12_0(1, 0), -e12_0(1, 1),     //
        e21_1(0, 0), e21_1(0, 1), -e22_0(0, 0), -e22_0(0, 1),     //
        e21_1(1, 0), e21_1(1, 1), -e22_0(1, 0), -e22_0(1, 1);

    Matrix4cd mat2;
    mat2 << e11_0(0, 0), e11_0(0, 1), -e12_1(0, 0), -e12_1(0, 1), //
        e11_0(1, 0), e11_0(1, 1), -e12_1(1, 0), -e12_1(1, 1),     //
        e21_0(0, 0), e21_0(0, 1), -e22_1(0, 0), -e22_1(0, 1),     //
        e21_0(1, 0), e21_0(1, 1), -e22_1(1, 0), -e22_1(1, 1);

    Matrix4cd mat3;
    mat3 << exp_d00(0, 0), exp_d00(0, 1), 0, 0, //
        exp_d00(1, 0), exp_d00(1, 1), 0, 0,     //
        0, 0, exp_u10(0, 0), exp_u10(0, 1),     //
        0, 0, exp_u10(1, 0), exp_u10(1, 1);

    Matrix4cd result = mat1.inverse() * (mat2 * mat3);

    t_d_[i] << result(0, 0), result(0, 1), result(1, 0), result(1, 1);

    r_ud_[i] << result(0, 2), result(0, 3), result(1, 2), result(1, 3);

    r_du_[i] << result(2, 0), result(2, 1), result(3, 0), result(3, 1);

    t_u_[i] << result(2, 2), result(2, 3), result(3, 2), result(3, 3);
  }

  // the last interface
  auto &e11_N = e11_[nl_ - 1];
  auto &e21_N = e21_[nl_ - 1];
  auto &e11_N_1 = e11_[nl_ - 2];
  auto &e12_N_1 = e12_[nl_ - 2];
  auto &e21_N_1 = e21_[nl_ - 2];
  auto &e22_N_1 = e22_[nl_ - 2];

  Matrix4cd mat1;
  mat1 << e11_N(0, 0), e11_N(0, 1), -e12_N_1(0, 0), -e12_N_1(0, 1), //
      e11_N(1, 0), e11_N(1, 1), -e12_N_1(1, 0), -e12_N_1(1, 1),     //
      e21_N(0, 0), e21_N(0, 1), -e22_N_1(0, 0), -e22_N_1(0, 1),     //
      e21_N(1, 0), e21_N(1, 1), -e22_N_1(1, 0), -e22_N_1(1, 1);

  Matrix2cd exp_dN_1N_1 = get_Ad(z_(nl_ - 1), nl_ - 2);
  Matrix2cd mat2_1 = e11_N_1 * exp_dN_1N_1;
  Matrix2cd mat2_2 = e21_N_1 * exp_dN_1N_1;

  Mat42cd mat2;
  mat2 << mat2_1(0, 0), mat2_1(0, 1), mat2_1(1, 0), mat2_1(1, 1), //
      mat2_2(0, 0), mat2_2(0, 1), mat2_2(1, 0), mat2_2(1, 1);

  Mat42cd result = mat1.inverse() * mat2;
  t_d_[nl_ - 1] << result(0, 0), result(0, 1), result(1, 0), result(1, 1);
  r_du_[nl_ - 1] << result(2, 0), result(2, 1), result(3, 0), result(3, 1);
}

void GRTCoeff::compute_grtc() {
  for (auto i = nl_ - 1; i >= 1; --i) {
    Matrix2cd mat1 = matI_ - r_ud_[i] * gr_du_[i + 1];
    gt_d_[i] = mat1.inverse() * t_d_[i];
    gr_du_[i] = r_du_[i] + t_u_[i] * gr_du_[i + 1] * gt_d_[i];
  }
  gr_ud_[0] = -e21_[0].inverse() * e22_[0] * get_Au(z_(0), 0);
  for (auto i = 1; i < nl_; ++i) {
    Matrix2cd mat1 = matI_ - r_du_[i] * gr_ud_[i - 1];
    gt_u_[i] = mat1.inverse() * t_u_[i];
    gr_ud_[i] = r_ud_[i] + t_d_[i] * gr_ud_[i - 1] * gt_u_[i];
  }
}

void GRTCoeff::compute_CdCu() {
  Matrix2cd mat1 = matI_ - gr_ud_[0] * gr_du_[1];

  complex_d norm = sqrt(pow(mat1(0, 0), 2) + pow(mat1(0, 1), 2));
  Cd_[0] << mat1(0, 1) / norm, -mat1(0, 0) / norm;
  Cu_[0] = gr_du_[1] * Cd_[0];

  for (auto i = 1; i < nl_ - 1; ++i) {
    Cd_[i] = gt_d_[i] * Cd_[i - 1];
    Cu_[i] = gr_du_[i + 1] * Cd_[i];
  }
  Cd_[nl_ - 1] = gt_d_[nl_ - 1] * Cd_[nl_ - 2];
}

IntegralLayer::IntegralLayer(const Ref<const ArrayXXd> model, const double freq,
                             const double c)
    : grtc_(std::make_unique<GRTCoeff>(model, freq, c)), nl_(model.rows()),
      k_(2.0 * PI * freq / c), pvel_(c), z_(model.col(1)), alpha_(model.col(4)),
      beta_(model.col(3)), rho_(model.col(2)), mu_(rho_ * beta_ * beta_),
      lamb_(rho_ * alpha_ * alpha_ - 2.0 * mu_), gamma_(grtc_->gamma_),
      nv_(grtc_->nv_), thickness_(nl_ - 1), Cd_(grtc_->Cd_), Cu_(grtc_->Cu_),
      matE_(nl_, MatrixXcd::Zero(2, 4)), matP_u_u_(nl_, Matrix4cd::Zero()),
      matP_uc_u_(nl_, Matrix4cd::Zero()), matP_uc_uc_(nl_, Matrix4cd::Zero()),
      matP_du_du_(nl_, Matrix4cd::Zero()), matP_duc_du_(nl_, Matrix4cd::Zero()),
      matP_duc_duc_(nl_, Matrix4cd::Zero()), matP_u_du_(nl_, Matrix4cd::Zero()),
      matP_uc_du_(nl_, Matrix4cd::Zero()), matP_u_duc_(nl_, Matrix4cd::Zero()),
      matP_uc_duc_(nl_, Matrix4cd::Zero()),
      sigma_x_sigma_top_(nl_, Matrix4cd::Zero()),
      sigmac_x_sigma_top_(nl_, Matrix4cd::Zero()),
      sigma_x_sigmac_top_(nl_, Matrix4cd::Zero()),
      sigmac_x_sigmac_top_(nl_, Matrix4cd::Zero()),
      sigma_x_sigma_bottom_(nl_, Matrix4cd::Zero()),
      sigmac_x_sigma_bottom_(nl_, Matrix4cd::Zero()),
      sigma_x_sigmac_bottom_(nl_, Matrix4cd::Zero()),
      sigmac_x_sigmac_bottom_(nl_, Matrix4cd::Zero()),
      int_us2_(ArrayXd::Zero(nl_)), int_ur2_(ArrayXd::Zero(nl_)),
      int_dus2_(ArrayXd::Zero(nl_)), int_dur2_(ArrayXd::Zero(nl_)),
      int_urdus_(ArrayXd::Zero(nl_)), int_usdur_(ArrayXd::Zero(nl_)) {
  for (int i = 0; i < nl_ - 1; ++i) {
    thickness_(i) = z_(i + 1) - z_(i);
  }
  for (int i = 0; i < nl_; ++i) {
    matE_[i] << grtc_->e11_[i], grtc_->e12_[i];
  }

  initialize_P();
  initialize_sigma();

  integrate_us2();
  integrate_dus2();
  integrate_ur2();
  integrate_dur2();
  integrate_usdur();
  integrate_urdus();
}

void IntegralLayer::initialize_P() {
  for (int i = 0; i < nl_; ++i) {
    const complex_d ga0 = gamma_(i);
    const complex_d nv0 = nv_(i);
    const complex_d ga0_2 = ga0 * ga0;
    const complex_d nv0_2 = nv0 * nv0;
    const complex_d ga1 = conj(ga0);
    const complex_d nv1 = conj(nv0);
    const complex_d ga1_2 = ga1 * ga1;
    const complex_d nv1_2 = nv1 * nv1;
    matP_u_u_[i] << 1. / (-ga0 - ga0), 1. / (-ga0 - nv0), 1., 1. / (-ga0 + nv0),
        1. / (-nv0 - ga0), 1. / (-nv0 - nv0), 1. / (-nv0 + ga0), 1., 1.,
        1. / (ga0 - nv0), 1. / (ga0 + ga0), 1. / (ga0 + nv0), 1. / (nv0 - ga0),
        1., 1. / (nv0 + ga0), 1. / (nv0 + nv0);
    matP_uc_u_[i] << 1. / (-ga1 - ga0), 1. / (-ga1 - nv0), 1. / (-ga1 + ga0),
        1. / (-ga1 + nv0), 1. / (-nv1 - ga0), 1. / (-nv1 - nv0),
        1. / (-nv1 + ga0), 1. / (-nv1 + nv0), 1. / (ga1 - ga0),
        1. / (ga1 - nv0), 1. / (ga1 + ga0), 1. / (ga1 + nv0), 1. / (nv1 - ga0),
        1. / (nv1 - nv0), 1. / (nv1 + ga0), 1. / (nv1 + nv0);
    matP_uc_uc_[i] << 1. / (-ga1 - ga1), 1. / (-ga1 - nv1), 1.,
        1. / (-ga1 + nv1), 1. / (-nv1 - ga1), 1. / (-nv1 - nv1),
        1. / (-nv1 + ga1), 1., 1., 1. / (ga1 - nv1), 1. / (ga1 + ga1),
        1. / (ga1 + nv1), 1. / (nv1 - ga1), 1., 1. / (nv1 + ga1),
        1. / (nv1 + nv1);
    matP_du_du_[i] << ga0_2 / (-ga0 - ga0), ga0 * nv0 / (-ga0 - nv0), -ga0_2,
        -ga0 * nv0 / (-ga0 + nv0), nv0 * ga0 / (-nv0 - ga0),
        nv0_2 / (-nv0 - nv0), -nv0 * ga0 / (-nv0 + ga0), -nv0_2, -ga0_2,
        -ga0 * nv0 / (ga0 - nv0), ga0_2 / (ga0 + ga0), ga0 * nv0 / (ga0 + nv0),
        -ga0 * nv0 / (nv0 - ga0), -nv0_2, ga0 * nv0 / (nv0 + ga0),
        nv0_2 / (nv0 + nv0);
    matP_duc_du_[i] << ga1 * ga0 / (-ga1 - ga0), ga1 * nv0 / (-ga1 - nv0),
        -ga1 * ga0 / (-ga1 + ga0), -ga1 * nv0 / (-ga1 + nv0),
        nv1 * ga0 / (-nv1 - ga0), nv1 * nv0 / (-nv1 - nv0),
        -nv1 * ga0 / (-nv1 + ga0), -nv1 * nv0 / (-nv1 + nv0),
        -ga1 * ga0 / (ga1 - ga0), -ga1 * nv0 / (ga1 - nv0),
        ga1 * ga0 / (ga1 + ga0), ga1 * nv0 / (ga1 + nv0),
        -nv1 * ga0 / (nv1 - ga0), -nv1 * nv0 / (nv1 - nv0),
        nv1 * ga0 / (nv1 + ga0), nv1 * nv0 / (nv1 + nv0);
    matP_duc_duc_[i] << ga1_2 / (-ga1 - ga1), ga1 * nv1 / (-ga1 - nv1), -ga1_2,
        -ga1 * nv1 / (-ga1 + nv1), nv1 * ga1 / (-nv1 - ga1),
        nv1_2 / (-nv1 - nv1), -nv1 * ga1 / (-nv1 + ga1), -nv1_2, -ga1_2,
        -ga1 * nv1 / (ga1 - nv1), ga1_2 / (ga1 + ga1), ga1 * nv1 / (ga1 + nv1),
        -ga1 * nv1 / (nv1 - ga1), -nv1_2, ga1 * nv1 / (nv1 + ga1),
        nv1_2 / (nv1 + nv1);
    matP_u_du_[i] << -ga0 / (-ga0 - ga0), -nv0 / (-ga0 - nv0), ga0,
        nv0 / (-ga0 + nv0), -ga0 / (-nv0 - ga0), -nv0 / (-nv0 - nv0),
        ga0 / (-nv0 + ga0), nv0, -ga0, -nv0 / (ga0 - nv0), ga0 / (ga0 + ga0),
        nv0 / (ga0 + nv0), -ga0 / (nv0 - ga0), -nv0, ga0 / (nv0 + ga0),
        nv0 / (nv0 + nv0);
    matP_uc_du_[i] << -ga0 / (-ga1 - ga0), -nv0 / (-ga1 - nv0),
        ga0 / (-ga1 + ga0), nv0 / (-ga1 + nv0), -ga0 / (-nv1 - ga0),
        -nv0 / (-nv1 - nv0), ga0 / (-nv1 + ga0), nv0 / (-nv1 + nv0),
        -ga0 / (ga1 - ga0), -nv0 / (ga1 - nv0), ga0 / (ga1 + ga0),
        nv0 / (ga1 + nv0), -ga0 / (nv1 - ga0), -nv0 / (nv1 - nv0),
        ga0 / (nv1 + ga0), nv0 / (nv1 + nv0);
    matP_u_duc_[i] << -ga1 / (-ga0 - ga1), -nv1 / (-ga0 - nv1),
        ga1 / (-ga0 + ga1), nv1 / (-ga0 + nv1), -ga1 / (-nv0 - ga1),
        -nv1 / (-nv0 - nv1), ga1 / (-nv0 + ga1), nv1 / (-nv0 + nv1),
        -ga1 / (ga0 - ga1), -nv1 / (ga0 - nv1), ga1 / (ga0 + ga1),
        nv1 / (ga0 + nv1), -ga1 / (nv0 - ga1), -nv1 / (nv0 - nv1),
        ga1 / (nv0 + ga1), nv1 / (nv0 + nv1);
    matP_uc_duc_[i] << -ga1 / (-ga1 - ga1), -nv1 / (-ga1 - nv1), ga1,
        nv1 / (-ga1 + nv1), -ga1 / (-nv1 - ga1), -nv1 / (-nv1 - nv1),
        ga1 / (-nv1 + ga1), nv1, -ga1, -nv1 / (ga1 - nv1), ga1 / (ga1 + ga1),
        nv1 / (ga1 + nv1), -ga1 / (nv1 - ga1), -nv1, ga1 / (nv1 + ga1),
        nv1 / (nv1 + nv1);
    if (pvel_ > alpha_(i)) {
      matP_uc_u_[i](0, 0) = 1.;
      matP_uc_u_[i](2, 2) = 1.;
      matP_duc_du_[i](0, 0) = ga1 * ga0;
      matP_duc_du_[i](2, 2) = ga1 * ga0;
      matP_uc_du_[i](0, 0) = -ga0;
      matP_uc_du_[i](2, 2) = ga0;
      matP_u_duc_[i](0, 0) = -ga1;
      matP_u_duc_[i](2, 2) = ga1;
    } else {
      matP_uc_u_[i](0, 2) = 1.;
      matP_uc_u_[i](2, 0) = 1.;
      matP_duc_du_[i](0, 2) = -ga1 * ga0;
      matP_duc_du_[i](2, 0) = -ga1 * ga0;
      matP_uc_du_[i](0, 2) = ga0;
      matP_uc_du_[i](2, 0) = -ga0;
      matP_u_duc_[i](0, 2) = ga1;
      matP_u_duc_[i](2, 0) = -ga1;
    }
    if (pvel_ > beta_(i)) {
      matP_uc_u_[i](1, 1) = 1.;
      matP_uc_u_[i](3, 3) = 1.;
      matP_duc_du_[i](1, 1) = nv1 * nv0;
      matP_duc_du_[i](3, 3) = nv1 * nv0;
      matP_uc_du_[i](1, 1) = -nv0;
      matP_uc_du_[i](3, 3) = nv0;
      matP_u_duc_[i](1, 1) = -nv1;
      matP_u_duc_[i](3, 3) = nv1;
    } else {
      matP_uc_u_[i](1, 3) = 1.;
      matP_uc_u_[i](3, 1) = 1.;
      matP_duc_du_[i](1, 3) = -nv1 * nv0;
      matP_duc_du_[i](3, 1) = -nv1 * nv0;
      matP_uc_du_[i](1, 3) = nv0;
      matP_uc_du_[i](3, 1) = -nv0;
      matP_u_duc_[i](1, 3) = nv1;
      matP_u_duc_[i](3, 1) = -nv1;
    }
  }
}

void IntegralLayer::initialize_sigma() {
  MatrixXcd sigma_bottom = MatrixXcd::Zero(4, nl_);
  MatrixXcd sigma_top = MatrixXcd::Zero(4, nl_);
  for (auto i = 0; i < nl_ - 1; ++i) {
    sigma_bottom.col(i) << Cd_[i](0), Cd_[i](1),
        Cu_[i](0) * exp(-gamma_(i) * thickness_(i)),
        Cu_[i](1) * exp(-nv_(i) * thickness_(i));
    sigma_top.col(i) << Cd_[i](0) * exp(-gamma_(i) * thickness_(i)),
        Cd_[i](1) * exp(-nv_(i) * thickness_(i)), Cu_[i](0), Cu_[i](1);
  }
  sigma_bottom.col(nl_ - 1) << Cd_[nl_ - 1](0), Cd_[nl_ - 1](1), 0, 0;
  sigma_top.col(nl_ - 1).fill(0);

  for (auto id_layer = 0; id_layer < nl_; ++id_layer) {
    auto cd = Cd_[id_layer];
    auto cu = Cu_[id_layer];
    for (auto i = 0; i < 4; ++i) {
      for (auto j = 0; j < 4; ++j) {
        sigma_x_sigma_top_[id_layer](i, j) =
            sigma_top(i, id_layer) * sigma_top(j, id_layer);
        sigmac_x_sigma_top_[id_layer](i, j) =
            conj(sigma_top(i, id_layer)) * sigma_top(j, id_layer);
        sigma_x_sigmac_top_[id_layer](i, j) =
            sigma_top(i, id_layer) * conj(sigma_top(j, id_layer));
        sigmac_x_sigmac_top_[id_layer](i, j) =
            conj(sigma_top(i, id_layer)) * conj(sigma_top(j, id_layer));

        sigma_x_sigma_bottom_[id_layer](i, j) =
            sigma_bottom(i, id_layer) * sigma_bottom(j, id_layer);
        sigmac_x_sigma_bottom_[id_layer](i, j) =
            conj(sigma_bottom(i, id_layer)) * sigma_bottom(j, id_layer);
        sigma_x_sigmac_bottom_[id_layer](i, j) =
            sigma_bottom(i, id_layer) * conj(sigma_bottom(j, id_layer));
        sigmac_x_sigmac_bottom_[id_layer](i, j) =
            conj(sigma_bottom(i, id_layer)) * conj(sigma_bottom(j, id_layer));
      }
    }
    if (id_layer != nl_ - 1) {
      sigma_x_sigma_top_[id_layer](0, 2) *= thickness_(id_layer);
      sigma_x_sigma_top_[id_layer](2, 0) *= thickness_(id_layer);
      sigma_x_sigma_top_[id_layer](1, 3) *= thickness_(id_layer);
      sigma_x_sigma_top_[id_layer](3, 1) *= thickness_(id_layer);
      sigmac_x_sigmac_top_[id_layer](0, 2) *= thickness_(id_layer);
      sigmac_x_sigmac_top_[id_layer](2, 0) *= thickness_(id_layer);
      sigmac_x_sigmac_top_[id_layer](1, 3) *= thickness_(id_layer);
      sigmac_x_sigmac_top_[id_layer](3, 1) *= thickness_(id_layer);
      if (pvel_ > alpha_(id_layer)) {
        sigmac_x_sigma_top_[id_layer](0, 0) =
            conj(cd(0)) * cd(0) * thickness_(id_layer);
        sigmac_x_sigma_top_[id_layer](2, 2) =
            conj(cu(0)) * cu(0) * thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](0, 0) =
            cd(0) * conj(cd(0)) * thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](2, 2) =
            cu(0) * conj(cu(0)) * thickness_(id_layer);
      } else {
        sigmac_x_sigma_top_[id_layer](0, 2) *= thickness_(id_layer);
        sigmac_x_sigma_top_[id_layer](2, 0) *= thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](0, 2) *= thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](2, 0) *= thickness_(id_layer);
      }
      if (pvel_ > beta_(id_layer)) {
        sigmac_x_sigma_top_[id_layer](1, 1) =
            conj(cd(1)) * cd(1) * thickness_(id_layer);
        sigmac_x_sigma_top_[id_layer](3, 3) =
            conj(cu(1)) * cu(1) * thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](1, 1) =
            cd(1) * conj(cd(1)) * thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](3, 3) =
            cu(1) * conj(cu(1)) * thickness_(id_layer);
      } else {
        sigmac_x_sigma_top_[id_layer](1, 3) *= thickness_(id_layer);
        sigmac_x_sigma_top_[id_layer](3, 1) *= thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](1, 3) *= thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](3, 1) *= thickness_(id_layer);
      }
      sigma_x_sigma_bottom_[id_layer](0, 2) = 0.;
      sigma_x_sigma_bottom_[id_layer](2, 0) = 0.;
      sigma_x_sigma_bottom_[id_layer](1, 3) = 0.;
      sigma_x_sigma_bottom_[id_layer](3, 1) = 0.;
      sigmac_x_sigmac_bottom_[id_layer](0, 2) = 0.;
      sigmac_x_sigmac_bottom_[id_layer](2, 0) = 0.;
      sigmac_x_sigmac_bottom_[id_layer](1, 3) = 0.;
      sigmac_x_sigmac_bottom_[id_layer](3, 1) = 0.;
      if (pvel_ > alpha_(id_layer)) {
        sigmac_x_sigma_bottom_[id_layer](0, 0) = 0.;
        sigmac_x_sigma_bottom_[id_layer](2, 2) = 0.;
        sigma_x_sigmac_bottom_[id_layer](0, 0) = 0.;
        sigma_x_sigmac_bottom_[id_layer](2, 2) = 0.;
      } else {
        sigmac_x_sigma_bottom_[id_layer](0, 2) = 0.;
        sigmac_x_sigma_bottom_[id_layer](2, 0) = 0.;
        sigma_x_sigmac_bottom_[id_layer](0, 2) = 0.;
        sigma_x_sigmac_bottom_[id_layer](2, 0) = 0.;
      }
      if (pvel_ > beta_(id_layer)) {
        sigmac_x_sigma_bottom_[id_layer](1, 1) = 0.;
        sigmac_x_sigma_bottom_[id_layer](3, 3) = 0.;
        sigma_x_sigmac_bottom_[id_layer](1, 1) = 0.;
        sigma_x_sigmac_bottom_[id_layer](3, 3) = 0.;
      } else {
        sigmac_x_sigma_bottom_[id_layer](1, 3) = 0.;
        sigmac_x_sigma_bottom_[id_layer](3, 1) = 0.;
        sigma_x_sigmac_bottom_[id_layer](1, 3) = 0.;
        sigma_x_sigmac_bottom_[id_layer](3, 1) = 0.;
      }
    }
  }
}

double IntegralLayer::intker_us2_top(int id_layer) {
  complex_d result = 0;

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      result += matP_u_u_[id_layer](i, j) * matE_[id_layer](0, i) *
                    matE_[id_layer](0, j) * sigma_x_sigma_top_[id_layer](i, j) +
                2. * matP_uc_u_[id_layer](i, j) * conj(matE_[id_layer](0, i)) *
                    matE_[id_layer](0, j) *
                    sigmac_x_sigma_top_[id_layer](i, j) +
                matP_uc_uc_[id_layer](i, j) * conj(matE_[id_layer](0, i)) *
                    conj(matE_[id_layer](0, j)) *
                    sigmac_x_sigmac_top_[id_layer](i, j);
    }
  }
  return std::real(result) / 4;
}

double IntegralLayer::intker_us2_bottom(int id_layer) {
  complex_d result = 0;

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      result +=
          matP_u_u_[id_layer](i, j) * matE_[id_layer](0, i) *
              matE_[id_layer](0, j) * sigma_x_sigma_bottom_[id_layer](i, j) +
          2. * matP_uc_u_[id_layer](i, j) * conj(matE_[id_layer](0, i)) *
              matE_[id_layer](0, j) * sigmac_x_sigma_bottom_[id_layer](i, j) +
          matP_uc_uc_[id_layer](i, j) * conj(matE_[id_layer](0, i)) *
              conj(matE_[id_layer](0, j)) *
              sigmac_x_sigmac_bottom_[id_layer](i, j);
    }
  }
  return std::real(result) / 4;
}

double IntegralLayer::intker_ur2_top(int id_layer) {
  complex_d result = 0;

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      result += matP_u_u_[id_layer](i, j) * matE_[id_layer](1, i) *
                    matE_[id_layer](1, j) * sigma_x_sigma_top_[id_layer](i, j) +
                2. * matP_uc_u_[id_layer](i, j) * conj(matE_[id_layer](1, i)) *
                    matE_[id_layer](1, j) *
                    sigmac_x_sigma_top_[id_layer](i, j) +
                matP_uc_uc_[id_layer](i, j) * conj(matE_[id_layer](1, i)) *
                    conj(matE_[id_layer](1, j)) *
                    sigmac_x_sigmac_top_[id_layer](i, j);
    }
  }
  return std::real(result) / 4;
}

double IntegralLayer::intker_ur2_bottom(int id_layer) {
  complex_d result = 0;

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      result +=
          matP_u_u_[id_layer](i, j) * matE_[id_layer](1, i) *
              matE_[id_layer](1, j) * sigma_x_sigma_bottom_[id_layer](i, j) +
          2. * matP_uc_u_[id_layer](i, j) * conj(matE_[id_layer](1, i)) *
              matE_[id_layer](1, j) * sigmac_x_sigma_bottom_[id_layer](i, j) +
          matP_uc_uc_[id_layer](i, j) * conj(matE_[id_layer](1, i)) *
              conj(matE_[id_layer](1, j)) *
              sigmac_x_sigmac_bottom_[id_layer](i, j);
    }
  }
  return std::real(result) / 4;
}

double IntegralLayer::intker_dus2_top(int id_layer) {
  complex_d result = 0;

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      result += matP_du_du_[id_layer](i, j) * matE_[id_layer](0, i) *
                    matE_[id_layer](0, j) * sigma_x_sigma_top_[id_layer](i, j) +
                2. * matP_duc_du_[id_layer](i, j) *
                    conj(matE_[id_layer](0, i)) * matE_[id_layer](0, j) *
                    sigmac_x_sigma_top_[id_layer](i, j) +
                matP_duc_duc_[id_layer](i, j) * conj(matE_[id_layer](0, i)) *
                    conj(matE_[id_layer](0, j)) *
                    sigmac_x_sigmac_top_[id_layer](i, j);
    }
  }
  return std::real(result) / 4.;
}

double IntegralLayer::intker_dus2_bottom(int id_layer) {
  complex_d result = 0;

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      result +=
          matP_du_du_[id_layer](i, j) * matE_[id_layer](0, i) *
              matE_[id_layer](0, j) * sigma_x_sigma_bottom_[id_layer](i, j) +
          2. * matP_duc_du_[id_layer](i, j) * conj(matE_[id_layer](0, i)) *
              matE_[id_layer](0, j) * sigmac_x_sigma_bottom_[id_layer](i, j) +
          matP_duc_duc_[id_layer](i, j) * conj(matE_[id_layer](0, i)) *
              conj(matE_[id_layer](0, j)) *
              sigmac_x_sigmac_bottom_[id_layer](i, j);
    }
  }
  return std::real(result) / 4.;
}

double IntegralLayer::intker_dur2_top(int id_layer) {
  complex_d result = 0;

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      result += matP_du_du_[id_layer](i, j) * matE_[id_layer](1, i) *
                    matE_[id_layer](1, j) * sigma_x_sigma_top_[id_layer](i, j) +
                2. * matP_duc_du_[id_layer](i, j) *
                    conj(matE_[id_layer](1, i)) * matE_[id_layer](1, j) *
                    sigmac_x_sigma_top_[id_layer](i, j) +
                matP_duc_duc_[id_layer](i, j) * conj(matE_[id_layer](1, i)) *
                    conj(matE_[id_layer](1, j)) *
                    sigmac_x_sigmac_top_[id_layer](i, j);
    }
  }
  return std::real(result) / 4;
}

double IntegralLayer::intker_dur2_bottom(int id_layer) {
  complex_d result = 0;

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      result +=
          matP_du_du_[id_layer](i, j) * matE_[id_layer](1, i) *
              matE_[id_layer](1, j) * sigma_x_sigma_bottom_[id_layer](i, j) +
          2. * matP_duc_du_[id_layer](i, j) * conj(matE_[id_layer](1, i)) *
              matE_[id_layer](1, j) * sigmac_x_sigma_bottom_[id_layer](i, j) +
          matP_duc_duc_[id_layer](i, j) * conj(matE_[id_layer](1, i)) *
              conj(matE_[id_layer](1, j)) *
              sigmac_x_sigmac_bottom_[id_layer](i, j);
    }
  }
  return std::real(result) / 4;
}

double IntegralLayer::intker_urdus_top(int id_layer) {
  complex_d result = 0;

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      result += matP_u_du_[id_layer](i, j) * matE_[id_layer](1, i) *
                    matE_[id_layer](0, j) * sigma_x_sigma_top_[id_layer](i, j) +
                matP_u_duc_[id_layer](i, j) * matE_[id_layer](1, i) *
                    conj(matE_[id_layer](0, j)) *
                    sigma_x_sigmac_top_[id_layer](i, j) +
                matP_uc_du_[id_layer](i, j) * conj(matE_[id_layer](1, i)) *
                    matE_[id_layer](0, j) *
                    sigmac_x_sigma_top_[id_layer](i, j) +
                matP_uc_duc_[id_layer](i, j) * conj(matE_[id_layer](1, i)) *
                    conj(matE_[id_layer](0, j)) *
                    sigmac_x_sigmac_top_[id_layer](i, j);
    }
  }
  return std::real(result) / 4.;
}

double IntegralLayer::intker_urdus_bottom(int id_layer) {
  complex_d result = 0;

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      result +=
          matP_u_du_[id_layer](i, j) * matE_[id_layer](1, i) *
              matE_[id_layer](0, j) * sigma_x_sigma_bottom_[id_layer](i, j) +
          matP_u_duc_[id_layer](i, j) * matE_[id_layer](1, i) *
              conj(matE_[id_layer](0, j)) *
              sigma_x_sigmac_bottom_[id_layer](i, j) +
          matP_uc_du_[id_layer](i, j) * conj(matE_[id_layer](1, i)) *
              matE_[id_layer](0, j) * sigmac_x_sigma_bottom_[id_layer](i, j) +
          matP_uc_duc_[id_layer](i, j) * conj(matE_[id_layer](1, i)) *
              conj(matE_[id_layer](0, j)) *
              sigmac_x_sigmac_bottom_[id_layer](i, j);
    }
  }
  return std::real(result) / 4.;
}

double IntegralLayer::intker_usdur_top(int id_layer) {
  complex_d result = 0;

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      result += matP_u_du_[id_layer](i, j) * matE_[id_layer](0, i) *
                    matE_[id_layer](1, j) * sigma_x_sigma_top_[id_layer](i, j) +
                matP_u_duc_[id_layer](i, j) * matE_[id_layer](0, i) *
                    conj(matE_[id_layer](1, j)) *
                    sigma_x_sigmac_top_[id_layer](i, j) +
                matP_uc_du_[id_layer](i, j) * conj(matE_[id_layer](0, i)) *
                    matE_[id_layer](1, j) *
                    sigmac_x_sigma_top_[id_layer](i, j) +
                matP_uc_duc_[id_layer](i, j) * conj(matE_[id_layer](0, i)) *
                    conj(matE_[id_layer](1, j)) *
                    sigmac_x_sigmac_top_[id_layer](i, j);
    }
  }
  return std::real(result) / 4.;
}

double IntegralLayer::intker_usdur_bottom(int id_layer) {
  complex_d result = 0;

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      result +=
          matP_u_du_[id_layer](i, j) * matE_[id_layer](0, i) *
              matE_[id_layer](1, j) * sigma_x_sigma_bottom_[id_layer](i, j) +
          matP_u_duc_[id_layer](i, j) * matE_[id_layer](0, i) *
              conj(matE_[id_layer](1, j)) *
              sigma_x_sigmac_bottom_[id_layer](i, j) +
          matP_uc_du_[id_layer](i, j) * conj(matE_[id_layer](0, i)) *
              matE_[id_layer](1, j) * sigmac_x_sigma_bottom_[id_layer](i, j) +
          matP_uc_duc_[id_layer](i, j) * conj(matE_[id_layer](0, i)) *
              conj(matE_[id_layer](1, j)) *
              sigmac_x_sigmac_bottom_[id_layer](i, j);
    }
  }
  return std::real(result) / 4.;
}

void IntegralLayer::integrate_us2() {
  for (int id_layer = 0; id_layer < nl_; ++id_layer) {
    int_us2_(id_layer) = intker_us2_top(id_layer) - intker_us2_bottom(id_layer);
  }
}

void IntegralLayer::integrate_ur2() {
  for (int id_layer = 0; id_layer < nl_; ++id_layer) {
    int_ur2_(id_layer) = intker_ur2_top(id_layer) - intker_ur2_bottom(id_layer);
  }
}

void IntegralLayer::integrate_dus2() {
  for (int id_layer = 0; id_layer < nl_; ++id_layer) {
    int_dus2_(id_layer) =
        intker_dus2_top(id_layer) - intker_dus2_bottom(id_layer);
  }
}

void IntegralLayer::integrate_dur2() {
  for (int id_layer = 0; id_layer < nl_; ++id_layer) {
    int_dur2_(id_layer) =
        intker_dur2_top(id_layer) - intker_dur2_bottom(id_layer);
  }
}

void IntegralLayer::integrate_usdur() {
  for (int id_layer = 0; id_layer < nl_; ++id_layer) {
    int_usdur_(id_layer) =
        intker_usdur_top(id_layer) - intker_usdur_bottom(id_layer);
  }
}

void IntegralLayer::integrate_urdus() {
  for (int id_layer = 0; id_layer < nl_; ++id_layer) {
    int_urdus_(id_layer) =
        intker_urdus_top(id_layer) - intker_urdus_bottom(id_layer);
  }
}

double IntegralLayer::compute_I1() {
  ArrayXd ker = rho_ * (int_us2_ + int_ur2_);
  double i1 = 0.5 * ker.sum();
  return i1;
}

double IntegralLayer::compute_I2() {
  ArrayXd ker = (lamb_ + 2.0 * mu_) * int_us2_ + mu_ * int_ur2_;
  double i2 = 0.5 * ker.sum();
  return i2;
}

double IntegralLayer::compute_I3() {
  ArrayXd ker = lamb_ * int_usdur_ - mu_ * int_urdus_;
  double i3 = 0.5 * ker.sum();
  return i3;
}

ArrayXd IntegralLayer::compute_kvs() {
  double k2 = std::pow(k_, 2);
  ArrayXd kvs(nl_);
  for (int i = 0; i < nl_; ++i) {
    kvs(i) = 0.5 * rho_(i) * beta_(i) *
             (int_ur2_(i) + 1.0 / k2 * int_dus2_(i) - 2.0 / k_ * int_urdus_(i) -
              4.0 / k_ * int_usdur_(i));
  }
  return kvs;
}

GradientPSV::~GradientPSV() = default;

ArrayXd GradientPSV::compute(const double freq, const double c) const {
  double k = 2.0 * PI * freq / c;
  IntegralLayer intl(model_, freq, c);
  double I2 = intl.compute_I2();
  double I3 = intl.compute_I3();
  ArrayXd kvs = intl.compute_kvs();
  kvs *= c / (I2 + I3 / k);
  return kvs;
}

} // namespace grad_psv


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

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

inline void bench(unsigned n)
{
    typedef mtl::mat::parameters<> mat_para;
    mtl::dense2D<double, mat_para> A(n, n), B(n, n), C(n, n);
    mtl::dense_vector<double> x(n, 1.0), b(n);

    for ( unsigned i= 0; i < n; i++)
	for ( unsigned  j= 0; j < n; j++){
	    A[i][j]= 1;
	    B[i][j]= 2;
	}

    boost::timer t;
    C= A * B;
    b= C * x;
    double t1= 1000. * t.elapsed();

    t.restart();
    b=  B * x;
    x= A * b;
    double t2= 1000. * t.elapsed();
    std::cout << n << " " << t1 << " " << t2 <<"\n";
}

int main(int , char**)
{
    bench(1000);

#if 0
    bench(300);
    bench(400);
    bench(500);
    bench(600);
    bench(700);
    bench(800);
    bench(900);
    bench(1000);
    bench(1200);
    bench(1400);
    bench(1600);
    bench(1800);
    bench(2000);
    bench(2200);
    bench(2400);
    bench(2600);
#endif

    return 0;
}


#ifndef MPLLIBS_METAPARSE_V1_INT__HPP
#define MPLLIBS_METAPARSE_V1_INT__HPP

// Copyright Abel Sinkovics (abel@sinkovics.hu)  2011.
// 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 <mpllibs/metaparse/v1/digit_val.hpp>
#include <mpllibs/metaparse/v1/foldl1.hpp>

#include <boost/mpl/lambda.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/plus.hpp>

namespace mpllibs
{
  namespace metaparse
  {
    namespace v1
    {
      typedef
        foldl1<
          digit_val,
          boost::mpl::int_<0>,
          // I need to wrap it with lambda, because int_ may be used
          // in an apply and turned into a lambda expression too early.
          boost::mpl::lambda<
            boost::mpl::plus<
              boost::mpl::times<boost::mpl::_2, boost::mpl::int_<10> >,
              boost::mpl::_1
            >
          >::type
        >
        int_;
    }
  }
}

#endif


#include <boost/graph/geodesic_distance.hpp>


#include <iostream>
#include <functional>
#include <thread>
#include <vector>
#include <fstream>
#include <mutex>
#include <condition_variable>

#include <kfr/base.hpp>
#include <kfr/dft.hpp>
#include <kfr/dsp.hpp>

#include <Eigen/Dense>

#include <portaudio.h>

namespace {
std::mutex m;
std::condition_variable cv;
bool shouldStop_ = false;

std::array<int, 4> channelMap_ordinary = {0,1,2,3};
std::array<int, 4> channelMap_PS_eye={0,2,1,3};

}

template<size_t SAMPLE_RATE, size_t NB_SAMPLES_PER_CHANNEL>
class MicArrayRunner {
private:
    PaStream* paStream_ = nullptr;
    PaStreamParameters  paInputParameters_;
    std::function<void(const int16_t * inputBuffer, const size_t framesPerBuffer, const size_t nbChannel)> callback_ = nullptr;
    int finalNbChannel_ = -1;

public:
    MicArrayRunner(decltype(callback_) callback)
        :callback_(callback)
    { }

    bool micArrayFilter(const PaDeviceInfo* deviceInfo) {
        return (deviceInfo->maxInputChannels < 6 && deviceInfo->maxInputChannels > 2);
    }

    void init() {
        auto err = Pa_Initialize();
        if( err != paNoError ) {
            throw;
        }

        auto numDevices = Pa_GetDeviceCount();
        auto finalIndex = -1;
        for(auto i=0; i<numDevices; i++ ){
            auto deviceInfo = Pa_GetDeviceInfo( i );

            if(micArrayFilter(deviceInfo)){
                printf("deviceID=%02d: %s\n \t\t nbChannel=%02d\n", i, deviceInfo->name, deviceInfo->maxInputChannels);
                finalIndex = i;
                finalNbChannel_ = deviceInfo->maxInputChannels;
            }
        }

        paInputParameters_.device = finalIndex;
        paInputParameters_.channelCount = finalNbChannel_;
        paInputParameters_.sampleFormat = paInt16;
        paInputParameters_.suggestedLatency = Pa_GetDeviceInfo( paInputParameters_.device )->defaultLowInputLatency;
        paInputParameters_.hostApiSpecificStreamInfo = NULL;
        err = Pa_OpenStream(&paStream_, &paInputParameters_, nullptr, SAMPLE_RATE, NB_SAMPLES_PER_CHANNEL, paClipOff, MicArrayRunner::theCallback, this);
    }

    void start() {
        Pa_StartStream(paStream_);
    }

    void stop() {
        Pa_StopStream(paStream_);
    }

    void deInit() {
        stop();
        Pa_Terminate();
    }

    ~MicArrayRunner() {
        deInit();
    }

private:
    static int theCallback(const void *inputBuffer, void *outputBuffer,
                           unsigned long framesPerBuffer,
                           const PaStreamCallbackTimeInfo* timeInfo,
                           PaStreamCallbackFlags statusFlags,
                           void *userData ) {
        auto _runner = (MicArrayRunner*)userData;
        _runner->callback_((const int16_t*)inputBuffer, framesPerBuffer, _runner->finalNbChannel_);
        return 0;
    }
};

template<size_t SAMPLE_RATE, size_t NB_CHANNEL, size_t NB_SAMPLES_PER_CHANNEL, const size_t CORR_RESULT_SIZE = 2 * NB_SAMPLES_PER_CHANNEL - 1>
class MultichannelCrossCorrelationCoefficientAlgorithm {
private:
    kfr::univector<kfr::fbase, NB_CHANNEL*NB_SAMPLES_PER_CHANNEL> kfrFbase_;
    kfr::univector<kfr::univector<kfr::fbase, NB_SAMPLES_PER_CHANNEL>, NB_CHANNEL> splitedAudio_;
    kfr::univector<kfr::univector<kfr::univector<kfr::fbase, CORR_RESULT_SIZE>,NB_CHANNEL>, NB_CHANNEL> raP_;
    kfr::univector<Eigen::Matrix<double, NB_CHANNEL, NB_CHANNEL>, CORR_RESULT_SIZE> eigenMatrix_;
    kfr::univector<kfr::fbase, CORR_RESULT_SIZE> detRaP_;

    std::array<int, NB_CHANNEL> channelMap_;
private:
    void calcCorr(const kfr::univector<kfr::fbase, NB_SAMPLES_PER_CHANNEL> &a,
                  const kfr::univector<kfr::fbase, NB_SAMPLES_PER_CHANNEL> &b,
                  kfr::univector<kfr::fbase, CORR_RESULT_SIZE> &result
                  ){
        result = correlate(a,b);
    }

    void constructMatrix(const size_t p,
                         const kfr::univector<kfr::univector<kfr::univector<kfr::fbase, CORR_RESULT_SIZE>, NB_CHANNEL> ,NB_CHANNEL> &RaP,
                         Eigen::Matrix<double, NB_CHANNEL, NB_CHANNEL> &outputMatrix
                         ){
        for (int row = 0; row < NB_CHANNEL; row++){
            for (int col = 0; col < NB_CHANNEL; col++){
                outputMatrix(row,col) = RaP[row][col][p];
            }
        }
    }

    void paraCalcCorr(){

    }

    void captureAudio(kfr::univector<kfr::fbase, NB_CHANNEL*NB_SAMPLES_PER_CHANNEL> &samples){
        // fill samples
    }

    void int16ToKfrFbase(const int16_t * inputBuffer,
                         kfr::univector<kfr::fbase, NB_CHANNEL*NB_SAMPLES_PER_CHANNEL> &outputKfrFbase ){
        for (int i = 0; i < NB_CHANNEL*NB_SAMPLES_PER_CHANNEL; i++){
            outputKfrFbase[i]=(kfr::fbase)inputBuffer[i]/(1<<15);
        }

    }

    void splitChannels(
            const kfr::univector<kfr::fbase, NB_CHANNEL*NB_SAMPLES_PER_CHANNEL> &samples,
            kfr::univector<kfr::univector<kfr::fbase, NB_SAMPLES_PER_CHANNEL>, NB_CHANNEL> &splitedAudio
            ) {
        for(int channel = 0; channel < NB_CHANNEL; channel++){
            for (int sample = 0; sample < NB_SAMPLES_PER_CHANNEL; sample++){
                splitedAudio[channel][sample] = samples[NB_CHANNEL*sample + channelMap_[channel]];
            }
        }
    }

public:
    MultichannelCrossCorrelationCoefficientAlgorithm(const decltype(channelMap_) channelMap)
        :channelMap_(channelMap)
    { }

    void entry(){
        MicArrayRunner<SAMPLE_RATE, NB_SAMPLES_PER_CHANNEL> runner(
                    [this](const int16_t * inputBuffer, const size_t framesPerBuffer, const size_t nbChannel)
        {
            int16ToKfrFbase(inputBuffer, kfrFbase_);
            splitChannels(kfrFbase_, splitedAudio_);
            for (auto row = 0; row < NB_CHANNEL; row++){
                for (auto col = 0; col < NB_CHANNEL; col++){
                    calcCorr(splitedAudio_[row], splitedAudio_[col], raP_[row][col]);
                }
            }

            for(int p = 0; p<CORR_RESULT_SIZE; p++){
                constructMatrix(p, raP_, eigenMatrix_[p]);
                detRaP_[p]= eigenMatrix_[p].determinant();
            }

            {
                int argMinP = 0;
                double minDetRaP = detRaP_[argMinP];
                for(int p = 1; p<CORR_RESULT_SIZE; p++){
                    if (detRaP_[p] < minDetRaP){
                        argMinP=p;
                        minDetRaP=detRaP_[p];
                    }
                }
                auto tDOA = (double)((int)argMinP - (int)NB_SAMPLES_PER_CHANNEL)/(int)SAMPLE_RATE;

                {
                    printf("tDOA=%f \t  argMinP=%d \t minDetRaP=%f\n", tDOA, argMinP, minDetRaP);
                }
            }
        });

        runner.init();
        runner.start();

        cv.notify_one();

        // wait for the worker
        {
            std::unique_lock<std::mutex> lk(m);
            cv.wait(lk, []{return shouldStop_;});
        }

        runner.stop();
    }
};

auto test00(){
    kfr::univector<kfr::fbase, 4> a({ 1, 2, 3, 4 });
    kfr::univector<kfr::fbase, 4> b({ 1, 2, 3, 4 });
    auto c = correlate(a, b);
    kfr::println(c);
    //CHECK(c.size() == 9u);
    //CHECK(rms(c - kfr::univector<kfr::fbase>({ 1.5, 1., 1.5, 2.5, 3.75, -4., 7.75, 3.5, 1.25 })) < 0.0001);
}

int main(void){
    MultichannelCrossCorrelationCoefficientAlgorithm<16000, 4, 1<<8> handler(channelMap_PS_eye);
    handler.entry();

    return 0;
}


// arma_numpy_test.hpp
// A collection of simple C++ functions to test arma_numpy.i

#include <armadillo>
#include <string>

// 1D

double sum_vec( arma::vec v); // handle input of vec and output of something easily recognised
arma::sword sum_vec(arma::ivec v); // handle input of ivec and output of an armadillo typedef, plus handle overloading
arma::vec get_vec( int size ); // given something easily recognised, output vec
arma::vec reverse_vec( arma::vec v); // given a vec, return a vec
arma::icolvec reverse_vec( arma::ivec v); // given an ivec, return a typedef over ivec
arma::uvec reverse_vec( arma::uvec v); // given a uvec, return a uvec. Tests if numpy casting works properly.

double sum_vec_by_ref( const arma::vec& v);
double sum_vec_by_ptr( const arma::vec* v);
void set_to_zero_by_ref( arma::vec& v);
void set_to_zero_by_ptr( arma::vec* v);

std::size_t get_memptr( arma::vec* v);
std::size_t get_memptr_const( const arma::vec* v);

arma::vec& get_static_vec();

// 2D

double sum_mat( arma::mat m);
arma::sword sum_mat( arma::imat m);
arma::mat transpose_mat( arma::mat m);
void set_to_zero_by_ref( arma::mat& m);
void set_to_zero_by_ptr( arma::mat* m);
double sum_mat_by_const_ref( const arma::mat& m);
double sum_mat_by_const_ptr( const arma::mat* m);

std::size_t get_memptr( arma::mat* m);
std::size_t get_memptr_const( const arma::mat* m);

// 3D

double sum_cube( arma::cube c);
arma::sword sum_cube( arma::icube c);
arma::cube do_nothing( arma::cube c);
arma::mat get_second_slice( arma::cube c);


#pragma once

#include <polyfem/Mesh.hpp>

#include <Eigen/Dense>

namespace polyfem
{
	class BoundarySampler
	{
	public:
		static void sample_parametric_quad_edge(int index, int n_samples, Eigen::MatrixXd &uv, Eigen::MatrixXd &samples);
		static void sample_parametric_tri_edge(int index, int n_samples, Eigen::MatrixXd &uv, Eigen::MatrixXd &samples);

		static void sample_parametric_quad_face(int index, int n_samples, Eigen::MatrixXd &uv, Eigen::MatrixXd &samples);
		static void sample_parametric_tri_face(int index, int n_samples, Eigen::MatrixXd &uv, Eigen::MatrixXd &samples);

		static void sample_polygon_edge(int face_id, int edge_id, int n_samples, const Mesh &mesh, Eigen::MatrixXd &uv, Eigen::MatrixXd &samples);

		static void quadrature_for_quad_edge(int index, int order, const int gid, const Mesh &mesh, Eigen::MatrixXd &uv, Eigen::MatrixXd &points, Eigen::VectorXd &weights);
		static void quadrature_for_tri_edge(int index, int order, const int gid, const Mesh &mesh, Eigen::MatrixXd &uv, Eigen::MatrixXd &points, Eigen::VectorXd &weights);

		static void quadrature_for_quad_face(int index, int order, const int gid, const Mesh &mesh, Eigen::MatrixXd &uv, Eigen::MatrixXd &points, Eigen::VectorXd &weights);
		static void quadrature_for_tri_face(int index, int order, const int gid, const Mesh &mesh, Eigen::MatrixXd &uv, Eigen::MatrixXd &points, Eigen::VectorXd &weights);

		static void quadrature_for_polygon_edge(int face_id, int edge_id, int order, const Mesh &mesh, Eigen::MatrixXd &uv, Eigen::MatrixXd &points, Eigen::VectorXd &weights);

		static void normal_for_quad_edge(int index, Eigen::MatrixXd &normal);
		static void normal_for_tri_edge(int index, Eigen::MatrixXd &normal);

		static void normal_for_quad_face(int index, Eigen::MatrixXd &normal);
		static void normal_for_tri_face(int index, Eigen::MatrixXd &normal);

		static void normal_for_polygon_edge(int face_id, int edge_id, const Mesh &mesh, Eigen::MatrixXd &normal);

		static Eigen::MatrixXd tet_local_node_coordinates_from_face(int lf);
		static Eigen::MatrixXd hex_local_node_coordinates_from_face(int lf);
	};
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testImuFactor.cpp
 * @brief   Unit test for ImuFactor
 * @author  Luca Carlone, Stephen Williams, Richard Roberts
 */

#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>

#include <boost/bind.hpp>
#include <list>

using namespace std;
using namespace gtsam;

// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;

/* ************************************************************************* */
namespace {
Vector callEvaluateError(const ImuFactor& factor,
    const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
    const imuBias::ConstantBias& bias)
{
  return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias);
}

Rot3 evaluateRotationError(const ImuFactor& factor,
    const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
    const imuBias::ConstantBias& bias)
{
  return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
}

ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
    const imuBias::ConstantBias& bias,
    const list<Vector3>& measuredAccs,
    const list<Vector3>& measuredOmegas,
    const list<double>& deltaTs,
    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
    )
{
  ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
      Matrix3::Identity(), Matrix3::Identity());

  list<Vector3>::const_iterator itAcc = measuredAccs.begin();
  list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
  list<double>::const_iterator itDeltaT = deltaTs.begin();
  for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
    result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
  }

  return result;
}

Vector3 evaluatePreintegratedMeasurementsPosition(
    const imuBias::ConstantBias& bias,
    const list<Vector3>& measuredAccs,
    const list<Vector3>& measuredOmegas,
    const list<double>& deltaTs,
    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
  return evaluatePreintegratedMeasurements(bias,
      measuredAccs, measuredOmegas, deltaTs).deltaPij;
}

Vector3 evaluatePreintegratedMeasurementsVelocity(
    const imuBias::ConstantBias& bias,
    const list<Vector3>& measuredAccs,
    const list<Vector3>& measuredOmegas,
    const list<double>& deltaTs,
    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
  return evaluatePreintegratedMeasurements(bias,
      measuredAccs, measuredOmegas, deltaTs).deltaVij;
}

Rot3 evaluatePreintegratedMeasurementsRotation(
    const imuBias::ConstantBias& bias,
    const list<Vector3>& measuredAccs,
    const list<Vector3>& measuredOmegas,
    const list<double>& deltaTs,
    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
  return evaluatePreintegratedMeasurements(bias,
      measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij;
}

Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT)
{
  return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
}

Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta)
{
  return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
}

}

/* ************************************************************************* */
TEST( ImuFactor, PreintegratedMeasurements )
{
  // Linearization point
  imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases

  // Measurements
  Vector3 measuredAcc(0.1, 0.0, 0.0);
  Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
  double deltaT = 0.5;

  // Expected preintegrated values
  Vector3 expectedDeltaP1; expectedDeltaP1 << 0.5*0.1*0.5*0.5, 0, 0;
  Vector3 expectedDeltaV1(0.05, 0.0, 0.0);
  Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0);
  double expectedDeltaT1(0.5);

  bool use2ndOrderIntegration = true;
  // Actual preintegrated values
  ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
  actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij), 1e-6));
  EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij), 1e-6));
  EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij, 1e-6));
  DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij, 1e-6);

  // Integrate again
  Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0;
  Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * measuredAcc * 0.5;
  Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0);
  double expectedDeltaT2(1);

  // Actual preintegrated values
  ImuFactor::PreintegratedMeasurements actual2 = actual1;
  actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij), 1e-6));
  EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij), 1e-6));
  EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij, 1e-6));
  DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij, 1e-6);
}

/* ************************************************************************* */
TEST( ImuFactor, Error )
{
  // Linearization point
  imuBias::ConstantBias bias; // Bias
  Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
  Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));

  // Measurements
  Vector3 gravity; gravity << 0, 0, 9.81;
  Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
  Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0;
  Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector();
  double deltaT = 1.0;
  bool use2ndOrderIntegration = true;
  ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
  pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  // Create factor
  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);

  Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);

  // Expected error
  Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
  EXPECT(assert_equal(errorExpected, errorActual, 1e-6));

  // Expected Jacobians
  Matrix H1e = numericalDerivative11<Pose3>(
      boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
  Matrix H2e = numericalDerivative11<LieVector>(
      boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
  Matrix H3e = numericalDerivative11<Pose3>(
      boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
  Matrix H4e = numericalDerivative11<LieVector>(
      boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
  Matrix H5e = numericalDerivative11<imuBias::ConstantBias>(
      boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);

  // Check rotation Jacobians
  Matrix RH1e = numericalDerivative11<Rot3,Pose3>(
      boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
  Matrix RH3e = numericalDerivative11<Rot3,Pose3>(
      boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);

  // Actual Jacobians
  Matrix H1a, H2a, H3a, H4a, H5a;
  (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);

  // positions and velocities
  Matrix H1etop6 =  H1e.topRows(6);
  Matrix H1atop6 =  H1a.topRows(6);
  EXPECT(assert_equal(H1etop6, H1atop6));
  // rotations
  EXPECT(assert_equal(RH1e, H1a.bottomRows(3), 1e-5));  // 1e-5 needs to be added only when using quaternions for rotations

  EXPECT(assert_equal(H2e, H2a));

  // positions and velocities
  Matrix H3etop6 =  H3e.topRows(6);
  Matrix H3atop6 =  H3a.topRows(6);
  EXPECT(assert_equal(H3etop6, H3atop6));
  // rotations
  EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5));  // 1e-5 needs to be added only when using quaternions for rotations

  EXPECT(assert_equal(H4e, H4a));
//  EXPECT(assert_equal(H5e, H5a));
}

/* ************************************************************************* */
TEST( ImuFactor, ErrorWithBiases )
{
  // Linearization point
//  Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
//  Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
//  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
//  Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
//  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));

  imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
  Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));

  // Measurements
  Vector3 gravity; gravity << 0, 0, 9.81;
  Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
  Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
  Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
  double deltaT = 1.0;

  ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

//  ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
//    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

    // Create factor
    ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);

    SETDEBUG("ImuFactor evaluateError", false);
    Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
    SETDEBUG("ImuFactor evaluateError", false);

    // Expected error
    Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
//    EXPECT(assert_equal(errorExpected, errorActual, 1e-6));

    // Expected Jacobians
    Matrix H1e = numericalDerivative11<Pose3>(
        boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
    Matrix H2e = numericalDerivative11<LieVector>(
        boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
    Matrix H3e = numericalDerivative11<Pose3>(
        boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
    Matrix H4e = numericalDerivative11<LieVector>(
        boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
    Matrix H5e = numericalDerivative11<imuBias::ConstantBias>(
        boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);

    // Check rotation Jacobians
    Matrix RH1e = numericalDerivative11<Rot3,Pose3>(
        boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
    Matrix RH3e = numericalDerivative11<Rot3,Pose3>(
        boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
    Matrix RH5e = numericalDerivative11<Rot3,imuBias::ConstantBias>(
        boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias);

    // Actual Jacobians
    Matrix H1a, H2a, H3a, H4a, H5a;
    (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);

    EXPECT(assert_equal(H1e, H1a));
    EXPECT(assert_equal(H2e, H2a));
    EXPECT(assert_equal(H3e, H3a));
    EXPECT(assert_equal(H4e, H4a));
    EXPECT(assert_equal(H5e, H5a));
}

/* ************************************************************************* */
TEST( ImuFactor, PartialDerivativeExpmap )
{
  // Linearization point
  Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias

  // Measurements
  Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
  double deltaT = 0.5;

  // Compute numerical derivatives
  Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, LieVector>(boost::bind(
      &evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega));

  const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);

   Matrix3  actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign

  // Compare Jacobians
  EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3));  // 1e-3 needs to be added only when using quaternions for rotations

}

/* ************************************************************************* */
TEST( ImuFactor, PartialDerivativeLogmap )
{
  // Linearization point
  Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias

  // Measurements
  Vector3 deltatheta; deltatheta << 0, 0, 0;

  // Compute numerical derivatives
  Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(boost::bind(
      &evaluateLogRotation, thetahat, _1), LieVector(deltatheta));

  const Vector3 x = thetahat; // parametrization of so(3)
  const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
  double normx = norm_2(x);
  const Matrix3  actualDelFdeltheta = Matrix3::Identity() +
       0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;

//  std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl;
//  std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl;

  // Compare Jacobians
  EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));

}

/* ************************************************************************* */
TEST( ImuFactor, fistOrderExponential )
{
  // Linearization point
    Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias

    // Measurements
    Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
    double deltaT = 1.0;

    // change w.r.t. linearization point
    double alpha = 0.0;
    Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha;

    const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);

     Matrix3  delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign

     const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();

     const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
     const Matrix3 actualRot =
         hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
         //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));

    // Compare Jacobians
    EXPECT(assert_equal(expectedRot, actualRot));
}

/* ************************************************************************* */
TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
{
  // Linearization point
  imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases

  Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));

  // Measurements
  list<Vector3> measuredAccs, measuredOmegas;
  list<double> deltaTs;
  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
  measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
  deltaTs.push_back(0.01);
  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
  measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
  deltaTs.push_back(0.01);
  for(int i=1;i<100;i++)
  {
    measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
    measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
    deltaTs.push_back(0.01);
  }

  // Actual preintegrated values
  ImuFactor::PreintegratedMeasurements preintegrated =
      evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));

  // Compute numerical derivatives
  Matrix expectedDelPdelBias = numericalDerivative11<imuBias::ConstantBias>(
      boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
  Matrix expectedDelPdelBiasAcc   = expectedDelPdelBias.leftCols(3);
  Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);

  Matrix expectedDelVdelBias = numericalDerivative11<imuBias::ConstantBias>(
      boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
  Matrix expectedDelVdelBiasAcc   = expectedDelVdelBias.leftCols(3);
  Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);

  Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
      boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
  Matrix expectedDelRdelBiasAcc   = expectedDelRdelBias.leftCols(3);
  Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);

  // Compare Jacobians
  EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc));
  EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega));
  EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc));
  EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega));
  EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
  EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}

//#include <gtsam/linear/GaussianFactorGraph.h>
///* ************************************************************************* */
//TEST( ImuFactor, LinearizeTiming)
//{
//  // Linearization point
//  Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
//  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
//  Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
//  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
//  imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
//
//  // Pre-integrator
//  imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10));
//  Vector3 gravity; gravity << 0, 0, 9.81;
//  Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01;
//  ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
//
//  // Pre-integrate Measurements
//  Vector3 measuredAcc(0.1, 0.0, 0.0);
//  Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
//  double deltaT = 0.5;
//  for(size_t i = 0; i < 50; ++i) {
//    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
//  }
//
//  // Create factor
//  noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.preintegratedMeasurementsCovariance());
//  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
//
//  Values values;
//  values.insert(X(1), x1);
//  values.insert(X(2), x2);
//  values.insert(V(1), v1);
//  values.insert(V(2), v2);
//  values.insert(B(1), bias);
//
//  Ordering ordering;
//  ordering.push_back(X(1));
//  ordering.push_back(V(1));
//  ordering.push_back(X(2));
//  ordering.push_back(V(2));
//  ordering.push_back(B(1));
//
//  GaussianFactorGraph graph;
//  gttic_(LinearizeTiming);
//  for(size_t i = 0; i < 100000; ++i) {
//    GaussianFactor::shared_ptr g = factor.linearize(values, ordering);
//    graph.push_back(g);
//  }
//  gttoc_(LinearizeTiming);
//  tictoc_finishedIteration_();
//  std::cout << "Linear Error: " << graph.error(values.zeroVectors(ordering)) << std::endl;
//  tictoc_print_();
//}

/* ************************************************************************* */
TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
{

  imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
  Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));

  // Measurements
  Vector3 gravity; gravity << 0, 0, 9.81;
  Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
  Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
  Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
  double deltaT = 1.0;

  const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0));

//  ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
//        Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmega);

  ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
        Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());

  pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

    // Create factor
    ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);

    // Expected Jacobians
    Matrix H1e = numericalDerivative11<Pose3>(
        boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
    Matrix H2e = numericalDerivative11<LieVector>(
        boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
    Matrix H3e = numericalDerivative11<Pose3>(
        boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
    Matrix H4e = numericalDerivative11<LieVector>(
        boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
    Matrix H5e = numericalDerivative11<imuBias::ConstantBias>(
        boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);

    // Check rotation Jacobians
    Matrix RH1e = numericalDerivative11<Rot3,Pose3>(
        boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
    Matrix RH3e = numericalDerivative11<Rot3,Pose3>(
        boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
    Matrix RH5e = numericalDerivative11<Rot3,imuBias::ConstantBias>(
        boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias);

    // Actual Jacobians
    Matrix H1a, H2a, H3a, H4a, H5a;
    (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);

    EXPECT(assert_equal(H1e, H1a));
    EXPECT(assert_equal(H2e, H2a));
    EXPECT(assert_equal(H3e, H3a));
    EXPECT(assert_equal(H4e, H4a));
    EXPECT(assert_equal(H5e, H5a));
}

/* ************************************************************************* */
  int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


/*=============================================================================
    Copyright (c) 2001-2003 Daniel Nuffer
    http://spirit.sourceforge.net/

    Use, modification and distribution is 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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  Demonstrates the ASTs. This is discussed in the
//  "Trees" chapter in the Spirit User's Guide.
//
///////////////////////////////////////////////////////////////////////////////
#define BOOST_SPIRIT_DUMP_PARSETREE_AS_XML

#include <boost/spirit/include/classic_core.hpp>
#include <boost/spirit/include/classic_ast.hpp>
#include <boost/spirit/include/classic_tree_to_xml.hpp>
#include <boost/assert.hpp>
#include "tree_calc_grammar.hpp"

#include <iostream>
#include <stack>
#include <functional>
#include <string>

#if defined(BOOST_SPIRIT_DUMP_PARSETREE_AS_XML)
#include <map>
#endif

// This example shows how to use an AST.
////////////////////////////////////////////////////////////////////////////
using namespace std;
using namespace BOOST_SPIRIT_CLASSIC_NS;

typedef char const*         iterator_t;
typedef tree_match<iterator_t> parse_tree_match_t;
typedef parse_tree_match_t::tree_iterator iter_t;

////////////////////////////////////////////////////////////////////////////
long evaluate(parse_tree_match_t hit);
long eval_expression(iter_t const& i);

long evaluate(tree_parse_info<> info)
{
    return eval_expression(info.trees.begin());
}

long eval_expression(iter_t const& i)
{
    cout << "In eval_expression. i->value = " <<
        string(i->value.begin(), i->value.end()) <<
        " i->children.size() = " << i->children.size() << endl;

    if (i->value.id() == calculator::integerID)
    {
        BOOST_ASSERT(i->children.size() == 0);

        // extract integer (not always delimited by '\0')
        string integer(i->value.begin(), i->value.end());

        return strtol(integer.c_str(), 0, 10);
    }
    else if (i->value.id() == calculator::factorID)
    {
        // factor can only be unary minus
        BOOST_ASSERT(*i->value.begin() == '-');
        return - eval_expression(i->children.begin());
    }
    else if (i->value.id() == calculator::termID)
    {
        if (*i->value.begin() == '*')
        {
            BOOST_ASSERT(i->children.size() == 2);
            return eval_expression(i->children.begin()) *
                eval_expression(i->children.begin()+1);
        }
        else if (*i->value.begin() == '/')
        {
            BOOST_ASSERT(i->children.size() == 2);
            return eval_expression(i->children.begin()) /
                eval_expression(i->children.begin()+1);
        }
        else
            BOOST_ASSERT(0);
    }
    else if (i->value.id() == calculator::expressionID)
    {
        if (*i->value.begin() == '+')
        {
            BOOST_ASSERT(i->children.size() == 2);
            return eval_expression(i->children.begin()) +
                eval_expression(i->children.begin()+1);
        }
        else if (*i->value.begin() == '-')
        {
            BOOST_ASSERT(i->children.size() == 2);
            return eval_expression(i->children.begin()) -
                eval_expression(i->children.begin()+1);
        }
        else
            BOOST_ASSERT(0);
    }
    else
    {
        BOOST_ASSERT(0); // error
    }

    return 0;
}

////////////////////////////////////////////////////////////////////////////
int
main()
{
    // look in tree_calc_grammar for the definition of calculator
    calculator calc;

    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tThe simplest working calculator...\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "Type an expression...or [q or Q] to quit\n\n";

    string str;
    while (getline(cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        tree_parse_info<> info = ast_parse(str.c_str(), calc);

        if (info.full)
        {
#if defined(BOOST_SPIRIT_DUMP_PARSETREE_AS_XML)
            // dump parse tree as XML
            std::map<parser_id, std::string> rule_names;
            rule_names[calculator::integerID] = "integer";
            rule_names[calculator::factorID] = "factor";
            rule_names[calculator::termID] = "term";
            rule_names[calculator::expressionID] = "expression";
            tree_to_xml(cout, info.trees, str.c_str(), rule_names);
#endif

            // print the result
            cout << "parsing succeeded\n";
            cout << "result = " << evaluate(info) << "\n\n";
        }
        else
        {
            cout << "parsing failed\n";
        }
    }

    cout << "Bye... :-) \n\n";
    return 0;
}


#define BOOST_TEST_MODULE modular_arithmetic
#include <boost/test/included/unit_test.hpp>

#include <fc/exception/exception.hpp>
#include <fc/crypto/hex.hpp>
#include <fc/crypto/modular_arithmetic.hpp>
#include <fc/utility.hpp>

using namespace fc;
#include "test_utils.hpp"

namespace std {
std::ostream& operator<<(std::ostream& st, const std::variant<fc::modular_arithmetic_error, bytes>& err)
{
    if(std::holds_alternative<fc::modular_arithmetic_error>(err))
        st << static_cast<int32_t>(std::get<fc::modular_arithmetic_error>(err));
    else
        st << fc::to_hex(std::get<bytes>(err));
    return st;
}
}

BOOST_AUTO_TEST_SUITE(modular_arithmetic)
BOOST_AUTO_TEST_CASE(modexp) try {

    using modexp_test = std::tuple<std::vector<string>, std::variant<fc::modular_arithmetic_error, bytes>>;

    const std::vector<modexp_test> tests {
        //test1
        {
            {
                "03",
                "fffffffffffffffffffffffffffffffffffffffffffffffffffffffefffffc2e",
                "fffffffffffffffffffffffffffffffffffffffffffffffffffffffefffffc2f",
            },
            to_bytes("0000000000000000000000000000000000000000000000000000000000000001"),
        },

        //test2
        {
            {
                "",
                "fffffffffffffffffffffffffffffffffffffffffffffffffffffffefffffc2e",
                "fffffffffffffffffffffffffffffffffffffffffffffffffffffffefffffc2f",
            },
            to_bytes("0000000000000000000000000000000000000000000000000000000000000000")
        },

        //test3
        {
            {
                "01",
                "fffffffffffffffffffffffffffffffffffffffffffffffffffffffefffffc2e",
                "",
            },
            modular_arithmetic_error::modulus_len_zero
        },

    };

    for(const auto& test : tests) {
        const auto& parts           = std::get<0>(test);
        const auto& expected_result = std::get<1>(test);

        auto base = to_bytes(parts[0]);
        auto exponent = to_bytes(parts[1]);
        auto modulus = to_bytes(parts[2]);

        auto res = fc::modexp(base, exponent, modulus);
        BOOST_CHECK_EQUAL(res, expected_result);
    }

} FC_LOG_AND_RETHROW();

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (c) 2016-2017, Rafael Ballester-Ripoll
 *                          (Visualization and MultiMedia Lab, University of Zurich),
 *                          rballester@ifi.uzh.ch
 *
 * Licensed under the LGPLv3.0 (https://github.com/rballester/tthresh/blob/master/LICENSE)
 */

#ifndef __TUCKER_HPP__
#define __TUCKER_HPP__

#include "memtrace.h"

#define _USE_MATH_DEFINES
#include <math.h>
#include <iostream>

#include "Slice.hpp"
#include <Eigen/Dense>

//using namespace std;
//using namespace Eigen;

namespace tthresh
{

    // Projects an unfolded core M into M_proj using the transformation matrix U.
    // U is an output parameter and is computed as the HOSVD of the tensor (left singular
    // vectors of M) and M is compressed using U.transpose().
    inline void project(Eigen::MatrixXd& M, Eigen::MatrixXd& U, Eigen::MatrixXd& M_proj)
    {
        Eigen::SelfAdjointEigenSolver <Eigen::MatrixXd > es(M * M.transpose()); // M*M^T is symmetric -> faster eigenvalue computation
        Eigen::VectorXd eigenvalues = es.eigenvalues().real();
        Eigen::MatrixXd U_unsorted = es.eigenvectors().real();
        uint32_t s = M.rows();
        U = Eigen::MatrixXd(s, s);
        // We sort the (eigenvalue, eigenvector) pairs in descending order
        std::vector < std::pair < double, uint32_t >>eigenvalues_sorted(s);
        for (uint32_t i = 0; i < s; ++i)
            eigenvalues_sorted[i] = std::pair < double, uint32_t >(-eigenvalues(i), i);
        std::sort(eigenvalues_sorted.begin(), eigenvalues_sorted.end());
        for (uint32_t i = 0; i < s; ++i)
            U.col(i) = U_unsorted.col(eigenvalues_sorted[i].second);
        M_proj = U.transpose() * M;
    }

    // U is an input parameter and M is decompressed using U (sliced as appropriate)
    inline void unproject(Eigen::MatrixXd& M, Eigen::MatrixXd& U, Eigen::MatrixXd& M_proj, Slice slice) {
        if (!slice.is_standard()) {
            if (slice.points[0] < 0 || slice.points[1] > U.rows()) { // TODO put in decompress.hpp
                std::cout << "Error: the slicing falls out of the tensor size range" << std::endl;
                exit(1);
            }
            int8_t sign = (0 < slice.points[2]) - (slice.points[2] < 0);
            if ((sign < 0 && slice.points[0] < slice.points[1]) || (sign > 0 && slice.points[0] > slice.points[1])) {
                std::cout << "Error: unfeasible slicing" << std::endl;
                exit(1);
            }
            Eigen::MatrixXd convolution = Eigen::MatrixXd::Zero(slice.get_size(), U.rows()); // convolution*U convolves U along the columns
            const int32_t sliceSize = slice.get_size();
#pragma omp parallel for
            for (int32_t i = 0; i < slice.get_size(); ++i) {
                switch (slice.reduction) {
                case Downsampling: {
                    convolution(i, slice.points[0] + i * slice.points[2]) = 1; // Delta kernel
                    break;
                }
                case Box: {
                    int32_t start = slice.points[0] + i * slice.points[2] - slice.points[2] / 2;
                    int32_t end = std::max(
                        std::min(static_cast<Eigen::Index>(slice.points[0] + i * slice.points[2] + (slice.points[2] - slice.points[2] / 2)), U.rows()),
                        Eigen::Index(0));
                    double kernel_sum = 1. / abs(end - start);
                    for (int32_t j = start; sign * j < sign * end; j += sign)
                        convolution(i, j) = kernel_sum; // Box kernel
                    break;
                }
                case Lanczos: {
                    double a = 2 * slice.points[2]; // Upscaled Lanczos window
                    int32_t start = std::max(
                        std::min(static_cast<long long>(slice.points[0] + i * slice.points[2] - a), U.rows() - 1ll),
                        0ll); // Kernel support: [-a, a], clamped
                    int32_t end = std::max(
                        std::min(static_cast<Eigen::Index>(slice.points[0] + i * slice.points[2] + a + 1), U.rows()),
                        static_cast<Eigen::Index>(-1ll));
                    double center = slice.points[0] + i * slice.points[2];
                    double kernel_sum = 0;
                    for (int32_t j = start; sign * j < sign * end; j += sign) {
                        double x = (j - center) / abs(slice.points[2]); // Upscaled x
                        if (x == 0)
                            convolution(i, j) = 1;
                        else
                            convolution(i, j) = a * sin(M_PI * x) * sin(M_PI * x / a) / (M_PI * M_PI * x * x); // Lanczos 2 kernel
                        kernel_sum += convolution(i, j);
                    }
                    for (int32_t j = start; sign * j < sign * end; j += sign)
                        convolution(i, j) /= kernel_sum; // Normalize the kernel so that it adds up to 1
                    break;
                }
                }
            }
            M_proj = (convolution * U) * M;
        }
        else
            M_proj = U * M;
    }

    // Reads a tensor in the buffer data of size s, and compresses it.
    // The factor matrices are output parameters
    inline void hosvd_compress(double* data, std::vector<Eigen::MatrixXd>& Us,
        const std::vector<uint32_t>& s, const std::vector<size_t>& sprod, bool verbose)
    {
        char n = s.size();

        // First unfolding: special case (elements are already arranged as we want)
        if (verbose) std::cout << "\tUnfold (1)... " << std::flush;
        Eigen::MatrixXd M = Eigen::MatrixXd::Map(data, s[0], sprod[n] / s[0]);
        Eigen::MatrixXd M_proj;
        if (verbose) std::cout << "Project (1)..." << std::flush;
        project(M, Us[0], M_proj);
        if (verbose) std::cout << std::endl;

        // Remaining unfoldings: all of them go matrix -> matrix
        // Input: matrix of size s[dim-1] x (s[0] * ... * s[dim-2] * s[dim] * ... * s[N])
        // Output: matrix of size s[dim] x (s[0] * ... * s[dim-1] * s[dim+1] * ... * s[N])
        for (uint8_t dim = 1; dim < n; ++dim) {
            if (verbose)  std::cout << "\tUnfold (" << dim + 1 << ")... " << std::flush;
            M = Eigen::MatrixXd(s[dim], sprod[n] / s[dim]); // dim-th factor matrix
#pragma omp parallel for
            for (int64_t j = 0; j < M_proj.cols(); ++j) {
                uint32_t write_i = (j / sprod[dim - 1]) % s[dim];
                size_t base_write_j = j % sprod[dim - 1] + j / (sprod[dim - 1] * s[dim]) * sprod[dim];
                for (int32_t i = 0; i < M_proj.rows(); ++i)
                    M(write_i, base_write_j + i * sprod[dim - 1]) = M_proj(i, j);
            }
            if (verbose)  std::cout << "\tProject (" << dim + 1 << ")... " << std::flush;
            project(M, Us[dim], M_proj);
            if (verbose)  std::cout << std::endl;
        }

        // We fold back from matrix into ND tensor
        if (verbose) std::cout << "\tFold... " << std::flush << std::endl;
#pragma omp parallel for
        for (int32_t i = 0; i < s[n - 1]; i++)
            for (size_t j = 0; j < sprod[n - 1]; j++)
                data[i * sprod[n - 1] + j] = M_proj(i, j);
    }

    // Reads a tensor in the buffer data of size s, and decompresses it in-place
    inline void hosvd_decompress(std::vector<double>& data, std::vector<Eigen::MatrixXd>& Us,
        const std::vector<uint32_t>& r, const std::vector<size_t>& rprod, const std::vector<size_t>& snewprod,
        bool verbose, std::vector<Slice>& cutout)
    {
        size_t n = r.size();
        if (rprod[n] == 0) { // Extreme case: 0 ranks
            data = std::vector<double>(snewprod[n], 0); // Produce a 0 reconstruction of the expected size, and leave
            return;
        }

        // First unfolding: special case (elements are already arranged as we want)
        if (verbose) std::cout << "\tUnfold (1)... " << std::flush;
        Eigen::MatrixXd M = Eigen::MatrixXd::Map(data.data(), r[0], rprod[n] / r[0]);
        Eigen::MatrixXd M_proj;
        if (verbose) {
            std::cout << "\tUnproject (" << 1 << ")";
            if (!cutout[0].is_standard())
                std::cout << " with cutout " << cutout[0];
            std::cout << "... " << std::flush;
        }
        unproject(M, Us[0], M_proj, cutout[0]);
        if (verbose) std::cout << std::endl;

        // Remaining unfoldings: all of them go matrix -> matrix
        // Input: matrix of size s[dim-1] x (s[0] * ... * s[dim-2] * s[dim] * ... * s[N])
        // Output: matrix of size s[dim] x (s[0] * ... * s[dim-1] * s[dim+1] * ... * s[N])
        for (uint8_t dim = 1; dim < n; ++dim) {
            if (verbose) std::cout << "\tUnfold (" << dim + 1 << ")... " << std::flush;
            M = Eigen::MatrixXd(r[dim], snewprod[dim] * rprod[n] / rprod[dim + 1]); // dim-th factor matrix
#pragma omp parallel for
            for (int64_t j = 0; j < M_proj.cols(); ++j) {
                uint32_t write_i = (j / snewprod[dim - 1]) % r[dim];
                size_t base_write_j = j % snewprod[dim - 1] + j / (snewprod[dim - 1] * r[dim]) * snewprod[dim];
                for (int32_t i = 0; i < M_proj.rows(); ++i)
                    M(write_i, base_write_j + i * snewprod[dim - 1]) = M_proj(i, j);
            }
            if (verbose) {
                std::cout << "\tUnproject (" << dim + 1 << ")";
                if (!cutout[dim].is_standard())
                    std::cout << " with cutout " << cutout[dim];
                std::cout << "... " << std::flush;
            }
            unproject(M, Us[dim], M_proj, cutout[dim]);
            if (verbose) std::cout << std::endl;
        }

        // We fold back from matrix into ND tensor
        if (verbose) std::cout << "\tFold... " << std::flush << std::endl;
        data.resize(snewprod[n]);
        data.shrink_to_fit();
#pragma omp parallel for
        for (ptrdiff_t i = 0; i < snewprod[n]; i++)
            data[i] = M_proj(i / snewprod[n - 1], i % snewprod[n - 1]);
    }

}

#endif // TUCKER_HPP


#include "mview.h"
#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgcodecs.hpp>

static std::pair<GrayImage, RgbImage> ReadImageFromFile(std::string);
static float ColourToGray(Eigen::Vector3f rgb);
static RgbImage convertOpenCVToRgb(cv::Mat rgbMat);

auto read_image(CameraParameter cameraParameter) -> Image
{
  Image image;
  image.intrinsics = cameraParameter.intrinsics;
  image.extrinsics = cameraParameter.extrinsics;
  std::tie(image.gray_pixels, image.rgb_pixels) = ReadImageFromFile(cameraParameter.filename);

  return image;
}

std::pair<GrayImage, RgbImage> ReadImageFromFile(std::string filename) {
	std::cout << filename << std::endl;
	cv::Mat rgb_mat = cv::imread(filename, cv::IMREAD_COLOR);
	rgb_mat.assignTo(rgb_mat, CV_32F);
	rgb_mat /= 255.;

	RgbImage rgb_target = convertOpenCVToRgb(rgb_mat);
	GrayImage gray_target = rgb_target.unaryExpr([](auto pixel) { return ColourToGray(pixel); });

	return {gray_target, rgb_target};
}

float ColourToGray(Eigen::Vector3f rgb) {
	static constexpr float GAMMA = 2.2;

	const Eigen::Vector3f gamma_correct = Eigen::Array3f(rgb).pow(GAMMA);
	return gamma_correct.dot(Eigen::Vector3f { .2126, .7152, .0722 });
}

RgbImage convertOpenCVToRgb(const cv::Mat rgbMat){
    cv::Mat rgb[3];
    cv::split(rgbMat, rgb);

	int width = rgbMat.cols;
	int height = rgbMat.rows;
    GrayImage r(height, width), g(height, width), b(height, width);

    cv::cv2eigen(rgb[0], r);
    cv::cv2eigen(rgb[1], g);
    cv::cv2eigen(rgb[2], b);
	std::cout << "converted\n";

    RgbImage rgbImage (g.rows(), g.cols());

    for(int row=0;row<g.rows();row++){
        for(int col=0;col<g.cols();col++){
            rgbImage(row,col)<<r(row,col),g(row,col),b(row,col);
        }
    }

    return rgbImage;
}


#pragma once

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

namespace tu
{

  /**
   * Represents subsets of row and column index sets
   */

  struct submatrix_indices
  {
    typedef boost::numeric::ublas::vector <size_t> vector_type;
    typedef boost::numeric::ublas::indirect_array <vector_type> indirect_array_type;

    indirect_array_type rows;
    indirect_array_type columns;
  };

  /**
   * Integer matrix
   */

  typedef boost::numeric::ublas::matrix <long long> integer_matrix;

  /**
   * Indirect integer matrix
   */

  typedef boost::numeric::ublas::matrix_indirect <const integer_matrix, submatrix_indices::indirect_array_type> integer_submatrix;

  enum log_level
  {
    LOG_QUIET, LOG_PROGRESSIVE, LOG_VERBOSE
  };

} /* namespace tu */