//Authors: Dario Cattaruzza, Alessandro Abate, Peter Schrammel, Daniel Kroening
//University of Oxford 2016
//This code is supplied under the BSD license agreement (see license.txt)

#include <math.h>

#include <Eigen/Eigenvalues>

#include <boost/timer.hpp>

#include "JordanMatrix.h"
#include "MatrixToString.h"

namespace abstract{

template <class scalar>
scalar  JordanMatrix<scalar>::ms_half(0.5);

template <class scalar>
scalar  JordanMatrix<scalar>::ms_one(1);

template <class scalar>
scalar  JordanMatrix<scalar>::ms_two(2);

template <class scalar>
typename JordanMatrix<scalar>::complexS JordanMatrix<scalar>::ms_complexOne(1,0);

template <class scalar>
MatToStr<scalar>  JordanMatrix<scalar>::ms_logger(true);

template <class scalar>
MatToStr<scalar>  JordanMatrix<scalar>::ms_decoder(false);

template <class scalar>
typename JordanMatrix<scalar>::MatrixS JordanMatrix<scalar>::ms_emptyMatrix(0,0);

template <class scalar>
traceDynamics_t JordanMatrix<scalar>::ms_trace_dynamics=eTraceNoDynamics;

template <class scalar>
bool JordanMatrix<scalar>::ms_trace_time=false;

/// Constructs an empty matrix
template <class scalar>
JordanMatrix<scalar>::JordanMatrix(int dimension) :
  m_dimension(dimension),
  m_zero(func::ms_weakZero),
  m_largeZero(dimension*dimension*func::ms_weakZero),
  m_dynamics(dimension,dimension),
  m_refDynamics(dimension,dimension),
  m_eigenSpace(dimension),
  m_minSeparation(dimension,1),
  m_jordanTime(0)
{
  m_pseudoEigenVectors=MatrixS(m_dimension,m_dimension);
  m_invPseudoEigenVectors=MatrixS(m_dimension,m_dimension);
}

/// Changes the default dimension of the system
template <class scalar>
void JordanMatrix<scalar>::changeDimensions(const int dimension)
{
  if (dimension!=m_dimension) {
    m_dimension=dimension;
    m_dynamics.conservativeResize(dimension,dimension);
    m_minSeparation.conservativeResize(dimension,1);
    m_refDynamics.resize(dimension,dimension);
    m_pseudoEigenVectors.resize(dimension,dimension);
    m_invPseudoEigenVectors.resize(dimension,dimension);
  }
}

/// Loads a matrix from a given description
template <class scalar>
int JordanMatrix<scalar>::load(const std::string &data,size_t pos)
{
  int result=ms_logger.StringToMat(m_dynamics,data,pos);
  //changeDimensions(m_dynamics.rows());
  if ((result>0) && !calculateJordanForm()) return -1;
  return result;
}

template <class scalar>
bool JordanMatrix<scalar>::loadFromRef(const MatrixR &matrix)
{
  changeDimensions(matrix.rows());
  for (int row=0;row<matrix.rows();row++) {
    for (int col=0;col<matrix.cols();col++) {
      m_dynamics.coeffRef(row,col)=matrix.coeff(row,col);
    }
  }
  return calculateJordanForm();
}

template <class scalar>
bool JordanMatrix<scalar>::load(const MatrixS &dynamics)
{
  changeDimensions(dynamics.rows());
  m_dynamics=dynamics;
  return calculateJordanForm();
}

template <class scalar>
bool JordanMatrix<scalar>::loadJordan(const MatrixS &matrix)
{
  boost::timer timer;
  changeDimensions(matrix.rows());
  m_dynamics=matrix;
  m_zero=calculateEpsilon(m_dynamics);
  func::setZero(m_zero);
  m_largeZero=m_zero*m_dimension*m_dimension;
  interToRef(m_refDynamics,m_dynamics);
  m_pseudoEigenValues=m_dynamics;
  m_pseudoEigenVectors=MatrixS::Identity(m_dimension,m_dimension);
  m_invPseudoEigenVectors=m_pseudoEigenVectors;
  m_hasOnes=false;
  m_hasMultiplicities=false;
  m_isOne.resize(2*m_dimension);
  m_conjugatePair.resize(2*m_dimension);
  m_jordanIndex.resize(2*m_dimension);
  for (int i=0;i<m_dimension;i++) {
    m_conjugatePair[i]=-1;
    m_conjugatePair[i+m_dimension]=-1;
    if ((i<m_dimension-1) && !func::isZero(m_dynamics.coeff(i,i+1))) m_conjugatePair[i]=i+1;
    else if ((i>0) && !func::isZero(m_dynamics.coeff(i-1,i)))        m_conjugatePair[i]=i-1;
    int mult=(m_conjugatePair[i]<0) ? 1 : 2;
    m_jordanIndex[i]=0;
    if ((i>=mult) && !func::isZero(m_dynamics.coeff(i,i-mult))) m_jordanIndex[i-mult]+1;
    m_jordanIndex[i+m_dimension]=0;
    if (m_jordanIndex[i]>0) m_hasMultiplicities=true;
    m_isOne[i]=func::isZero(ms_one-m_dynamics.coeff(i,i)) && (m_conjugatePair[i]<0);
    m_hasOnes|=m_isOne[i];
  }
  m_eigenValues=pseudoToJordan(m_pseudoEigenValues,eToEigenValues);
  m_eigenNorms.resize(m_eigenValues.rows(),1);
  for (int i=0;i<m_eigenValues.rows();i++) m_eigenNorms.coeffRef(i,0)=func::norm2(m_eigenValues.coeff(i,i));
  m_eigenVectors=MatrixC::Identity(m_dimension,m_dimension);
  m_invEigenVectors=m_eigenVectors;
  m_error=func::ms_hardZero;
  m_jordanTime=timer.elapsed()*1000;
  if (ms_trace_time) ms_logger.logData(m_jordanTime,"Pole Extraction time:",true);
  return true;
}

/// calculates the estimated roundoff error of a matrix operation
template <class scalar>
template <class MatrixType> inline typename JordanMatrix<scalar>::refScalar JordanMatrix<scalar>::calculateEpsilon(const MatrixType &matrix)
{
  if (matrix.rows()>0) {
    refScalar max=func::toUpper(func::norm2(matrix.coeff(0,0)));
    refScalar min=func::toLower(func::norm2(matrix.coeff(0,0)));
    for (int row=0;row<matrix.rows();row++) {
      for (int col=0;col<matrix.cols();col++) {
        refScalar upper=func::toUpper(func::norm2(matrix.coeff(row,col)));
        refScalar lower=func::toLower(func::norm2(matrix.coeff(row,col)));
        if (upper>max) max=upper;
        if (lower<min) min=lower;
      }
    }
    //scalar max=matrix.maxCoeff();
    //scalar min=matrix.minCoeff();
    if (-min>max) max=-min;
    return max*func::ms_weakEpsilon;
  }
  return 0;
}

/// Loads the transformation matrix for the state space
template <class scalar>
bool JordanMatrix<scalar>::calculateJordanForm(bool includeSvd)
{
  boost::timer timer;
  m_zero=calculateEpsilon(m_dynamics);
  func::setZero(m_zero);
  m_largeZero=m_zero*m_dimension*m_dimension;
  interToRef(m_refDynamics,m_dynamics);

  m_eigenSpace.computeJordan(m_refDynamics);
  if (ms_trace_time) ms_logger.logData(timer.elapsed()*1000,"Jordan Form:",true);
  if (m_eigenSpace.info()!=Eigen::Success) {
    if (ms_trace_dynamics>=eTraceDynamics) ms_logger.logData("Failed to find Jordan Form");
    return false;
  }
  refToInter(m_eigenValues,m_eigenSpace.getEigenValues());
  refToInter(m_eigenVectors,m_eigenSpace.getEigenVectors());

  m_jordanIndex=m_eigenSpace.getJordanIndeces();
  m_conjugatePair=m_eigenSpace.getConjugatePairs();
  m_isOne=m_eigenSpace.getOnes();
  m_hasOnes=m_eigenSpace.hasOnes();
  m_hasMultiplicities=m_eigenSpace.hasMultiplicities();
  m_eigenNorms.resize(m_eigenValues.rows(),1);
  for (int i=0;i<m_eigenValues.rows();i++) m_eigenNorms.coeffRef(i,0)=func::norm2(m_eigenValues.coeff(i,i));
  try {
    m_invEigenVectors=m_eigenVectors.inverse();
  }
  catch(...) {
    refToInter(m_invEigenVectors,m_eigenSpace.getEigenVectors().inverse());
  }
  if (ms_trace_dynamics>=eTraceDynamics) {
    ms_logger.logData(m_dynamics,"Dynamics:");
    ms_logger.logData(m_eigenValues,"EigenValues:");
    ms_logger.logData(m_eigenVectors,"EigenVectors:");
    ms_logger.logData(m_invEigenVectors,"InvEigenVectors:");
  }

  m_pseudoEigenValues=jordanToPseudoJordan(m_eigenValues,eToEigenValues);
  m_pseudoEigenVectors=jordanToPseudoJordan(m_eigenVectors,eToEigenVectors);
  m_invPseudoEigenVectors=m_pseudoEigenVectors.inverse();
  if (ms_trace_dynamics>=eTraceDynamics) {
    MatrixS pseudoCalculated=m_pseudoEigenVectors*m_pseudoEigenValues*m_invPseudoEigenVectors;
    ms_logger.logData(m_pseudoEigenValues,"PseudoEigenValues");
    ms_logger.logData(m_pseudoEigenVectors,"PseudoEigenVectors");
    ms_logger.logData(m_invPseudoEigenVectors,"InvPseudoEigenVectors");
    ms_logger.logData(pseudoCalculated,"PseudoCalc");
  }
  if (includeSvd) {
    calculateBlockSVD();
    m_minSigma=func::toLower(m_blockSingularValues.coeff(0,0));
    m_maxSigma=func::toUpper(m_blockSingularValues.coeff(0,0));
    for (int row=1;row<m_blockSingularValues.rows();row++) {
      if (func::toUpper(m_blockSingularValues.coeff(row,0))>m_maxSigma) {
        m_maxSigma=func::toUpper(m_blockSingularValues.coeff(row,0));
      }
      if (func::toLower(m_blockSingularValues.coeff(row,0))<m_minSigma) {
        m_minSigma=func::toLower(m_blockSingularValues.coeff(row,0));
      }
    }
    m_jordanTime=timer.elapsed()*1000;
    if (ms_trace_time) ms_logger.logData(m_jordanTime,"SVD time:",true);
  }
  calculateEigenError();
  m_jordanTime=timer.elapsed()*1000;
  if (ms_trace_time) ms_logger.logData(m_jordanTime,"Jordan Error time:",true);
  for (int row=0;row<m_invPseudoEigenVectors.rows();row++) {
    for (int col=0;col<m_invPseudoEigenVectors.cols();col++) {
      if (func::isNan(m_invPseudoEigenVectors.coeff(row,col))) {
        return false;
      }
    }
  }
  return true;
}

/// Retrieves an equivalent real Jordan from a complex representation
template <class scalar>
typename JordanMatrix<scalar>::MatrixS JordanMatrix<scalar>::jordanToPseudoJordan(const MatrixC &source,const pseudoType_t conversionType)
{
  MatrixS result=source.real();
  if (conversionType==eToEigenValues) {
    for (int col=0;col<source.rows();col++) {
      if (m_conjugatePair[col]>col) {
        result.coeffRef(col+1,col)=-source.coeff(col,col).imag();
        result.coeffRef(col,col+1)=source.coeff(col,col).imag();
        for (int offset=1;offset<=m_jordanIndex[col];offset++) {
          int row=col-2*offset;
          result.coeffRef(row+1,col)=-source.coeff(row,col).imag();
          result.coeffRef(row,col+1)=source.coeff(row,col).imag();
        }
        col++;
      }
    }
  }
  else if (conversionType==eToEigenVectors) {
    for (int col=0;col<source.cols();col++) {
      if (m_conjugatePair[col]>col) {
        result.col(col+1)=source.col(col).imag();
        col++;
      }
    }
  }
  else {
      for (int row=0;row<source.rows();row++) {
        if (m_conjugatePair[row]>row) {
          result.row(row+1)=source.row(row).imag();
          row++;
        }
      }
  }
  return result;
}

/// Retrieves an equivalent complex Jordan from a real representation
template <class scalar>
typename JordanMatrix<scalar>::MatrixC JordanMatrix<scalar>::pseudoToJordan(const MatrixS &source,const pseudoType_t conversionType)
{
  MatrixC result=MatrixC::Zero(source.rows(),source.cols());
  if (conversionType==eToEigenValues) {
    int mult=1;
    for (int row=0;row<source.rows();row+=mult) {
      mult=(m_conjugatePair[row]<0) ? 1 : 2;
      if (m_jordanIndex[row+mult]>0) {
        if (m_conjugatePair[row]<0) {
          result.coeffRef(row,row)=complexS(source.coeff(row,row),func::ms_hardZero);
        }
        else {
          result.coeffRef(row,row)=complexS(source.coeff(row,row),source.coeff(row,row+1));
          result.coeffRef(row+1,row+mult+1)=ms_complexOne;
        }
        result.coeffRef(row,row+mult)=ms_complexOne;
      }
      else {
        result.coeffRef(row,row)=complexS(source.coeff(row,row),source.coeff(row,row+1));
      }
    }
  }
  else if (conversionType==eToEigenVectors) {
    for (int col=0;col<source.cols();col++) {
      result.col(col).real()=source.col(col);
      if (m_conjugatePair[col]>col) {
        result.col(col).imag()=source.col(col+1);
        col++;
        result.col(col).real()=source.col(col-1);
        result.col(col).imag()=-source.col(col);
      }
    }
  }
  else {
    for (int row=0;row<source.rows();row++) {
      result.row(row).real()=source.row(row);
      if (m_conjugatePair[row]>row) {
        result.row(row).imag()=source.row(row+1);
        row++;
        result.row(row).real()=source.row(row-1);
        result.row(row).imag()=-source.row(row);
      }
    }
  }
  return result;
}

/// Retrieves a scalar matrix from a refScalar one
template <class scalar>
void JordanMatrix<scalar>::interToRef(SolverMatrixType &dest,const MatrixS &source)
{
  dest.conservativeResize(source.rows(),source.cols());
  for (int row=0;row<source.rows();row++) {
    for (int col=0;col<source.cols();col++) {
      refScalar coef=func::toCentre(source.coeff(row,col));
      dest.coeffRef(row,col)=coef;
    }
  }
}

/// Retrieves a scalar matrix from a refScalar one
template <class scalar>
void JordanMatrix<scalar>::refToInter(MatrixC &dest,const SolverComplexMatrixType &source)
{
  dest.conservativeResize(source.rows(),source.cols());
  for (int row=0;row<source.rows();row++) {
    for (int col=0;col<source.cols();col++) {
      dest.coeffRef(row,col)=func::toScalar(source.coeff(row,col));
    }
  }
}

/// Retrieves a scalar matrix from a refScalar one
template <class scalar>
void JordanMatrix<scalar>::refToInter(MatrixS &dest,const SolverMatrixType &source)
{
  dest.conservativeResize(source.rows(),source.cols());
  for (int row=0;row<source.rows();row++) {
    for (int col=0;col<source.cols();col++) {
      dest.coeffRef(row,col)=source.coeff(row,col);
    }
  }
}

/// Transforms the matrix to Reduced Row Echelon Form
template <class scalar>
int JordanMatrix<scalar>::toRREF(MatrixC &matrix)
{
  int rank=m_dimension;
  int col=0;
  for (int row=0;col<matrix.rows();row++,col++) {
    while (func::isZero(func::norm2(matrix.coeff(row,col)),m_zero)) {
      for (int row2=row+1;row2<matrix.rows();row2++) {
        if (!func::isZero(func::norm2(matrix.coeff(row2,col)),m_zero)) {
          matrix.row(row)+=matrix.row(row2);
          break;
        }
      }
      if (func::isZero(func::norm2(matrix.coeff(row,col)),m_zero)) {
        col++;
        if (col==matrix.rows()) break;
      }
    }
    if (col==matrix.rows()) break;
    complexR multiplier=func::toCentre(matrix.coeff(row,col));
    if (!func::isZero(func::norm2(multiplier),m_zero)) {
      for (int col2=0;col2<matrix.cols();col2++) matrix.coeffRef(row,col2)/=multiplier;
      rank--;
    }
    for (int row2=row+1;row2<matrix.rows();row2++) {
      complexS multiplier=matrix.coeff(row2,col);
      matrix.row(row2)-=multiplier*matrix.row(row);
    }
  }
  col=0;
  for (int row=1;(row<matrix.rows()) && (col<matrix.cols());row++) {
    while ((col<matrix.cols()) && func::isZero(func::norm2(matrix.coeff(row,col)),m_zero)) col++;
    if (col<matrix.cols()) {
      for (int row2=0;row2<row;row2++) {
        if (!func::isZero(func::norm2(matrix.coeff(row2,col)),m_zero)) {
          complexS multiplier=matrix.coeff(row2,col);
          matrix.row(row2)-=multiplier*matrix.row(row);
        }
      }
    }
  }
  return rank;
}

/// Returns the description of a complex matrix
template <class scalar>
std::string JordanMatrix<scalar>::getMatrix(const MatrixC &matrix,bool brackets)
{
  if (brackets) return ms_logger.MatToString(matrix);
  return ms_decoder.MatToString(matrix);
}

/// Returns the description of a matrix
template <class scalar>
std::string JordanMatrix<scalar>::getMatrix(const MatrixS &matrix,bool brackets)
{
  if (brackets) return ms_logger.MatToString(matrix);
  return ms_decoder.MatToString(matrix);
}

/// Returns the complex eigenvector matrix (S)
template <class scalar>
std::string JordanMatrix<scalar>::getEigenVectorsDesc(bool pseudo)
{
  if (pseudo) return getMatrix(m_pseudoEigenVectors,false);
  return getMatrix(m_eigenVectors,false);
}

/// Returns the singular values of the matrix
template <class scalar>
std::string JordanMatrix<scalar>::getSingularValuesDesc()
{
  return getMatrix(m_blockSingularValues,false);
}

/// Returns the inverse complex eigenvector matrix (S^-1)
template <class scalar>
std::string JordanMatrix<scalar>::getInvEigenVectorsDesc(bool pseudo)
{
  if (pseudo) return getMatrix(m_invPseudoEigenVectors,false);
  return getMatrix(m_invEigenVectors,false);
}

/// Returns the schur decomposition of the dynamics
template <class scalar>
std::string JordanMatrix<scalar>::getSJinvS()
{
  MatrixC matrix=m_eigenVectors;
  std::string result=getMatrix(matrix);
  result+=getMatrix(m_eigenValues);
  matrix=m_eigenVectors.inverse();
  result+=getMatrix(matrix);
  return result;
}

/// Retrieves the inverse of the dynamics
template <class scalar>
typename JordanMatrix<scalar>::MatrixS JordanMatrix<scalar>::getPseudoInverse(const MatrixS &matrix,bool &hasInverse)
{
  load(matrix);
  MatrixC diag=m_eigenValues;
  hasInverse=true;
  for (int i=0;i<m_dimension;i++) {
    char sign=func::hardSign(func::norm2(diag.coeff(i,i)));
    if (sign!=0) {
      diag.coeffRef(i,i)=ms_complexOne/diag.coeff(i,i);
      for (int j=1;j<=m_jordanIndex[i];j++) {
        diag.coeffRef(i-j,i)=-func::c_pow(-diag.coeffRef(i,i),j+1);
      }
    }
    else hasInverse=false;
  }
  if (hasInverse) return m_dynamics.inverse();
  diag=m_invEigenVectors*diag*m_eigenVectors;
  return diag.real();
}

/// Calculates the pseudoinverse of a matrix
template <class scalar>
typename JordanMatrix<scalar>::MatrixS JordanMatrix<scalar>::getSVDpseudoInverse(const MatrixS &matrix,bool &hasInverse)
{
  interToRef(m_refDynamics,matrix);
  m_svdSpace.compute(m_refDynamics, Eigen::ComputeFullU | Eigen::ComputeFullV);
  SolverMatrixType d=m_svdSpace.singularValues().asDiagonal();
  SolverMatrixType u=m_svdSpace.matrixU();
  SolverMatrixType v=m_svdSpace.matrixV();
  MatrixS diag;
  MatrixS matrixU;
  MatrixS matrixV;
  refToInter(diag,d);
  refToInter(matrixU,u);
  refToInter(matrixV,v);
  if (ms_trace_dynamics>=eTraceDynamics) {
    ms_logger.logData(matrix,"Inverse:");
    ms_logger.logData(matrixU);
    ms_logger.logData(diag);
    ms_logger.logData(matrixV);
  }
  for (int i=0;i<matrix.rows();i++) {
    if (func::isZero(diag.coeff(i,i))) hasInverse=false;
    else diag.coeffRef(i,i)=scalar(1)/diag.coeff(i,i);
  }
  if (matrix.rows()!=matrix.cols()) {
    diag.conservativeResize(matrixV.cols(),matrixU.rows());
    if (matrixV.cols()>matrixU.rows()) {
      diag.block(matrixU.rows(),0,matrixV.cols()-matrixU.rows(),matrixU.rows())=MatrixS::Zero(matrixV.cols()-matrixU.rows(),matrixU.rows());
    }
    else {
      diag.block(0,matrixV.cols(),matrixV.cols(),matrixU.rows()-matrixV.cols())=MatrixS::Zero(matrixV.cols(),matrixU.rows()-matrixV.cols());
    }
    hasInverse=false;
  }
  if (hasInverse) return matrix.inverse();
  return matrixV*diag*matrixU.adjoint();
}

/// Calculates a lower bound for the minimum separation between any two jordan blocks
template <class scalar>
typename JordanMatrix<scalar>::refScalar JordanMatrix<scalar>::calculateMinSeparation()
{
  MatrixS base;
  refToInter(base,m_eigenSpace.getSchur());
  scalar nonDiagNorm2=0;
  for (int row=0;row<m_dimension-1;row++) {
    if (m_conjugatePair[row]<row) nonDiagNorm2+=func::squared(base.coeff(row,row+1));//*base.coeff(row,row+1);
    for (int col=row+2;col<m_dimension;col++) {
      nonDiagNorm2+=func::squared(base.coeff(row,col));//*base.coeff(row,col);
    }
  }
  scalar dimension=m_dimension;
  scalar factor=sqrt(func::pow((dimension-ms_one)/dimension,m_dimension-1));
  factor*=sqrt(func::pow(ms_two,(m_dimension-1)*(m_dimension+1)));//The smallest col/row is ignored

  for (int i=0;i<m_dimension;i++) {
    scalar diagNorm2=0;
    scalar det=ms_one;
    int mult=(m_conjugatePair[i]<0) ? 1 : 2;
    m_minSeparation.coeffRef(i,0)=func::toLower(func::norm2(m_eigenValues.coeff(i,i)));
    for (int j=0;j<m_dimension;j++) {
      if (i+mult*m_jordanIndex[j]==j) continue;
      scalar sep=func::norm2(m_eigenValues.coeff(i,i)-m_eigenValues.coeff(j,j));
      det*=sep;
      diagNorm2+=func::squared(sep);//*sep;
    }
    if (func::toLower(det)==0) func::imprecise(det,func::ms_hardZero);
    diagNorm2+=nonDiagNorm2;
    scalar maxColOrRowProd=func::pow(sqrt(diagNorm2),m_dimension-1);
    while (m_jordanIndex[i+mult]>0) i+=mult;
    if (m_jordanIndex[i]>0) {
      maxColOrRowProd*=func::pow(dimension,m_dimension);
      m_minSeparation.coeffRef(i,0)=func::toLower(factor*func::pow(det,m_jordanIndex[i])/maxColOrRowProd);
      for (int j=0;j<m_jordanIndex[i]*mult;j++) m_minSeparation.coeffRef(i-j,0)=m_minSeparation.coeffRef(i,0);
    }
    else m_minSeparation.coeffRef(i,0)=func::toLower(factor*det/maxColOrRowProd);
    if (m_conjugatePair[i]>i) {
      m_minSeparation.coeffRef(i+1,0)=m_minSeparation.coeffRef(i,0);
      i++;
    }
  }
  return m_minSeparation.minCoeff();
}

/// Calculates the maximum error for the numerical approximation of the eigencvalues
template <class scalar>
scalar JordanMatrix<scalar>::calculateEigenError()
{
  scalar kP=m_pseudoEigenVectors.norm()*m_invPseudoEigenVectors.norm();
  MatrixS calculated=m_pseudoEigenVectors*m_pseudoEigenValues*m_invPseudoEigenVectors;
  if (ms_trace_dynamics>=eTraceErrors) ms_logger.logData(calculated,"Calculated:");
  calculated-=m_dynamics;
  scalar errorNorm=calculated.norm();
  m_error=errorNorm*kP;
  if (func::toUpper(m_error)>m_zero) func::imprecise(m_error,m_zero);

  m_boundForError=0;
  if (m_hasMultiplicities) return m_error;

  if (ms_trace_dynamics>=eTraceErrors) ms_logger.logData(m_error,"Error:",true);
  m_error=func::setpm(m_error);
  complexS complexError=m_error;
  for (int i=0;i<m_dimension;i++) {
    m_eigenValues.coeffRef(i,i)+=complexError;
//    m_eigenValues.coeffRef(i,i).real()+=m_error;
//    if (m_conjugatePair[i]>=0) m_eigenValues.coeffRef(i,i).imag()+=m_error;
  }
  m_pseudoEigenValues=jordanToPseudoJordan(m_eigenValues,eToEigenValues);

  calculateMinSeparation();
  for (int i=0;i<m_dimension;i++) {
    if (m_minSeparation.coeff(i,0)>0) {
      scalar angleError=errorNorm/m_minSeparation.coeff(i,0);
      scalar cosTheta=func::toLower(func::cosine(angleError));
      scalar invCosTheta=ms_one/cosTheta;
      scalar vError=func::getHull(cosTheta,invCosTheta);
      m_eigenVectors.col(i)*=vError;
    }
  }
  m_pseudoEigenVectors=jordanToPseudoJordan(m_eigenVectors,eToEigenVectors);
  m_invPseudoEigenVectors=m_pseudoEigenVectors.inverse();//jordanToPseudoJordan(m_invEigenVectors,eToInvEigenVectors);
  if (ms_trace_dynamics>=eTraceDynamics) {
    ms_logger.logData(m_pseudoEigenValues,"PseudoEigenValues");
    ms_logger.logData(m_pseudoEigenVectors,"PseudoEigenVectors");
    ms_logger.logData(m_invPseudoEigenVectors,"InvPseudoEigenVectors");
  }
  return m_error;
}

/// Calculates the maximum error for the numerical approximation of the matrix to the nth power
/// @return the maximum variation of the eigenvalues
template <class scalar>
scalar JordanMatrix<scalar>::calculateBoundedEigenError(scalar iteration)
{
  if (func::isNegative(iteration)) iteration=-iteration;
  if (func::isZero(iteration-m_boundForError)) return m_error;
  m_pseudoEigenVectors=jordanToPseudoJordan(m_eigenVectors,eToEigenVectors);
  m_invPseudoEigenVectors=m_pseudoEigenVectors.inverse();//jordanToPseudoJordan(m_invEigenVectors,eToInvEigenVectors);
  if (ms_trace_dynamics>=eTraceDynamics) {
      ms_logger.logData(m_pseudoEigenVectors,"S");
      ms_logger.logData(m_pseudoEigenValues,"J");
      ms_logger.logData(m_invPseudoEigenVectors,"invS");
  }
  MatrixS calculated=m_pseudoEigenVectors*m_pseudoEigenValues*m_invPseudoEigenVectors;
  if (ms_trace_dynamics>=eTraceErrors) ms_logger.logData(calculated,"Calculated");
  MatrixS jordanError=m_invPseudoEigenVectors*m_dynamics*m_pseudoEigenVectors;
  if (ms_trace_dynamics>=eTraceErrors) ms_logger.logData(jordanError,"Calculated Jordan");
  jordanError-=m_pseudoEigenValues;
  if (ms_trace_dynamics>=eTraceErrors) ms_logger.logData(jordanError,"Jordan Error");
  m_error=jordanError.norm();
  scalar theta=acos((ms_one-m_error)/(ms_one+m_error));
  scalar nTheta=iteration*theta;
  if (func::toUpper(nTheta)>m_zero) func::imprecise(nTheta,m_zero);
  scalar cosn=func::toLower(func::cosine(nTheta));
  scalar invCosN=ms_one/cosn;
  scalar vError=func::getHull(cosn,invCosN);
  m_pseudoEigenVectors*=vError;
  m_invPseudoEigenVectors=m_pseudoEigenVectors.inverse();
  return m_error;
}

/// Calculates the singular values
template <class scalar>
bool JordanMatrix<scalar>::calculateSVD()
{
  boost::timer timer;
  MatrixS dynamicsSq=m_dynamics*m_dynamics.transpose();
  interToRef(m_refDynamics,dynamicsSq);
  m_eigenSpace.computeJordan(m_refDynamics);
  if (ms_trace_time) ms_logger.logData(timer.elapsed()*1000,"Full Svd:",true);
  if (m_eigenSpace.info()!=Eigen::Success) return false;
  MatrixS singularValues,singularVectors,inverseVectors;
  refToInter(singularValues,m_eigenSpace.getEigenValues().real());
  refToInter(singularVectors,m_eigenSpace.getEigenVectors().real());
  inverseVectors=singularVectors.inverse();
  scalar kP=singularVectors.norm()*inverseVectors.norm();
  dynamicsSq-=singularVectors*singularValues*singularVectors.inverse();
  scalar error=dynamicsSq.norm()*kP;
  if (func::toUpper(m_error)>m_zero) func::imprecise(m_error,m_zero);
  error=func::setpm(error);
  for (int i=0;i<m_dimension;i++) {
    singularValues.coeffRef(i,i)+=error;
    singularValues.coeffRef(i,i)=sqrt(singularValues.coeff(i,i));
  }
  return true;
}

/// Calculates the singular values for each Jordan Block
template <class scalar>
void JordanMatrix<scalar>::calculateBlockSVD()
{
  m_blockSingularValues.resize(m_dimension,2);
  for (int row=m_dimension-1;row>=0;row--) {
    if (m_jordanIndex[row]>0) {
      int blockSize=(m_jordanIndex[row]+1);
      MatrixR jordanBlock=MatrixR::Zero(m_dimension,m_dimension);
      jordanBlock.coeffRef(0,0)=func::toUpper(m_eigenNorms.coeff(row,0));
      for (int i=1;i<blockSize;i++) {
        jordanBlock.coeffRef(i,i)=jordanBlock.coeff(0,0);
        jordanBlock.coeffRef(i,i-1)=1;
      }
      m_svdSpace.compute(jordanBlock);
      scalar norm=m_svdSpace.singularValues().coeff(0)*(ms_one+m_dimension*m_dimension*Eigen::NumTraits<refScalar>::epsilon());
      if (m_conjugatePair[row]>=0) blockSize*=2;
      for (int j=0;j<blockSize;j++) m_blockSingularValues.coeffRef(row--,0)=norm;
      row++;
    }
    else {
      m_blockSingularValues.coeffRef(row,0)=m_eigenNorms.coeff(row,0);
    }
  }
  m_blockSingularValues.col(1)=MatrixS::Ones(m_dimension,1);
  for (int row=1;row<m_dimension;row++) {
    if (m_jordanIndex[row]>0)
    {
      int mult=(m_conjugatePair[row]>=0) ? 2 : 1;
      m_blockSingularValues.coeffRef(row,1)=m_blockSingularValues.coeff(row-mult,1)/(this->ms_one-norm(m_eigenValues.coeff(row,row)));
      m_blockSingularValues.coeffRef(row-mult*m_jordanIndex[row],1)+=m_blockSingularValues.coeff(row,1);
    }
  }
}

#ifdef USE_LDOUBLE
  #ifdef USE_SINGLES
    template class JordanMatrix<long double>;
  #endif
  #ifdef USE_INTERVALS
    template class JordanMatrix<ldinterval>;
  #endif
#endif
#ifdef USE_MPREAL
  #ifdef USE_SINGLES
    template class JordanMatrix<mpfr::mpreal>;
  #endif
  #ifdef USE_INTERVALS
    template class JordanMatrix<mpinterval>;
  #endif
#endif
}


