#include "mex.h"

#include <Eigen/Dense>
#include <cmath>
#include <limits>

#define FIXNAN() unaryExpr(std::ptr_fun(fixNaN))

double fixNaN(double x) {

    return std::isnan(x) ? 0 : x;
}

double fixLogInf(double x) {

    return std::isinf(x) ? -500 : x;
}

void printMatrix(char *s, Eigen::MatrixXd M) {

  mexPrintf("Matrix: %s\n", s);

  for (int i = 0; i < M.rows(); i++) {

    for (int j = 0; j < M.cols(); j++) {
      mexPrintf("\t%f", M(i, j));
    }
    mexPrintf("\n");
  }
  mexPrintf("\n\n");
}

Eigen::VectorXd logsumexp(Eigen::MatrixXd x) {

  Eigen::VectorXd y = x.colwise().maxCoeff();

  Eigen::VectorXd s = (x.rowwise() - y.transpose()).array().exp().colwise().sum().log();

  return y + s;
}

double logdet(Eigen::MatrixXd M) {

  Eigen::LLT<Eigen::MatrixXd> llt(M);

  // D = 2 * sum(log(diag(chol(M))));

  Eigen::VectorXd res = ((Eigen::MatrixXd) llt.matrixU()) // Calc chol())
          .diagonal().array().log() // Log of diagonal
          .colwise().sum(); // Calc sum of logs

  return 2 * res[0];
}

static inline double max(double a, double b) {
  return a < b ? b : a;
}

static inline double log_relative_Gauss(double z, double &e, int &exit_flag) {

  double logphi, logPhi;

  if (z < -6) {

    e = 1;
    logPhi = -1.0e12;
    exit_flag = -1;

  } else if (z > 6) {

    e = 0;
    logPhi = 0;
    exit_flag = 1;

  } else {

    logphi = -0.5 * (z * z + log(M_PI * 2)); // Const function call gets optimized away
    logPhi = log(0.5 * erfc(-z * M_SQRT1_2));
    e = exp(logphi - logPhi);
    exit_flag = 0;
  }
  return logPhi;
}

static void lt_factor(int s, int l, Eigen::VectorXd M, Eigen::MatrixXd V, double mp, double p, double gam,
        Eigen::VectorXd &Mnew, Eigen::MatrixXd &Vnew, double &pnew, double &mpnew, double &logS, double &d) {

  // rank 1 projected cavity parameters
  Eigen::VectorXd Vc = (V.col(l) - V.col(s)) * M_SQRT1_2;
  double cVc = (V(l, l) - 2 * V(s, l) + V(s, s)) / 2;
  double cM = (M(l) - M(s)) * M_SQRT1_2;

  double cVnic = max(0, cVc / (1 - p * cVc));

  double cmni = cM + cVnic * (p * cM - mp);

  // rank 1 calculation: step factor
  double z = cmni / sqrt(cVnic);

  double e;
  int exit_flag;
  double lP = log_relative_Gauss(z, e, exit_flag);

  double alpha, beta, r, dp, dmp;

  switch (exit_flag) {

    case 0:

      alpha = e / sqrt(cVnic);
      beta = alpha * (alpha * cVnic + cmni);
      r = beta / (1 - beta);

      // new message
      pnew = r / cVnic;
      mpnew = r * (alpha + cmni / cVnic) + alpha;

      // update terms
      dp = max(-p + DBL_EPSILON, gam * (pnew - p));
      dmp = max(-mp + DBL_EPSILON, gam * (mpnew - mp));
      d = max(dmp, dp); // for convergence measures

      pnew = p + dp;
      mpnew = mp + dmp;

      // project out to marginal
      Vnew = V - dp / (1 + dp * cVc) * (Vc * Vc.transpose());
      Mnew = M + (dmp - cM * dp) / (1 + dp * cVc) * Vc;

      // normalization constant
      //logS  = lP - 0.5 * (log(beta) - log(pnew)) + (alpha * alpha) / (2*beta);

      // there is a problem here, when z is very large
      logS = lP - 0.5 * (log(beta) - log(pnew) - log(cVnic)) + (alpha * alpha) / (2 * beta) * cVnic;

      break;

    case -1: // impossible combination

      d = NAN;

      //Mnew = 0;
      //Vnew = 0;

      pnew = 0;
      mpnew = 0;
      logS = -INFINITY;
      break;

    case 1: // uninformative message

      pnew = 0;
      mpnew = 0;

      // update terms
      dp = -p; // at worst, remove message
      dmp = -mp;
      d = max(dmp, dp); // for convergence measures

      // project out to marginal
      Vnew = V - dp / (1 + dp * cVc) * (Vc * Vc.transpose());
      Mnew = M + (dmp - cM * dp) / (1 + dp * cVc) * Vc;

      logS = 0;
      break;
  }
}

double min_factor(Eigen::VectorXd Mu, Eigen::MatrixXd Sigma, int k, double gam,
        Eigen::VectorXd &dlogZdMu, Eigen::VectorXd &dlogZdSigma, Eigen::MatrixXd &dlogZdMudMu) {

  int D = Mu.size();

  double logZ;

  // messages (in natural parameters)
  Eigen::VectorXd logS = Eigen::VectorXd::Zero(D - 1); // normalization constant (determines zeroth moment)
  Eigen::VectorXd MP = Eigen::VectorXd::Zero(D - 1); // mean times precision (determines first moment)
  Eigen::VectorXd P = Eigen::VectorXd::Zero(D - 1); // precision (determines second moment)

  // TODO: check if copy is really necessary here
  // marginal:
  Eigen::VectorXd M(Mu);
  Eigen::MatrixXd V(Sigma);

  double mpm;
  double s;
  double rSr;
  double dts;

  //Eigen::VectorXd dMdMu;
  //Eigen::VectorXd dMdSigma;
  //Eigen::VectorXd dVdSigma;
  Eigen::MatrixXd _dlogZdSigma;

  Eigen::MatrixXd R;
  Eigen::VectorXd r;

  Eigen::MatrixXd IRSR;
  Eigen::MatrixXd A;
  Eigen::MatrixXd A_;
  Eigen::VectorXd b;
  Eigen::VectorXd Ab;

  Eigen::VectorXd btA;

  Eigen::MatrixXd C;

  double pnew;
  double mpnew;

  double Diff = 0, diff = 0;

  int l, count = 0;

  // mvmin = Eigen::VectorXd(2);

  while (true) {

    count++;

    Diff = 0;

    for (int i = 0; i < D - 1; i++) {

      if (i < k)
        l = i;
      else
        l = i + 1;

      lt_factor(k, l, M, V, MP[i], P[i], gam, // IN
              M, V, pnew, mpnew, logS[i], diff); // OUT

      // Write back vector elements
      P[i] = pnew;
      MP[i] = mpnew;

      if (std::isnan(diff))
        goto done; // found impossible combination

      Diff = Diff + std::abs(diff);
    }

    if (count > 50) {
      mexPrintf("EP iteration ran over iteration limit. Stopped.\n");
      goto done;
    }
    if (Diff < 1.0e-3) {
      goto done;
    }
  }

done:

  if (std::isnan(diff)) {

    logZ = -INFINITY;
    dlogZdMu = Eigen::VectorXd::Zero(D);
    dlogZdSigma = Eigen::VectorXd::Zero(0.5 * (D * (D + 1)));
    dlogZdMudMu = Eigen::MatrixXd::Zero(D, D);
    //mvmin << Mu(k), Sigma(k, k);
    //dMdMu = Eigen::VectorXd::Zero(D);
    //dMdSigma = Eigen::VectorXd::Zero(0.5 * (D * (D + 1)));
    //dVdSigma = Eigen::VectorXd::Zero(0.5 * (D * (D + 1)));

  } else {

    // evaluate log Z:

    // C = eye(D) ./ sqrt(2); C(k,:) = -1/sqrt(2); C(:,k) = [];
    C = Eigen::MatrixXd::Zero(D, D - 1);
    for (int i = 0; i < D - 1; i++) {

      C(i + (i >= k), i) = M_SQRT1_2;
      C(k, i) = -M_SQRT1_2;
    }

    R = C.array().rowwise() * P.transpose().array().sqrt();
    r = (C.array().rowwise() * MP.transpose().array()).rowwise().sum();
    mpm = (MP.array() * MP.array() / P.array()).FIXNAN().sum();
    s = logS.sum();

    IRSR = R.transpose() * Sigma * R;
    IRSR.diagonal().array() += 1; // Add eye()

    rSr = r.dot(Sigma * r);

    A_ = R * IRSR.llt().solve(R.transpose());
    A = 0.5 * (A_.transpose() + A_); // ensure symmetry.

    b = (Mu + Sigma * r);
    Ab = A * b;

    dts = logdet(IRSR);
    logZ = 0.5 * (rSr - b.dot(Ab) - dts) + Mu.dot(r) + s - 0.5 * mpm;

    if (true /*TODO: needs derivative? */) {

      dlogZdSigma = Eigen::VectorXd(0.5 * (D * (D + 1)));

      btA = b.transpose() * A;

      dlogZdMu = r - Ab;
      dlogZdMudMu = -A;

      _dlogZdSigma = -A - 2 * r * Ab.transpose() + r * r.transpose() + btA * Ab.transpose();

      Eigen::MatrixXd diag = _dlogZdSigma.diagonal().asDiagonal();

      _dlogZdSigma = 0.5 * (_dlogZdSigma + _dlogZdSigma.transpose() - diag);

      // dlogZdSigma = dlogZdSigma(logical(triu(ones(D,D))));
      for (int x = 0, i = 0; x < D; x++) {

        for (int y = 0; y <= x; y++) {
          dlogZdSigma[i++] = _dlogZdSigma(y, x);
        }
      }
    }
  }

  return logZ;
}

void joint_min(Eigen::VectorXd Mu, Eigen::MatrixXd Sigma,
        Eigen::VectorXd &logP, Eigen::MatrixXd &dlogPdMu, Eigen::MatrixXd &dlogPdSigma, Eigen::MatrixXd **dlogPdMudMu) {

  Eigen::VectorXd dlPdM;
  Eigen::VectorXd dlPdS;
  Eigen::MatrixXd dlPdMdM;

  double gam = 1;
  int D = Mu.size();

  Eigen::MatrixXd gg = Eigen::MatrixXd(D, D);

  logP = Eigen::VectorXd(D);

  dlogPdMu = Eigen::MatrixXd(D, D);
  dlogPdSigma = Eigen::MatrixXd(D, D * (D + 1) / 2);
  *dlogPdMudMu = new Eigen::MatrixXd[D]; // Create an array of matrizes

  for (int k = 0; k < D; k++) {

#ifdef DEBUG_PRINTF
    if (k % 10 == 0)
      DEBUG_PRINTF('#');
#endif

    logP(k) = min_factor(Mu, Sigma, k, gam, // IN
            dlPdM, dlPdS, dlPdMdM); // OUT

    dlogPdMu.row(k) = dlPdM;
    dlogPdSigma.row(k) = dlPdS;

    (*dlogPdMudMu)[k] = dlPdMdM;
  }

  // Sanity check for INF values
  logP = logP.unaryExpr(std::ptr_fun(fixLogInf));

  // re-normalize at the end, to smooth out numerical imbalances:
  double Z = logP.array().exp().sum();

  Eigen::VectorXd Zm = (dlogPdMu.array().colwise() * logP.array().exp()).colwise().sum() /Z;
  Eigen::VectorXd Zs = (dlogPdSigma.array().colwise() * logP.array().exp()).colwise().sum() /Z;

  Eigen::MatrixXd Zij = Zm * Zm.transpose();

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

    for (int j = i; j < D; j++) {

      Eigen::MatrixXd Mj = (*dlogPdMudMu)[j];

      for (int k = 0; k < D; k++) {
        gg(i, j) -= (dlogPdMu(k, i) * dlogPdMu(k, j) + Mj(k, i)) * exp(logP(k));
      }
      gg(j, i) = // Hesse Matrix is symmetric
              gg(i, j) = gg(i, j) / Z + Zij(i, j);
    }
  }

  for (int i = 0; i < D; i++) {
    (*dlogPdMudMu)[i].array() *= gg.array();
  }

  dlogPdMu = dlogPdMu.array().rowwise() - Zm.transpose().array();
  dlogPdSigma = dlogPdSigma.array().rowwise() - Zs.transpose().array();

  logP = logP.array() - logsumexp(logP)(0, 0);
}

static void copyMatrix(Eigen::MatrixXd from, mxArray **out) {

  int r = from.rows();
  int c = from.cols();

  *out = mxCreateDoubleMatrix(r, c, mxREAL);
  double *write = mxGetPr(*out);
  for (int i = 0; i < r; i++) {

    for (int j = 0; j < c; j++) {
      write[j * r + i] = from(i, j);
    }
  }
}