#pragma once
///@file simpleClusterization.hpp
///@brief This is a simple library that provides basic functionality to clusterize data according to either kmeans or fuzzy cmeans algorithms

#include <Eigen/Dense>
#ifndef FCM_MAX_ITERATIONS
///The maximum number of iterations to do if the algorithm doesn't otherwise converge
//@note It's wrapped in an #ifndef to allow the user to override it at compile time
#define FCM_MAX_ITERATIONS  20
#endif

#ifndef FCM_THRESHOLD
///The threshold below which real numbers are deemed identical, used to avoid infinite weights and determine when the algorithm has converged
//@note It's wrapped in an #ifndef to allow the user to override it at compile time
#define FCM_THRESHOLD   1.0E-19
#endif

#include <simpleClusterization_common.hpp>
using namespace Eigen;

///A constant used in offsetting the centroids of one-datapoint clusters in fuzzy-cmeans, to avoid infinite weights
const float offsetConstant                  = 0.05;
///The number of different seeds to try for each value of clusters number when attempting a clusterization
const int   attemptsPerClustersNumber       = 3;
///If empty clusters happen, the algorithm for that number of clusters fails, so we try up to this number of times if this happens.
const int   maxIterationPerClustersNumber   = 5;

/*!
 * @brief       This function calculates the weights of a fuzzy c-means clusterization, according to the next formula:
 *              w_ij = 1 / (norm(centroids(i), entities(j)))
 * @note        The weights are not normalized, as depending on the use case the normalization will be columns-wise or row-wise
 * @param[in]   entities    The datapoints
 * @param[in]   centroids   The centroids of the clusters
 * @param[out]  weights     The resulting weights
 * @param[in]   norm        A pointer to the norm function you want to use
*/
void calculateFuzzyWeights(
        const Ref<const MatrixXf>   &entities,
        const Ref<const MatrixXf>   &centroids,
        Ref<MatrixXfR>              weights,
        squaredNorm_t               *norm
        );

/*!
 * @brief Generates a fixed number of clusters and their fuzzy weights
 * @param[in]       entities    The datapoints
 * @param[out]      centroids   The centroids of the clusters. The number of rows are the required clusters to find
 * @param[out]      weights     The weights associated with the clusterization (array of floats)
 * @param[in]       norm        A pointer to the norm function you want to use
*/
void FCMGenerator(
        const Ref<const MatrixXf>   &entities,
        Ref<MatrixXf>               centroids,
        Ref<MatrixXfR>              weights,
        squaredNorm_t               *norm
    );

/*!
 * @brief       Returns a measure of the fit of the clusterization, it's strictly positive and the smaller it is, the best the fit
 * @details     The Davies-Boulding Index defines a measure of the "goodness" of a clusterization of a data population based on the following quantities:
                The scatter vector S_i= (1/T_i * sum_j (norm(C_i, X_j)))^(1/2) where T_i is the population size of the i-th cluster and the sum runs over the datapoints belonging to the i-th cluster
                The Cluster Separation Matrix M_ij = (norm(C_i, C_j))^(1/2)
                The Davies-Bouldin Matrix R_ij = (S_i + S_j)/M_ij
                The Davies-Bouldin Vector R_i = max_(j!=i) R_ij
                The Davies-Bouldin index is, in terms of the previous quantities, R = 1/N * sum_i R_i where N is the number of clusters
 * @param[in]   entities    The datapoints
 * @param[in]   centroids   The centroids of the clusters
 * @param[out]  weights     The weights associating each centroid to its cluster (it's an array of bools)
 * @param[in]   norm        A pointer to the norm function you want to use
 * @return     The Davies-Boulding index of the provided clusterization
*/
float daviesBouldinIndex(
        const Ref<const MatrixXf>    &entities,
        const Ref<const MatrixXf>    &centroids,
        const Ref<const MatrixXb>    &weights,
        squaredNorm_t                *norm
    );

/*!
 * @brief Returns a measure of how well the clusters fit the data
 * @warning TODO <b>Not implemented</b>
 * @param[in]   entities     The datapoints
 * @param[in]   clusters     The centroids of the clusters
 * @param[out]  weights      The weights associating each centroid to its cluster (it's an array of floats)
 * @param[in]   norm         A pointer to the norm function you want to use
 * @return     the fitness of the clusterization
*/
float silhouetteTest(
        const Ref<const MatrixXf>   &entities,
        const Ref<const MatrixXf>   &clusters,
        const Ref<const MatrixXfR>  &weights,
        squaredNorm_t               *norm
        );

/*!
 * @brief       Given a dataset and centroids, returns a weights matrix that is true if the j-th centroid is the closest to the i-th datapoint, and false otherwise
 * @param[in]   entities    The datapoints
 * @param[in]   centroids   The centroids of the clusters
 * @param[out]  weights     The weights associating each centroid to its cluster (it's an array of bools)
 * @param[in]   norm        A pointer to the norm function you want to use
*/
void calculateBooleanWeights(
        const Ref<const MatrixXf>   &entities,
        const Ref<const MatrixXf>   &centroids,
        Ref<MatrixXbR>              weights,
        squaredNorm_t               *norm
    );

/*!
 * @brief       Given a dataset and a centroids matrix of k rows, it tries to identify the most probable k centroids to represent the dataset
 * @param[in]   entities    The datapoints
 * @param[out]   centroids   The centroids of the clusters
 * @param[out]  weights     The weights associating each centroid to its cluster (it's an array of bools)
 * @param[in]   norm        A pointer to the norm function you want to use
*/
void kmeansGenerator(
        const Ref<const MatrixXf>   &entities,
        Ref<MatrixXf>               centroids,
        Ref<MatrixXbR>              weights,
        squaredNorm_t               *norm
    );

/*!
 * @brief Finds the best fitting number of clusters for the given datapoints, up to the number of rows of centroids through an approximated algorithm compared to full fuzzy c-means
 * @param[in]   entities    The datapoints
 * @param[out]  centroids   The centroids of the clusters
 * @param[out]  weights     The weights associating each centroid to its cluster (it's an array of floats)
 * @param[out]  boolWeights The weights associating each centroid to its cluster (it's an array of bools)
 * @param[in]   norm        A pointer to the norm function you want to use
 * @return     the number of clusters generated
*/
int clusterGeneratorApproximate(
        const Ref<const MatrixXf>   &entities,
        Ref<MatrixXf>               centroids,
        Ref<MatrixXfR>              weights,
        Ref<MatrixXbR>              boolWeights,
        squaredNorm_t               *norm
    );