static void copyCube(Eigen::MatrixXd *from, int elms, mxArray **out) {

  // They're all of the same dimension
  int r = from->rows();
  int c = from->cols();

  mwSize dims[3];
  dims[0] = r;
  dims[1] = c;
  dims[2] = elms;

  *out = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
  double *write = mxGetPr(*out);
  for (int k = 0; k < elms; k++) {

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

      for (int j = 0; j < c; j++) {
        write[k * r * c + i * c + j] = from[k](i, j);
      }
    }
  }
}

void mexFunction(int nlhs, mxArray * plhs[],
        int nrhs, const mxArray * prhs[]) {

  if (nrhs != 2)
    mexErrMsgIdAndTxt("MATLAB:xtimesy:invalidNumInputs",
          "Two inputs required.");

  if (nlhs != 4)
    mexErrMsgIdAndTxt("MATLAB:xtimesy:invalidNumOutputs",
          "Four outputs required.");

  if (mxGetM(prhs[0]) != 1 && mxGetN(prhs[0]) != 1) {
    mexErrMsgIdAndTxt("MATLAB:xtimesy:invalidNumOutputs",
            "Vector for param 1 required");
  }

  // Input vars
  Eigen::Map<Eigen::VectorXd> Mu(mxGetPr(prhs[0]), mxGetM(prhs[0]) == 1 ? mxGetN(prhs[0]) : mxGetM(prhs[0]));
  Eigen::Map<Eigen::MatrixXd> Sigma(mxGetPr(prhs[1]), mxGetM(prhs[1]), mxGetN(prhs[1]));

  // Output vars
  Eigen::VectorXd logP;
  Eigen::MatrixXd dlogPdMu;
  Eigen::MatrixXd dlogPdSigma;
  Eigen::MatrixXd *dlogPdMudMu;

  // Do the heavy work
  joint_min(Mu, Sigma,
          logP, dlogPdMu, dlogPdSigma, &dlogPdMudMu);

  // Output results
  copyMatrix(logP, &(plhs[0]));
  copyMatrix(dlogPdMu, &(plhs[1]));
  copyMatrix(dlogPdSigma, &(plhs[2]));
  copyMatrix(dlogPdSigma, &(plhs[3]));
  copyCube(dlogPdMudMu, Mu.size(), &(plhs[3]));

  // Endpoint, delete array
  delete[] dlogPdMudMu;
}


#include "sv/util/math.h"

#include <Eigen/Geometry>

namespace sv {

void MakeRightHanded(Eigen::Vector3d& eigvals, Eigen::Matrix3d& eigvecs) {
  auto hand = eigvecs.col(0).cross(eigvecs.col(1)).dot(eigvecs.col(2));
  if (hand < 0) {
    eigvecs.col(0).swap(eigvecs.col(1));
    eigvals.row(0).swap(eigvals.row(1));
  }
}

}  // namespace sv


///\file shooting-method.cpp
///\author Ethan Knox
///\date 8/2/2020.

#include <iostream>
#include <fstream>
#include <limits>
#include <iomanip>
#include <functional>

#include <boost/numeric/odeint.hpp>
#include <boost/math/tools/roots.hpp>

#include "potentials.h"
#include "determine_domain_from_potential.h"

using namespace std::placeholders;

const double m = 1.0;
const double k = 1.0;
const double hbar = 1.0;
const double omega = 1.0;

typedef std::vector<double> state_t;
typedef boost::numeric::odeint::runge_kutta_fehlberg78<state_t> stepper_rkf78_t;
typedef boost::numeric::odeint::runge_kutta4<state_t> rk4;

void TISE(state_t &psi, state_t &dpsi_dx, double x, std::function<double(double)> V, double E) {
    dpsi_dx[0] = psi[1];
    dpsi_dx[1] = (2.0 * m / pow(hbar, 2.0)) * (V(x) - E) * psi[0];
}

int eigenE_shooting(state_t& psi, double boundary_condition, std::string Vfunc, double L, double h_0, double eps_rel, double eps_abs, double E)
{
    // Potential
    std::function<double(double)> V;
    std::function<double(double)> dV;
    if (Vfunc == "QHO") {
        V = quantum_harmonic_oscillator;
        dV = d_quantum_harmonic_oscillator;
    }
    else { // Vfunc == "ISW"
        V = infinite_square_well;
        dV = d_infinite_square_well;
    }

    // Domain
    std::pair<double, double> domain = determine_domain_from_potential(Vfunc, L);
    const double xi = domain.first;
    const double xf = domain.second;
    double x = xi;

    // Initial Condition
    state_t psi_copy(psi);
    state_t dpsi_dx(2);
    double h_0 = 1.0e-4;

    std::function<double(double)> defect = [x, xf, h_0, psi_copy, &V](double E) {
        boost::numeric::odeint::integrate_const(rk4(), std::bind(TISE, _1, _2, _3, V, E), psi_copy, x, xf, h_0);
        return psi_copy[0];
    };
    boost::math::tools::eps_tolerance<double> tol;
    std::pair<double, double> r = boost::math::tools::bisect(defect, 0.0, 10.0, tol);
    return r.first + (r.second - r.first) / 2;
}


#ifndef SE3_OPS_HPP
#define SE3_OPS_HPP

#include <tuple>
#include <vector>
#include <Eigen/StdVector>
#include <Eigen/Core>
#include <sophus/se3.hpp>
#include <math.h>
#include <opencv2/core.hpp>
#include <opencv2/hdf.hpp>
#include <opencv2/core/eigen.hpp>

#include "math_utils.hpp"