/*!
 * @brief Finds the best fitting number of clusters for the given datapoints, up to the number of rows of centroids using the fuzzy c-means algorithm
 * @param[in]   entities    The datapoints
 * @param[out]  centroids   The centroids of the clusters
 * @param[out]  weights     The weights associating each centroid to its cluster (it's an array of floats)
 * @param[in]   norm        A pointer to the norm function you want to use
 * @return     the number of clusters generated
*/
int clusterGeneratorExact(
        const Ref<const MatrixXf>   &entities,
        Ref<MatrixXf>               centroids,
        MatrixXfR                   &weights,
        squaredNorm_t               *norm
    );


#include "line_interval.hpp"
#include "shape2d.hpp"
#include <Eigen/Dense>
#include <vector>
#include <array>
#include <iostream>
#define _USE_MATH_DEFINES
#include <math.h>

using namespace Eigen;
using namespace std;

array<double, 2> polarEquation(Vector2d v0, Vector2d v1){
    array<double, 3> line={v0[1]-v1[1], v1[0]-v0[0],-(v0[0]*v1[1]-v1[0]*v0[1])};

    double normalAngle=atan2(line[1], line[0]);
    double normalDist=line[2] / sqrt(line[0] * line[0] + line[1] * line[1]);

    if(normalDist<0){
	normalDist = -normalDist;
	normalAngle += M_PI;
	if(normalAngle>=M_PI){
	    normalAngle -= 2 * M_PI;
	}
    }

    array<double, 2> polar={normalDist, normalAngle};
    return polar;
}

LineInterval::LineInterval(Vector2d _point){
    point = _point;
    angleStart = atan2(point[1], point[0]);
    distLowerBound = 0.0;
    distUpperBound = 0.0;
    intervalMaxDists = {0.0, 0.0};
}

LineInterval::LineInterval(double _angleStart, double _angleEnd){
    angleStart=_angleStart;
    angleEnd=_angleEnd;
    distLowerBound = 0.0;
    distUpperBound = 0.0;
    intervalMaxDists = {0.0, 0.0};
}

double LineInterval::DistAt(array<double, 2> edge, int side){
    double angle=(side==0 ? angleStart : angleEnd);
    return edge[0]/cos(angle-edge[1]);
}

array<double, 3> LineInterval::Divide(){
    double mid=(angleStart+IntervalAngleEnd())/2;
    if(mid>=M_PI){
	mid -= 2 * M_PI;
    }
    return {angleStart, mid, angleEnd};
}

bool LineInterval::IntersectsEdge(double v0Angle, double v1Angle){
    double edgeAngleStart=v0Angle;
    double edgeAngleEnd=v1Angle;
    if(edgeAngleStart>edgeAngleEnd){
	if(angleStart<0){
	    edgeAngleStart -= M_PI * 2;
	} else {
	    edgeAngleEnd += M_PI * 2;
	}
    }
    return (edgeAngleStart<=angleStart && edgeAngleEnd>=IntervalAngleEnd());
}

double LineInterval::IntervalAngleEnd(){
    double intervalAngleEnd= (angleStart>angleEnd ? angleEnd + (2 * M_PI) : angleEnd);
    return intervalAngleEnd;
}

bool LineInterval::containsNormal(array<double, 2> edge){

    if(angleStart>angleEnd){
	return (edge[1]>=angleStart && edge[1]<=M_PI) || (edge[1]<=angleEnd && edge[1] >= -M_PI);
    } else {
	return (edge[1] >= angleStart && edge[1] <= angleEnd);
    }
}

array<double, 3> LineInterval::FunctionsAt(array<double, 2> edge, int side){
    double angle=(side==0 ? angleStart : angleEnd);
    //double dist=edge[0]/cos(angle-edge[1]);
    double dist=DistAt(edge, side);
    double distPrime=tan(angle-edge[1])*dist;

    double angleSin=sin(angle-edge[1]);
    double angleCos=cos(angle-edge[1]);

    double distPrime2=edge[0]*(1+angleSin*angleSin)/(angleCos*angleCos*angleCos);
    return {dist, distPrime, distPrime2};
}

double LineInterval::ApproxRoot(double distStart, double distEnd, double derivStart, double derivEnd){
    //double intervalAngleEnd= (angleStart>angleEnd ? angleEnd + (2 * M_PI) : angleEnd);

    double bStart=distStart - (angleStart * derivStart);
    double bEnd=distEnd - (IntervalAngleEnd() * derivEnd);

    Matrix2d A;
    A << derivStart, -1,   derivEnd, -1;
    Vector2d b;
    b << -bStart, -bEnd;

    return A.colPivHouseholderQr().solve(b)[1];
}

void LineInterval::SetAngleEnd(Vector2d _point){
    angleEnd = atan2(_point[1], _point[0]);
}

void LineInterval::update(double upperBound, double lowerBound, double distStart, double distEnd, unsigned long int shapeId){
    distUpperBound+=upperBound;
    distLowerBound+=lowerBound;

    if(distStart > max(intervalMaxDists[0], intervalMaxDists[1]) || distEnd > max(intervalMaxDists[0], intervalMaxDists[1])){
	intervalMaxDists={distStart, distEnd};
    }
    shapeIds.push_back(shapeId);
}

double LineInterval::MaxWidth(){
    //double intervalAngleEnd= (angleStart>angleEnd ? angleEnd + (2 * M_PI) : angleEnd);
    double b=intervalMaxDists[0];
    double c=intervalMaxDists[1];
    return sqrt( b*b + c*c - 2*b*c*cos(IntervalAngleEnd()-angleStart) );
}

array<Vector2d, 2> LineInterval::EndPoints(){
    Vector2d v0(intervalMaxDists[0]*cos(angleStart), intervalMaxDists[0]*sin(angleStart));
    Vector2d v1(intervalMaxDists[1]*cos(angleEnd), intervalMaxDists[1]*sin(angleEnd));
    return {v0, v1};
}

vector<unsigned long int> LineInterval::ShapeIds(){
    return shapeIds;
}

double LineInterval::LowerBound(){
    return distLowerBound;
}

double LineInterval::UpperBound(){
    return distUpperBound;
}

Vector2d LineInterval::Point(){
    return point;
}


#include <nori/object.h>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <boost/math/special_functions/fpclassify.hpp>

#if defined(PLATFORM_LINUX)
#include <malloc.h>
#endif

#if defined(PLATFORM_WINDOWS)
#include <windows.h>
#endif

#if defined(PLATFORM_MACOS)
#include <sys/sysctl.h>
#endif

#if !defined(L1_CACHE_LINE_SIZE)
#define L1_CACHE_LINE_SIZE 64
#endif

NORI_NAMESPACE_BEGIN

Color3f Color3f::toSRGB() const {
	Color3f result;

	for (int i=0; i<3; ++i) {
		float value = coeff(i);

		if (value <= 0.0031308f)
			result[i] = 12.92f * value;
		else
			result[i] = (1.0f + 0.055f)
				* std::pow(value, 1.0f/2.4f) -  0.055f;
	}

	return result;
}

Color3f Color3f::toLinearRGB() const {
	Color3f result;

	for (int i=0; i<3; ++i) {
		float value = coeff(i);

		if (value <= 0.04045f)
			result[i] = value * (1.0f / 12.92f);
		else
			result[i] = std::pow((value + 0.055f)
				* (1.0f / 1.055f), 2.4f);
	}

	return result;
}

bool Color3f::isValid() const {
	for (int i=0; i<3; ++i) {
		float value = coeff(i);
		int cl = boost::math::fpclassify(value);
		if (value < 0 || cl == FP_INFINITE || cl == FP_NAN)
			return false;
	}
	return true;
}

float Color3f::getLuminance() const {
	return coeff(0) * 0.212671f + coeff(1) * 0.715160f + coeff(2) * 0.072169f;
}

Transform::Transform(const Eigen::Matrix4f &trafo)
	: m_transform(trafo), m_inverse(trafo.inverse()) { }

QString Transform::toString() const {
	std::ostringstream oss;
	oss << m_transform.format(Eigen::IOFormat(4, 0, ", ", ";\n", "", "", "[", "]"));
	return QString(oss.str().c_str());
}
QString Transform::toLineString() const {
	std::ostringstream oss;
        for(int row = 0; row < 4; ++row){
            if(row > 0) oss << "; ";
            for(int col = 0; col < 4; ++col){
                if(col > 0) oss << ", ";
                oss << m_transform(row, col);
            }
        }
	return QString(oss.str().c_str());
}

Vector3f squareToUniformSphere(const Point2f &sample) {
	float z = 1.0f - 2.0f * sample.y();
	float r = std::sqrt(std::max((float) 0.0f, 1.0f - z*z));
	float sinPhi, cosPhi;
	sincosf(2.0f * M_PI * sample.x(), &sinPhi, &cosPhi);
	return Vector3f(r * cosPhi, r * sinPhi, z);
}

Vector3f squareToUniformHemisphere(const Point2f &sample) {
	float cosTheta = sample.x();
	float sinTheta = std::sqrt(std::max((float) 0, 1-cosTheta*cosTheta));

	float sinPhi, cosPhi;
	sincosf(2.0f * M_PI * sample.y(), &sinPhi, &cosPhi);

	return Vector3f(cosPhi * sinTheta, sinPhi * sinTheta, cosTheta);
}

Point2f squareToUniformDisk(const Point2f &sample) {
	float r = std::sqrt(sample.x());
	float sinPhi, cosPhi;
	sincosf(2.0f * M_PI * sample.y(), &sinPhi, &cosPhi);

	return Point2f(
		cosPhi * r,
		sinPhi * r
	);
}

Point2f squareToUniformDiskConcentric(const Point2f &sample) {
	float r1 = 2.0f*sample.x() - 1.0f;
	float r2 = 2.0f*sample.y() - 1.0f;

	Point2f coords;
	if (r1 == 0 && r2 == 0) {
		coords = Point2f(0, 0);
	} else if (r1 > -r2) { /* Regions 1/2 */
		if (r1 > r2)
			coords = Point2f(r1, (M_PI/4.0f) * r2/r1);
		else
			coords = Point2f(r2, (M_PI/4.0f) * (2.0f - r1/r2));
	} else { /* Regions 3/4 */
		if (r1<r2)
			coords = Point2f(-r1, (M_PI/4.0f) * (4.0f + r2/r1));
		else
			coords = Point2f(-r2, (M_PI/4.0f) * (6.0f - r1/r2));
	}

	Point2f result;
	sincosf(coords.y(), &result[1], &result[0]);
	return result*coords.x();
}

Point2f squareToUniformTriangle(const Point2f &sample) {
	float a = std::sqrt(1.0f - sample.x());
	return Point2f(1 - a, a * sample.y());
}

float intervalToTent(float sample) {
	float sign;

	if (sample < 0.5f) {
		sign = 1;
		sample *= 2;
	} else {
		sign = -1;
		sample = 2 * (sample - 0.5f);
	}

	return sign * (1 - std::sqrt(sample));
}

Point2f squareToTent(const Point2f &sample) {
	return Point2f(
		intervalToTent(sample.x()),
		intervalToTent(sample.y())
	);
}

Point2f squareToTriangle(const Point2f &sample) {
	float a = std::sqrt(1.0f - sample.x());
	return Point2f(1 - a, a * sample.y());
}

Vector3f sphericalDirection(float theta, float phi) {
	float sinTheta, cosTheta, sinPhi, cosPhi;

	sincosf(theta, &sinTheta, &cosTheta);
	sincosf(phi, &sinPhi, &cosPhi);

	return Vector3f(
		sinTheta * cosPhi,
		sinTheta * sinPhi,
		cosTheta
	);
}

Point2f sphericalCoordinates(const Vector3f &v) {
	Point2f result(
		std::acos(v.z()),
		std::atan2(v.y(), v.x())
	);
	if (result.y() < 0)
		result.y() += 2*M_PI;
	return result;
}

void coordinateSystem(const Vector3f &a, Vector3f &b, Vector3f &c) {
	if (std::abs(a.x()) > std::abs(a.y())) {
		float invLen = 1.0f / std::sqrt(a.x() * a.x() + a.z() * a.z());
		c = Vector3f(a.z() * invLen, 0.0f, -a.x() * invLen);
	} else {
		float invLen = 1.0f / std::sqrt(a.y() * a.y() + a.z() * a.z());
		c = Vector3f(0.0f, a.z() * invLen, -a.y() * invLen);
	}
	b = c.cross(a);
}

void *allocAligned(size_t size) {
#if defined(PLATFORM_WINDOWS)
	return _aligned_malloc(size, L1_CACHE_LINE_SIZE);
#elif defined(PLATFORM_MACOS)
	/* OSX malloc already returns 16-byte aligned data suitable
	   for AltiVec and SSE computations */
	return malloc(size);
#else
	return memalign(L1_CACHE_LINE_SIZE, size);
#endif
}

void freeAligned(void *ptr) {
#if defined(PLATFORM_WINDOWS)
	_aligned_free(ptr);
#else
	free(ptr);
#endif
}

int getCoreCount() {
#if defined(PLATFORM_WINDOWS)
	SYSTEM_INFO sys_info;
	GetSystemInfo(&sys_info);
	return sys_info.dwNumberOfProcessors;
#elif defined(PLATFORM_MACOS)
	int nprocs;
	size_t nprocsSize = sizeof(int);
	if (sysctlbyname("hw.activecpu", &nprocs, &nprocsSize, NULL, 0))
		throw NoriException("Could not detect the number of processors!");
	return (int) nprocs;
#else
	return sysconf(_SC_NPROCESSORS_CONF);
#endif
}

QString indent(const QString &string, int amount) {
	QString result = string;
	result.replace("\n", QString("\n") + QString(" ").repeated(amount));
	return result;
}

float fresnel(float cosThetaI, float extIOR, float intIOR) {
	float etaI = extIOR, etaT = intIOR;

	if (extIOR == intIOR)
		return 0.0f;

	/* Swap the indices of refraction if the interaction starts
	   at the inside of the object */
	if (cosThetaI < 0.0f) {
		std::swap(etaI, etaT);
		cosThetaI = -cosThetaI;
	}

	/* Using Snell's law, calculate the squared sine of the
	   angle between the normal and the transmitted ray */
	float eta = etaI / etaT,
		  sinThetaTSqr = eta*eta * (1-cosThetaI*cosThetaI);

	if (sinThetaTSqr > 1.0f)
		return 1.0f;  /* Total internal reflection! */

	float cosThetaT = std::sqrt(1.0f - sinThetaTSqr);

	float Rs = (etaI * cosThetaI - etaT * cosThetaT)
	         / (etaI * cosThetaI + etaT * cosThetaT);
	float Rp = (etaT * cosThetaI - etaI * cosThetaT)
	         / (etaT * cosThetaI + etaI * cosThetaT);

	return (Rs * Rs + Rp * Rp) / 2.0f;
}

NORI_NAMESPACE_END


//
//  mex_top_eig.cpp
//
//
//  Created by Bo_Royce on 8/17/16.
//
//
#include <mex.h>
#include <stdio.h>
#include <math.h>
#include <iostream>
#include <Eigen/SparseCore>
#include <Eigen/Core>
#include <SymEigsSolver.h>
#include <MatOp/SparseSymMatProd.h>
#include <time.h>
using namespace Eigen;
using namespace Spectra;
void make_top_eigenvectors(double *val,double *ind, int KK, int NN, int NK, double *eigenvectors,double *eigenvalues){
    clock_t begin = clock();
    Eigen::SparseMatrix<double> mat((const int) NN,(const int) NN);         // default is column major
    mat.reserve(Eigen::VectorXi::Constant((const int) NN, (const int) NK));
    typedef Eigen::Triplet<double> T;
    std::vector<T> tripletList;
    tripletList.reserve((const int) NN*NK);
    for(int i=0; i<NN; i++){
        for (int j = 0; j< NK; j++){
            tripletList.push_back(T((int) ind[i+NN*j]-1,i,val[i+j*NN]));
        }
    }
    mat.setFromTriplets(tripletList.begin(), tripletList.end());
    mat += Eigen::SparseMatrix<double>(mat.transpose());
    clock_t end = clock();
    //printf("Elapsed time in initialization is %f seconds\n", (double)(end - begin)/CLOCKS_PER_SEC);

    SparseSymMatProd<double> op(mat);
    begin = clock();
    // Construct eigen solver object, requesting the largest KK eigenvalues
    SymEigsSolver< double, LARGEST_ALGE, SparseSymMatProd<double> > eigs(&op, KK, 2*KK);

    // Initialize and compute
    eigs.init();
    int nconv = eigs.compute();

    // Retrieve results

    Eigen::VectorXd evalues;
    Eigen::MatrixXd evectors;
    if(eigs.info() == SUCCESSFUL){
        evalues = eigs.eigenvalues();
        evectors = eigs.eigenvectors();
    }
    //std::cout << "Eigenvalues found:\n" << evalues << std::endl;
    end = clock();
    printf("Elapsed time in eigen-decomposition is %f seconds\n", (double)(end - begin)/CLOCKS_PER_SEC);
    ///
    begin = clock();
    for (int j = 0; j< KK; j++){
        eigenvalues[j] = evalues[j];
        for (int i = 0; i < NN; i++){
            eigenvectors[i+NN*j] = evectors.col(j)[i];
        }
    }
   end = clock();
    printf("Elapsed time in copying eigenvectors is %f seconds\n", (double)(end - begin)/CLOCKS_PER_SEC);
    ///
}

/// usage: eigenvectors = mex_top_eig(val, ind, KK);
/// input: val of size NxK, the value of transition matrix
///        ind of size NxK, the index of the values (Note ind starts with 0);
///        KK , the number of eigenvectors
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
    double *val, *ind, *eigenvectors,*eigenvalues;
    int KK, NN, NK;
    val = mxGetPr(prhs[0]);
    ind = mxGetPr(prhs[1]); //
    KK = (int) mxGetScalar(prhs[2]); //number of eigenvalues
    NN = mxGetM(prhs[0]);
    NK = mxGetN(prhs[1]);
    plhs[0] = mxCreateDoubleMatrix(NN,KK,mxREAL);
    eigenvectors = mxGetPr(plhs[0]);
    plhs[1] = mxCreateDoubleMatrix(KK,1,mxREAL);
    eigenvalues = mxGetPr(plhs[1]);
    make_top_eigenvectors(val,ind, KK,  NN, NK, eigenvectors,eigenvalues);
}


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

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

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

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

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

/*
*
*   Tutorial: Calculation of eigenvalues using Lanczos' method (lanczos.cpp and lanczos.cu are identical, the latter being required for compilation using CUDA nvcc)
*
*/

// include necessary system headers
#include <iostream>

#ifndef NDEBUG
  #define NDEBUG
#endif

#define VIENNACL_WITH_UBLAS

//include basic scalar and vector types of ViennaCL
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/compressed_matrix.hpp"

#include "viennacl/linalg/lanczos.hpp"
#include "viennacl/io/matrix_market.hpp"
// Some helper functions for this tutorial:
#include <iostream>
#include <fstream>
#include <limits>
#include <string>
#include <iomanip>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>

template <typename MatrixType>
std::vector<double> initEig(MatrixType const & A)
{
  viennacl::linalg::lanczos_tag ltag(0.75, 10, viennacl::linalg::lanczos_tag::partial_reorthogonalization, 1700);
  std::vector<double> lanczos_eigenvalues = viennacl::linalg::eig(A, ltag);
  for(std::size_t i = 0; i< lanczos_eigenvalues.size(); i++){
          std::cout << "Eigenvalue " << i+1 << ": " << std::setprecision(10) << lanczos_eigenvalues[i] << std::endl;
  }

  return lanczos_eigenvalues;
}