namespace orcvio
{

template<typename T>
  using vector_eigen = std::vector<T, Eigen::aligned_allocator<T>>;

/**
 * @brief converts vector to skew symmetric matrix in batch
 *
 * @param a: size n x 3, input vector
 *
 * @return : size n x 3 x 3, skew symmetric matrix
 */
template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 3, 3> > skew(const vector_eigen<Eigen::Matrix<Scalar, 3, 1> > &a)
{

    vector_eigen<Eigen::Matrix<Scalar, 3, 3> > S;

    for (const auto& w : a)
    {
        Eigen::Matrix<Scalar, 3, 3> w_x = skewSymmetric(w);
        S.push_back(w_x);
    }

    return S;

}

/**
 * @brief converts 6-vector to 4x4 hat form in se(3) in batch
 *
 * @param x: size n x 6, n se3 elements
 *
 * @return : size n x 4 x 4, n elements of se(3)
 */
template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 4, 4> > axangle2twist(const vector_eigen<Eigen::Matrix<Scalar, 6, 1> > &a)
{

    vector_eigen<Eigen::Matrix<Scalar, 4, 4> > T;

    for (auto v : a)
    {
        Eigen::Matrix<Scalar, 4, 4> v_x = Eigen::Matrix4d::Zero();
        v_x(0, 1) = -v(5);
        v_x(0, 2) = v(4);
        v_x(0, 3) = v(0);
        v_x(1, 0) = v(5);
        v_x(1, 2) = -v(3);
        v_x(1, 3) = v(1);
        v_x(2, 0) = -v(4);
        v_x(2, 1) = v(3);
        v_x(2, 3) = v(2);

        T.push_back(v_x);
    }

    return T;

}

/**
 * @brief converts se3 element to SE3 in batch
 *
 * @param x: size n x 6, n se3 elements
 *
 * @return : size n x 4 x 4, n elements of SE(3)
 */
template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 4, 4> > axangle2pose(const vector_eigen<Eigen::Matrix<Scalar, 6, 1> > &a)
{

    vector_eigen<Eigen::Matrix<Scalar, 4, 4> > T;

    for (auto x : a)
    {

        Sophus::SE3d T_temp = Sophus::SE3d::exp(x);
        T.push_back(T_temp.matrix());

    }

    return T;

}

/**
 * @brief converts axis angle to SO3 in batch
 *
 * @param a = n x 3 = n axis-angle elements
 *
 * @return : R = n x 3 x 3 = n elements of SO(3)
 */
template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 3, 3> > axangle2rot(const vector_eigen<Eigen::Matrix<Scalar, 3, 1> > &a)
{

    vector_eigen<Eigen::Matrix<Scalar, 3, 3> > R;

    for (auto x : a)
    {

        Sophus::SO3d R_temp = Sophus::SO3d::exp(x);
        R.push_back(R_temp.matrix());

    }

    return R;

}

/**
 * @brief performs batch inverse of transform matrix
 *
 * @param T: size n x 4 x 4, n elements of SE(3)
 *
 * @return : size n x 4 x 4, inverse of T
 */
template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 4, 4> > inversePose(const vector_eigen<Eigen::Matrix<Scalar, 4, 4> > &T)
{

    vector_eigen<Eigen::Matrix<Scalar, 4, 4> > iT;

    for (auto T_temp : T)
    {
        Eigen::Matrix<Scalar, 4, 4> iT_temp = Eigen::Matrix4d::Zero();

        iT_temp(0, 0) = T_temp(0, 0);
        iT_temp(0, 1) = T_temp(1, 0);
        iT_temp(0, 2) = T_temp(2, 0);

        iT_temp(1, 0) = T_temp(0, 1);
        iT_temp(1, 1) = T_temp(1, 1);
        iT_temp(1, 2) = T_temp(2, 1);

        iT_temp(2, 0) = T_temp(0, 2);
        iT_temp(2, 1) = T_temp(1, 2);
        iT_temp(2, 2) = T_temp(2, 2);

        iT_temp.block(0, 3, 3, 1) = -1 * iT_temp.block(0, 0, 3, 3) * T_temp.block(0, 3, 3, 1);

        iT_temp.block(3, 0, 1, 4) = T_temp.block(3, 0, 1, 4);

        iT.push_back(iT_temp);

    }

    return iT;

}

/**
 * @brief odot operator
 *
 * \f{align*}{
 *  \underline{x}^{\odot} = \begin{bmatrix} I_{3\times 3} & -x_{\times} \\ 0 & 0\end{bmatrix}
 @f}
 *
 * @param ph = 4 = point in homogeneous coordinates
 *
 * @return : odot(ph) = 4 x 6
 */
template<typename Derived>
 Eigen::Matrix<typename Derived::Scalar, 4, 6> odotOperator(const Eigen::MatrixBase<Derived>& x) {
     assert(x.rows() == 4);
     assert(x.cols() == 1);

   Eigen::Matrix<typename Derived::Scalar, 4, 6> temp;
   temp.setZero();

   temp.block(0, 3, 3, 3) = -1 * skewSymmetric(x.template head<3>());

   temp(0, 0) = x(3);
   temp(1, 1) = x(3);
   temp(2, 2) = x(3);
   return temp;
 }

/**
 * @brief odot operator
 *
 * \f{align*}{
 *  \underline{x}^{\odot} = \begin{bmatrix} I_{3\times 3} & -x_{\times} \\ 0 & 0\end{bmatrix}
  @f}
 *
 * @param ph = n x 4 = points in homogeneous coordinates
 *
 * @return : odot(ph) = n x 4 x 6
 */
 template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 4, 6> > odotOperator(const vector_eigen<Eigen::Matrix<Scalar, 4, 1> > &ph)
{

    vector_eigen<Eigen::Matrix<Scalar, 4, 6> > zz;

    for (const auto& x : ph)
    {
      zz.emplace_back(odotOperator(x));
    }
    return zz;
}

/**
  * @brief circle dot operator
  *
  * @param ph = 4 = points in homogeneous coordinates
  *
  * @return : circledCirc(ph) = 6 x 4
  */
template<typename Derived>
Eigen::Matrix<typename Derived::Scalar, 6, 4> circledCirc(const Eigen::MatrixBase<Derived>& x) {
    static_assert(Derived::RowsAtCompileTime == 4, "x is not 4D vector");
    static_assert(Derived::ColsAtCompileTime == 1, "x is not a vector");
    Eigen::Matrix<typename Derived::Scalar, 6, 4> temp;
    temp.setZero();

    temp.block(3, 0, 3, 3) = -1 * skewSymmetric(x.template block<3, 1>(0, 0));

    temp.block(0, 3, 3, 1) = x.template block<3, 1>(0, 0);
    return temp;
}

/**
 * @brief circle dot operator
 *
 * @param ph = n x 4 = points in homogeneous coordinates
 *
 * @return : circledCirc(ph) = n x 6 x 4
 */
template<typename Scalar>
vector_eigen<Eigen::Matrix<Scalar, 6, 4> > circledCirc(const vector_eigen<Eigen::Matrix<Scalar, 4, 1> > &ph)
{

    vector_eigen<Eigen::Matrix<Scalar, 6, 4>  > zz;

    for (const auto& x : ph)
    {
      zz.push_back(circledCirc(x));
    }

    return zz;

}

/**
 * @brief only keep yaw and zero z
 *
 * @param T_SE3: 4 x 4
 *
 * @return T_SE2: 4 x 4
 */

// template<typename Scalar>
// Eigen::Matrix<Scalar, 4, 4> poseSE32SE2(const Eigen::Matrix<Scalar, 4, 4> &T_SE3)
// {

//     Eigen::Matrix<Scalar, 4, 4> T_SE2 = Eigen::Matrix4d::Identity();

//     // yaw: alpha=arctan(r21/r11)
//     Scalar yaw = M_PI/atan2(T_SE3(1, 0), T_SE3(0, 0));

//     // deal with the case when yaw is nan
//     if (!std::isfinite(yaw))
//       yaw = 0;

//     T_SE2(0, 0) = cos(yaw);
//     T_SE2(0, 1) = -sin(yaw);
//     T_SE2(0, 3) = T_SE3(0, 3);

//     T_SE2(1, 0) = sin(yaw);
//     T_SE2(1, 1) = cos(yaw);
//     T_SE2(1, 3) = T_SE3(1, 3);

//     return T_SE2;

// }

template<typename Derived>
Eigen::Matrix<typename Derived::Scalar, 2, Eigen::Dynamic>
project_image(const Eigen::MatrixBase<Derived>& uv_hom) {
  auto den = uv_hom.template bottomRows<1>().array().template replicate<2, 1>();
  auto num = uv_hom.template topRows<2>().array();
  return ( num / den ).matrix();
}

template<typename Scalar>
using Matrix23 = Eigen::Matrix<Scalar, 2, 3>;

/**
 * @brief Differentiate \f$ \pi([x, y, z]^\top) = [x/z, y/z] \f$
 *
 * \f{align*}{
 * \frac{\partial \pi([x, y, z]^\top)}{\partial \mathbf{x}}
 * = [1/z, 0,   -x/z^2]
 *   [0,   1/z, -y/z^2]
 * @f}

 * @param x
 * @return Jacobian
 */
template <typename Derived>
Matrix23<typename Derived::Scalar>
  project_image_df(const Eigen::MatrixBase<Derived>& x)
{
    static_assert(Derived::RowsAtCompileTime == 3, " need 3 x 1 vector");
    static_assert(Derived::ColsAtCompileTime == 1, " need 3 x 1 vector");
    typedef typename Derived::Scalar Scalar;
   Scalar z = x(2, 0);
  Scalar zsq = z * z;
  Matrix23<typename Derived::Scalar> df;
  df <<
    1/z,   0, -x(0, 0)/zsq,
    0  , 1/z, -x(1, 0)/zsq;
  return df;
}

/**
 * @brief project_object_points
 *
 * @param P   : Camera projection matrix (3 x 4)
 * @param wTo : Object to world transform (4 x 4)
 * @param points_w : Points (n x 4), note this is points in object frame with homo coord
 * @return Points (n x 2)
 */
template <typename D1, typename D2, typename D3>
Eigen::Matrix<typename D3::Scalar, Eigen::Dynamic, 2>
project_object_points(const Eigen::MatrixBase<D1>& P,
                   const Eigen::MatrixBase<D2>& wTo, const Eigen::MatrixBase<D3>& points_w) {
  auto uv_hom = P * (wTo * points_w.transpose());
  return project_image(uv_hom).transpose();
}

/**
 * @brief Computes the derivative of projection operation wrt object pose
 *
 * \f[
 * \frac{\partial \pi(K ^CT_w(\chi) x_w)}{\partial \xi} = \frac{\partial \pi(K T x_w)}{\partial T} @ \frac{^cT_w(\chi)}{\partial \xi}
 * \f]
 *
 * @param [in] P   : Camera projection matrix (3 x 4)
 * @param [in] wTo : World transform (4 x 4)
 * @param [in] points_o : Points in object frame (n x 4)
 *
 * @return Jacobians (n x 2 x 6)
 */
template<typename D1, typename D2, typename D3>
Eigen::Matrix<typename D3::Scalar, Eigen::Dynamic, 6>
  project_object_points_df_object(const Eigen::MatrixBase<D1>& P, const Eigen::MatrixBase<D2>& wTo,
                           const Eigen::MatrixBase<D3>& points_o,
                           const bool use_left_perturbation_flag) {
  auto X_o = points_o.transpose();
  Eigen::Matrix<typename D3::Scalar, Eigen::Dynamic, 6> jacobians(2*points_o.rows(), 6);
  for (int i = 0; i < points_o.rows(); ++i) {

    auto dpibydx = project_image_df(P * wTo * X_o.col(i).template topLeftCorner<4, 1>());

    Eigen::MatrixXd jac;
    if (use_left_perturbation_flag)
    {
      // using left perturbation
      jac = dpibydx * P * odotOperator(wTo * X_o.col(i));
    }
    else
    {
      // using right perturbation
      jac = dpibydx * P * wTo * odotOperator(X_o.col(i));
    }

    assert(jac.rows() == 2 && jac.cols() == 6);
    jacobians.template block<2, 6>(2*i, 0) = jac.template block<2, 6>(0,0);

  }

  return jacobians;
}

/**
 * @brief Computes the derivative of projection operation wrt camera pose
 * @param [in] P   : Camera projection matrix (3 x 4)
 * @param [in] wTo : Object frame to World frame transformation (4 x 4)
 * @param [in] cTw : world frame to camera frame transformation (4 x 4)
 * @param [in] points_o : Points in object frame (n x 4)
 *
 * @return Jacobians ((nk x 2) x 6)
 */
template<typename D1, typename D2, typename D3>
Eigen::Matrix<typename D3::Scalar, Eigen::Dynamic, 6>
  project_object_points_df_camera(const Eigen::MatrixBase<D1>& P,
                           const Eigen::MatrixBase<D2>& wTo,
                           const Eigen::MatrixBase<D2>& cTw,
                           const Eigen::MatrixBase<D3>& points_o,
                           const bool use_left_perturbation_flag) {

  auto X_o = points_o.transpose();
  Eigen::Matrix<typename D3::Scalar, Eigen::Dynamic, 6> jacobians(2*points_o.rows(), 6);

  Eigen::MatrixXd ps_puline_s = Eigen::Matrix<double, 3, 4>::Zero();
  ps_puline_s.block<3,3>(0,0) = Eigen::Matrix3d::Identity();

  for (int i = 0; i < points_o.rows(); ++i) {

    auto dpibydx = project_image_df(P * wTo * X_o.col(i).template topLeftCorner<4, 1>());

    Eigen::MatrixXd jac;
    if (use_left_perturbation_flag)
    {
      // using left perturbation
      jac = -1 * dpibydx * ps_puline_s * cTw * odotOperator(wTo * X_o.col(i));
      // or equivalently
      // jac = -1 * dpibydx * P * odotOperator(wTo * X_o.col(i));
      // std::cerr << "jac equivalent " << odotOperator(wTo * X_o.col(i)) << "\n";
    }
    else
    {
      // using right perturbation
      jac = -1 * dpibydx * ps_puline_s * odotOperator(cTw * wTo * X_o.col(i));
    }

    assert(jac.rows() == 2 && jac.cols() == 6);
    jacobians.template block<2, 6>(2*i, 0) = jac.template block<2, 6>(0,0);

    // for debugging
    // std::cerr << "jac " << jac << "\n";

  }

  return jacobians;
}

/**
 * @brief Read eigen matrices from hdfio object
 *
 * @param h5io
 * @param name
 * @return
 */
template<typename T = cv::Ptr<cv::hdf::HDF5>>
Eigen::MatrixXd
dsread(const T& h5io, const std::string& name) {
  cv::Mat m;
  if (! h5io->hlexists(name))
    throw std::runtime_error("Unable to find dataset " + name);
  h5io->dsread( m, name );
  if (m.dims > 2)
    throw std::runtime_error("Cannot handle more than 2 dims, found " + std::to_string(m.dims));
  Eigen::MatrixXd m_e;
  cv::cv2eigen(m, m_e);
  return m_e;
}

/**
 * @brief Computes the distance between two SE3 transforms
 *
 * @param T1: size 4 x 4, input transform
 * @param T2: size 4 x 4, input transform
 *
 * @return : (3-tr(R))/2, |t₁ - t₂|₂
 */
template <typename D1, typename D2>
std::tuple<typename D1::Scalar, typename D1::Scalar>
displacement(const Eigen::MatrixBase<D1>& T1, const Eigen::MatrixBase<D2>& T2)
{
  using Scalar = typename D1::Scalar;
  // tr(R) = 1 + 2 cos θ
  // 1 - cos θ = 3/2 - tr(R)/2 ∈ [0, 2]
  auto R1 = T1.template block<3,3>(0,0);
  auto R2 = T2.template block<3,3>(0,0);
  Scalar dispR = (3 - (R1.transpose() * R2).trace()) / 2;
  Scalar dispt = (T1.template topRightCorner<3,1>() - T2.template topRightCorner<3,1>()).norm();
  return std::make_tuple(dispR, dispt);
}

/**
 * @brief odot operator
 *
 * \f{align*}{
 *  \underline{x}^{\odot} = \begin{bmatrix} I_{3\times 3} & -x_{\times} \\ 0 & 0\end{bmatrix}
 @f}
 *
 * @param ph = 4 = point in homogeneous coordinates
 *
 * @return : odot(ph) = 4 x 6
 */
inline Eigen::Matrix<double, 4, 6> odotOperator(const Eigen::Vector4d& x) {
  Eigen::Matrix<double, 4, 6> temp;
  temp.setZero();
  temp.block(0, 3, 3, 3) = -1 * skewSymmetric(x.head(3));

  temp(0, 0) = x(3);
  temp(1, 1) = x(3);
  temp(2, 2) = x(3);
  return temp;
}

/**
 * @brief Computes the derivative of camera se3 wrt IMU se3
 * @param [in] R_b2c : rotation of body frame to camera frame
 * @param [in] t_c_b : position of camera frame in body frame
 * @param [in] R_w2c : rotation of world frame to camera frame
 * @param [in] t_b_w : position of body frame in world frame
 * @param [in] use_left_perturbation_flag : which perturbation to use
 *
 * @return Jacobians (6 x 6)
 */
inline Eigen::Matrix<double, 6, 6> get_cam_wrt_imu_se3_jacobian(const Eigen::Matrix3d& R_b2c, const Eigen::Vector3d& t_c_b, const Eigen::Matrix3d& R_w2c, const Eigen::Vector3d& t_b_w, const bool use_left_perturbation_flag)
{
  Eigen::Matrix<double, 6, 6> p_cxi_p_ixi = Eigen::Matrix<double, 6, 6>::Zero();
  if (use_left_perturbation_flag)
  {

    p_cxi_p_ixi.block<3, 3>(0, 0) = skewSymmetric(t_b_w);
    p_cxi_p_ixi.block<3, 3>(3, 0) = Eigen::Matrix<double, 3, 3>::Identity();
    p_cxi_p_ixi.block<3, 3>(0, 3) = Eigen::Matrix<double, 3, 3>::Identity();

  }
  else
  {

    p_cxi_p_ixi.block<3, 3>(0, 0) = -1 * R_b2c * skewSymmetric(t_c_b);
    p_cxi_p_ixi.block<3, 3>(3, 0) = R_b2c;
    p_cxi_p_ixi.block<3, 3>(0, 3) = R_w2c;

  }

  return p_cxi_p_ixi;
}

} // namespace orcvio
#endif // SE3_OPS_HPP


#include <NTL/ZZ_pXFactoring.h>

NTL_CLIENT

long compare(const ZZ_pX& a, const ZZ_pX& b)
{
   if (deg(a) < deg(b))
      return 0;

   if (deg(a) > deg(b))
      return 1;

   long n = a.rep.length();
   long i;

   for (i = 0; i < n; i++) {
      if (rep(a.rep[i]) < rep(b.rep[i])) return 0;
      if (rep(a.rep[i]) > rep(b.rep[i])) return 1;
   }

   return 0;
}

void sort(vec_pair_ZZ_pX_long& v)
{
   long n = v.length();
   long i, j;

   for (i = 0; i < n-1; i++)
      for (j = 0; j < n-1-i; j++)
         if (compare(v[j].a, v[j+1].a)) {
            swap(v[j].a, v[j+1].a);
            swap(v[j].b, v[j+1].b);
         }
}