int main()
{
  typedef double     ScalarType;

  boost::numeric::ublas::compressed_matrix<ScalarType> ublas_A;

  if (!viennacl::io::read_matrix_market_file(ublas_A, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return 0;
  }

  std::cout << "Running Lanczos algorithm (this might take a while)..." << std::endl;
  std::vector<double> eigenvalues = initEig(ublas_A);
}


// Includes
// ========
#include <iostream>
// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/find_flow_cost.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
  boost::no_property, boost::property<boost::edge_weight_t, long> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;

// Graph Type with nested interior edge properties for flow algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>> flow_graph;

typedef traits::vertex_descriptor vertex_desc;
typedef traits::edge_descriptor edge_desc;

// Custom edge adder class
class edge_adder {
 weighted_graph &G;

 public:
  explicit edge_adder(weighted_graph &G) : G(G) {}
  void add_edge(int from, int to, long weight) {
    auto w_map = boost::get(boost::edge_weight, G); // new!
    const edge_desc e = boost::add_edge(from, to, G).first;
    w_map[e] = weight;   // new assign cost
  }
};

// Custom edge adder class, highly recommended
class edge_adder_flow {
  flow_graph &G;

 public:
  explicit edge_adder_flow(flow_graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

void testcase() {
    int n, m, a, s, c, d;
    std::cin >> n >> m >> a >> s >> c >> d;
    weighted_graph G(n);
    edge_adder adder(G);

    for(int i = 0; i < m; i++) {
        char w; int x, y, z;
        std::cin >> w >> x >> y >> z;
        adder.add_edge(x, y, z);
        if(w == 'L') {
            adder.add_edge(y, x, z);
        }
    }

    std::vector<int> agent(a);
    for(int i = 0; i < a; i++) {
        std::cin >> agent[i];
    }

    std::vector<int> shelter(s);
    for(int i = 0; i < s; i++) {
        std::cin >> shelter[i];
    }

    std::vector<std::vector<long>> distance_to(a, std::vector<long>(s));
    long max_dist = 0;
    for(int i = 0; i < a; i++) {
        std::vector<int> dist_map(n);
        boost::dijkstra_shortest_paths(G, agent[i],
            boost::distance_map(boost::make_iterator_property_map(
            dist_map.begin(), boost::get(boost::vertex_index, G))));
        for(int j = 0; j < s; j++) {
            distance_to[i][j] = dist_map[shelter[j]];
            if(distance_to[i][j] < std::numeric_limits<int>::max()) {
                max_dist = std::max(max_dist, distance_to[i][j]);
            }
        }
    }

    int l = 0, r = max_dist + c * d;

    while(l < r) {
        // compute matching for all edges dist + c * d <= mid
        flow_graph G_f(a + c * s);
        edge_adder_flow adder_f(G_f);
        int mid = (l + r) / 2;
        const int v_source = boost::add_vertex(G_f);
        const int v_sink = boost::add_vertex(G_f);
        for(int i = 0; i < a; i++) {
            adder_f.add_edge(v_source, i, 1);
            for(int j = 0; j < s; j++) {
                if(distance_to[i][j] + d <= mid) adder_f.add_edge(i, a + j, 1);
                if(c == 2) {
                    if(distance_to[i][j] + 2 * d <= mid) adder_f.add_edge(i, a + s + j, 1);
                }
            }
        }
        for(int j = 0; j < s; j++) {
            adder_f.add_edge(a + j, v_sink, 1);
            if(c == 2) adder_f.add_edge(a + s + j, v_sink, 1);
        }
        long flow = boost::push_relabel_max_flow(G_f, v_source, v_sink);
        if(flow == a) { // matching maximal -> all agents arrive at shelter
            r = mid;
        } else {
            l = mid + 1;
        }
    }
    std::cout << l << std::endl;
    return;
}

int main() {
    std::ios_base::sync_with_stdio(false);

    int t;
    std::cin >> t;
    for (int i = 0; i < t; ++i)
        testcase();
}


#include "gcd.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include <boost/variant.hpp>
#include <stdexcept>
#include <algorithm>
#include <cmath>
namespace HT
{
    void gcd(PASTNode astnode, ParsersHelper& ph)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()<2)
          throw std::runtime_error("Gcd should have at least 1 parameter");
        auto  secondCh = (++astnode->ch.begin() );
        myParserHelper.parse(*secondCh);
        if ((*secondCh)->token.tokenType != Complex) throw std::runtime_error("the argument of gcd must be complex"+ (*secondCh)->token.raw);
        auto cast = boost::get<ComplexType>((*secondCh)->token.info);
        if (!cast.isInt())
          throw std::runtime_error("the argument of gcd must be int");
        auto now = cast.toInt();

        std::for_each(++secondCh, astnode->ch.end(), [&](PASTNode an)
                    {
                    myParserHelper.parse(an);
                    if (an->token.tokenType!=Complex) throw std::runtime_error("the argument of gcd must be complex" + an->token.raw);
                    cast = boost::get<ComplexType>(an->token.info);
                    if (!cast.isInt())
                        throw std::runtime_error("the argument of gcd must be int");
                    if (!cast.toInt().isZero())
                    now = gcd( now, cast.toInt());
                    });

        astnode->token.info = ComplexType(now.setSign(true));
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        astnode->remove();
    }
}


#include "teca_laplacian.h"

#include "teca_cartesian_mesh.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"

#include <algorithm>
#include <iostream>
#include <string>
#include <cmath>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

using std::string;
using std::vector;
using std::cerr;
using std::endl;
using std::cos;
using std::tan;

//#define TECA_DEBUG

namespace {

template <typename num_t>
constexpr num_t deg_to_rad() { return num_t(M_PI)/num_t(180); }

template <typename num_t>
constexpr num_t earth_radius() { return num_t(6371.0e3); }

// compute the laplacian. This  assumes fixed mesh spacing. Here we add periodic
// bc in lon and apply unit stride vector optimization strategy to loops
template <typename num_t, typename pt_t>
void laplacian(num_t *w, const pt_t *lon, const pt_t *lat,
    const num_t *f, unsigned long n_lon,
    unsigned long n_lat, bool periodic_lon=true)
{
    size_t n_bytes = n_lat*sizeof(num_t);
    num_t *delta_lon_sq = static_cast<num_t*>(malloc(n_bytes));

    // delta lon squared as a function of latitude
    num_t d_lon = (lon[1] - lon[0]) * deg_to_rad<num_t>() * earth_radius<num_t>();
    // tan(lat)
    num_t *tan_lat = static_cast<num_t*>(malloc(n_bytes));
    for (unsigned long j = 0; j < n_lat; ++j)
    {
        delta_lon_sq[j] = pow(d_lon * cos(lat[j] * deg_to_rad<num_t>()),2);
    	tan_lat[j] = tan(lat[j] * deg_to_rad<num_t>());
    }

    // delta lat squared
    num_t delta_v = (lat[1] - lat[0]) * deg_to_rad<num_t>() * earth_radius<num_t>();
    num_t dlat = num_t(2)*delta_v;
    num_t dlat_sq = delta_v*delta_v;
    dlat *= earth_radius<num_t>(); // scale dlat by R for the tan term

    unsigned long max_i = n_lon - 1;
    unsigned long max_j = n_lat - 1;

    // laplacian
    for (unsigned long j = 1; j < max_j; ++j)
    {
	// set the current row in the u/v/w arrays
        unsigned long jj = j*n_lon;
	/*
	 * The following f_* variables describe the field
	 * f in a grid oriented fashion:
	 *
	 *	f_ipjm	f_ipj	f_ipjp
	 *
	 *	f_ijm	f_ji	f_ijp
	 *
	 *	f_imjm	f_imj	f_imjp
	 *
	 * The 'j' direction represents longitude, the
	 * 'i' direciton represents latitude.
	 *
	 * Note: The laplacian represented here uses the chain
	 * rule to separate the (1/cos(lat)*d(cos(lat)*df/dlat)/dlat
	 * term into two terms.
	 *
	 */
	// Set array pointer locations so that index 'i' refers to the
	// shifted location in all variables
        const num_t *f_ij = f + jj;          // i,j
        const num_t *f_ipj = f + jj + n_lon; // i+1, j
        const num_t *f_imj = f + jj - n_lon; // i-1, j
        const num_t *f_ijp = f + jj + 1;     // i,   j + 1
        const num_t *f_ijm = f + jj - 1;     // i,   j - 1

	// set the pointer index for the output field w
	// ... this is index i,j
        num_t *ww = w + jj;
	// create a dummy variable for u**2
        num_t dlon_sq = delta_lon_sq[j];

        for (unsigned long i = 1; i < max_i; ++i)
        {
	    // calculate the laplacian in spherical coordinates, assuming
	    // constant radius R.
            ww[i] = (f_imj[i] - num_t(2)*f_ij[i] + f_ipj[i])/dlat_sq -
		    tan_lat[j]*(f_ipj[i]-f_imj[i])/dlat +
                    (f_ijm[i] - num_t(2)*f_ij[i] + f_ijp[i])/dlon_sq;
        }
    }

    if (periodic_lon)
    {
        // periodic in longitude; leftmost boundary
        for (unsigned long j = 1; j < max_j; ++j)
        {
	    // set the current row in the u/v/w arrays
            unsigned long jj = j*n_lon;
	    // Set array pointer locations so that index 'i' refers to the
	    // shifted location in all variables
            const num_t *f_ij = f + jj;          // i,j
            const num_t *f_ipj = f + jj + n_lon; // i+1, j
            const num_t *f_imj = f + jj - n_lon; // i-1, j
            const num_t *f_ijp = f + jj + 1;     // i,   j + 1
            const num_t *f_ijm = f + jj - max_i; // i,   j - 1

	    // set the pointer index for the output field w
	    // ... this is index i,j
            num_t *ww = w + jj;
	    // create a dummy variable for u**2
            num_t dlon_sq = delta_lon_sq[j];

	    // calculate the laplacian in spherical coordinates, assuming
	    // constant radius R.
            ww[0] = (f_imj[0] - num_t(2)*f_ij[0] + f_ipj[0])/dlat_sq -
		    tan_lat[j]*(f_ipj[0]-f_imj[0])/dlat +
                    (f_ijm[0] - num_t(2)*f_ij[0] + f_ijp[0])/dlon_sq;
        }

        // periodic in longitude; rightmost boundary
        for (unsigned long j = 1; j < max_j; ++j)
        {
	    // set the current row in the u/v/w arrays
            unsigned long jj = j*n_lon;

	    // Set array pointer locations so that index 'i' refers to the
	    // shifted location in all variables
            const num_t *f_ij = f + jj + max_i;          // i,j
            const num_t *f_ipj = f + jj + max_i + n_lon; // i+1, j
            const num_t *f_imj = f + jj + max_i - n_lon; // i-1, j
            const num_t *f_ijp = f + jj;                 // i,   j + 1
            const num_t *f_ijm = f + jj - max_i;         // i,   j - 1

	    // set the pointer index for the output field w
	    // ... this is index i,j
            num_t *ww = w + jj + max_i;
	    // create a dummy variable for u**2
            num_t dlon_sq = delta_lon_sq[j];

	    // calculate the laplacian in spherical coordinates, assuming
	    // constant radius R.
            ww[0] = (f_imj[0] - num_t(2)*f_ij[0] + f_ipj[0])/dlat_sq -
		    tan_lat[j]*(f_ipj[0]-f_imj[0])/dlat +
                    (f_ijm[0] - num_t(2)*f_ij[0] + f_ijp[0])/dlon_sq;
        }
    }
    else
    {
        // zero it out
        for (unsigned long j = 1; j < max_j; ++j)
            w[j*n_lon] = num_t();

        for (unsigned long j = 1; j < max_j; ++j)
            w[j*n_lon + max_i] = num_t();
    }

    // extend values into lat boundaries
    num_t *dest = w;
    num_t *src = w + n_lon;
    for (unsigned long i = 0; i < n_lon; ++i)
        dest[i] = src[i+n_lon];

    dest = w + max_j*n_lon;
    src = dest - n_lon;
    for (unsigned long i = 0; i < n_lon; ++i)
        dest[i] = src[i];

    free(delta_lon_sq);
    free(tan_lat);

    return;
}
};

// --------------------------------------------------------------------------
teca_laplacian::teca_laplacian() :
    component_0_variable(),
    laplacian_variable("laplacian")
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_laplacian::~teca_laplacian()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_laplacian::get_properties_description(
    const string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_laplacian":prefix));

    opts.add_options()
        TECA_POPTS_GET(std::string, prefix, component_0_variable,
            "array containing the input variable")
        TECA_POPTS_GET(std::string, prefix, laplacian_variable,
            "array to store the computed laplacian in")
        ;

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_laplacian::set_properties(
    const string &prefix, variables_map &opts)
{
    TECA_POPTS_SET(opts, std::string, prefix, component_0_variable)
    TECA_POPTS_SET(opts, std::string, prefix, laplacian_variable)
}
#endif

// --------------------------------------------------------------------------
std::string teca_laplacian::get_component_0_variable(
    const teca_metadata &request)
{
    std::string comp_0_var = this->component_0_variable;

    if (comp_0_var.empty() &&
        request.has("teca_laplacian::component_0_variable"))
            request.get("teca_laplacian::component_0_variable", comp_0_var);

    return comp_0_var;
}

// --------------------------------------------------------------------------
std::string teca_laplacian::get_laplacian_variable(
    const teca_metadata &request)
{
    std::string lapl_var = this->laplacian_variable;

    if (lapl_var.empty())
    {
        if (request.has("teca_laplacian::laplacian_variable"))
            request.get("teca_laplacian::laplacian_variable", lapl_var);
        else
            lapl_var = "laplacian";
    }

    return lapl_var;
}

// --------------------------------------------------------------------------
teca_metadata teca_laplacian::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_laplacian::get_output_metadata" << endl;
#endif
    (void)port;

    // add in the array we will generate
    teca_metadata out_md(input_md[0]);
    out_md.append("variables", this->laplacian_variable);

    return out_md;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_laplacian::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
    (void)port;
    (void)input_md;

    vector<teca_metadata> up_reqs;

    // get the name of the arrays we need to request
    std::string comp_0_var = this->get_component_0_variable(request);
    if (comp_0_var.empty())
    {
        TECA_ERROR("component 0 array was not specified")
        return up_reqs;
    }

    // copy the incoming request to preserve the downstream
    // requirements and add the arrays we need
    teca_metadata req(request);

    std::set<std::string> arrays;
    if (req.has("arrays"))
        req.get("arrays", arrays);

    arrays.insert(this->component_0_variable);

    // capture the array we produce
    arrays.erase(this->get_laplacian_variable(request));

    // update the request
    req.set("arrays", arrays);

    // send it up
    up_reqs.push_back(req);
    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_laplacian::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_laplacian::execute" << endl;
#endif
    (void)port;

    // get the input mesh
    const_p_teca_cartesian_mesh in_mesh
        = std::dynamic_pointer_cast<const teca_cartesian_mesh>(input_data[0]);

    if (!in_mesh)
    {
        TECA_ERROR("teca_cartesian_mesh is required")
        return nullptr;
    }

    // get component 0 array
    std::string comp_0_var = this->get_component_0_variable(request);

    if (comp_0_var.empty())
    {
        TECA_ERROR("component_0_variable was not specified")
        return nullptr;
    }

    const_p_teca_variant_array comp_0
        = in_mesh->get_point_arrays()->get(comp_0_var);

    if (!comp_0)
    {
        TECA_ERROR("requested array \"" << comp_0_var << "\" not present.")
        return nullptr;
    }

    // get the input coordinate arrays
    const_p_teca_variant_array lon = in_mesh->get_x_coordinates();
    const_p_teca_variant_array lat = in_mesh->get_y_coordinates();

    if (!lon || !lat)
    {
        TECA_ERROR("lat lon mesh cooridinates not present.")
        return nullptr;
    }

    // allocate the output array
    p_teca_variant_array lapl = comp_0->new_instance();
    lapl->resize(comp_0->size());

    // compute laplacian
    NESTED_TEMPLATE_DISPATCH_FP(
        const teca_variant_array_impl,
        lon.get(), 1,

        const NT1 *p_lon = dynamic_cast<const TT1*>(lon.get())->get();
        const NT1 *p_lat = dynamic_cast<const TT1*>(lat.get())->get();

        NESTED_TEMPLATE_DISPATCH_FP(
            teca_variant_array_impl,
            lapl.get(), 2,

            const NT2 *p_comp_0 = dynamic_cast<const TT2*>(comp_0.get())->get();
            NT2 *p_lapl = dynamic_cast<TT2*>(lapl.get())->get();

            ::laplacian(p_lapl, p_lon, p_lat,
                p_comp_0, lon->size(), lat->size());
            )
        )

    // create the output mesh, pass everything through, and
    // add the laplacian array
    p_teca_cartesian_mesh out_mesh = teca_cartesian_mesh::New();

    out_mesh->shallow_copy(
        std::const_pointer_cast<teca_cartesian_mesh>(in_mesh));

    out_mesh->get_point_arrays()->append(
        this->get_laplacian_variable(request), lapl);

    return out_mesh;
}


/** $Id: polygon.cxx 137064 2015-08-31 18:24:47Z jvansanten $
 * @file
 * @author Jakob van Santen <jakob.van.santen@desy.de>
 *
 * $Revision: 137064 $
 * $Date: 2015-08-31 12:24:47 -0600 (Mon, 31 Aug 2015) $
 */

#include <phys-services/surfaces/detail/polygon.h>
#include <boost/utility.hpp>

namespace I3Surfaces { namespace polygon {

namespace {

/// A counterclockwise curve is the basic building block of a convex hull
class ccw_curve : public std::vector<vec2> {
public:
	// Add a point to the curve
	void operator()(const vec2 &p)
	{
		// Remove points until the curve will be counterclockwise
		while (size() >= 2 && !ccw((*this)[size()-2], (*this)[size()-1], p))
			pop_back();
		push_back(p);
	}
private:
	static bool
	ccw(const vec2 &o, const vec2 &a, const vec2 &b)
	{
		// 2D cross product of OA and OB vectors, i.e. z-component of their 3D cross product.
		// positive, if OAB makes a counter-clockwise turn,
		// negative for clockwise turn, and zero if the points are collinear.
		return (a.x - o.x) * (b.y - o.y) - (a.y - o.y) * (b.x - o.x) > 0;
	}
};

}

/// Lifted from http://code.icecube.wisc.edu/svn/sandbox/ckopper/eventinjector/python/util/__init__.py
std::vector<vec2>
convex_hull(const std::vector<I3Position> &positions)
{
	std::vector<vec2> hull;

	// Build a set of unique points, sorted lexicographically
	std::set<vec2> points;
	std::transform(positions.begin(), positions.end(),
	    std::inserter(points, points.end()), vec2::from_I3Position);

	// Boring case: 1 point (perhaps repeated)
	if (points.size() <= 1) {
		std::copy(points.begin(), points.end(), std::back_inserter(hull));
		return hull;
	}

	// Build lower and upper hulls
	std::vector<vec2> lower = std::for_each(points.begin(), points.end(), ccw_curve());
	std::vector<vec2> upper = std::for_each(points.rbegin(), points.rend(), ccw_curve());

	// Concatenation of the lower and upper hulls gives the convex hull.
	// Last point of each list is omitted because it is repeated at the
	// beginning of the other list.
	std::copy(lower.begin(), lower.end()-1, std::back_inserter(hull));
	std::copy(upper.begin(), upper.end()-1, std::back_inserter(hull));

	return hull;
}

std::vector<vec2>
expand_polygon(const std::vector<vec2> &hull, double padding)
{
	std::vector<vec2> points;
	for (std::vector<vec2>::const_iterator p = hull.begin(); p != hull.end(); p++) {
		std::vector<vec2>::const_iterator next = boost::next(p);
		if (next == hull.end())
			next = hull.begin();
		std::vector<vec2>::const_iterator prev = boost::prior(
		    p == hull.begin() ? hull.end() : p);
		// normalized vector connecting this vertex to the next one
		vec2 d = vec2::normalized(next->x-p->x, next->y-p->y);
		// and the previous vertex to this one
		vec2 prev_d = vec2::normalized(p->x-prev->x, p->y-prev->y);
		// sine of the inner angle between the segments that meet here
		double det = prev_d.x*d.y - prev_d.y*d.x;
		if (det == 0.)
			log_fatal("Edges can't be [anti]parallel");
		vec2 outwards(prev_d.x-d.x, prev_d.y-d.y);
		points.push_back(vec2(p->x + outwards.x*padding/det, p->y + outwards.y*padding/det));
	}

	return points;
}

vec2::vec2(double xi, double yi) : x(xi), y(yi)
{}

template <typename Archive>
void vec2::serialize(Archive &ar, unsigned version)
{
	if (version > 0)
		log_fatal_stream("Version "<<version<<" is from the future");

	ar & make_nvp("X", x);
	ar & make_nvp("Y", y);
}

vec2
vec2::from_I3Position(const I3Position &p)
{
	return vec2(p.GetX(), p.GetY());
}

vec2
vec2::normalized(double xi, double yi)
{
	double l = hypot(xi, yi);
	return vec2(xi/l, yi/l);
}

bool
operator<(const vec2 &a, const vec2 &b)
{
	if (a.x < b.x)
		return true;
	else if (a.x > b.x)
		return false;
	else if (a.y < b.y)
		return true;
	else
		return false;
}

side::side(const vec2 &p, const vec2 &np) : origin(p),
    vector(np.x-p.x, np.y-p.y), length(hypot(vector.x, vector.y)),
	normal(vector.y/length, -vector.x/length, 0.)
{}

}}

I3_SERIALIZABLE(I3Surfaces::polygon::vec2);


/*
 * This file is part of the Visual Computing Library (VCL) release under the
 * MIT license.
 *
 * Copyright (c) 2014 Basil Fierz
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

// VCL configuration
#include <vcl/config/global.h>

// C++ standard library
#include <iostream>
#include <random>

// Eigen library
#include <Eigen/Dense>

// VCL
#include <vcl/core/simd/vectorscalar.h>
#include <vcl/core/interleavedarray.h>
#include <vcl/math/jacobieigen33_selfadjoint.h>
#include <vcl/math/jacobieigen33_selfadjoint_quat.h>
#include <vcl/util/precisetimer.h>

template<typename Scalar>
Vcl::Core::InterleavedArray<Scalar, 3, 3, -1> createProblems(size_t nr_problems)
{
	// Random number generator
	std::mt19937_64 rng;
	std::uniform_real_distribution<float> d;

	Vcl::Core::InterleavedArray<Scalar, 3, 3, -1> F(nr_problems);

	// Initialize data
	for (size_t i = 0; i < nr_problems; i++)
	{
		Eigen::Matrix<Scalar, 3, 3> rnd;
		rnd << d(rng), d(rng), d(rng),
			   d(rng), d(rng), d(rng),
			   d(rng), d(rng), d(rng);
		F.template at<Scalar>(i) = rnd.transpose() * rnd;
	}

	return std::move(F);
}

void perfEigenEigen
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& resU,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& resS
)
{
	Vcl::Util::PreciseTimer timer;
	timer.start();
#ifdef _OPENMP
#	pragma omp parallel for
#endif /* _OPENMP */
	for (size_t i = 0; i < nr_problems; i++)
	{
		// Map data
		Vcl::Matrix3f A = F.at<float>(i);

		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
		solver.compute(A, Eigen::ComputeEigenvectors);

		resU.at<float>(i) = solver.eigenvectors();
		resS.at<float>(i) = solver.eigenvalues();
	}
	timer.stop();
	std::cout << "Eigen Jacobi SVD: " << timer.interval() / nr_problems * 1e9 << "[ns]" << std::endl;
}

void perfEigenEigenDirect
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& resU,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& resS
)
{
	Vcl::Util::PreciseTimer timer;
	timer.start();
#ifdef _OPENMP
#	pragma omp parallel for
#endif /* _OPENMP */
	for (size_t i = 0; i < nr_problems; i++)
	{
		// Map data
		Vcl::Matrix3f A = F.at<float>(i);

		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
		solver.computeDirect(A, Eigen::ComputeEigenvectors);

		resU.at<float>(i) = solver.eigenvectors();
		resS.at<float>(i) = solver.eigenvalues();
	}
	timer.stop();
	std::cout << "Eigen Jacobi SVD: " << timer.interval() / nr_problems * 1e9 << "[ns]" << std::endl;
}

template<typename WideScalar>
void perfJacobiEigen
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& resU,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& resS
)
{
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;

	size_t width = sizeof(real_t) / sizeof(float);

	Vcl::Util::PreciseTimer timer;
	timer.start();
	int avg_nr_iter = 0;
#ifdef _OPENMP
#	pragma omp parallel for
#endif /* _OPENMP */
	for (size_t i = 0; i < nr_problems / width; i++)
	{
		// Map data
		auto U = resU.at<real_t>(i);
		auto S = resS.at<real_t>(i);

		// Compute SVD using 2-sided Jacobi iterations (Brent)
		matrix3_t SV = F.at<real_t>(i);
		matrix3_t matU = matrix3_t::Identity();

		avg_nr_iter += Vcl::Mathematics::SelfAdjointJacobiEigen(SV, matU);

		// Store results
		U = matU;
		S = SV.diagonal();
	}
	timer.stop();
	std::cout << "Self-adjoint Jacobi Eigen Decomposition: " << timer.interval() / nr_problems * 1e9 << "[ns], Avg. iterations: " << (double) (avg_nr_iter * width) / (double) nr_problems << std::endl;
}

template<typename WideScalar>
void perfJacobiEigenQuat
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& resU,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& resS
)
{
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;

	size_t width = sizeof(real_t) / sizeof(float);

	Vcl::Util::PreciseTimer timer;
	timer.start();
	int avg_nr_iter = 0;
#ifdef _OPENMP
#	pragma omp parallel for
#endif /* _OPENMP */
	for (size_t i = 0; i < nr_problems / width; i++)
	{
		// Map data
		auto U = resU.at<real_t>(i);
		auto S = resS.at<real_t>(i);

		// Compute SVD using Jacobi iterations and QR decomposition
		matrix3_t SV = F.at<real_t>(i);
		matrix3_t matU = matrix3_t::Identity();

		avg_nr_iter += Vcl::Mathematics::SelfAdjointJacobiEigenQuat(SV, matU);

		// Store results
		U = matU;
		S = SV.diagonal();
	}
	timer.stop();
	std::cout << "Self-adjoint Jacobi Quaternion Eigen Decomposition: " << timer.interval() / nr_problems * 1e9 << "[ns], Avg. iterations: " << (double) (avg_nr_iter * width) / (double) nr_problems << std::endl;
}
int main(int, char**)
{
	size_t nr_problems = 1024*1024;

	Vcl::Core::InterleavedArray<float, 3, 3, -1> resU(nr_problems);
	Vcl::Core::InterleavedArray<float, 3, 1, -1> resS(nr_problems);

	// Initialize data
	auto F = createProblems<float>(nr_problems);

	// Test Performance: Eigen Jacobi Decomposition
	perfEigenEigen(nr_problems, F, resU, resS);
	perfEigenEigenDirect(nr_problems, F, resU, resS);

	// Test Performance: Jacobi Eigenvalue Decomposition
	perfJacobiEigen<float>(nr_problems, F, resU, resS);
	perfJacobiEigen<Vcl::float4>(nr_problems, F, resU, resS);
	perfJacobiEigen<Vcl::float8>(nr_problems, F, resU, resS);
	perfJacobiEigen<Vcl::float16>(nr_problems, F, resU, resS);

	// Test Performance: Jacobi Eigenvalue Decomposition using quaternions
	perfJacobiEigenQuat<float>(nr_problems, F, resU, resS);
	perfJacobiEigenQuat<Vcl::float4>(nr_problems, F, resU, resS);
	perfJacobiEigenQuat<Vcl::float8>(nr_problems, F, resU, resS);
	perfJacobiEigenQuat<Vcl::float16>(nr_problems, F, resU, resS);
}


#include "solvers.hpp"

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Sparse>

#include <Eigen/QR>
#include <Eigen/LU>
#include <Eigen/SparseLU>

// -----------------------------------------------------------------------------

typedef Eigen::Triplet<double, int> Triplet;
/// declares a column-major sparse matrix type of double
typedef Eigen::SparseMatrix<double> Sparse_mat;

// -----------------------------------------------------------------------------

/// Are angles obtuse between e0 and e1 (meaning a < PI/2)
static bool check_obtuse(const Vec3& e0,
                         const Vec3& e1)
{
    return e0.normalized().dot( e1.normalized() ) >= 0.f;
}

// -----------------------------------------------------------------------------

/**
 * @brief Check if a triangle is obtuse (every angles < PI/2)
 * Here how edges must be defined :
   @code
      p0
      |\
      | \
      |  \
      |   \
      |____\
     p1     p2

    Vec3 e0 = p1 - p0;
    Vec3 e1 = p2 - p0;
    Vec3 e2 = p2 - p1;
   @endcode
 */
static bool check_obtuse(const Vec3& e0,
                         const Vec3& e1,
                         const Vec3& e2)
{
    return check_obtuse(e0, e1) && check_obtuse(-e0, e2) && check_obtuse(-e1, -e2);
}

// -----------------------------------------------------------------------------

inline static
double mixed_voronoi_area(const Vec3& pi,
                          const Vec3& pj0,
                          const Vec3& pj1)
{
    double area = 0.;
    Vec3 e0 = pj0 - pi ;
    Vec3 e1 = pj1 - pi;
    Vec3 e2 = pj1 - pj0;

    if( check_obtuse(e0, e1, e2) )
    {
        area = (1/8.) * (double)(e0.norm_squared() * (-e0).cotan( e2) +
                                 e1.norm_squared() * (-e1).cotan(-e2));
    }
    else
    {
        const double ta = (double)e0.cross( e1 ).norm() / 2.; // Tri area
        area = ta / (check_obtuse(e0, e1) ? 2. : 4.);
    }
    return area;
}

// -----------------------------------------------------------------------------

static
double get_cell_area(int vidx,
                     const std::vector< Vec3 >& vertices,
                     const std::vector< std::vector<int> >& edges )
{
    double area = 0.0;
    const Vec3 c_pos = vertices[vidx];
    //get triangles areas
    for(int e = 0; e < (int)edges[vidx].size(); ++e)
    {
        int ne = (e + 1) % edges[vidx].size();

        if(true){
            // Supposidely more precise (but a bit more complex to compute)
            Vec3 p0 = vertices[edges[vidx][e ] ];
            Vec3 p1 = vertices[edges[vidx][ne]];
            area += mixed_voronoi_area(c_pos, p0, p1 );
        }else{
            // This should give descent results as well:
            Vec3 edge0 = vertices[edges[vidx][e ]] - c_pos;
            Vec3 edge1 = vertices[edges[vidx][ne]] - c_pos;
            area += (edge0.cross(edge1)).norm() / 2.f;
        }
    }
    return area;
}

// -----------------------------------------------------------------------------

static
float angle_between(const Vec3& v1, const Vec3& v2)
{
    float cosa = v1.dot(v2);
    if(cosa >= 1.f)
        return 0.f;
    else if(cosa <= -1.f)
        return M_PI;
    else
        return std::acos(cosa);
}

// -----------------------------------------------------------------------------