int main()
{
   ZZ p;
   cin >> p;
   ZZ_p::init(p);
   ZZ_pX f;
   cin >> f;

   vec_pair_ZZ_pX_long factors;

   double t = GetTime();
   berlekamp(factors, f, 1);
   t = GetTime()-t;
   cerr << "total time: " << t << "\n";

   ZZ_pX ff;

   mul(ff, factors);
   if (f != ff)
      Error("Incorrect factorization!!");

   sort(factors);

   cerr << "factorization pattern:";
   long i;

   for (i = 0; i < factors.length(); i++) {
      cerr << " ";
      long k = factors[i].b;
      if (k > 1)
         cerr << k << "*";
      cerr << deg(factors[i].a);
   }

   cerr << "\n";

   cout << factors << "\n";

   return 0;
}


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_POSV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_POSV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/numeric/bindings/traits/detail/symm_herm_traits.hpp>
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B
    // with A symmetric or Hermitian positive definite matrix
    //
    /////////////////////////////////////////////////////////////////////

    /*
     * posv() computes the solution to a system of linear equations
     * A * X = B, where A is an N-by-N symmetric or Hermitian positive
     * definite matrix and X and B are N-by-NRHS matrices.
     *
     * The Cholesky decomposition is used to factor A as
     *   A = U^T * U or A = U^H * U,  if UPLO = 'U',
     *   A = L * L^T or A = L * L^H,  if UPLO = 'L',
     * where U is an upper triangular matrix and L is a lower triangular
     * matrix. The factored form of A is then used to solve the system of
     * equations A * X = B.
     *
     * If UPLO = 'U', the leading N-by-N upper triangular part of A
     * contains the upper triangular part of the matrix A, and the
     * strictly lower triangular part of A is not referenced.
     * If UPLO = 'L', the leading N-by-N lower triangular part of A
     * contains the lower triangular part of the matrix A, and the
     * strictly upper triangular part of A is not referenced.
     */

    namespace detail {

      inline
      void posv (char const uplo, int const n, int const nrhs,
                 float* a, int const lda,
                 float* b, int const ldb, int* info)
      {
        LAPACK_SPOSV (&uplo, &n, &nrhs, a, &lda, b, &ldb, info);
      }

      inline
      void posv (char const uplo, int const n, int const nrhs,
                 double* a, int const lda,
                 double* b, int const ldb, int* info)
      {
        LAPACK_DPOSV (&uplo, &n, &nrhs, a, &lda, b, &ldb, info);
      }

      inline
      void posv (char const uplo, int const n, int const nrhs,
                 traits::complex_f* a, int const lda,
                 traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CPOSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda,
                      traits::complex_ptr (b), &ldb, info);
      }

      inline
      void posv (char const uplo, int const n, int const nrhs,
                 traits::complex_d* a, int const lda,
                 traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZPOSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda,
                      traits::complex_ptr (b), &ldb, info);
      }

      template <typename SymmMatrA, typename MatrB>
      inline
      int posv (char const uplo, SymmMatrA& a, MatrB& b) {
        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));
        int info;
        posv (uplo, n, traits::matrix_size2 (b),
              traits::matrix_storage (a),
              traits::leading_dimension (a),
              traits::matrix_storage (b),
              traits::leading_dimension (b),
              &info);
        return info;
      }

    }

    template <typename SymmMatrA, typename MatrB>
    inline
    int posv (char const uplo, SymmMatrA& a, MatrB& b) {

      assert (uplo == 'U' || uplo == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmMatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      return detail::posv (uplo, a, b);
    }

    template <typename SymmMatrA, typename MatrB>
    inline
    int posv (SymmMatrA& a, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef traits::matrix_traits<SymmMatrA> matraits;
      typedef typename matraits::value_type val_t;
      BOOST_STATIC_ASSERT( (traits::detail::symm_herm_compatible< val_t, typename matraits::matrix_structure >::value ) ) ;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      return detail::posv (uplo, a, b);
    }

    /*
     * potrf() computes the Cholesky factorization of a symmetric
     * or Hermitian positive definite matrix A. The factorization has
     * the form
     *   A = U^T * U or A = U^H * U,  if UPLO = 'U',
     *   A = L * L^T or A = L * L^H,  if UPLO = 'L',
     * where U is an upper triangular matrix and L is lower triangular.
     */

    namespace detail {

      inline
      void potrf (char const uplo, int const n,
                  float* a, int const lda, int* info)
      {
        LAPACK_SPOTRF (&uplo, &n, a, &lda, info);
      }

      inline
      void potrf (char const uplo, int const n,
                  double* a, int const lda, int* info)
      {
        LAPACK_DPOTRF (&uplo, &n, a, &lda, info);
      }

      inline
      void potrf (char const uplo, int const n,
                  traits::complex_f* a, int const lda, int* info)
      {
        LAPACK_CPOTRF (&uplo, &n, traits::complex_ptr (a), &lda, info);
      }

      inline
      void potrf (char const uplo, int const n,
                  traits::complex_d* a, int const lda, int* info)
      {
        LAPACK_ZPOTRF (&uplo, &n, traits::complex_ptr (a), &lda, info);
      }

      template <typename SymmMatrA>
      inline
      int potrf (char const uplo, SymmMatrA& a) {
        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        int info;
        potrf (uplo, n, traits::matrix_storage (a),
               traits::leading_dimension (a), &info);
        return info;
      }

    }

    template <typename SymmMatrA>
    inline
    int potrf (char const uplo, SymmMatrA& a) {

      assert (uplo == 'U' || uplo == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmMatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      return detail::potrf (uplo, a);
    }

    template <typename SymmMatrA>
    inline
    int potrf (SymmMatrA& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef traits::matrix_traits<SymmMatrA> matraits;
      typedef typename matraits::value_type val_t;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename matraits::matrix_structure,
        typename traits::detail::symm_herm_t<val_t>::type
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      return detail::potrf (uplo, a);
    }

    /*
     * potrs() solves a system of linear equations A*X = B with
     * a symmetric or Hermitian positive definite matrix A using
     * the Cholesky factorization computed by potrf().
     */

    namespace detail {

      inline
      void potrs (char const uplo, int const n, int const nrhs,
                  float const* a, int const lda,
                  float* b, int const ldb, int* info)
      {
        LAPACK_SPOTRS (&uplo, &n, &nrhs, a, &lda, b, &ldb, info);
      }

      inline
      void potrs (char const uplo, int const n, int const nrhs,
                  double const* a, int const lda,
                  double* b, int const ldb, int* info)
      {
        LAPACK_DPOTRS (&uplo, &n, &nrhs, a, &lda, b, &ldb, info);
      }

      inline
      void potrs (char const uplo, int const n, int const nrhs,
                  traits::complex_f const* a, int const lda,
                  traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CPOTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (a), &lda,
                       traits::complex_ptr (b), &ldb, info);
      }

      inline
      void potrs (char const uplo, int const n, int const nrhs,
                  traits::complex_d const* a, int const lda,
                  traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZPOTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (a), &lda,
                       traits::complex_ptr (b), &ldb, info);
      }

      template <typename SymmMatrA, typename MatrB>
      inline
      int potrs (char const uplo, SymmMatrA const& a, MatrB& b) {
        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));
        int info;
        potrs (uplo, n, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::matrix_storage (a),
#else
               traits::matrix_storage_const (a),
#endif
               traits::leading_dimension (a),
               traits::matrix_storage (b),
               traits::leading_dimension (b),
               &info);
        return info;
      }

    }

    template <typename SymmMatrA, typename MatrB>
    inline
    int potrs (char const uplo, SymmMatrA const& a, MatrB& b) {

      assert (uplo == 'U' || uplo == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmMatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      return detail::potrs (uplo, a, b);
    }

    template <typename SymmMatrA, typename MatrB>
    inline
    int potrs (SymmMatrA const& a, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef traits::matrix_traits<SymmMatrA> matraits;
      typedef traits::matrix_traits<MatrB> mbtraits;
      typedef typename matraits::value_type val_t;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename matraits::matrix_structure,
        typename traits::detail::symm_herm_t<val_t>::type
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename mbtraits::matrix_structure, traits::general_t
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      return detail::potrs (uplo, a, b);
    }

    // TO DO: potri()

  }

}}}

#endif


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file linear_programming_test.cpp
 * @brief
 * @author Piotr Godlewski
 * @version 1.0
 * @date 2014-04-07
 */

#include "test_utils/logger.hpp"

#include "paal/lp/glp.hpp"
#include "paal/utils/floating.hpp"
#include "paal/utils/irange.hpp"

#include <boost/mpl/list.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/test/unit_test.hpp>

#include <unordered_map>
#include <vector>

using namespace paal;

using lp_types = boost::mpl::list<lp::glp>;

template <typename LP>
void log_solution(lp::problem_type status, const LP &lp_instance) {
    if (status == lp::OPTIMAL) {
        LOGLN("Optimal solution cost: " << lp_instance.get_obj_value());
        for (auto column : lp_instance.get_columns()) {
            boost::ignore_unused_variable_warning(column);
            LOGLN(lp_instance.get_col_name(column)
                  << " = " << lp_instance.get_col_value(column));
        }
    } else {
        LOGLN("Optimal solution not found");
    }
}

BOOST_AUTO_TEST_SUITE(linear_programming_test)