/// @return A sparse representation of the Laplacian matrix 'L'
/// list[ith_row][list of columns] = Triplet(ith_row, jth_column, matrix value)
static
std::vector<std::vector<Triplet>>
get_laplacian(const std::vector< Vec3 >& vertices,
              const std::vector< std::vector<int> >& edges )
{
    std::cout << "BUILD LAPLACIAN MATRIX" << std::endl;
    unsigned nv = unsigned(vertices.size());
    std::vector<std::vector<Triplet>> mat_elemts(nv);
    for(int i = 0; i < nv; ++i)
        mat_elemts[i].reserve(10);

    for(int i = 0; i < nv; ++i)
    {
        const Vec3 c_pos = vertices[i];

        //get laplacian
        double sum = 0.;
        int nb_edges = edges[i].size();
        for(int e = 0; e < nb_edges; ++e)
        {
            int next_edge = (e + 1           ) % nb_edges;
            int prev_edge = (e + nb_edges - 1) % nb_edges;

            /*                                 next_edge
                                    e ◀---v4---(cotan2)
                                   ◥ ◤         /
                                  /   \       /
                                 v2    v5    v3
                                /       \   /
                               /         \ ◣
                        (cotan1)----v1---▶c_pos
                       prev_edge
            */
            Vec3 v1 = c_pos - vertices[edges[i][prev_edge]];
            Vec3 v3 = c_pos - vertices[edges[i][next_edge]];
            double w = 0.0;
            if(true)
            {
                /* Cotangent weights
                 * (may be negative and undesirable in certain situations)
                */
                Vec3 v2 = vertices[edges[i][e]] - vertices[edges[i][prev_edge]];
                Vec3 v4 = vertices[edges[i][e]] - vertices[edges[i][next_edge]];

                double cotan1 = (v1.dot(v2)) / (1e-6 + (v1.cross(v2)).norm() );
                double cotan2 = (v3.dot(v4)) / (1e-6 + (v3.cross(v4)).norm() );

                // TODO: check for edge cases such as
                // the mesh corners and boundaries and adjust cotan weights
                // appropriatly ...
                w = (cotan1 + cotan2) * 0.5f;
            } else {
                // Mean value coordinations weights:
                // doesn't really work something must be wrong
                Vec3 v5 = c_pos - vertices[edges[i][e]];
                v1.normalize();
                v3.normalize();
                float v5_norm = v5.normalize();
                double tan1 = std::tan(angle_between(-v1, v5)*0.5f);
                double tan2 = std::tan(angle_between(-v3, v5)*0.5f);
                w = (tan1 + tan2) / (1e-6 + v5_norm);
            }

            // Disable / Enable multiplying against the inverse of
            // the Mass matrix 'M':
            if(false)
            {
                // If we want to return M^{-1}.L instead of just L
                // Then we can do it here since its more efficient
                // than building M^{-1} and then do the product M^{-1}.L
                // Since we solve for harmonic weights
                // M^{-1}.L = 0 can be simplified to L = 0
                // and this step safely ignored
                double area = get_cell_area(i, vertices, edges);
                area = 1. / ((1e-10 + area));
                w *= area;
            }

            sum += w;

            mat_elemts[i].push_back( Triplet(i, edges[i][e], w) );
        }

        mat_elemts[i].push_back( Triplet(i, i, -sum) );
    }
    return mat_elemts;
}

//------------------------------------------------------------------------------

/*
    Alternate implementation of the Laplacian matrix using only the
    list of triangles instead of the first ring neighboors.
*/
static
std::vector<std::vector<Triplet>>
get_laplacian(const std::vector< Vec3 >& vertices,
              const std::vector< Tri_face >& triangles )
{

    unsigned nv = unsigned(vertices.size());
    std::vector<std::vector<Triplet>> mat_elemts(nv);
    for(int i = 0; i < nv; ++i)
        mat_elemts[i].reserve(10);

    for( const Tri_face& f : triangles)
    {
        struct Edge { int i, j, org; };
        std::vector<Edge> edges =
        {
            {f.a, f.b, f.c},
            {f.b, f.c, f.a},
            {f.c, f.a, f.b},
        };

        for(Edge edge : edges)
        {
            /*
                                    j
                                   ◥
                                  /  \
                                 v2   \
                                /      \
                               /        \
                        (cotan)----v1---▶ i
                           org
            */
            Vec3 v1 = vertices[edge.org] - vertices[edge.i];
            Vec3 v2 = vertices[edge.org] - vertices[edge.j];
            double cotan = (v1.dot(v2)) / (1e-6 + (v1.cross(v2)).norm() );
            float w = cotan * 0.5f;

            int i = edge.i;
            int j = edge.j;
            // Note Eigen::setFromTriplets will sum up duplicate elements for us
            mat_elemts[i].push_back( Triplet(i, j,  w) );
            mat_elemts[j].push_back( Triplet(j, i,  w) );
            mat_elemts[i].push_back( Triplet(i, i, -w) );
            mat_elemts[j].push_back( Triplet(j, j, -w) );
        }
    }
    return mat_elemts;
}

//------------------------------------------------------------------------------

// Compute harmonic weights
void solve_laplace_equation(const std::vector< Vec3 >& vertices,
        const std::vector< std::vector<int> >& edges,
        const std::vector<Tri_face>& triangles,
        const std::vector<std::pair<Vert_idx, float> >& boundaries,
        std::vector<double>& harmonic_weight_map)
{
    std::cout << "COMPUTE LAPLACE EQUATION" << std::endl;

    int nv = vertices.size();

    // compute laplacian matrix of the mesh
    /*
        We can build the laplacian 'L' either from the half edge data structure
        (edges) or simply the list of triangles.
        For reference both versions are implemented here.
    */
    assert(edges.size() > 0 || triangles.size() > 0 );
    std::vector<std::vector<Triplet>> mat_elemts;
    if( edges.size() > 0)
        mat_elemts = get_laplacian(vertices, edges);
    else if( triangles.size() > 0 )
        mat_elemts = get_laplacian(vertices, triangles);

    // Set boundary conditions
    Eigen::VectorXd rhs = Eigen::VectorXd::Constant(nv, 0.);
    // Initialize handle
    for(const std::pair<int, float>& elt : boundaries){
        rhs( elt.first ) = double(elt.second);
        // Set row to 0.0f
        mat_elemts[elt.first].clear();
        // Set
        mat_elemts[elt.first].push_back( Triplet(elt.first, elt.first, 1.0) );
    }

#if 0
    // Solving with a dense matrix is Extremely slow
    Eigen::MatrixXd L = Eigen::MatrixXd::Constant(nv, nv, 0.);
    for( const std::vector<Triplet>& row : mat_elemts)
        for( const Triplet& elt : row )
            L(elt.row(), elt.col()) = elt.value();

    //Eigen::ColPivHouseholderQR<Eigen::MatrixXd> llt;
    Eigen::FullPivLU<Eigen::MatrixXd> solver;
    std::cout << "BEGIN MATRIX FACTORIZATION" << std::endl;
    solver.compute( L );
    std::cout << "END MATRIX FACTORIZATION" << std::endl;

#else
    Sparse_mat L(nv, nv);
    // Convert to triplets
    std::vector<Triplet> triplets;
    triplets.reserve(nv * 10);
    for( const std::vector<Triplet>& row : mat_elemts)
        for( const Triplet& elt : row )
            triplets.push_back( elt );

    L.setFromTriplets(triplets.begin(), triplets.end());

    Eigen::SparseLU<Sparse_mat> solver;
    std::cout << "BEGIN SPARSE MATRIX FACTORIZATION" << std::endl;
    solver.compute( L );
    std::cout << "END SPARSE MATRIX FACTORIZATION" << std::endl;
#endif

    harmonic_weight_map.resize(nv);
    Eigen::VectorXd res = solver.solve( rhs );
    for(int i = 0; i < nv; ++i)
        harmonic_weight_map[i] = res(i);

    return;
}

// -----------------------------------------------------------------------------


// Author: Tucker Haydon

#include "gnuplot-iostream.h"

#include <cstdlib>
#include <Eigen/Dense>

#include "polynomial_solver.h"
#include "polynomial_sampler.h"

using namespace p4;

int main(int argc, char** argv) {

  // Time in seconds
  const std::vector<double> times = {0, 1, 2.5};

  // NodeEqualityBound(dimension_idx, node_idx, derivative_idx, value)
  const std::vector<NodeEqualityBound> node_equality_bounds = {
    // Constraining position and velocity of first node to zero
    NodeEqualityBound(0,0,0,0),
    NodeEqualityBound(1,0,0,0),
    NodeEqualityBound(2,0,0,0),
    NodeEqualityBound(0,0,1,0),
    NodeEqualityBound(1,0,1,0),
    NodeEqualityBound(2,0,1,0),

    // Other nodes may constrain whatever they want
    // The second node is constraining position to (1,0,0)
    NodeEqualityBound(0,1,0,1),
    NodeEqualityBound(1,1,0,0),
    NodeEqualityBound(2,1,0,0),

    // The third node is contraining position to (1,1,free)
    NodeEqualityBound(0,2,0,1),
    NodeEqualityBound(1,2,0,1),
  };

  // NodeInequalityBound(dimension_idx, node_idx, derivative_idx, lower, upper)
  const std::vector<NodeInequalityBound> node_inequality_bounds = {
    // Constraining the z value of the third node above 0.5
    NodeInequalityBound(2,2,0,0.5,NodeInequalityBound::INFTY),
  };

  // SegmentInequalityBound(segment_idx, derivative_idx, mapping, value)
  // Segment inequality bounds constrain a derivative of a segment to
  //   dot(a,x) < b
  const std::vector<SegmentInequalityBound> segment_inequality_bounds = {
    // Constraining the x-acceleration of the first segment below 4 m/s^2
    SegmentInequalityBound(0,2,Eigen::Vector3d(1,0,0),4),
  };

  // Configure solver options
  PolynomialSolver::Options solver_options;
  solver_options.num_dimensions   = 3;   // 3D
  solver_options.polynomial_order = 8;   // Fit an 8th-order polynomial
  solver_options.continuity_order = 4;   // Require continuity through the 4th derivative
  solver_options.derivative_order = 2;   // Minimize the 2nd derivative (acceleration)

  // Configure the OSQP settings
  // Reference: https://osqp.org/docs/interfaces/cc++#settings
  solver_options.osqp_settings.polish = true;       // Polish the solution, getting the best answer possible
  solver_options.osqp_settings.verbose = false;     // Suppress the printout

  // Solve
  PolynomialSolver solver(solver_options);
  const PolynomialSolver::Solution solution
    = solver.Run(
        times,
        node_equality_bounds,
        node_inequality_bounds,
        segment_inequality_bounds);

  // Print some output info
  // Reference: https://osqp.org/docs/interfaces/cc++#info
  std::cout << "Status:                    " << solution.workspace->info->status << std::endl;
  std::cout << "Status Val (1 == success): " << solution.workspace->info->status_val << std::endl;
  std::cout << "Optimal Cost:              " << solution.workspace->info->obj_val << std::endl;

  // Sampling and Plotting
  { // Plot acceleration profiles
    PolynomialSampler::Options sampler_options;
    sampler_options.frequency = 100;
    sampler_options.derivative_order = 2;

    PolynomialSampler sampler(sampler_options);
    Eigen::MatrixXd samples = sampler.Run(times, solution);

    std::vector<double> t_hist, x_hist, y_hist, z_hist;
    for(size_t time_idx = 0; time_idx < samples.cols(); ++time_idx) {
      t_hist.push_back(samples(0,time_idx));
      x_hist.push_back(samples(1,time_idx));
      y_hist.push_back(samples(2,time_idx));
      z_hist.push_back(samples(3,time_idx));
    }

    Gnuplot gp;
    gp << "plot '-' using 1:2 with lines title 'X-Acceleration'";
    gp << ", '-' using 1:2 with lines title 'Y-Acceleration'";
    gp << ", '-' using 1:2 with lines title 'Z-Acceleration'";
    gp << std::endl;
    gp.send1d(boost::make_tuple(t_hist, x_hist));
    gp.send1d(boost::make_tuple(t_hist, y_hist));
    gp.send1d(boost::make_tuple(t_hist, z_hist));
    gp << "set grid" << std::endl;
    gp << "replot" << std::endl;
  }

  { // Plot velocity profiles
    PolynomialSampler::Options sampler_options;
    sampler_options.frequency = 100;
    sampler_options.derivative_order = 1;

    PolynomialSampler sampler(sampler_options);
    Eigen::MatrixXd samples = sampler.Run(times, solution);

    std::vector<double> t_hist, x_hist, y_hist, z_hist;
    for(size_t time_idx = 0; time_idx < samples.cols(); ++time_idx) {
      t_hist.push_back(samples(0,time_idx));
      x_hist.push_back(samples(1,time_idx));
      y_hist.push_back(samples(2,time_idx));
      z_hist.push_back(samples(3,time_idx));
    }

    Gnuplot gp;
    gp << "plot '-' using 1:2 with lines title 'X-Velocity'";
    gp << ", '-' using 1:2 with lines title 'Y-Velocity'";
    gp << ", '-' using 1:2 with lines title 'Z-Velocity'";
    gp << std::endl;
    gp.send1d(boost::make_tuple(t_hist, x_hist));
    gp.send1d(boost::make_tuple(t_hist, y_hist));
    gp.send1d(boost::make_tuple(t_hist, z_hist));
    gp << "set grid" << std::endl;
    gp << "replot" << std::endl;
  }

  { // Plot 3D position
    PolynomialSampler::Options sampler_options;
    sampler_options.frequency = 50;
    sampler_options.derivative_order = 0;

    PolynomialSampler sampler(sampler_options);
    Eigen::MatrixXd samples = sampler.Run(times, solution);

    std::vector<double> t_hist, x_hist, y_hist, z_hist;
    for(size_t time_idx = 0; time_idx < samples.cols(); ++time_idx) {
      t_hist.push_back(samples(0,time_idx));
      x_hist.push_back(samples(1,time_idx));
      y_hist.push_back(samples(2,time_idx));
      z_hist.push_back(samples(3,time_idx));
    }

    Gnuplot gp;
    gp << "splot '-' using 1:2:3 with lines title 'Trajectory'" << std::endl;
    gp.send1d(boost::make_tuple(x_hist, y_hist, z_hist));
    gp << "set grid" << std::endl;
    gp << "replot" << std::endl;

    // Must keep position gp in scope to rotate 3D graph
    std::cout << "Press enter to exit." << std::endl;
    std::cin.get();
  }

  return EXIT_SUCCESS;
}