BOOST_AUTO_TEST_CASE_TEMPLATE(linear_programming_example, LP, lp_types) {
    // sample problem
    LP lp_instance;

    lp_instance.set_optimization_type(lp::MAXIMIZE);
    auto X = lp_instance.add_column(500, 0, lp::lp_traits::PLUS_INF, "x");
    auto Y = lp_instance.add_column(300, 0, lp::lp_traits::PLUS_INF, "y");

    auto expr = X + Y;
    lp_instance.add_row(expr >= 7);
    auto row = lp_instance.add_row(expr <= 10);
    lp_instance.add_row(15 <= 200 * X + 100 * Y <= 1200);

    // solve it
    auto status = lp_instance.solve_simplex();
    log_solution(status, lp_instance);

    // add new row
    expr += Y;
    lp_instance.add_row(expr == 12);

    // resolve it
    status = lp_instance.resolve_simplex(lp::DUAL);
    log_solution(status, lp_instance);

    // delete row
    lp_instance.delete_row(row);

    // resolve it
    status = lp_instance.resolve_simplex();
    log_solution(status, lp_instance);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(linear_programming_expressions, LP, lp_types) {
    LP lp_instance;

    lp_instance.set_optimization_type(lp::MAXIMIZE);
    auto X = lp_instance.add_column(500);
    auto Y = lp_instance.add_column(300);
    auto Z = lp_instance.add_column(150);
    auto T = lp_instance.add_column(200);

    auto expr1 = X + Y + 2 * T;
    expr1 -= Y;
    auto expr2 = 2 * expr1 - 3 * Z;
    expr1 += Y * 0.5;
    expr2 += T;
    auto expr3 = expr2 * 0.1 - (Y * 0.3 + 2 * X) * 0.5 + 1 * expr1;

    auto row1 = lp_instance.add_row(expr1 >= 7);
    auto row2 = lp_instance.add_row(expr1 <= 10);
    auto row3 = lp_instance.add_row(expr2 == 8.5);
    auto row4 = lp_instance.add_row(-10 <= expr3 <= 20);

    expr1 = lp::linear_expression();
    expr1 += X;
    expr1 -= T * 1;
    expr1 = expr1 + Z;
    expr1 = T + expr1;

    auto row5 = lp_instance.add_row(expr1 <= 100);

    auto U = lp_instance.add_column(-50);

    auto expr4 =
        -U + U + 0.2 * (X * 7 + (U + X) * 3) + U + 2 * U + 5 * X + (T - T) * 11;

    auto row6 = lp_instance.add_row(expr4 >= 3);

    lp_instance.set_row_expression(
        row2, lp_instance.get_row_expression(row2) + 5 * U);
    lp_instance.set_row_expression(
        row3, lp_instance.get_row_expression(row3) - 2 * U);

    // solve it
    lp_instance.solve_simplex();

    std::vector<lp::col_id> columns = { X, Y, Z, T, U };
    std::vector<lp::row_id> rows = { row1, row2, row3, row4, row5, row6 };
    std::vector<std::vector<double>> coefs = {
        { 1, 0.5, 0, 2, 0 }, { 1, 0.5, 0, 2, 5 }, { 2, 0, -3, 5, -2 },
        { 0.2, 0.35, -0.3, 2.5, 0 }, { 1, 0, 1, 0, 0 }, { 7, 0, 0, 0, 3.6 },
    };
    std::vector<int> row_degrees = { 3, 4, 4, 4, 2, 2 };
    std::vector<int> col_degrees = { 6, 3, 3, 4, 3 };
    const double MAX = lp::lp_traits::PLUS_INF;
    const double MIN = lp::lp_traits::MINUS_INF;
    std::vector<double> lower_bounds = { 7, MIN, 8.5, -10, MIN, 3 };
    std::vector<double> upper_bounds = { MAX, 10, 8.5, 20, 100, MAX };

    for (int row_num : paal::irange(rows.size())) {
        auto expr = lp_instance.get_row_expression(rows[row_num]);
        for (int col_num : paal::irange(columns.size())) {
            BOOST_CHECK_SMALL(expr.get_coefficient(columns[col_num]) -
                                  coefs[row_num][col_num],
                              std::numeric_limits<double>::epsilon());
        }
        if (lower_bounds[row_num] == MIN) {
            BOOST_CHECK(lp_instance.get_row_lower_bound(rows[row_num]) ==
                        lower_bounds[row_num]);
        } else {
            BOOST_CHECK_SMALL(lp_instance.get_row_lower_bound(rows[row_num]) -
                                  lower_bounds[row_num],
                              std::numeric_limits<double>::epsilon());
        }
        if (upper_bounds[row_num] == MAX) {
            BOOST_CHECK(lp_instance.get_row_upper_bound(rows[row_num]) ==
                        upper_bounds[row_num]);
        } else {
            BOOST_CHECK_SMALL(lp_instance.get_row_upper_bound(rows[row_num]) -
                                  upper_bounds[row_num],
                              std::numeric_limits<double>::epsilon());
        }
        BOOST_CHECK_EQUAL(lp_instance.get_row_degree(rows[row_num]),
                          row_degrees[row_num]);
    }

    for (int col_num : paal::irange(columns.size())) {
        BOOST_CHECK_EQUAL(lp_instance.get_col_degree(columns[col_num]),
                          col_degrees[col_num]);
    }
}

template <typename LP>
void run_single_solve_test(lp::simplex_type solve_type,
                           lp::simplex_type resolve_type) {
    LP lp("test instance", lp::MAXIMIZE);
    auto X = lp.add_column(500, 0, lp::lp_traits::PLUS_INF, "x");
    auto Y = lp.add_column(300, 0, lp::lp_traits::PLUS_INF, "y");
    lp.add_row(10 >= X + Y >= 7);
    lp.add_row(200 * X + 100 * Y <= 1200);

    auto status = lp.solve_simplex(solve_type);
    BOOST_CHECK_EQUAL(status, lp::OPTIMAL);
    BOOST_CHECK_SMALL(lp.get_obj_value() - 3400,
                      std::numeric_limits<double>::epsilon());
    BOOST_CHECK_SMALL(lp.get_col_value(X) - 2,
                      4 * std::numeric_limits<double>::epsilon());
    BOOST_CHECK_SMALL(lp.get_col_value(Y) - 8,
                      10 * std::numeric_limits<double>::epsilon());

    lp.add_row(12 >= X + 2 * Y);

    status = lp.resolve_simplex(resolve_type);
    BOOST_CHECK_EQUAL(status, lp::OPTIMAL);
    BOOST_CHECK_SMALL(lp.get_obj_value() - 3200,
                      std::numeric_limits<double>::epsilon());
    BOOST_CHECK_SMALL(lp.get_col_value(X) - 4,
                      std::numeric_limits<double>::epsilon());
    BOOST_CHECK_SMALL(lp.get_col_value(Y) - 4,
                      std::numeric_limits<double>::epsilon());
}

BOOST_AUTO_TEST_CASE_TEMPLATE(linear_programming_solve, LP, lp_types) {
    run_single_solve_test<LP>(lp::PRIMAL, lp::PRIMAL);
    run_single_solve_test<LP>(lp::PRIMAL, lp::DUAL);
    run_single_solve_test<LP>(lp::DUAL, lp::PRIMAL);
    run_single_solve_test<LP>(lp::DUAL, lp::DUAL);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(linear_programming_methods, LP, lp_types) {
    LP lp("test instance", lp::MAXIMIZE);
    // add rows and colums
    auto X = lp.add_column(12, -9, 2.5, "x");
    auto Y = lp.add_column(33, 5, lp::lp_traits::PLUS_INF, "y");
    auto row1 = lp.add_row(X + 3.5 * Y, "row1");
    auto row2 = lp.add_row(0 >= (Y + 2 * X) * 0.1, "row2");
    auto row3 = lp.add_row(1 <= 0.04 * X + 23.7 * Y, "row3");
    auto row4 = lp.add_row(-1 <= Y + 0.2 * X <= 11.2, "row4");

    BOOST_CHECK_EQUAL(lp.columns_number(), 2);
    std::unordered_map<lp::col_id, std::string> columns(
        { { X, "x" }, { Y, "y" } });
    for (auto col : lp.get_columns()) {
        BOOST_CHECK(columns.find(col) != columns.end());
        BOOST_CHECK_EQUAL(lp.get_col_name(col), columns.find(col)->second);
        BOOST_CHECK_EQUAL(lp.get_col_degree(col), 4);
        BOOST_CHECK_EQUAL(boost::distance(lp.get_rows_in_column(col)), 4);
    }

    BOOST_CHECK_EQUAL(lp.rows_number(), 4);
    std::unordered_map<lp::row_id, std::string> rows(
        { { row1, "row1" }, { row2, "row2" }, { row3, "row3" },
          { row4, "row4" } });
    for (auto row : lp.get_rows()) {
        BOOST_CHECK(rows.find(row) != rows.end());
        BOOST_CHECK_EQUAL(lp.get_row_name(row), rows.find(row)->second);
        BOOST_CHECK_EQUAL(lp.get_row_degree(row), 2);
        BOOST_CHECK_EQUAL(lp.get_row_expression(row).non_zeros(), 2);
    }

    // delete some rows and colums
    lp.delete_row(row2);
    BOOST_CHECK_EQUAL(lp.rows_number(), 3);

    lp.delete_col(X);
    BOOST_CHECK_EQUAL(lp.columns_number(), 1);

    lp.delete_row(row3);
    BOOST_CHECK_EQUAL(lp.rows_number(), 2);

    columns = std::unordered_map<lp::col_id, std::string>({ { Y, "y" } });
    for (auto col : lp.get_columns()) {
        BOOST_CHECK(columns.find(col) != columns.end());
        BOOST_CHECK_EQUAL(lp.get_col_name(col), columns.find(col)->second);
        BOOST_CHECK_EQUAL(lp.get_col_degree(col), 2);
        BOOST_CHECK_EQUAL(boost::distance(lp.get_rows_in_column(col)), 2);
    }

    rows = std::unordered_map<lp::row_id, std::string>(
        { { row1, "row1" }, { row4, "row4" } });
    for (auto row : lp.get_rows()) {
        BOOST_CHECK(rows.find(row) != rows.end());
        BOOST_CHECK_EQUAL(lp.get_row_name(row), rows.find(row)->second);
        BOOST_CHECK_EQUAL(lp.get_row_degree(row), 1);
        BOOST_CHECK_EQUAL(lp.get_row_expression(row).non_zeros(), 1);
    }

    // set new bounds and costs
    lp.set_row_lower_bound(row1, 7);
    lp.set_row_upper_bound(row1, 10);
    lp.set_row_lower_bound(row4, lp::lp_traits::MINUS_INF);
    lp.set_row_upper_bound(row4, 12);

    lp.set_col_lower_bound(Y, 0);
    lp.set_col_upper_bound(Y, lp::lp_traits::PLUS_INF);
    lp.set_col_cost(Y, 30);

    // add new column
    auto Z = lp.add_column(50, 0, lp::lp_traits::PLUS_INF, "z");

    // set row linear expressions
    lp.set_row_expression(row1, Y + Z);
    lp.set_row_expression(row4, 2 * Z + Y);

    // solve
    auto status = lp.solve_simplex(lp::PRIMAL);
    BOOST_CHECK_EQUAL(status, lp::OPTIMAL);
    BOOST_CHECK_SMALL(lp.get_obj_value() - 340,
                      std::numeric_limits<double>::epsilon());
    BOOST_CHECK_SMALL(lp.get_row_sum(row1) - 10,
                      std::numeric_limits<double>::epsilon());
    BOOST_CHECK_SMALL(lp.get_row_sum(row4) - 12,
                      std::numeric_limits<double>::epsilon());
}

BOOST_AUTO_TEST_CASE_TEMPLATE(linear_programming_zeros, LP, lp_types) {
    LP lp("test instance", lp::MAXIMIZE);
    auto X = lp.add_column();
    auto Y = lp.add_column();
    auto Z = lp.add_column();
    auto expr = 1e-8 * X + Y;
    BOOST_CHECK_EQUAL(expr.non_zeros(), 2);
    expr += 0 * Z;
    BOOST_CHECK_EQUAL(expr.non_zeros(), 2);
    expr *= 1e-8;
    BOOST_CHECK_EQUAL(expr.non_zeros(), 1);

    expr = (1e-8 * X + Y) / 1e8;
    BOOST_CHECK_EQUAL(expr.non_zeros(), 1);

    expr = X + Y - Y;
    BOOST_CHECK_EQUAL(expr.non_zeros(), 1);
}

BOOST_AUTO_TEST_SUITE_END()


#include "Plane.h"
#include "Ray.h"
#include "Intersection.h"

#include <Eigen/Dense>

std::unique_ptr<Intersection> Plane::intersect(const Ray& ray) {
    auto p = ray.p;
    auto d = ray.d;

    if (n.dot(d) > 0.0f) {
        return nullptr;
    } else {
        auto a = this->p;
        float t = (a-p).dot(n) / d.dot(n);
        return std::unique_ptr<Intersection>(new Intersection(this, n, t));
    }
}


/*=================================================================
*
* compute various kinds of Laplacian weight
*
* usage:
		L = perform_mesh_weight(verts, faces, type, options);
* inputs:
		verts: 3*nverts
		faces: 3*faces
		type: type is either
			%       0: 'combinatorial' or 'graph': W(i,j)=1 is vertex i is conntected to vertex j.
			%       1: 'distance': W(i,j) = 1/d_ij^2 where d_ij is distance between vertex
			%           i and j.
			%       2: 'spring': W(i,j) = 1/d_ij where d_ij is distance between vertex
			%           i and j.
			%       3: 'conformal' or 'dcp': W(i,j) = (cot(alpha_ij)+cot(beta_ij))/2 where alpha_ij and
			%           beta_ij are the adjacent angle to edge (i,j). (do not offer W(i,j) = cot(alpha_ij)+cot(beta_ij) anymore)
			%           Refer to Computing discrete minimal surfaces and their conjugates_93,
			%           Lemma 2 of On the convergence of metric and geometric properties of polyhedral surfaces_06 and
			%           Characterizing Shape Using Conformal Factors_08.
			%           Refer to Skeleton Extraction by Mesh Extraction_08, and Intrinsic Parameterizations of Surface Meshes_02.
			%       4: 'Mean_curvature' or 'Laplace-Beltrami': W(i,j) = (1/area_i)*(cot(alpha_ij)+cot(beta_ij))/2 where alpha_ij and
			%           beta_ij are the adjacent angle to edge (i,j), area_i is the area of vertex i's Voroni vicinity.
			%           Refer to Discrete Differential-Geometry Operators_for triangulated 2-manifolds_02
			%       5: 'Manifold-harmonic': W(i,j) = (1/sqrt(area_i*area_j))*(cot(alpha_ij)+cot(beta_ij))/2 where alpha_ij and
			%           beta_ij are the adjacent angle to edge (i,j).
			%           Refer to Spectral Geometry Processing with Manifold Harmonics_08
			%       6: 'mvc': W(i,j) = [tan(/_kij/2)+tan(/_jil/2)]/d_ij where /_kij and /_jil are angles at i
		options.?:
*
*           4 or 5 can be built on 3 in matlab.
*           just 0 & 3 are handled here. 1, 2 and 6 are left as todo.
*
* JJCAO, 2013
*
*=================================================================*/

#include <mex.h>
#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <vector>

using namespace Eigen;
using namespace std;

typedef Triplet<double> T;
typedef SparseMatrix<double>::Index Index;
typedef SparseMatrix<double>::Scalar Scalar;

/// Return cotangent of (P,Q,R) corner (ie cotan of QP,QR angle).
double cotangent(const Vector3d& P,
                    const Vector3d& Q,
                    const Vector3d& R)
{

    Vector3d u = P - Q;
    Vector3d v = R - Q;
    // (u . v)/((u x v).len)
    double dot = u.dot(v);
    Vector3d cross_vector = u.cross(v);
    double cross_norm = std::sqrt(cross_vector.dot(cross_vector));
    if(cross_norm != 0.0)
        return (dot/cross_norm);
    else
        return 0.0; // undefined
}

////                                                  -> ->
///// Return tangent of (P,Q,R) corner (ie tangent of QP,QR angle).
//double tangent(const Point_3& P,
//                const Point_3& Q,
//                const Point_3& R)
//{
//    Vector_3 u = P - Q;
//    Vector_3 v = R - Q;
//    // (u . v)/((u x v).len)
//    double dot = (u*v);
//    CGAL_surface_mesh_parameterization_assertion(dot != 0.0);
//    Vector_3 cross_vector = CGAL::cross_product(u,v);
//    double cross_norm = std::sqrt(cross_vector*cross_vector);
//    if(dot != 0.0)
//        return (cross_norm/dot);
//    else
//        return 0.0; // undefined
//}

void compute_dcp_weight(double* verts, int* faces, int nverts, int nfaces, SparseMatrix<double>& sm)
{
	int i,j,m;
	double dtmp;
	std::vector<T> coef(nverts*6);
	for (int k = 0; k < nfaces; ++k)
	{
		int tmp = 3*k;
		int face[3] = {faces[tmp],faces[tmp+1],faces[tmp+2]};
		for (int l = 0; l < 3; ++l)
		{
			i = face[l]; j = face[(l+1)%3]; m = face[(l+2)%3];
			Vector3d p(verts[3*i],verts[3*i+1],verts[3*i+2]);
			Vector3d q(verts[3*m],verts[3*m+1],verts[3*m+2]);
			Vector3d r(verts[3*j],verts[3*j+1],verts[3*j+2]);
			dtmp = 0.5*cotangent(p, q, r);
			coef.push_back(T(i,j,dtmp));
		}
	}
	{
		SparseMatrix<double> smtmp(nverts, nverts);
		smtmp.setFromTriplets(coef.begin(), coef.end());
		sm = SparseMatrix<double>(smtmp.transpose()) + smtmp;
	}
}

void perform_mesh_weight(double* verts, int nverts, int *faces, int nfaces, int type, double *vert_areas, SparseMatrix<double>& sm)
{
	switch(type)
	{
	case 0: //'combinatorial' or 'graph'
		// ���ܴ�������
		{
		int i,j;
		std::vector<T> coef(nverts*6);
		for (int k = 0; k < nfaces; ++k)
		{
			int tmp = 3*k;
			int face[3] = {faces[tmp],faces[tmp+1],faces[tmp+2]};
			for (int l = 0; l < 3; ++l)
			{
				i = face[l]; j = face[(l+1)%3];
				coef.push_back(T(i,j,1));
			}
		}
		sm.setFromTriplets(coef.begin(), coef.end());
		//sm.coeffRef(1,1) = 2;sm.coeffRef(2,2) = 3;//sm.coeffRef(0,0) = 1;
		}
		break;
	case 3: //'conformal' or 'dcp'
		compute_dcp_weight(verts, faces, nverts, nfaces, sm);
		break;
	//case 4: // 'Mean_curvature' or 'Laplace-Beltrami'
	//	//if (vert_areas == 0)
	//	//{
	//	//	stringstream ss("options.vert_areas is not offered! ");
	//	//	mexErrMsgTxt(ss.str().c_str());
	//	//}
	//	compute_dcp_weight(verts, faces, nverts, nfaces, sm);
	//	//{
	//	//	for ( int i = 0; i < sm.rows(); ++i)
	//	//	{
	//	//		sm.row(i) = sm.row(i) * (1.0/vert_areas[i]);
	//	//	}
	//	//}
	//	break;
	default:
		stringstream ss("type: ");
		ss << type << " is not supported!";
		mexErrMsgTxt(ss.str().c_str());
	}

	sm.makeCompressed();
}

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray*prhs[])
{
	///////////// Error Check
	if ( nrhs < 3)
		mexErrMsgTxt("Number of input should be > 2");
	if (1 != nlhs)
		mexErrMsgTxt("Number of output should be 1");

	///////////// input & output arguments
	// input 0: verts: 3*nverts
	int row = mxGetM(prhs[0]);
	int nverts = mxGetN(prhs[0]);
	if(row != 3)
		mexErrMsgTxt("The mesh must be triangle mesh! it is excepted to be 3*n");

	double *verts = mxGetPr(prhs[0]);

	// input 1: faces: 3*nfaces
	row = mxGetM(prhs[1]);
	int nfaces = mxGetN(prhs[1]);
	if(row != 3)
		mexErrMsgTxt("The mesh must be triangle mesh! it is excepted to be 3*n");

	double* dfaces = mxGetPr(prhs[1]);
    int* faces = new int[nfaces*3];
    for(int i = 0; i < nfaces*3; ++i)
    {
        faces[i] = int(dfaces[i]);
		--faces[i];
    }

	// input 3: type
	double* type = mxGetPr(prhs[2]);

	// input 4: options
	double* vert_areas(0);
	if ( nrhs > 3)
	{
		mxArray* tmp;
		const mxArray *options = prhs[3];
		if ( mxSTRUCT_CLASS != mxGetClassID(options))
			mexErrMsgTxt("4th arguments is not a structure!");
		else
		{
			// options.vert_areas: 1*nverts
			tmp = mxGetField(options,0,"vert_areas");
			if (tmp)
				vert_areas = mxGetPr(tmp);// not used!
			//mexPrintf("%f, %f, %f\n", vert_areas[0],vert_areas[1],vert_areas[2]);
		}
	}

	///////////////////////////////////////////////
	SparseMatrix<double> sm(nverts, nverts);
	perform_mesh_weight(verts, nverts, faces, nfaces, *type, vert_areas, sm);

	SparseMatrix<double>::StorageIndex* innerInd = sm.innerIndexPtr();
	SparseMatrix<double>::StorageIndex* outerInd = sm.outerIndexPtr();
	Scalar* valuePtr = sm.valuePtr();

	///////////////////////////////////////////////
	// output 0
	//plhs[0] = mxCreateDoubleMatrix( nverts, nverts, mxREAL);
    mwSize nzmax= sm.nonZeros();
	plhs[0] = mxCreateSparse( nverts, nverts, nzmax, mxREAL);
	double *L = mxGetPr(plhs[0]);
	mwIndex *irs = mxGetIr(plhs[0]);//row index
    mwIndex *jcs = mxGetJc(plhs[0]);//column index

	for (int k = 0; k<nzmax; ++k)
	{
		//double dtmp = valuePtr[k];
		//long ltmp = innerInd[k];
		L[k] = valuePtr[k];
		irs[k] = innerInd[k];
		//mexPrintf("%f %f\n", L[k], irs[k]);
	}

	for (int k = 0; k<nverts; ++k)
	{
		//long ltmp = outerInd[k];
		jcs[k] = outerInd[k];
	}
	//long ltmp = outerInd[nverts];
	jcs[nverts]=outerInd[nverts];

    delete[] faces;
}


// Copyright (c) 2020, Viktor Larsson
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of the copyright holder nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "univariate.h"

#include <Eigen/Eigen>
#include <complex>

namespace poselib {
namespace univariate {
/* Solves the quadratic equation a*x^2 + b*x + c = 0 */
void solve_quadratic(double a, double b, double c, std::complex<double> roots[2]) {

    std::complex<double> b2m4ac = b * b - 4 * a * c;
    std::complex<double> sq = std::sqrt(b2m4ac);

    // Choose sign to avoid cancellations
    roots[0] = (b > 0) ? (2 * c) / (-b - sq) : (2 * c) / (-b + sq);
    roots[1] = c / (a * roots[0]);
}

/* Solves the quadratic equation a*x^2 + b*x + c = 0 */
int solve_quadratic_real(double a, double b, double c, double roots[2]) {

    double b2m4ac = b * b - 4 * a * c;
    if (b2m4ac < 0)
        return 0;

    double sq = std::sqrt(b2m4ac);

    // Choose sign to avoid cancellations
    roots[0] = (b > 0) ? (2 * c) / (-b - sq) : (2 * c) / (-b + sq);
    roots[1] = c / (a * roots[0]);

    return 2;
}

/* Sign of component with largest magnitude */
inline double sign2(const std::complex<double> z) {
    if (std::abs(z.real()) > std::abs(z.imag()))
        return z.real() < 0 ? -1.0 : 1.0;
    else
        return z.imag() < 0 ? -1.0 : 1.0;
}

/* Sign of component with largest magnitude */
inline double sign(const double z) { return z < 0 ? -1.0 : 1.0; }

void solve_cubic_single_real(double c2, double c1, double c0, double &root) {
    double a = c1 - c2 * c2 / 3.0;
    double b = (2.0 * c2 * c2 * c2 - 9.0 * c2 * c1) / 27.0 + c0;
    double c = b * b / 4.0 + a * a * a / 27.0;
    if (c > 0) {
        c = std::sqrt(c);
        b *= -0.5;
        root = std::cbrt(b + c) + std::cbrt(b - c) - c2 / 3.0;
    } else {
        c = 3.0 * b / (2.0 * a) * std::sqrt(-3.0 / a);
        root = 2.0 * std::sqrt(-a / 3.0) * std::cos(std::acos(c) / 3.0) - c2 / 3.0;
    }
}

int solve_cubic_real(double c2, double c1, double c0, double roots[3]) {
    double a = c1 - c2 * c2 / 3.0;
    double b = (2.0 * c2 * c2 * c2 - 9.0 * c2 * c1) / 27.0 + c0;
    double c = b * b / 4.0 + a * a * a / 27.0;
    int n_roots;
    if (c > 0) {
        c = std::sqrt(c);
        b *= -0.5;
        roots[0] = std::cbrt(b + c) + std::cbrt(b - c) - c2 / 3.0;
        n_roots = 1;
    } else {
        c = 3.0 * b / (2.0 * a) * std::sqrt(-3.0 / a);
        double d = 2.0 * std::sqrt(-a / 3.0);
        roots[0] = d * std::cos(std::acos(c) / 3.0) - c2 / 3.0;
        roots[1] = d * std::cos(std::acos(c) / 3.0 - 2.09439510239319526263557236234192) - c2 / 3.0; // 2*pi/3
        roots[2] = d * std::cos(std::acos(c) / 3.0 - 4.18879020478639052527114472468384) - c2 / 3.0; // 4*pi/3
        n_roots = 3;
    }

    // single newton iteration
    for (int i = 0; i < n_roots; ++i) {
        double x = roots[i];
        double x2 = x * x;
        double x3 = x * x2;
        double dx = -(x3 + c2 * x2 + c1 * x + c0) / (3 * x2 + 2 * c2 * x + c1);
        roots[i] += dx;
    }
    return n_roots;
}

/* Solves the quartic equation x^4 + b*x^3 + c*x^2 + d*x + e = 0 */
void solve_quartic(double b, double c, double d, double e, std::complex<double> roots[4]) {

    // Find depressed quartic
    std::complex<double> p = c - 3.0 * b * b / 8.0;
    std::complex<double> q = b * b * b / 8.0 - 0.5 * b * c + d;
    std::complex<double> r = (-3.0 * b * b * b * b + 256.0 * e - 64.0 * b * d + 16.0 * b * b * c) / 256.0;

    // Resolvent cubic is now
    // U^3 + 2*p U^2 + (p^2 - 4*r) * U - q^2
    std::complex<double> bb = 2.0 * p;
    std::complex<double> cc = p * p - 4.0 * r;
    std::complex<double> dd = -q * q;

    // Solve resolvent cubic
    std::complex<double> d0 = bb * bb - 3.0 * cc;
    std::complex<double> d1 = 2.0 * bb * bb * bb - 9.0 * bb * cc + 27.0 * dd;

    std::complex<double> C3 = (d1.real() < 0) ? (d1 - sqrt(d1 * d1 - 4.0 * d0 * d0 * d0)) / 2.0
                                              : (d1 + sqrt(d1 * d1 - 4.0 * d0 * d0 * d0)) / 2.0;

    std::complex<double> C;
    if (C3.real() < 0)
        C = -std::pow(-C3, 1.0 / 3);
    else
        C = std::pow(C3, 1.0 / 3);

    std::complex<double> u2 = (bb + C + d0 / C) / -3.0;

    // std::complex<double> db = u2 * u2 * u2 + bb * u2 * u2 + cc * u2 + dd;

    std::complex<double> u = sqrt(u2);

    std::complex<double> s = -u;
    std::complex<double> t = (p + u * u + q / u) / 2.0;
    std::complex<double> v = (p + u * u - q / u) / 2.0;

    roots[0] = (-u - sign2(u) * sqrt(u * u - 4.0 * v)) / 2.0;
    roots[1] = v / roots[0];
    roots[2] = (-s - sign2(s) * sqrt(s * s - 4.0 * t)) / 2.0;
    roots[3] = t / roots[2];

    for (int i = 0; i < 4; i++) {
        roots[i] = roots[i] - b / 4.0;

        // do one step of newton refinement
        std::complex<double> x = roots[i];
        std::complex<double> x2 = x * x;
        std::complex<double> x3 = x * x2;
        std::complex<double> dx =
            -(x2 * x2 + b * x3 + c * x2 + d * x + e) / (4.0 * x3 + 3.0 * b * x2 + 2.0 * c * x + d);
        roots[i] = x + dx;
    }
}

/* Solves the quartic equation x^4 + b*x^3 + c*x^2 + d*x + e = 0 */
int solve_quartic_real(double b, double c, double d, double e, double roots[4]) {

    // Find depressed quartic
    double p = c - 3.0 * b * b / 8.0;
    double q = b * b * b / 8.0 - 0.5 * b * c + d;
    double r = (-3.0 * b * b * b * b + 256.0 * e - 64.0 * b * d + 16.0 * b * b * c) / 256.0;

    // Resolvent cubic is now
    // U^3 + 2*p U^2 + (p^2 - 4*r) * U - q^2
    double bb = 2.0 * p;
    double cc = p * p - 4.0 * r;
    double dd = -q * q;

    // Solve resolvent cubic
    double u2;
    solve_cubic_single_real(bb, cc, dd, u2);

    if (u2 < 0)
        return 0;

    double u = sqrt(u2);

    double s = -u;
    double t = (p + u * u + q / u) / 2.0;
    double v = (p + u * u - q / u) / 2.0;

    int sols = 0;
    double disc = u * u - 4.0 * v;
    if (disc > 0) {
        roots[0] = (-u - sign(u) * std::sqrt(disc)) / 2.0;
        roots[1] = v / roots[0];
        sols += 2;
    }
    disc = s * s - 4.0 * t;
    if (disc > 0) {
        roots[sols] = (-s - sign(s) * std::sqrt(disc)) / 2.0;
        roots[sols + 1] = t / roots[sols];
        sols += 2;
    }

    for (int i = 0; i < sols; i++) {
        roots[i] = roots[i] - b / 4.0;

        // do one step of newton refinement
        double x = roots[i];
        double x2 = x * x;
        double x3 = x * x2;
        double dx = -(x2 * x2 + b * x3 + c * x2 + d * x + e) / (4.0 * x3 + 3.0 * b * x2 + 2.0 * c * x + d);
        roots[i] = x + dx;
    }
    return sols;
}

}; // namespace univariate
}; // namespace poselib


#pragma once

#include <polyfem/Types.hpp>

#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace polyfem {

	// Show some stats about the matrix M: det, singular values, condition number, etc
	void show_matrix_stats(const Eigen::MatrixXd &M);

	template<typename T>
	T determinant(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, 0, 3, 3> &mat)
	{
		assert(mat.rows() == mat.cols());

		if(mat.rows() == 1)
			return mat(0);
		else if(mat.rows() == 2)
			return mat(0, 0) * mat(1, 1) - mat(0, 1) * mat(1, 0);
		else if(mat.rows() == 3)
			return mat(0,0)*(mat(1,1)*mat(2,2)-mat(1,2)*mat(2,1))-mat(0,1)*(mat(1,0)*mat(2,2)-mat(1,2)*mat(2,0))+mat(0,2)*(mat(1,0)*mat(2,1)-mat(1,1)*mat(2,0));

		assert(false);
		return T(0);
	}

    template<typename T>
	void read_matrix(const std::string &path, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &mat);

	Eigen::Vector4d compute_specturm(const StiffnessMatrix &mat);

} // namespace polyfem


#include "geometrycentral/stripes.h"
#include <Eigen/SparseCholesky>

// ONLY WORKS FOR MESHES WITHOUT BOUNDARY
Stripes::Stripes(HalfedgeMesh* m, Geometry<Euclidean>* g) : mesh(m), geom(g), phi(m), r(m), field(m),
                                                                singularities(m), branchCover(m), omega(m) {
    assert(mesh->nBoundaryLoops() == 0);
}

void Stripes::setup() {
    VertexData<double> s(mesh);
    // Compute s_i at each vertex
    for (VertexPtr v : mesh->vertices()) {
        if (v.isBoundary()) {
            s[v] = 1.0;
        } else {
            double sum = 0;
            for (HalfedgePtr he : v.outgoingHalfedges()) {
                sum += geom->angle(he.next());
            }
            s[v] = 2*M_PI / sum;
        }
    }

    // Compute transport at edges r_ij <- e^ip_ij
    for (VertexPtr v : mesh->vertices()) {
        HalfedgePtr he = v.halfedge();
        double angle = 0;
        double s_i = s[v];
        do {
            phi[he] = angle;
            angle += s_i * geom->angle(he.next());
            he = he.next().next().twin();
        } while (he != v.halfedge());
    }

    // Compute r_ij
    std::complex<double> i(0, 1);
    for (VertexPtr v : mesh->vertices()) {
        for (HalfedgePtr he : v.outgoingHalfedges()) {
            double theta_ij = phi[he];
            double theta_ji = phi[he.twin()] + M_PI;
            double rho_ij = theta_ij - theta_ji;
            r[he] = std::exp(i * n * rho_ij);
        }
    }
}

Eigen::SparseMatrix<std::complex<double>> Stripes::assembleM() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<std::complex<double>> M(n,n);
    std::vector<Eigen::Triplet<std::complex<double>>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (FacePtr f : mesh->faces()) {
        HalfedgePtr he_ij = f.halfedge();
        size_t i = vertexIndices[he_ij.vertex()];
        size_t j = vertexIndices[he_ij.next().vertex()];
        size_t k = vertexIndices[he_ij.prev().vertex()];

        double area = geom->area(f);
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i, i, area/3.));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j, j, area/3.));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k, k, area/3.));
    }
    M.setFromTriplets(triplets.begin(),triplets.end());
    return M;
}

Eigen::SparseMatrix<std::complex<double>> Stripes::assembleA() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<std::complex<double>> A(n,n);
    std::vector<Eigen::Triplet<std::complex<double>>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (FacePtr f : mesh->faces()) {
        HalfedgePtr he_ij = f.halfedge();
        HalfedgePtr he_jk = f.halfedge().next();
        HalfedgePtr he_ki = f.halfedge().prev();

        size_t i = vertexIndices[he_ij.vertex()];
        size_t j = vertexIndices[he_jk.vertex()];
        size_t k = vertexIndices[he_ki.vertex()];

        double a = geom->cotan(he_jk);
        double b = geom->cotan(he_ki);
        double c = geom->cotan(he_ij);

        std::complex<double> r_ij = r[he_ij];
        std::complex<double> r_ji = r[he_ij.twin()];
        std::complex<double> r_jk = r[he_jk];
        std::complex<double> r_kj = r[he_jk.twin()];
        std::complex<double> r_ki = r[he_ki];
        std::complex<double> r_ik = r[he_ki.twin()];

        // row i
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i,i,b + c));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i,j,-c * r_ij));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i,k,-b * r_ik));

        // row j
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j,i,-c * r_ji));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j,j,c + a));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j,k,-a * r_jk));

        // row k
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k,i,-b * r_ki));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k,j,-a * r_kj));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k,k,a + b));
    }
    A.setFromTriplets(triplets.begin(),triplets.end());
    return A;
}

Eigen::MatrixXcd Stripes::principalEigenvector(Eigen::SparseMatrix<std::complex<double>> A, Eigen::SparseMatrix<std::complex<double>> B) {
    // LL^T <- Cholesky(A)
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<std::complex<double>>> solver;
    solver.compute(A);

    // u <- UniformRand(-1,1)
    Eigen::MatrixXcd x = Eigen::MatrixXcd::Random(mesh->nVertices(),1);

    // inverse power iteration to find eigenvector belonging to the smallest eigenvalue
    for (int i = 0; i < nPowerIterations; i++) {
        x = solver.solve(B * x);
        std::complex<double> norm2 = (x.transpose() * B * x)(0,0);
        x = x / sqrt(norm2);
    }
    return x;
}

Eigen::MatrixXcd Stripes::principalEigenvector(Eigen::SparseMatrix<double> A, Eigen::SparseMatrix<double> B) {
    // LL^T <- Cholesky(A)
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
    solver.compute(A);

    // u <- UniformRand(-1,1)
    Eigen::MatrixXcd x = Eigen::MatrixXcd::Random(2*mesh->nVertices(),1);

    // inverse power iteration to find eigenvector belonging to the smallest eigenvalue
    for (int i = 0; i < nPowerIterations; i++) {
        x = solver.solve(B * x);
        std::complex<double> norm2 = (x.transpose() * B * x)(0,0);
        x = x / sqrt(norm2);
    }
    return x;
}

VertexData<std::complex<double>> Stripes::computeField() {
    std::cout << "Computing Cross Field... ";
    // Algorithm 1 : Setup
    setup();

    // Algorithm 2 : Smoothest Field
    Eigen::SparseMatrix<std::complex<double>> A = assembleA();
    Eigen::SparseMatrix<std::complex<double>> M = assembleM();
    A = A + eps * M;
    Eigen::MatrixXcd x = principalEigenvector(A,M);

    // map resulting vector to VertexData
    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (VertexPtr v : mesh->vertices()) {
        std::complex<double> c = x(vertexIndices[v],0);
        if (std::abs(c) == 0) {
            field[v] = 0;
        } else {
            field[v] = c / std::abs(c);
        }
    }
    std::cout << "Done!" << std::endl;
    return field;
}

FaceData<int> Stripes::computeSingularities() {
    std::cout << "Computing Singularities... ";
    // first, compute Omega_ijk <- arg(r_ij r_jk r_ki)
    FaceData<double> Omega(mesh);
    for (FacePtr f : mesh->faces()) {
        std::complex<double> r_ij = r[f.halfedge()];
        std::complex<double> r_jk = r[f.halfedge().next()];
        std::complex<double> r_ki = r[f.halfedge().prev()];
        Omega[f] = std::arg(r_ij * r_jk * r_ki);
    }

    // next, compute w_ij for each e_ij, such that u_j = e^iw_ij * r_ij * u_i
    // w_ij = arg(u_j / (r_ij * u_i))
    HalfedgeData<double> w(mesh);
    for (HalfedgePtr he : mesh->allHalfedges()) {
        std::complex<double> u_i = field[he.vertex()];
        std::complex<double> u_j = field[he.twin().vertex()];
        std::complex<double> r_ij = r[he];
        w[he] = std::arg(u_j * r_ij / u_i);
    }

    // finally, compute index for each triangle t
    // (1/2pi) * (w_ij + w_jk + w_ki + Omega_ijk)
    int total = 0;
    for (FacePtr f : mesh->faces()) {
        double w_ij = w[f.halfedge()];
        double w_jk = w[f.halfedge().next()];
        double w_ki = w[f.halfedge().prev()];
        double Omega_ijk = Omega[f];
        double phi = (w_ij + w_jk + w_ki - Omega_ijk) / (2.0 * M_PI);
        singularities[f] = std::round(phi);
        total += singularities[f];
    }
    std::cout << "Sum: " << total << std::endl;
    return singularities;
}

void Stripes::edgeData() {
    std::cout<< "Computing EdgeData... ";
    std::complex<double> i(0, 1);
    for (EdgePtr e : mesh->edges()) {
        HalfedgePtr he_ij = e.halfedge();
        HalfedgePtr he_ji = he_ij.twin();

        // disambiguate the big vectors
        std::complex<double> f_ij = std::pow(field[he_ij.vertex()], 1.0 / n);
        std::complex<double> f_ji = std::pow(field[he_ji.vertex()], 1.0 / n);

        // we need to recompute r_ij here without raising to the nth power,
        // as raising to the nth power and then taking the nth root is not always an identity operation
        double theta_ij = phi[he_ij];
        double theta_ji = phi[he_ij.twin()] + M_PI;
        double rho_ij = theta_ji - theta_ij;
        std::complex<double> r_ij = std::exp(i * rho_ij);
        std::complex<double> s_ij = f_ji / (f_ij * r_ij);
        double ang = std::arg(s_ij);

        double sign;
        if ( ang >= -M_PI_2 && ang < M_PI_2 ) {
            sign = 1;
            branchCover[e] = 0;
        } else {
            sign = -1;
            branchCover[e] = 1;
        }

        double phi_i = std::arg(f_ij);
        double phi_j = std::arg(sign * f_ji);
        double l_ij = geom->length(e);
        omega[e] = lambda * (l_ij / 2.0) * (cos(phi_i - theta_ij) + cos(phi_j - theta_ji));
    }
    std::cout << "Done!" << std::endl;
}

Eigen::SparseMatrix<double> Stripes::EnergyMatrix() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<double> A(2*n,2*n);
    std::vector<Eigen::Triplet<double>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (EdgePtr e : mesh->edges()) {
        HalfedgePtr he_ij = e.halfedge();

        // cotan weights
        double cotA = geom->cotan(he_ij);
        double cotB = geom->cotan(he_ij.twin());
        if (singularities[he_ij.face()] != 0) cotA = 0;
        if (singularities[he_ij.twin().face()] != 0) cotB = 0;
        double w = (cotA + cotB) / 2.0;

        // indices
        size_t i = 2 * vertexIndices[he_ij.vertex()];
        size_t j = 2 * vertexIndices[he_ij.twin().vertex()];

        // add diagonal terms
        triplets.push_back( Eigen::Triplet<double>(i,i,w) );
        triplets.push_back( Eigen::Triplet<double>(i+1,i+1,w) );
        triplets.push_back( Eigen::Triplet<double>(j,j,w) );
        triplets.push_back( Eigen::Triplet<double>(j+1,j+1,w) );

        // transport coefficient components
        double x = w * cos(omega[e]);
        double y = w * sin(omega[e]);

        // stays on same sheet
        if (branchCover[e] == 0) {
            // A_ij
            triplets.push_back( Eigen::Triplet<double>(i,j,-x) ); triplets.push_back( Eigen::Triplet<double>(i,j+1,-y) );
            triplets.push_back( Eigen::Triplet<double>(i+1,j,y) ); triplets.push_back( Eigen::Triplet<double>(i+1,j+1,-x) );
            // A_ji
            triplets.push_back( Eigen::Triplet<double>(j,i,-x) ); triplets.push_back( Eigen::Triplet<double>(j,i+1,y) );
            triplets.push_back( Eigen::Triplet<double>(j+1,i,-y) ); triplets.push_back( Eigen::Triplet<double>(j+1,i+1,-x) );
        } else {
            // A_ij
            triplets.push_back( Eigen::Triplet<double>(i,j,-x) ); triplets.push_back( Eigen::Triplet<double>(i,j+1,y) );
            triplets.push_back( Eigen::Triplet<double>(i+1,j,y) ); triplets.push_back( Eigen::Triplet<double>(i+1,j+1,x) );
            // A_ji
            triplets.push_back( Eigen::Triplet<double>(j,i,-x) ); triplets.push_back( Eigen::Triplet<double>(j,i+1,y) );
            triplets.push_back( Eigen::Triplet<double>(j+1,i,y) ); triplets.push_back( Eigen::Triplet<double>(j+1,i+1,x) );
        }
    }

    A.setFromTriplets(triplets.begin(),triplets.end());
    return A;
}

Eigen::SparseMatrix<double> Stripes::MassMatrix() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<double> M(2*n,2*n);
    std::vector<Eigen::Triplet<double>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (FacePtr f : mesh->faces()) {
        HalfedgePtr he_ij = f.halfedge();
        size_t i = vertexIndices[he_ij.vertex()];
        size_t j = vertexIndices[he_ij.next().vertex()];
        size_t k = vertexIndices[he_ij.prev().vertex()];

        double area = geom->area(f);
        triplets.push_back(Eigen::Triplet<double>(2*i, 2*i, area/3.));
        triplets.push_back(Eigen::Triplet<double>(2*i+1, 2*i+1, area/3.));
        triplets.push_back(Eigen::Triplet<double>(2*j, 2*j, area/3.));
        triplets.push_back(Eigen::Triplet<double>(2*j+1, 2*j+1, area/3.));
        triplets.push_back(Eigen::Triplet<double>(2*k, 2*k, area/3.));
        triplets.push_back(Eigen::Triplet<double>(2*k+1, 2*k+1, area/3.));
    }
    M.setFromTriplets(triplets.begin(),triplets.end());
    return M;
}

void Stripes::computeStripes() {
    std::cout << "Computing Stripes... ";
    Eigen::SparseMatrix<double> A = EnergyMatrix();
    Eigen::SparseMatrix<double> B = MassMatrix();
    Eigen::MatrixXcd x = principalEigenvector(A,B);
    std::cout << "Done!" << std::endl;
    std::cout << x.rows() << "," << x.cols() << std::endl;
}


#pragma once
// Add a backcheck on the correspondence

#include <vector>
#include <string>
#include <Eigen/Dense>
#include "nanoflannWrapper.hpp"
#include "utils/visualization/progressbar.h"

Eigen::MatrixXd sub_space(const Eigen::MatrixXd &cloud, std::vector<int> indices) {
    Eigen::MatrixXd out(3, indices.size());
    for (int i=0; i<indices.size(); i++)
        out.col(i) = cloud.col(indices.at(i));
    return out;
}

template <typename T>
T median_filter(std::vector<T> vector)
{
    std::sort(vector.begin(), vector.end());
    return vector[ std::round( vector.size()/2 ) ];
}

Eigen::Vector3d get_median_point( std::vector<int> pointIdxNKNSearch, Eigen::MatrixXd & cloud )
{
    int x=0, y=1, z=2;
    Eigen::Vector3d median_point;
    std::vector< double > x_vector, y_vector, z_vector;
    for (int i=0; i<pointIdxNKNSearch.size(); ++i)
    {
        x_vector.push_back( cloud(pointIdxNKNSearch[i], x) );
        y_vector.push_back( cloud(pointIdxNKNSearch[i], y) );
        z_vector.push_back( cloud(pointIdxNKNSearch[i], z) );
    }
    median_point(x) = median_filter(x_vector);
    median_point(y) = median_filter(y_vector);
    median_point(z) = median_filter(z_vector);
    return median_point;
}

Eigen::Vector3d point_association(std::string method,
                                Eigen::Vector3d p_i,
                                Eigen::Vector3d n_i,
	                            Eigen::MatrixXd & P,
	                            Eigen::MatrixXd & N)
{
    // used for readability
    int x=0, y=1, z=2;
	Eigen::VectorXd points_dist(P.cols());
	Eigen::VectorXd normals_dist(P.cols());
	for (int i = 0; i < P.cols(); ++i)
	{
		points_dist(i) = sqrt( (p_i(x) - P.col(i)(x)) * (p_i(x) - P.col(i)(x))
			                  +(p_i(y) - P.col(i)(y)) * (p_i(y) - P.col(i)(y))
			                  +(p_i(z) - P.col(i)(z)) * (p_i(z) - P.col(i)(z)));
		normals_dist(i) = n_i(x)*N.col(i)(x)
		                + n_i(y)*N.col(i)(y)
		                + n_i(z)*N.col(i)(z);
	}
	Eigen::MatrixXd::Index pointIndex;
	if (method == "closest_point")
	{
		points_dist.minCoeff(&pointIndex);
	}
	if (method == "normals_distances")
	{
		normals_dist.maxCoeff(&pointIndex);
	}
	if (method == "weighted")
	{
		( points_dist/0.1 - normals_dist ).minCoeff(&pointIndex);
	}
	if (method == "hybrid")
	{
		//pointIndex = ( points_dist.array().pow(31)/normals_dist ).maxCoeff(&pointIndex);
		pointIndex = 1;
		//pointIndex = ( pow(points_dist, 31)/normals_dist ).maxCoeff(&pointIndex);
	}

    return P.row(pointIndex);
}

int find_correspondence(std::string method,
                        Eigen::Vector3d v,
                        Eigen::Vector3d n,
	                    const Eigen::MatrixXd & cloud,
	                    const Eigen::MatrixXd & normals,
                        nanoflann_wrapper kd_tree,
                        int K)
{
    // downsampling of the cloud and normals with K closest points
    std::vector<int> closest_point_indices(K);
    closest_point_indices = kd_tree.return_k_closest_points(v, K);
    Eigen::MatrixXd sample_cloud = sub_space(cloud, closest_point_indices);
    Eigen::MatrixXd sample_normals = sub_space(normals, closest_point_indices);

    // build the distance between points and normals
    int x=0, y=1, z=2;
    Eigen::VectorXd points_distance(cloud.cols());
	Eigen::VectorXd normals_distance(cloud.cols());
    for (int i = 0; i < cloud.cols(); ++i)
	{
		points_distance(i) = sqrt( (v(x) - cloud.col(i)(x)) * (v(x) - cloud.col(i)(x))
			                      +(v(y) - cloud.col(i)(y)) * (v(y) - cloud.col(i)(y))
			                      +(v(z) - cloud.col(i)(z)) * (v(z) - cloud.col(i)(z)));
		normals_distance(i) = n(x)*normals.col(i)(x)
		                    + n(y)*normals.col(i)(y)
		                    + n(z)*normals.col(i)(z);
	}

    // return the min distance according to the selected method
	Eigen::MatrixXd::Index pointIndex;
	if (method == "closest_point")
	{
		points_distance.minCoeff(&pointIndex);
	}
	if (method == "normals_distances")
	{
		normals_distance.maxCoeff(&pointIndex);
	}
	if (method == "weighted")
	{
		( points_distance/0.1 - normals_distance ).minCoeff(&pointIndex);
	}

    return pointIndex;
}

void get_surface_association(Eigen::MatrixXd V_source, Eigen::MatrixXd N_source,
                             Eigen::MatrixXd V_target, Eigen::MatrixXd N_target,
                             Eigen::MatrixXd &source_position,
                             Eigen::MatrixXd &target_position)
{
    // this need to be moved out of hardcoded parameters into config file
    int x=0, y=1, z=2;                                  // used to access the points
    double distance_threshold = 0.05;                   // used to check that the correspondence of the correspondence is not too far
    int skip_points = 5;                               // pseudo downsampling
    int K = 50;                                         // limit the search space

    std::vector<Eigen::Vector3d> correspondences_on_source;
    std::vector<Eigen::Vector3d> correspondences_on_target;

    nanoflann_wrapper tree_target(V_target.transpose());
    nanoflann_wrapper tree_source(V_source.transpose());
    std::vector<int> closest_point_indices(K);

    // for loop
    progressbar bar(V_source.cols()/skip_points);
    for (int i = 0; i < V_source.cols(); i = i+skip_points) {
        int point_index_original = i*skip_points;

        // find the correspondence of the source on the target
        int correspondence_on_target = find_correspondence("weighted",
                                                           V_source.col(point_index_original),
                                                           N_source.col(point_index_original),
	                                                       V_target,
	                                                       N_target,
                                                           tree_target,
                                                           K);

        // find the correspondence of the source on the target
        int correspondence_on_source = find_correspondence("weighted",
                                                           V_target.col(correspondence_on_target),
                                                           N_target.col(correspondence_on_target),
	                                                       V_source,
	                                                       N_source,
                                                           tree_source,
                                                           K);

        // check the distance between the points
        if ( (V_source.col(point_index_original)-V_source.col(correspondence_on_source)).norm() < distance_threshold ) {
            correspondences_on_source.push_back(V_source.col(point_index_original));
            correspondences_on_target.push_back(V_target.col(correspondence_on_target));
        }

        bar.update();
    }

    // push back into an Eigen Matrices
    source_position.resize(3, correspondences_on_source.size());
    for (int i=0; i<correspondences_on_source.size(); i++)
        source_position.col(i) = correspondences_on_source.at(i);

    target_position.resize(3, correspondences_on_target.size());
    for (int i=0; i<correspondences_on_target.size(); i++)
        target_position.col(i) = correspondences_on_target.at(i);

    /*
    int x=0, y=1, z=2;
    //for (int i = 0; i < template_cloud->size(); i = i+10)
    //{
    //    template_cloud_downsampled->points.push_back(template_cloud->points[i]);
    //    template_normals_downsampled->points.push_back(template_normals->points[i]);
    //}
    double distance_threshold = 0.05;                   // used to check that the correspondence of the correspondence is not too far
    int skip_points = 10;                               // pseudo downsampling
    int K = 50;                                         // limit the search space
    int number_of_correspondences = int(round(V_source.cols()/skip_points));

    // generate downsampled cloud
    Eigen::MatrixXd V_source_downsampled, N_source_downsampled;
    V_source_downsampled.resize(3, number_of_correspondences);
    N_source_downsampled.resize(3, number_of_correspondences);
    for (int i = 0; i < number_of_correspondences; i++) {
        V_source_downsampled.col(i) = V_source.col(i*skip_points);
        N_source_downsampled.col(i) = N_source.col(i*skip_points);
    }

    // create kd-tree
    nanoflann_wrapper tree(V_target.transpose());
    std::vector<int> pointIdxNKNSearch(K);
    std::vector<float> pointNKNSquaredDistance(K);

    pointIdxNKNSearch.clear();

    Eigen::Vector3d median_point;

    source_position.resize(3, number_of_correspondences);
    target_position.resize(3, number_of_correspondences);

    for (int i = 0; i < number_of_correspondences; i++)
    {
        // WTF this is not used ????? LOOOOL
		pointIdxNKNSearch = tree.return_k_closest_points(V_source.col(i*skip_points).transpose(), K);

        //median_point = point_association("closest_point",
        //                                    V_source.col(i*skip_points),
        //                                    N_source.col(i*skip_points),
        //                                    V_target,
        //                                    N_target);

        median_point = V_target.col(pointIdxNKNSearch[0]);

        target_position.col(i) <<  median_point(x), median_point(y), median_point(z);
        source_position.col(i) << V_source.col(i*skip_points)(x),
                                  V_source.col(i*skip_points)(y),
                                  V_source.col(i*skip_points)(z);
    }
    */
}