#include <geometrycentral/surface/direction_fields.h>

#include "geometrycentral/numerical/linear_solvers.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <Eigen/SparseQR>

using std::cout;
using std::endl;

namespace geometrycentral {
namespace surface {

namespace {

SparseMatrix<std::complex<double>> computeVertexConnectionLaplacian(IntrinsicGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  geometry.requireVertexIndices();
  geometry.requireEdgeCotanWeights();
  geometry.requireTransportVectorsAlongHalfedge();

  int numInvalid = 0;

  std::vector<Eigen::Triplet<std::complex<double>>> triplets;
  for (Halfedge he : mesh.halfedges()) {

    size_t iTail = geometry.vertexIndices[he.vertex()];
    size_t iTip = geometry.vertexIndices[he.next().vertex()];

    // Levi-Civita connection between vertices
    Vector2 rot = geometry.transportVectorsAlongHalfedge[he.twin()].pow(nSym);
    double weight = geometry.edgeCotanWeights[he.edge()];
    if (!rot.isFinite() || !isfinite(weight)) {
#ifndef GC_NLINALG_DEBUG
      //std::cout << "computeVertexConnectionLaplacian: Oopsie at " << iTip << " x " << iTail << std::endl;
#endif
      weight = 0;
      rot = Vector2::zero();
      numInvalid++;
    }

    triplets.emplace_back(iTail, iTail, weight);
    triplets.emplace_back(iTail, iTip, -weight * rot);
  }
  if (numInvalid > 0) {
    std::cout << "computeVertexConnectionLaplacian: " << numInvalid << " invalid entried." << std::endl;
  }

  // assemble matrix from triplets
  Eigen::SparseMatrix<std::complex<double>> vertexConnectionLaplacian(mesh.nVertices(), mesh.nVertices());
  vertexConnectionLaplacian.setFromTriplets(triplets.begin(), triplets.end());

  // Shift to avoid singularity
  Eigen::SparseMatrix<std::complex<double>> eye(mesh.nVertices(), mesh.nVertices());
  eye.setIdentity();
  vertexConnectionLaplacian += 1e-9 * eye;

  return vertexConnectionLaplacian;
}

SparseMatrix<std::complex<double>> computeFaceConnectionLaplacian(IntrinsicGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  geometry.requireFaceIndices();
  geometry.requireTransportVectorsAcrossHalfedge();

  std::vector<Eigen::Triplet<std::complex<double>>> triplets;
  for (Face f : mesh.faces()) {
    size_t i = geometry.faceIndices[f];

    std::complex<double> weightDiagSum = 0;
    for (Halfedge he : f.adjacentHalfedges()) {
      if (he.twin().isInterior()) {

        Face neighFace = he.twin().face();
        size_t j = geometry.faceIndices[neighFace];

        // Levi-Civita connection between the faces
        Vector2 rot = geometry.transportVectorsAcrossHalfedge[he.twin()].pow(nSym);

        double weight = 1;
        triplets.emplace_back(i, j, -weight * rot);

        weightDiagSum += weight;
      }
    }

    triplets.emplace_back(i, i, weightDiagSum + 1e-9); // Shift to avoid singularity
  }
  // assemble matrix from triplets
  Eigen::SparseMatrix<std::complex<double>> faceConnectionLaplacian(mesh.nFaces(), mesh.nFaces());
  faceConnectionLaplacian.setFromTriplets(triplets.begin(), triplets.end());

  return faceConnectionLaplacian;
}

} // namespace

VertexData<Vector2> computeSmoothestVertexDirectionField(IntrinsicGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  geometry.requireVertexIndices();
  geometry.requireVertexGalerkinMassMatrix();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.vertexGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = computeVertexConnectionLaplacian(geometry, nSym);

  // Find the smallest eigenvector
  Vector<std::complex<double>> solution = smallestEigenvectorSquare(energyMatrix, massMatrix);

  // Copy the result to a VertexData vector
  VertexData<Vector2> toReturn(mesh);
  for (Vertex v : mesh.vertices()) {
    toReturn[v] = Vector2::fromComplex(solution(geometry.vertexIndices[v]));
    toReturn[v] = unit(toReturn[v]);
  }

  return toReturn;
}

FaceData<Vector2> computeSmoothestFaceDirectionField(IntrinsicGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  geometry.requireFaceIndices();
  geometry.requireFaceGalerkinMassMatrix();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.faceGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = computeFaceConnectionLaplacian(geometry, nSym);

  // Find the smallest eigenvector
  Vector<std::complex<double>> solution = smallestEigenvectorSquare(energyMatrix, massMatrix);

  // Copy the result to a FaceData vector
  FaceData<Vector2> toReturn(mesh);
  for (Face f : mesh.faces()) {
    toReturn[f] = Vector2::fromComplex(solution(geometry.faceIndices[f]));
    toReturn[f] = unit(toReturn[f]);
  }

  return toReturn;
}

VertexData<Vector2> computeSmoothestBoundaryAlignedVertexDirectionField(IntrinsicGeometryInterface& geometry,
                                                                        int nSym) {
  SurfaceMesh& mesh = geometry.mesh;

  if (!mesh.hasBoundary()) {
    throw std::logic_error("tried to compute smoothest boundary aligned direction field on a mesh without boundary");
  }

  geometry.requireVertexGalerkinMassMatrix();
  geometry.requireHalfedgeVectorsInVertex();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.vertexGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = computeVertexConnectionLaplacian(geometry, nSym);

  // Compute the boundary values
  VertexData<std::complex<double>> boundaryValues(mesh);
  for (Vertex v : mesh.vertices()) {
    if (v.isBoundary()) {

      // Find incoming and outgoing boundary vectors as tangent
      Halfedge heBoundaryA = v.halfedge();
      Halfedge heBoundaryB = heBoundaryA.twin().next();

      Vector2 vecA = geometry.halfedgeVectorsInVertex[heBoundaryA];
      Vector2 vecB = geometry.halfedgeVectorsInVertex[heBoundaryB];

      Vector2 tangentV = unit(-vecA + vecB);
      Vector2 normalV = tangentV.rotate90();

      boundaryValues[v] = normalV.pow(nSym);
    } else {
      boundaryValues[v] = 0;
    }
  }

  // Assemble right-hand side from these boundary values
  Vector<std::complex<double>> b(mesh.nVertices());
  b.setZero();

  for (Vertex v : mesh.vertices()) {
    if (!v.isBoundary()) {

      for (Halfedge he : v.incomingHalfedges()) {
        Face neighFace = he.twin().face();
        if (he.vertex().isBoundary()) {

          size_t i = geometry.vertexIndices[v];
          size_t j = geometry.vertexIndices[he.vertex()];

          // move boundary terms to the right-hand side and remove the corresponding matrix entries
          std::complex<double> Aij = energyMatrix.coeff(i, j);
          energyMatrix.coeffRef(i, j) = 0;
          std::complex<double> bVal = boundaryValues[he.vertex()];
          b(i) += -Aij * bVal;
        }
      }
    }
  }

  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
  Eigen::VectorXcd RHS = massMatrix * b;
  Eigen::VectorXcd solution = solveSquare(LHS, RHS);

  // Copy the result to a VertexData vector for both the boundary and interior
  VertexData<Vector2> toReturn(mesh);
  for (Vertex v : mesh.vertices()) {
    if (v.isBoundary()) {
      toReturn[v] = Vector2::fromComplex(boundaryValues[v]);
    } else {
      toReturn[v] = Vector2::fromComplex(solution(geometry.vertexIndices[v]));
      toReturn[v] = unit(toReturn[v]);
    }
  }

  return toReturn;
}

FaceData<Vector2> computeSmoothestBoundaryAlignedFaceDirectionField(IntrinsicGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  if (!mesh.hasBoundary()) {
    throw std::logic_error("tried to compute smoothest boundary aligned direction field on a mesh without boundary");
  }

  geometry.requireFaceGalerkinMassMatrix();
  geometry.requireHalfedgeVectorsInFace();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.faceGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = computeFaceConnectionLaplacian(geometry, nSym);

  // Compute boundary values
  FaceData<bool> isInterior(mesh);
  FaceData<std::complex<double>> boundaryValues(mesh);
  for (Face f : mesh.faces()) {
    bool isBoundary = false;
    for (Edge e : f.adjacentEdges()) {
      isBoundary |= e.isBoundary();
    }
    isInterior[f] = !isBoundary;

    if (isInterior[f]) {
      boundaryValues[f] = 0;
    } else {
      Vector2 bC = Vector2::zero();
      for (Halfedge he : f.adjacentHalfedges()) {
        if (he.edge().isBoundary()) {
          bC -= geometry.halfedgeVectorsInFace[he].rotate90(); // negate the vector to point outwards
        }
      }
      bC = unit(bC);
      boundaryValues[f] = bC.pow(nSym);
    }
  }

  // Assemble right-hand side from these boundary values
  Vector<std::complex<double>> b(mesh.nFaces());
  b.setZero();

  for (Face f : mesh.faces()) {
    if (isInterior[f]) {

      for (Halfedge he : f.adjacentHalfedges()) {
        Face neighFace = he.twin().face();
        if (!isInterior[neighFace]) {

          size_t i = geometry.faceIndices[f];
          size_t j = geometry.faceIndices[neighFace];

          // move boundary terms to the right-hand side and remove the corresponding matrix entries
          std::complex<double> Aij = energyMatrix.coeff(i, j);
          energyMatrix.coeffRef(i, j) = 0;
          std::complex<double> bVal = boundaryValues[neighFace];
          b(i) += -Aij * bVal;
        }
      }
    }
  }

  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
  Eigen::VectorXcd RHS = massMatrix * b;
  Eigen::VectorXcd solution = solveSquare(LHS, RHS);

  // Copy the result to a FaceData object
  FaceData<Vector2> field(mesh);
  for (Face f : mesh.faces()) {
    if (isInterior[f]) {
      field[f] = Vector2::fromComplex(solution(geometry.faceIndices[f]));
      field[f] = unit(field[f]);
    } else {
      field[f] = Vector2::fromComplex(boundaryValues[f]);
    }
  }

  return field;
}

VertexData<Vector2> computeCurvatureAlignedVertexDirectionField(ExtrinsicGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;
  size_t N = mesh.nVertices();

  geometry.requireVertexIndices();
  geometry.requireVertexGalerkinMassMatrix();
  geometry.requireVertexPrincipalCurvatureDirections();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.vertexGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = computeVertexConnectionLaplacian(geometry, nSym);

  Vector<std::complex<double>> dirVec(N);
  if (nSym == 2) {
    for (Vertex v : mesh.vertices()) {
      dirVec[geometry.vertexIndices[v]] = geometry.vertexPrincipalCurvatureDirections[v];
    }
  } else if (nSym == 4) {
    for (Vertex v : mesh.vertices()) {
      dirVec[geometry.vertexIndices[v]] =
          std::pow(std::complex<double>(geometry.vertexPrincipalCurvatureDirections[v]), 2);
    }
  } else {
    throw std::logic_error("ERROR: It only makes sense to align with curvature when nSym = 2 or 4");
  }

  // Normalize the alignment field
  double scale = std::sqrt(std::abs((dirVec.adjoint() * massMatrix * dirVec)[0]));
  dirVec /= scale;

  // this is something of a magical constant, see "Globally Optimal Direction Fields", eqn 16
  double lambdaT = 0;

  Eigen::VectorXcd RHS = massMatrix * dirVec;
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix - lambdaT * massMatrix;
  Eigen::VectorXcd solution = solveSquare(LHS, RHS);

  // Copy the result to a VertexData vector
  VertexData<Vector2> toReturn(mesh);
  for (Vertex v : mesh.vertices()) {
    toReturn[v] = Vector2::fromComplex(solution(geometry.vertexIndices[v]));
    toReturn[v] = unit(toReturn[v]);
  }

  return toReturn;
}

FaceData<Vector2> computeCurvatureAlignedFaceDirectionField(EmbeddedGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;
  const unsigned int N = mesh.nFaces();

  geometry.requireFaceIndices();
  geometry.requireFaceGalerkinMassMatrix();
  geometry.requireFacePrincipalCurvatureDirections();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.faceGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = computeFaceConnectionLaplacian(geometry, nSym);

  Eigen::VectorXcd dirVec(N);
  if (nSym == 2) {
    for (Face f : mesh.faces()) {
      dirVec[geometry.faceIndices[f]] = geometry.facePrincipalCurvatureDirections[f];
    }
  } else if (nSym == 4) {
    for (Face f : mesh.faces()) {
      dirVec[geometry.faceIndices[f]] = std::pow(std::complex<double>(geometry.facePrincipalCurvatureDirections[f]), 2);
    }
  } else {
    throw std::logic_error("ERROR: It only makes sense to align with curvature when nSym = 2 or 4");
  }

  // Normalize the alignment field
  double scale = std::sqrt(std::abs((dirVec.adjoint() * massMatrix * dirVec)[0]));
  dirVec /= scale;

  double lambdaT = 0.0; // this is something of a magical constant, see "Globally Optimal Direction Fields", eqn 16

  Eigen::VectorXcd RHS = massMatrix * dirVec;
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix - lambdaT * massMatrix;
  Eigen::VectorXcd solution = solveSquare(LHS, RHS);

  // Copy the result to a FaceData object
  FaceData<Vector2> field(mesh);
  for (Face f : mesh.faces()) {
    field[f] = Vector2::fromComplex(solution[geometry.faceIndices[f]]);
    field[f] = unit(field[f]);
  }

  return field;
}

FaceData<int> computeFaceIndex(IntrinsicGeometryInterface& geometry, const VertexData<Vector2>& directionField,
                               int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  geometry.requireTransportVectorsAlongHalfedge();
  geometry.requireFaceGaussianCurvatures();

  // Store the result here
  FaceData<int> indices(mesh);

  // TODO haven't tested that this correctly reports the index when it is larger
  // than +-1

  for (Face f : mesh.faces()) {
    // Trace the direction field around the face and see how many times it
    // spins!
    double totalRot = geometry.faceGaussianCurvatures[f] * nSym;

    for (Halfedge he : f.adjacentHalfedges()) {
      if (he.twin().isInterior()) {
        // Compute the rotation along the halfedge implied by the field
        Vector2 x0 = directionField[he.vertex()];
        Vector2 x1 = directionField[he.twin().vertex()];
        Vector2 rot = geometry.transportVectorsAlongHalfedge[he].pow(nSym);

        // Find the difference in angle
        double theta0 = (rot * x0).arg();
        double theta1 = x1.arg();
        double deltaTheta = regularizeAngle(theta1 - theta0 + PI) - PI; // regularize to [-PI,PI]

        totalRot += deltaTheta; // accumulate
      }
    }

    // Compute the net rotation and corresponding index
    int index = static_cast<int>(std::round(totalRot / (2 * PI))); // should be very close to a multiple of 2PI
    indices[f] = index;
  }

  return indices;
}

VertexData<int> computeVertexIndex(IntrinsicGeometryInterface& geometry, const FaceData<Vector2>& directionField,
                                   int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  geometry.requireTransportVectorsAcrossHalfedge();
  geometry.requireVertexGaussianCurvatures();

  // Store the result here
  VertexData<int> indices(mesh);

  // TODO haven't tested that this correctly reports the index when it is larger
  // than +-1

  for (Vertex v : mesh.vertices()) {

    // Trace the direction field around the face and see how many times it
    // spins!
    double totalRot = geometry.vertexGaussianCurvatures[v] * nSym;

    if (!v.isBoundary()) {
      for (Halfedge he : v.incomingHalfedges()) {
        // Compute the rotation along the halfedge implied by the field
        Vector2 x0 = directionField[he.face()];
        Vector2 x1 = directionField[he.twin().face()];
        Vector2 rot = geometry.transportVectorsAcrossHalfedge[he].pow(nSym);

        double deltaTheta = regularizeAngle(x1.arg() - (rot * x0).arg() + PI) - PI; // regularize to [-PI,PI]

        totalRot += deltaTheta;
      }
    }

    // Compute the net rotation and corresponding index
    int index = static_cast<int>(std::round(totalRot / (2 * PI))); // should be very close to a multiple of 2PI
    indices[v] = index;
  }

  return indices;
}
/*
VertexData<Vector2> computeSmoothestBoundaryAlignedVertexDirectionField(IntrinsicGeometryInterface& geometry,
                                                                        int nSym) {
  SurfaceMesh& mesh = geometry.mesh;

  if (!mesh.hasBoundary()) {
    throw std::logic_error("tried to compute smoothest boundary aligned direction field on a mesh without boundary");
  }

  geometry.requireVertexGalerkinMassMatrix();
  geometry.requireVertexConnectionLaplacian();
  geometry.requireHalfedgeVectorsInVertex();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.vertexGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = geometry.vertexConnectionLaplacian;

  // Compute the boundary values
  VertexData<std::complex<double>> boundaryValues(mesh);
  VertexData<char> isBoundary(mesh, false);
  for (Vertex v : mesh.vertices()) {
    if (v.isBoundary()) {
      isBoundary[v] = true;

      // Find incoming and outgoing boundary vectors as tangent
      Halfedge heBoundaryA = v.halfedge();
      Halfedge heBoundaryB = heBoundaryA.twin().next();

      Vector2 vecA = geometry.halfedgeVectorsInVertex[heBoundaryA];
      Vector2 vecB = geometry.halfedgeVectorsInVertex[heBoundaryB];

      Vector2 tangentV = unit(-vecA + vecB);
      Vector2 normalV = tangentV.rotate90();

      boundaryValues[v] = normalV.pow(nSym);
    } else {
      boundaryValues[v] = 0;
    }
  }
  Vector<std::complex<double>> b = boundaryValues.toVector();
  Vector<char> isBoundaryVec = isBoundary.toVector();

  // Block decompose problem

  // Compute the actual solution
  std::cout << "Solving linear problem..." << std::endl;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    geometry.requirePrincipalDirections();

    Eigen::VectorXcd dirVec(nInterior);
    for (Vertex v : mesh.vertices()) {
      if (v.isBoundary()) {
        continue;
      }

      Vector2 directionVal = geometry.principalDirections[v];
      if (nSym == 4) {
        directionVal = std::pow(directionVal, 2);
      }

      // Normalize the curvature vectors. By doing so, we lose the property of adjusting the strength of the alignment
      // based on the strength of the curvature, but resolve any scaling issues between the magnitude of the normals and
      // the magnitude of the desired field.  Be careful when interpreting this as opposed to the usual direction field
      // optimization.
      dirVec[geometry.interiorVertexIndices[v]] = directionVal / std::abs(directionVal);
    }

    double t = 0.01; // this is something of a magical constant, see "Globally
                     // Optimal Direction Fields", eqn 9

    Eigen::VectorXcd RHS = massMatrix * (t * dirVec + b);
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    solution = solveSquare(LHS, RHS);
  }
  // Otherwise find the general closest solution
  else {
    std::cout << "Solving smoothest field dirichlet problem..." << std::endl;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    Eigen::VectorXcd RHS = massMatrix * b;
    solution = solveSquare(LHS, RHS);
  }

  // Copy the result to a VertexData vector for both the boudary and interior
  VertexData<Vector2> toReturn(*mesh);
  for (Vertex v : mesh.vertices()) {
    if (v.isBoundary()) {
      toReturn[v] = boundaryValues[v];
    } else {
      toReturn[v] = unit(solution[geometry.interiorVertexIndices[v]]);
    }
  }

  return toReturn;
}

// Helpers for computing face-based direction fields
namespace {

FaceData<Vector2> computeSmoothestFaceDirectionField_noBoundary(IntrinsicGeometryInterface& geometry, int nSym = 1,
                                                                bool alignCurvature = false) {

  SurfaceMesh* mesh = geometry->getMesh();
  unsigned int N = mesh.nFaces();

  GeometryCache<Euclidean>& geometry = geometry->cache;
  geometry.requireFaceTransportCoefs();
  geometry.requireFaceNormals();
  geometry.requireFaceAreas();
  geometry.requireDihedralAngles();
  geometry.requireFaceIndices();

  // === Allocate matrices
  // Energy matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> energyMatrix(N, N);
  energyMatrix.reserve(Eigen::VectorXi::Constant(N, 4));

  // Mass matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> massMatrix(N, N);
  massMatrix.reserve(Eigen::VectorXi::Constant(N, 1));

  // === Build matrices

  // Build the mass matrix
  for (Face f : mesh.faces()) {
    size_t i = geometry.faceIndices[f];
    massMatrix.insert(i, i) = geometry.faceAreas[f];
  }

  // Build the energy matrix
  for (Face f : mesh.faces()) {
    size_t i = geometry.faceIndices[f];

    std::complex<double> weightISum = 0;
    for (Halfedge he : f.adjacentHalfedges()) {

      if (!he.twin().isInterior()) {
        continue;
      }

      Face neighFace = he.twin().face();
      unsigned int j = geometry.faceIndices[neighFace];

      // LC connection between the faces
      Vector2 rBar = std::pow(geometry.faceTransportCoefs[he.twin()], nSym);

      double weight = 1; // FIXME TODO figure out weights
      energyMatrix.insert(i, j) = -weight * rBar;
      weightISum += weight;
    }

    energyMatrix.insert(i, i) = weightISum;
  }

  // Shift to avoid singularity
  Eigen::SparseMatrix<Vector2> eye(N, N);
  eye.setIdentity();
  energyMatrix += 1e-4 * eye;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    Eigen::VectorXcd dirVec(N);
    for (Face f : mesh.faces()) {

      // Compute something like the principal directions
      double weightSum = 0;
      Vector2 sum = 0;

      for (Halfedge he : f.adjacentHalfedges()) {

        double dihedralAngle = std::abs(geometry.dihedralAngles[he.edge()]);
        double weight = norm(geometry->vector(he));
        weightSum += weight;
        double angleCoord = angleInPlane(geometry->vector(f.halfedge()), geometry->vector(he), geometry.faceNormals[f]);
        Vector2 coord = std::exp(angleCoord * IM_I *
                                 (double)nSym); // nsym should be 2 or 4, checked in the funciton which calls this

        sum += coord * weight * dihedralAngle;
      }

      sum /= weightSum;

      dirVec[geometry.faceIndices[f]] = sum;
    }

    // Normalize the alignment field
    double scale = std::sqrt(std::abs((dirVec.adjoint() * massMatrix * dirVec)[0]));
    dirVec /= scale;

    double lambdaT = 0.0; // this is something of a magical constant, see "Globally Optimal Direction Fields", eqn 16

    // Eigen::VectorXcd RHS = massMatrix * dirVec;
    Eigen::VectorXcd RHS = dirVec;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix - lambdaT * massMatrix;
    solution = solveSquare(LHS, RHS);

  }
  // Otherwise find the smallest eigenvector
  else {
    std::cout << "Solving smoothest field eigenvalue problem..." << std::endl;
    solution = smallestEigenvectorPositiveDefinite(energyMatrix, massMatrix);
  }

  // Copy the result to a FaceData object
  FaceData<Vector2> field(*mesh);
  for (Face f : mesh.faces()) {
    field[f] = solution[geometry.faceIndices[f]] / std::abs(solution[geometry.faceIndices[f]]);
  }

  return field;
}

FaceData<Vector2> computeSmoothestFaceDirectionField_boundary(IntrinsicGeometryInterface& geometry, int nSym = 1,
                                                              bool alignCurvature = false) {

  SurfaceMesh* mesh = geometry->getMesh();

  GeometryCache<Euclidean>& geometry = geometry->cache;
  geometry.requireFaceTransportCoefs();
  geometry.requireFaceNormals();
  geometry.requireFaceAreas();
  geometry.requireDihedralAngles();

  // Index interior faces
  size_t nInteriorFace = 0;
  FaceData<size_t> interiorFaceInd(*mesh, -77);
  FaceData<char> isInterior(*mesh);
  for (Face f : mesh.faces()) {
    bool isBoundary = false;
    for (Edge e : f.adjacentEdges()) {
      isBoundary |= e.isBoundary();
    }
    isInterior[f] = !isBoundary;
    if (!isBoundary) {
      interiorFaceInd[f] = nInteriorFace++;
    }
  }

  // Compute boundary values
  FaceData<Vector2> boundaryValues(*mesh);
  for (Face f : mesh.faces()) {
    if (isInterior[f]) {
      boundaryValues[f] = 0;
    } else {
      Vector3 bVec = Vector3::zero();
      for (Halfedge he : f.adjacentHalfedges()) {
        if (he.edge().isBoundary()) {
          bVec += geometry->vector(he).rotate_around(geometry.faceNormals[f], -PI / 2.0);
        }
      }
      Vector2 bC(dot(geometry.faceBases[f][0], bVec), dot(geometry.faceBases[f][1], bVec));
      bC = unit(bC);
      boundaryValues[f] = std::pow(bC, nSym);
    }
  }

  // === Allocate matrices
  // Energy matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> energyMatrix(nInteriorFace, nInteriorFace);
  energyMatrix.reserve(Eigen::VectorXi::Constant(nInteriorFace, 4));

  // Mass matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> massMatrix(nInteriorFace, nInteriorFace);
  massMatrix.reserve(Eigen::VectorXi::Constant(nInteriorFace, 1));

  // RHS
  Eigen::VectorXcd b(nInteriorFace);

  // === Build matrices

  // Build the mass matrix
  for (Face f : mesh.faces()) {
    if (isInterior[f]) {
      size_t i = interiorFaceInd[f];
      massMatrix.insert(i, i) = geometry.faceAreas[f];
    }
  }

  // Build the energy matrix
  for (Face f : mesh.faces()) {
    if (isInterior[f]) {
      size_t i = interiorFaceInd[f];

      std::complex<double> weightISum = 0;
      for (Halfedge he : f.adjacentHalfedges()) {

        Face neighFace = he.twin().face();
        double weight = 1; // FIXME TODO figure out weights
        Vector2 rBar = std::pow(geometry.faceTransportCoefs[he.twin()], nSym);

        if (isInterior[neighFace]) {
          size_t j = interiorFaceInd[neighFace];
          energyMatrix.insert(i, j) = -weight * rBar;
        } else {
          std::complex<double> bVal = boundaryValues[neighFace];
          b(i) += weight * rBar * bVal;
        }

        weightISum += weight;
      }

      energyMatrix.insert(i, i) = weightISum;
    }
  }

  // Shift to avoid singularity
  Eigen::SparseMatrix<Vector2> eye(nInteriorFace, nInteriorFace);
  eye.setIdentity();
  energyMatrix += 1e-4 * eye;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    Eigen::VectorXcd dirVec(nInteriorFace);
    for (Face f : mesh.faces()) {
      if (isInterior[f]) {

        // Compute something like the principal directions
        double weightSum = 0;
        Vector2 sum = 0;

        for (Halfedge he : f.adjacentHalfedges()) {

          double dihedralAngle = std::abs(geometry.dihedralAngles[he.edge()]);
          double weight = norm(geometry->vector(he));
          weightSum += weight;
          double angleCoord =
              angleInPlane(geometry->vector(f.halfedge()), geometry->vector(he), geometry.faceNormals[f]);
          Vector2 coord = std::exp(angleCoord * IM_I *
                                   (double)nSym); // nsym should be 2 or 4, checked in the funciton which calls this

          sum += coord * weight * dihedralAngle;
        }

        sum /= weightSum;

        // Normalize the curvature vectors. By doing so, we lose the property of adjusting the strength of the alignment
        // based on the strength of the curvature, but resolve any scaling issues between the magnitude of the normals
        // and the magnitude of the desired field.  Be careful when interpreting this as opposed to the usual direction
        // field optimization.
        dirVec[interiorFaceInd[f]] = unit(sum);
      }
    }

    double t = 0.1; // this is something of a magical constant, see "Globally
                    // Optimal Direction Fields", eqn 9
                    // NOTE: This value is different from the one used for vertex fields; seems to work better?

    std::cout << "Solving smoothest field dirichlet problem with curvature term..." << std::endl;
    Eigen::VectorXcd RHS = massMatrix * (t * dirVec + b);
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    solution = solveSquare(LHS, RHS);

  }
  // Otherwise find the general closest solution
  else {
    std::cout << "Solving smoothest field dirichlet problem..." << std::endl;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    Eigen::VectorXcd RHS = massMatrix * b;
    solution = solveSquare(LHS, RHS);
  }

  // Copy the result to a FaceData object
  FaceData<Vector2> field(*mesh);
  for (Face f : mesh.faces()) {
    if (isInterior[f]) {
      field[f] = unit(solution[interiorFaceInd[f]]);
    } else {
      field[f] = unit(boundaryValues[f]);
    }
  }

  return field;
}

} // namespace

FaceData<Vector2> computeSmoothestFaceDirectionField(Geometry<Euclidean>* geometry, int nSym, bool alignCurvature) {

  std::cout << "Computing globally optimal direction field in faces" << std::endl;

  if (alignCurvature && !(nSym == 2 || nSym == 4)) {
    throw std::logic_error("ERROR: It only makes sense to align with curvature when nSym = 2 or "
                           "4");
  }

  // Dispatch to either the boundary of no boundary variant depending on the mesh type
  bool hasBoundary = false;
  for (Vertex v : geometry->getMesh()->vertices()) {
    hasBoundary |= v.isBoundary();
  }

  if (hasBoundary) {
    std::cout << "Mesh has boundary, computing dirichlet boundary condition solution" << std::endl;
    return computeSmoothestFaceDirectionField_boundary(geometry, nSym, alignCurvature);
  } else {
    std::cout << "Mesh has no boundary, computing unit-norm solution" << std::endl;
    return computeSmoothestFaceDirectionField_noBoundary(geometry, nSym, alignCurvature);
  }
}

FaceData<int> computeFaceIndex(IntrinsicGeometryInterface& geometry, const VertexData<Vector2>& directionField,
                               int nSym) {

  SurfaceMesh* mesh = geometry->getMesh();

  GeometryCache<Euclidean>& geometry = geometry->cache;
  geometry.requireFaceTransportCoefs();

  // Store the result here
  FaceData<int> indices(*mesh);

  // TODO haven't tested that this correctly reports the index when it is larger
  // than +-1

  for (Face f : mesh.faces()) {
    // Trace the direction field around the face and see how many times it
    // spins!
    double totalRot = 0;

    for (Halfedge he : f.adjacentHalfedges()) {
      // Compute the rotation along the halfedge implied by the field
      Vector2 x0 = directionField[he.vertex()];
      Vector2 x1 = directionField[he.twin().vertex()];
      Vector2 transport = std::pow(geometry.vertexTransportCoefs[he], nSym);

      // Find the difference in angle
      double theta0 = std::arg(transport * x0);
      double theta1 = std::arg(x1);
      double deltaTheta = regularizeAngle(theta1 - theta0 + PI) - PI; // regularize to [-PI,PI]

      totalRot += deltaTheta; // accumulate
    }

    // Compute the net rotation and corresponding index
    int index = static_cast<int>(std::round(totalRot / (2 * PI))); // should be very close to a multiple of 2PI
    indices[f] = index;
  }

  return indices;
}

VertexData<int> computeVertexIndex(IntrinsicGeometryInterface& geometry, const FaceData<Vector2>& directionField,
                                   int nSym) {

  SurfaceMesh* mesh = geometry->getMesh();
  GeometryCache<Euclidean>& geometry = geometry->cache;
  geometry.requireFaceTransportCoefs();

  // Store the result here
  VertexData<int> indices(*mesh);

  // TODO haven't tested that this correctly reports the index when it is larger
  // than +-1

  for (Vertex v : mesh.vertices()) {

    // Trace the direction field around the face and see how many times it
    // spins!
    double totalRot = 0;

    for (Halfedge he : v.incomingHalfedges()) {
      // Compute the rotation along the halfedge implied by the field
      Vector2 x0 = directionField[he.face()];
      Vector2 x1 = directionField[he.twin().face()];
      Vector2 transport = std::pow(geometry.faceTransportCoefs[he], nSym);

      // Find the difference in angle
      double theta0 = std::arg(transport * x0);
      double theta1 = std::arg(x1);
      double deltaTheta = std::arg(x1 / (transport * x0));

      totalRot += deltaTheta;
    }

    double angleDefect = geometry->angleDefect(v);
    totalRot += angleDefect * nSym;

    // Compute the net rotation and corresponding index
    int index = static_cast<int>(std::round(totalRot / (2 * PI))); // should be very close to a multiple of 2PI
    indices[v] = index;
  }

  return indices;
}
*/

} // namespace surface
} // namespace geometrycentral


#pragma once
#include <boost/circular_buffer.hpp>
#include "DenseMatrix.hpp"
#include "Mapping.hpp"

namespace NonlinearSolvers {

	struct FixedPointData {
		Operator f; // residual operator
		Operator g; // iteration operator
	};

	inline void logFixedPoint(double r, Index i, Index max = 1) {
		auto& logger = SingletonLogger::instance();
		auto& w = std::setw(std::to_string(max).length());
		logger.buf << "|| f(x_" << std::setfill('0') << w << i << ") || = " << std::scientific << r;
		logger.log();
	}

	inline std::vector<double> FixedPointMethod(
		FixedPointData const & fp,
		std::vector<double> const & x_0,
		Index n = 100, // max numb of iters
		double eps = 1e-7
	) {
		auto& logger = SingletonLogger::instance();
		logger.log("start Fixed Point method");
		auto x = x_0;
		auto r_norm = norm(fp.f(x));
		logFixedPoint(r_norm, 0, n);
		if (r_norm < eps) {
			logger.log("stop Fixed Point method");
			return x;
		}
		Index i;
		for (i = 1; i <= n; ++i) {
			x = fp.g(x);
			r_norm = norm(fp.f(x));
			logFixedPoint(r_norm, i, n);
			if (r_norm < eps) break;
		}
		logger.log("stop Fixed Point method");
		if (i > n) logger.wrn("Fixed Point Method exceeded max numb of iterations");
		return x;
	}

	inline std::vector<double> AndersonMixingMethod(
			FixedPointData const & fp,
			std::vector<double> const & x_0,
			Index m = 0,
			Index n = 100, // max numb of iters
			double eps = 1e-7
		) {
		auto& logger = SingletonLogger::instance();
		if (m == 0) {
			logger.log("m = 0 -> Fixed Point Method");
			return FixedPointMethod(fp, x_0, n, eps);
		}
		boost::circular_buffer<std::vector<double>> F(m), G(m + 1);
		logger.log("start Fixed Point method");
		// i = 0
		auto x = x_0;
		auto r = fp.f(x);
		auto r_norm = norm(r);
		logFixedPoint(r_norm, 0, n);
		if (r_norm < eps) {
			logger.log("stop Fixed Point method");
			return x;
		}
		G.push_front(fp.g(x));
		// i = 1
		x = G[0];
		auto r_new = fp.f(x);
		r_norm = norm(r_new);
		logFixedPoint(r_norm, 1, n);
		if (r_norm < eps) {
			logger.log("stop Fixed Point method");
			return x;
		}
		G.push_front(fp.g(x));
		F.push_front(r_new - r);
		r = r_new;
		logger.log("stop Fixed Point method");
		// i = 2, 3, . . .
		logger.log("start Anderson Mixing method");
		Index i;
		for (i = 2; i <= n; ++i) {
			m = F.size();
			DenseMatrix<double> A(m);
			std::vector<double> b(m);
			for (Index k = 0; k < m; ++k) {
				b[k] = F[k] * r;
				for (Index j = 0; j < m; ++j)
					A(k, j) = F[k] * F[j];
			}
			auto beta = A.GaussElimination(b);
			decltype(beta) alpha(m + 1);
			alpha[0] = 1. - beta[0];
			for (Index k = 1; k < m; ++k) alpha[k] = beta[k - 1] - beta[k];
			alpha[m] = beta[m - 1];
			std::fill(x.begin(), x.end(), 0.);
			for (Index k = 0; k < m + 1; ++k) x += alpha[k] * G[k];
			r_new = fp.f(x);
			r_norm = norm(r_new);
			logFixedPoint(r_norm, i, n);
			if (r_norm < eps) break;
			G.push_front(fp.g(x));
			F.push_front(r_new - r);
			r = r_new;
		}
		logger.log("stop Anderson Mixing method");
		if (i > n) logger.wrn("Anderson Mixing Method exceeded max numb of iterations");
		return x;
	}

}


#include <ros/ros.h>
#include <iostream>
#include <ceres/ceres.h>
#include <Eigen/Core>
#include <fstream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <opencv2/opencv.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include <string>

#include "common.h"
#include "result_verify.h"

using namespace std;

typedef pcl::PointXYZRGB PointType;
Eigen::Matrix3d inner;
string lidar_path, photo_path, intrinsic_path, extrinsic_path;
int error_threshold;
vector<float> init;

void getParameters();

class external_cali {
public:
    external_cali(PnPData p) {
        pd = p;
    }

    template <typename T>
    bool operator()(const T *_q, const T *_t, T *residuals) const {
        Eigen::Matrix<T, 3, 3> innerT = inner.cast<T>();
        Eigen::Quaternion<T> q_incre{_q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ]};
        Eigen::Matrix<T, 3, 1> t_incre{_t[ 0 ], _t[ 1 ], _t[ 2 ]};

        Eigen::Matrix<T, 3, 1> p_l(T(pd.x), T(pd.y), T(pd.z));
        Eigen::Matrix<T, 3, 1> p_c = q_incre.toRotationMatrix() * p_l + t_incre;
        Eigen::Matrix<T, 3, 1> p_2 = innerT * p_c;

        residuals[0] = p_2[0]/p_2[2] - T(pd.u);
        residuals[1] = p_2[1]/p_2[2] - T(pd.v);

        return true;
    }

    static ceres::CostFunction *Create(PnPData p) {
        return (new ceres::AutoDiffCostFunction<external_cali, 2, 4, 3>(new external_cali(p)));
    }

private:
    PnPData pd;

};

void getParameters() {
    cout << "Get the parameters from the launch file" << endl;

    if (!ros::param::get("input_lidar_path", lidar_path)) {
        cout << "Can not get the value of input_lidar_path" << endl;
        exit(1);
    }
    if (!ros::param::get("input_photo_path", photo_path)) {
        cout << "Can not get the value of input_photo_path" << endl;
        exit(1);
    }
    if (!ros::param::get("intrinsic_path", intrinsic_path)) {
        cout << "Can not get the value of intrinsic_path" << endl;
        exit(1);
    }
    if (!ros::param::get("extrinsic_path", extrinsic_path)) {
        cout << "Can not get the value of extrinsic_path" << endl;
        exit(1);
    }
    if (!ros::param::get("error_threshold", error_threshold)) {
        cout << "Can not get the value of error_threshold" << endl;
        exit(1);
    }
    init.resize(12);
    if (!ros::param::get("init_value", init)) {
        cout << "Can not get the value of init_value" << endl;
        exit(1);
    }
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "getExt1");
    getParameters();

    vector<PnPData> pData;
    getData(lidar_path, photo_path, pData);

    Eigen::Matrix4d extrin;
    // set the intrinsic parameters
    vector<float> intrinsic;
    getIntrinsic(intrinsic_path, intrinsic);
    inner << intrinsic[0], intrinsic[1], intrinsic[2],
    intrinsic[3], intrinsic[4], intrinsic[5],
    intrinsic[6], intrinsic[7], intrinsic[8];

    // init the matrix of extrinsic, matrix of rotation and translation
    // keep this init value, or it is possible to get a local optimal result
    Eigen::Matrix3d R;
    R << init[0], init[1], init[2],
         init[4], init[5], init[6],
         init[8], init[9], init[10];
    Eigen::Quaterniond q(R);
    double ext[7];

    ext[0] = q.x();
    ext[1] = q.y();
    ext[2] = q.z();
    ext[3] = q.w();
    ext[4] = init[3];
    ext[5] = init[7];
    ext[6] = init[11];

    Eigen::Map<Eigen::Quaterniond> m_q = Eigen::Map<Eigen::Quaterniond>(ext);
    Eigen::Map<Eigen::Vector3d> m_t = Eigen::Map<Eigen::Vector3d>(ext + 4);

    ceres::LocalParameterization * q_parameterization = new ceres::EigenQuaternionParameterization();
    ceres::Problem problem;

    problem.AddParameterBlock(ext, 4, q_parameterization);
    problem.AddParameterBlock(ext + 4, 3);

    for(auto val : pData) {
        ceres::CostFunction *cost_function;
        cost_function = external_cali::Create(val);
        problem.AddResidualBlock(cost_function, NULL, ext, ext + 4);
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    cout << summary.BriefReport() << endl;

    Eigen::Matrix3d rot = m_q.toRotationMatrix();
    writeExt(extrinsic_path, rot, m_t);
    cout << rot << endl;
    cout << m_t << endl;

    cout << "Use the extrinsic result to reproject the data" << endl;
    float error[2] = {0, 0};
    getUVError(intrinsic_path, extrinsic_path, lidar_path, photo_path, error, error_threshold, 1, cv::Size(2464, 2056));

    cout << "u average error is: " << error[0] << endl;
    cout << "v average error is: " << error[1] << endl;
    if (error[0] + error[1] < error_threshold) {
        cout << endl << "The reprojection error smaller than the threshold, extrinsic result seems ok" << endl;
    }
    else {
        cout << endl << "The reprojection error bigger than the threshold, extrinsic result seems not ok" << endl;
    }

    return 0;
}


#include "writer.hpp"
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <cmath>
#include <iostream>
#include <stdexcept>

//! Sparse Matrix type. Makes using this type easier.
typedef Eigen::SparseMatrix<double> SparseMatrix;

//! Used for filling the sparse matrix.
typedef Eigen::Triplet<double> Triplet;

//! Vector type
typedef Eigen::VectorXd Vector;

//! Our function pointer, typedef'd to make it easier to use
typedef double (*FunctionPointer)(double, double);

//----------------poissonBegin----------------
//! Create the Poisson matrix for 2D finite difference.
//! @param[out] A will be the Poisson matrix (as in the exercise)
//! @param[in] N number of elements in the x-direction
void createPoissonMatrix2D(SparseMatrix &A, int N) {
	// Fill the matrix A using setFromTriplets - method (see other exercise
	// for how to use it).
	// (write your solution here)
}
//----------------poissonEnd----------------

//----------------RHSBegin----------------
//! Create the Right hand side for the 2D finite difference
//! @param[out] rhs will at the end contain the right hand side
//! @param[in] f the right hand side function f
//! @param[in] N the number of points in the x direction
//! @param[in] dx the cell width
void createRHS(Vector &rhs, FunctionPointer f, int N, double dx) {
	rhs.resize(N * N);
	// fill up RHS
	// remember that the index (i,j) corresponds to i*N+j
	// (write your solution here)
}
//----------------RHSEnd----------------

//----------------solveBegin----------------
//! Solve the Poisson equation in 2D
//! @param[out] u will contain the solution u
//! @param[in] f the function pointer to f
//! @param[in] N the number of points to use (in x direction)
void poissonSolve(Vector &u, FunctionPointer f, int N) {
	// Solve Poisson 2D here
	// (write your solution here)
}
//----------------solveEnd----------------

double F(double x, double y) {
	return 8 * M_PI * M_PI * sin(2 * M_PI * x) * sin(2 * M_PI * y);
}

int main(int, char **) {
	Vector u;
	poissonSolve(u, F, 100);
	writeToFile("u_fd.txt", u);
}


// Implements functions that fit planes to data.
// File: planefit.h
// Author: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>

// Includes
#include <rc_utils/planefit.h>
#include <Eigen/SVD>
#include <numeric>
#include <cmath>

// Namespaces
using namespace rc_utils;

//
// Plane fitting
//

// Fit a plane to 3D data
void PlaneFit::fitPlane(const Points3D& P, Eigen::Vector4d& coeff)
{
	// Calculate a point on the plane and the normal vector to it
	Eigen::Vector3d normal, mean;
	fitPlane(P, normal, mean);

	// Construct the coefficient vector (a,b,c,d) where the equation of the plane is ax + by + cz + d = 0
	coeff << normal, -normal.dot(mean);
}

// Fit a plane to 3D data
void PlaneFit::fitPlane(const Points3D& P, Eigen::Vector4d& coeff, Eigen::Vector3d& mean)
{
	// Calculate a point on the plane and the normal vector to it
	Eigen::Vector3d normal;
	fitPlane(P, normal, mean);

	// Construct the coefficient vector (a,b,c,d) where the equation of the plane is ax + by + cz + d = 0
	coeff << normal, -normal.dot(mean);
}

// Fit a plane to 3D data
void PlaneFit::fitPlane(const Points3D& P, Eigen::Vector3d& normal)
{
	// Calculate a point on the plane and the normal vector to it
	Eigen::Vector3d mean;
	fitPlane(P, normal, mean);
}

// Fit a plane to 3D data
void PlaneFit::fitPlane(const Points3D& P, Eigen::Vector3d& normal, Eigen::Vector3d& mean)
{
	// Retrieve how many data points there are
	size_t N = P.size();

	// Handle trivial cases
	if(N <= 0)
	{
		normal << 0.0, 0.0, 1.0;
		mean.setZero();
		return;
	}
	else if(N == 1)
	{
		normal << 0.0, 0.0, 1.0;
		mean = P[0];
		return;
	}

	// Calculate the mean of the data points (the plane of best fit always passes through the mean)
	mean = Eigen::Vector3d::Zero();
	if(N >= 1)
		mean = std::accumulate(P.begin(), P.end(), mean) / N;

	// Construct the matrix of zero mean data points
	Eigen::MatrixXd A(3, N);
	for(size_t i = 0; i < N; i++)
		A.col(i) = P[i] - mean;

	// Perform an SVD decomposition to determine the direction with the minimum variance
	unsigned int options = (N >= 3 ? Eigen::ComputeThinU : Eigen::ComputeFullU) | Eigen::ComputeThinV;
	normal = A.jacobiSvd(options).matrixU().col(2).normalized();
}

// Calculate the fitting error of a plane to certain 3D data
double PlaneFit::fitPlaneError(const Points3D& P, const Eigen::Vector4d& coeff)
{
	// Calculate the normalised equation coefficients (to put the resulting error in units of normal distance to plane)
	Eigen::Vector3d normal = coeff.head<3>();
	double norm = normal.norm();
	double scale = (norm > 0.0 ? norm : coeff.w());
	Eigen::Vector3d abc = normal / scale;
	double d = coeff.w() / scale;

	// Calculate the average distance to the plane of the data points
	double err = 0.0;
	size_t N = P.size();
	for(size_t i = 0; i < N; i++)
		err += fabs(abc.dot(P[i]) + d)/N;

	// Return the calculated error
	return err;
}

// Calculate the fitting error of a plane to certain 3D data
double PlaneFit::fitPlaneError(const Points3D& P, const Eigen::Vector3d& normal, Eigen::Vector3d& point)
{
	// Normalise the normal vector (to put the resulting error in units of normal distance to plane)
	Eigen::Vector3d n = normal.normalized();

	// Calculate the average distance to the plane of the data points
	double err = 0.0;
	size_t N = P.size();
	for(size_t i = 0; i < N; i++)
		err += fabs(n.dot(P[i] - point))/N;

	// Return the calculated error
	return err;
}
// EOF


#pragma once

#include "cppitertools/enumerate.hpp"
#include "cppitertools/range.hpp"

#include <Eigen/Core>

#include <functional>
#include <vector>

template<class T>
struct KalmanSmoother
{
    using DataTable = std::vector<std::vector<T>>;

    Eigen::VectorX<T> x;
    Eigen::MatrixX<T> P;
    Eigen::MatrixX<T> F;
    Eigen::MatrixX<T> H;
    Eigen::MatrixX<T> R;
    Eigen::MatrixX<T> Q;

    std::vector<Eigen::VectorX<T>> x_;
    std::vector<Eigen::VectorX<T>> _x;
    std::vector<Eigen::MatrixX<T>> P_;
    std::vector<Eigen::MatrixX<T>> _P;

    std::function<void(int, std::vector<T>)> function_beforeKFcorrect;
    std::function<void(int, std::vector<T>)> function_afterKFcorrect;

    KalmanSmoother(int dynamParams, int measureParams, int controlParams = 0);

    DataTable forwardPass(DataTable & dataSeries);
    DataTable backwardPass();

private:

    int m_dynamParams;
    int m_measureParams;
    int m_controlParams;

    std::vector<std::vector<T>> zk;
};

template<class T>
inline KalmanSmoother<T>::KalmanSmoother(int dynamParams, int measureParams, int controlParams)
{
    m_dynamParams   = dynamParams;
    m_measureParams = measureParams;
    m_controlParams = controlParams;

    x = Eigen::VectorX<T>(dynamParams);
    P = Eigen::MatrixX<T>(dynamParams, dynamParams);
    F = Eigen::MatrixX<T>(dynamParams, dynamParams);
    H = Eigen::MatrixX<T>(measureParams, dynamParams);
    R = Eigen::MatrixX<T>(measureParams, measureParams);
    Q = Eigen::MatrixX<T>(dynamParams, dynamParams);
}

template<class T>
inline std::vector<std::vector<T>>
  KalmanSmoother<T>::forwardPass(DataTable & dataSeries)
{
    for (auto & data : dataSeries) {
        if (data.size() != m_measureParams)
            throw std::length_error("One of the data point's dimension is inadequate.");
    }

    x_.clear();
    x_.resize(dataSeries.size() + 1, x);
    _x.clear();
    _x.resize(dataSeries.size() + 1, x);
    P_.clear();
    P_.resize(dataSeries.size() + 1, P);
    _P.clear();
    _P.resize(dataSeries.size() + 1, P);

    for (auto && [idx, data] : iter::enumerate(dataSeries)) {
        const int i = idx + 1;

        Eigen::Map<Eigen::VectorX<T>> z(data.data(), m_measureParams);

        // Kalman predict
        _x[i] = F * x_[i - 1];
        _P[i] = F * P_[i - 1] * F.transpose() + Q;

        // Kalman correct
        auto K = _P[i] * H.transpose() * (R + H * _P[i] * H.transpose()).inverse();
        auto I = Eigen::MatrixX<T>::Identity(m_dynamParams, m_dynamParams);
        x_[i]  = _x[i] + K * (z - H * _x[i]);
        P_[i]  = (I - K * H) * _P[i];
    }

    DataTable result;
    result.reserve(x_.size());
    for (auto & x : x_) {
        std::vector<T> temp;
        for (auto value : x) {
            temp.push_back(value);
        }
        result.push_back(temp);
    }

    return result;
}

template<class T>
inline std::vector<std::vector<T>>
  KalmanSmoother<T>::backwardPass()
{
    const int n = x_.size();

    std::vector<Eigen::VectorX<T>> xs;
    xs.resize(n, x_[n - 1]);
    std::vector<Eigen::MatrixX<T>> Ps;
    Ps.resize(n, P_[n - 1]);

    for (auto i : iter::range(n - 2, -1, -1)) {
        // Ps[i] = F * P_[i] * F.transpose() + Q;

        // auto K = P_[i] * F.transpose() * Ps[i].inverse();
        // xs[i] += K * (x_[i + 1] - F * _x[i]);
        // Ps[i] += K * (P_[i + 1] - P_[i]) * K.transpose();

        auto C = P_[i] * F.transpose() * _P[i + 1].inverse();

        xs[i] += x_[i] + C * (xs[i + 1] - _x[i + 1]);
        Ps[i] += P_[i] + C * (Ps[i + 1] - _P[i + 1]) * C.transpose();
    }

    DataTable result;
    result.reserve(xs.size());
    for (auto & x : xs) {
        std::vector<T> temp;
        for (auto value : x) {
            temp.push_back(value);
        }
        result.push_back(temp);
    }

    return result;
}


/*=================================================================
*
* todo!!!!!!!!
* compute "voronoi" area of each vertex
* reference: Discrete Differential Geometry Operators for triangulated 2-manifolds_02
* usage:
		va = vertex_area(verts, faces);
* inputs:
		verts: 3*nverts
		faces: 3*faces
		bVoronoi: use fast approximation or voronoi area
*
* output:
*		va: nverts*1
*
*
* JJCAO, 2013
*
*=================================================================*/

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

using namespace std;

void compute_vertex_area_fast(Eigen::MatrixXd& V, Eigen::MatrixXd& F, double* va)
{
	int n1,n2,n3;
	Eigen::Vector3d v1,v2,v3;
	Eigen::Vector3d e1,e2;
	Eigen::Vector3d tmpV;
	double tmp;
	double farea;
	for( int i = 0; i < F.rows(); ++i)
	{
		n1 = F.coeff(i,0)-1; n2 = F.coeff(i,1)-1; n3 = F.coeff(i,2)-1;
		v1 = V.row(n1); v2 = V.row(n2); v3 = V.row(n3);
		e1 = v2 - v1; e2 = v3 - v1;

		tmpV = e1.cross(e2);
		farea = tmpV.norm()*0.5;
		tmp = farea / 3.0;

		va[n1] += tmp;
		va[n2] += tmp;
		va[n3] += tmp;
	}
}
void compute_vertex_voronoi_area(Eigen::MatrixXd& V, Eigen::MatrixXd& F, double* va)
{
	mexErrMsgTxt("not implemented still !");
}
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray*prhs[])
{
	///////////// Error Check
	if ( nrhs < 2)
		mexErrMsgTxt("Number of input should > 1!");

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

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

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

	double* faces = mxGetPr(prhs[1]);

	bool bVoronoi(false);
	if ( nrhs > 2)
	{
		double* tmp = mxGetPr(prhs[2]);
		if ( *tmp != 0)
			bVoronoi = true;
	}

	///////////////////////////////////////////////
	// output 0
	plhs[0] = mxCreateDoubleMatrix( nverts, 1, mxREAL);
	double *va = mxGetPr(plhs[0]);
	for ( int i = 0; i < nverts; ++i)
	{
		va[i] = 0;
	}

	///////////////////////////////////////////////
	// process
	Eigen::MatrixXd V = Eigen::Map<Eigen::MatrixXd>(verts, nverts, 3);
	Eigen::MatrixXd F = Eigen::Map<Eigen::MatrixXd>(faces, nfaces, 3);

	if ( bVoronoi)
		compute_vertex_voronoi_area(V,F,va);
	else
		compute_vertex_area_fast(V,F,va);
}


/* Boost example/newton-raphson.cpp
 * Newton iteration for intervals
 *
 * Copyright 2003 Guillaume Melquiond
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/numeric/interval.hpp>
#include <vector>
#include <algorithm>
#include <utility>
#include <iostream>
#include <iomanip>

template <class I> I f(const I& x)
{ return x * (x - 1.) * (x - 2.) * (x - 3.) * (x - 4.); }
template <class I> I f_diff(const I& x)
{ return (((5. * x - 40.) * x + 105.) * x - 100.) * x + 24.; }

static const double max_width = 1e-10;
static const double alpha = 0.75;

using namespace boost;
using namespace numeric;
using namespace interval_lib;

// First method: no empty intervals

typedef interval<double> I1_aux;
typedef unprotect<I1_aux>::type I1;

std::vector<I1> newton_raphson(const I1& xs) {
  std::vector<I1> l, res;
  I1 vf, vd, x, x1, x2;
  l.push_back(xs);
  while (!l.empty()) {
    x = l.back();
    l.pop_back();
    bool x2_used;
    double xx = median(x);
    vf = f<I1>(xx);
    vd = f_diff<I1>(x);
    if (zero_in(vf) && zero_in(vd)) {
      x1 = I1::whole();
      x2_used = false;
    } else {
      x1 = xx - division_part1(vf, vd, x2_used);
      if (x2_used) x2 = xx - division_part2(vf, vd);
    }
    if (overlap(x1, x)) x1 = intersect(x, x1);
    else if (x2_used) { x1 = x2; x2_used = false; }
    else continue;
    if (x2_used)
      if (overlap(x2, x)) x2 = intersect(x, x2);
      else x2_used = false;
    if (x2_used && width(x2) > width(x1)) std::swap(x1, x2);
    if (!zero_in(f(x1)))
      if (x2_used) { x1 = x2; x2_used = false; }
      else continue;
    if (width(x1) < max_width) res.push_back(x1);
    else if (width(x1) > alpha * width(x)) {
      std::pair<I1, I1> p = bisect(x);
      if (zero_in(f(p.first))) l.push_back(p.first);
      x2 = p.second;
      x2_used = true;
    } else l.push_back(x1);
    if (x2_used && zero_in(f(x2)))
      if (width(x2) < max_width) res.push_back(x2);
      else l.push_back(x2);
  }
  return res;
}

// Second method: with empty intervals

typedef change_checking<I1_aux, checking_no_nan<double> >::type I2_aux;
typedef unprotect<I2_aux>::type I2;

std::vector<I2> newton_raphson(const I2& xs) {
  std::vector<I2> l, res;
  I2 vf, vd, x, x1, x2;
  l.push_back(xs);
  while (!l.empty()) {
    x = l.back();
    l.pop_back();
    double xx = median(x);
    vf = f<I2>(xx);
    vd = f_diff<I2>(x);
    if (zero_in(vf) && zero_in(vd)) {
      x1 = x;
      x2 = I2::empty();
    } else {
      bool x2_used;
      x1 = intersect(x, xx - division_part1(vf, vd, x2_used));
      x2 = intersect(x, xx - division_part2(vf, vd, x2_used));
    }
    if (width(x2) > width(x1)) std::swap(x1, x2);
    if (empty(x1) || !zero_in(f(x1)))
      if (!empty(x2)) { x1 = x2; x2 = I2::empty(); }
      else continue;
    if (width(x1) < max_width) res.push_back(x1);
    else if (width(x1) > alpha * width(x)) {
      std::pair<I2, I2> p = bisect(x);
      if (zero_in(f(p.first))) l.push_back(p.first);
      x2 = p.second;
    } else l.push_back(x1);
    if (!empty(x2) && zero_in(f(x2)))
      if (width(x2) < max_width) res.push_back(x2);
      else l.push_back(x2);
  }
  return res;
}

template<class T, class Policies>
std::ostream &operator<<(std::ostream &os,
                         const boost::numeric::interval<T, Policies> &x) {
  os << "[" << x.lower() << ", " << x.upper() << "]";
  return os;
}

int main() {
  {
    I1_aux::traits_type::rounding rnd;
    std::vector<I1> res = newton_raphson(I1(-1, 5.1));
    std::cout << "Results: " << std::endl << std::setprecision(12);
    for(std::vector<I1>::const_iterator i = res.begin(); i != res.end(); ++i)
      std::cout << "  " << *i << std::endl;
    std::cout << std::endl;
  }
  {
    I2_aux::traits_type::rounding rnd;
    std::vector<I2> res = newton_raphson(I2(-1, 5.1));
    std::cout << "Results: " << std::endl << std::setprecision(12);
    for(std::vector<I2>::const_iterator i = res.begin(); i != res.end(); ++i)
      std::cout << "  " << *i << std::endl;
    std::cout << std::endl;
  }
}


#include "xpbd/tetrahedron_volume_constraint.h"

#include <Eigen/Dense>

namespace xpbd {

tetrahedron_volume_constraint_t::tetrahedron_volume_constraint_t(
    std::initializer_list<index_type> indices,
    positions_type const& p,
    scalar_type const alpha)
    : base_type(indices, alpha), V0_{0.}
{
    assert(indices.size() == 4u);
    V0_ = volume(p);
}

tetrahedron_volume_constraint_t::scalar_type
tetrahedron_volume_constraint_t::volume(positions_type const& V) const
{
    Eigen::RowVector3d const p0 = V.row(indices()[0]);
    Eigen::RowVector3d const p1 = V.row(indices()[1]);
    Eigen::RowVector3d const p2 = V.row(indices()[2]);
    Eigen::RowVector3d const p3 = V.row(indices()[3]);

    auto const vol = (1. / 6.) * (p1 - p0).cross(p2 - p0).dot(p3 - p0);
    return std::abs(vol);
}

tetrahedron_volume_constraint_t::scalar_type
tetrahedron_volume_constraint_t::evaluate(positions_type const& p, masses_type const& m) const
{
    return volume(p) - V0_;
}

void tetrahedron_volume_constraint_t::project(
    positions_type& p,
    masses_type const& m,
    scalar_type& lagrange,
    scalar_type const dt) const
{
    auto const v0 = indices()[0];
    auto const v1 = indices()[1];
    auto const v2 = indices()[2];
    auto const v3 = indices()[3];

    auto const w0 = 1. / m(v0);
    auto const w1 = 1. / m(v1);
    auto const w2 = 1. / m(v2);
    auto const w3 = 1. / m(v3);

    Eigen::RowVector3d const p0 = p.row(v0);
    Eigen::RowVector3d const p1 = p.row(v1);
    Eigen::RowVector3d const p2 = p.row(v2);
    Eigen::RowVector3d const p3 = p.row(v3);

    auto const C = evaluate(p, m);

    Eigen::RowVector3d const grad0 = (1. / 6.) * (p1 - p2).cross(p3 - p2);
    Eigen::RowVector3d const grad1 = (1. / 6.) * (p2 - p0).cross(p3 - p0);
    Eigen::RowVector3d const grad2 = (1. / 6.) * (p0 - p1).cross(p3 - p1);
    Eigen::RowVector3d const grad3 = (1. / 6.) * (p1 - p0).cross(p2 - p0);

    auto const weighted_sum_of_gradients = w0 * grad0.squaredNorm() + w1 * grad1.squaredNorm() +
                                           w2 * grad2.squaredNorm() + w3 * grad3.squaredNorm();

    if (weighted_sum_of_gradients < 1e-5)
        return;

    scalar_type const alpha_tilde = alpha_ / (dt * dt);
    scalar_type const delta_lagrange =
        -(C + alpha_tilde * lagrange) / (weighted_sum_of_gradients + alpha_tilde);

    lagrange += delta_lagrange;
    p.row(v0) += w0 * grad0 * delta_lagrange;
    p.row(v1) += w1 * grad1 * delta_lagrange;
    p.row(v2) += w2 * grad2 * delta_lagrange;
    p.row(v3) += w3 * grad3 * delta_lagrange;
}

} // namespace xpbd


#ifndef EXACTSOLUTION_PARTITION_FUNCTION_CLASSICAL_XY_FULLYCONNECTED_HPP
#define EXACTSOLUTION_PARTITION_FUNCTION_CLASSICAL_XY_FULLYCONNECTED_HPP

#include <cmath>
#include <string>
#include <vector>
#include <boost/math/special_functions/bessel.hpp>
#include <clstatphys/rootfinder/false_position_method.hpp>
#include <clstatphys/rootfinder/newton_method.hpp>

namespace exactsolution{

class PartitionFunctionClassicalXYFullyConnected{
public :
  static std::string name() { return "Exact partition function, classical XY, Fully Connected"; }
  PartitionFunctionClassicalXYFullyConnected(double J,unsigned int num) : J_(J), num_(num) {}

  double operator()(double T){
    double value = 0.0;
    value = logZ(T);
    value = std::exp(value);
    return value;
  }

  double logZ(double T) {
    double value = 0.0;
    double M_temp = M(T);
    value -= 0.5 * std::log(2.0 * M_PI * T) - 0.5 * J_ / T - 0.5 * M_temp * M_temp / T * J_  + F(M_temp / T * J_);
    value *= num_;
    return value;
  }

  double T(double U){
    double T0 = 0.0;
    double T1 = 0.75;
    if(U >= 0.25 + 0.5 * J_){
      return 2.0 * U - J_;
    } else{
      //False Position Method
      optimization::FalsePositionMethod optimizer;
      double J = J_;
      auto diff_U = [&J,&U,this](double x) { return U - ( 0.5 * x + 0.5 * J * ( 1 - this->M(x) * this->M(x)));};
      optimizer.find_zero(diff_U, T0, T1);
      return optimizer.zero();
    }
  }

  double M(double T) {
    double M_tmp = 0.0;
    if(T >= J_ * 0.5) return 0.0;
    else if( T == 0.0) return 1.0;
    else{
      M_tmp = std::sqrt(1 - 2.0 * T / J_);
      optimization::NewtonMethod optimizer;
      double J = J_;
      auto diff_M = [&J,&T,this](double x) { return x - this->dF(x * J / T);};
      auto d_diff_M = [&J,&T,this](double x) { return 1 - J / T * this->ddF(x * J / T);};
      optimizer.find_zero(diff_M, d_diff_M, M_tmp);
      return optimizer.zero();
    }
  }

  double F(double value){
    return std::log(2.0 * M_PI * boost::math::cyl_bessel_i(0,value));
  }

  double dF(double value){
    double value_output = 0.0;
    value_output = boost::math::cyl_bessel_i(1.0,value) / boost::math::cyl_bessel_i(0.0,value);
    return value_output;
  }

  double ddF(double value){
    double value_output = 0.0;
    value_output = 0.5 * ( boost::math::cyl_bessel_i(0.0,value) + boost::math::cyl_bessel_i(2.0,value) ) * boost::math::cyl_bessel_i(0.0,value);
    value_output -=  boost::math::cyl_bessel_i(1.0,value) * boost::math::cyl_bessel_i(1.0,value);
    value_output /= boost::math::cyl_bessel_i(0.0,value) * boost::math::cyl_bessel_i(0.0,value);
    return value_output;
  }

private :
  double J_;
  unsigned int num_;
  mutable double U_;
  mutable double T_;
}; //end harmonic_chain

}
#endif //EXACTSOLUTION_PARTITION_FUNCTION_CLASSICAL_XY_FULLYCONNECTED_HPP


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

using Eigen::MatrixXf;

vector<vector<double> > inverse(vector<vector<double> > m) {
  int dim = m.size();
  MatrixXf m_(dim, dim);
  for (int i=0; i<dim; i++)
    for (int j=0; j<dim; j++)
      m_(i, j) = m[i][j];
  MatrixXf inv_ = m_.inverse();
  vector<vector<double> > inv(dim, vector<double>(dim));
  for (int i=0; i<dim; i++)
    for (int j=0; j<dim; j++)
      inv[i][j] = inv_(i, j);
  return inv;
}

vector<vector<double> > pseudo_inv(vector<vector<double> > m) {
  int dim = m.size();
  MatrixXf m_(dim, dim);
  for (int i=0; i<dim; i++)
    for (int j=0; j<dim; j++)
      m_(i, j) = m[i][j];
  MatrixXf inv_ = m_.partialPivLu().solve(MatrixXf::Identity(dim, dim));
  vector<vector<double> > inv(dim, vector<double>(dim));
  for (int i=0; i<dim; i++)
    for (int j=0; j<dim; j++)
      inv[i][j] = inv_(i, j);
  return inv;
}


#pragma once

// Eigen includes
#include <Eigen/Core>
#include <Eigen/LU>

// STL includes
#include <vector>
#include <random>
#ifdef USE_THREADS
#include <thread>
#endif

// Local include
#include "DirectionsSampling.hpp"

/* This header provide multiple ways to project spherical function to Spherical
 * Harmonics vectors.
 *
 *    + 'ProjectToSH' return the SH coefficient vector when the Functor to be
 *    projected is bandlimited to the max order of SH. Warning: do not use with
 *    highly varying functions! This projection will probably behave very
 *    badly. In such case, it is advised to use the 'ProjectToShMC' method.
 *
 *    + 'ProjectToShMC' return the SH coefficient vector by integrating <f·Ylm>
 *    using Monte Carlo integration. It is possible to choose the type of
 *    random or quasi- random sequence for the integration.
 *
 *    + 'TripleTensorProduct' return the premultiplied triple tensor product:
 *    the integral of <Ylm x Ylm x Ylm> multiplied by the SH coefficients 'clm'
 *    of 'f'.  The integral is computed using Monte Carlo, as 'ProjectToShMC'
 *    does.
 *
 * TODO: Template the direction sequence.
 */

#define USE_FIBONACCI_SEQ
//#define USE_BLUENOISE_SEQ

/* From a set of `basis` vector directions, and a spherical function `Functor`,
 * generate the Spherical Harmonics projection of the `Functor`. This algorithm
  * works as follows:
 *
 *  1. Evaluate the matrix Ylm(w_i) for each SH index and each direction
 *  2. Evaluate the vector of the functor [f(w_0), ..., f(w_n)]
 *  3. Return the product Ylm(w_i)^{-1} [f(w_0), ..., f(w_n)]
 *
 *  Requierement: `basis` must have the dimension of the output SH vector.
 *                `f` must be real valued. Higher dimension functor are not
 *                handled yet.
 */
template<class Functor, class Vector, class SH>
inline Eigen::VectorXf ProjectToSH(const Functor& f,
                                   const std::vector<Vector>& basis) {

   // Get the number of elements to compute
   const int dsize = basis.size();
   const int order = sqrt(dsize)-1;

   Eigen::MatrixXf Ylm(dsize, dsize);
   Eigen::VectorXf flm(dsize);

   for(unsigned int i=0; i<basis.size(); ++i) {

      const Vector& w = basis[i];

      auto ylm = SH::FastBasis(w, order);
      Ylm.row(i) = ylm;

      flm[i] = f(w);
   }

   return Ylm.inverse() * flm;
}

/* Project a spherical function `f` onto Spherical Harmonics coefficients up
 * to order `order` using Monte-Carlo integration.
 */
template<class Functor, class Vector, class SH>
inline Eigen::VectorXf ProjectToShMC(const Functor& f, int order, int M=100000) {

   std::mt19937 gen(0);
   std::uniform_real_distribution<float> dist(0.0,1.0);

   Eigen::VectorXf shCoeffs((order+1)*(order+1));
#if   defined(USE_FIBONACCI_SEQ)
   const std::vector<Vector> directions = SamplingFibonacci<Vector>(M);
#elif defined(USE_BLUENOISE_SEQ)
   const std::vector<Vector> directions = SamplingBlueNoise<Vector>(M);
#else
   const std::vector<Vector> directions = SamplingRandom<Vector>(M);
#endif
   for(auto& w : directions) {
      // Evaluate the function and the basis vector
      shCoeffs += f(w) * SH::FastBasis(w, order);
   }
   shCoeffs *= 4.0*M_PI / float(M);

   return shCoeffs;
}

/* Compute the triple tensor product \int Ylm * Ylm * Ylm
 *
 * ## Arguments:
 *
 *    + 'ylm' is the input spherical function SH coeffcients. They will be
 *      prefactored to the triple product tensor to build the matrix.
 *
 *    + 'truncated' allows to export either the truncated matrix (up to order
 *      'n', where 'n' is the order the input SH coeffs 'ylm') or the full matrix
 *      that is order '2n-1'.
 */
template<class SH, class Vector>
inline Eigen::MatrixXf TripleTensorProduct(const Eigen::VectorXf& ylm,
                                           bool truncated=true,
                                           int nDirections=100000) {

   // Compute the max order
   const int vsize = ylm.size();
   const int order = (truncated) ? sqrt(vsize)-1 : 2*sqrt(vsize)-2;
   const int msize = (truncated) ? vsize         : SH::Terms(order);

   Eigen::MatrixXf res = Eigen::MatrixXf::Zero(msize, msize);
   Eigen::VectorXf clm(msize);

   // Take a uniformly distributed point sequence and integrate the triple tensor
   // for each SH band
#if   defined(USE_FIBONACCI_SEQ)
   const std::vector<Vector> directions = SamplingFibonacci<Vector>(nDirections);
#elif defined(USE_BLUENOISE_SEQ)
   const std::vector<Vector> directions = SamplingBlueNoise<Vector>(nDirections);
#else
   const std::vector<Vector> directions = SamplingRandom<Vector>(nDirections);
#endif
   for(auto& w : directions) {
      SH::FastBasis(w, order, clm);
      res += clm.segment(0, vsize).dot(ylm) * clm * clm.transpose();
   }

   res *= 4.0f * M_PI / float(nDirections);
   return res;
}

/* Compute the triple tensor product \int Ylm * Ylm * Ylm for a bunch of
 * function projected on Spherical Harmonics. This method is specially
 * interesting to construct product of (R,G,B) component where each component
 * is stored in a separate vector.
 *
 * ## Arguments:
 *
 *    + 'ylm' is the input spherical function SH coeffcients. They will be
 *      prefactored to the triple product tensor to build the matrix.
 *
 *    + 'truncated' allows to export either the truncated matrix (up to order
 *      'n', where 'n' is the order the input SH coeffs 'ylm') or the full matrix
 *      that is order '2n-1'.
 */
template<class SH, class Vector>
inline std::vector<Eigen::MatrixXf> TripleTensorProduct(
                                       const std::vector<Eigen::VectorXf>& ylms,
                                       bool truncated=true,
                                       int nDirections=100000) {

#ifdef USE_THREADS
   struct TTPThread : public std::thread {

      std::vector<Eigen::MatrixXf> res;

      TTPThread(const std::vector<Vector>& dirs, unsigned int start, unsigned int end,
                const std::vector<Eigen::VectorXf>& ylms, int order) :
         std::thread(&TTPThread::run, this, dirs, start, end, ylms, order) {}

      void run(const std::vector<Vector>& dirs, unsigned int start, unsigned int end,
               const std::vector<Eigen::VectorXf>& ylms, int order) {
         const int vsize = ylms[0].size();
         const float fact = 4.0f * M_PI / float(dirs.size());

         const int msize = SH::Terms(order);
         Eigen::MatrixXf mat(msize, msize);
         Eigen::VectorXf clm(msize);

         res.reserve(3);
         res.push_back(Eigen::MatrixXf::Zero(msize, msize));
         res.push_back(Eigen::MatrixXf::Zero(msize, msize));
         res.push_back(Eigen::MatrixXf::Zero(msize, msize));

         for(unsigned int k=start; k<end; ++k) {
            // Get the vector
            const Vector& w = dirs[k];

            // Construct the matrix
            SH::FastBasis(w, order, clm);
            mat = clm * clm.transpose();

            // For each SH vector apply the weight to the matrix and sum it
            for(unsigned int i=0; i<ylms.size(); ++i) {
               res[i] += fact * clm.segment(0, vsize).dot(ylms[i]) * mat;
            }
         }
      }
   };
#endif

   // Compute the max order
   const int vsize = ylms[0].size();
   const int order = (truncated) ? sqrt(vsize)-1 : 2*sqrt(vsize)-2;
   const int msize = (truncated) ? vsize         : SH::Terms(order);

   std::vector<Eigen::MatrixXf> res(ylms.size(), Eigen::MatrixXf::Zero(msize, msize));

   // Take a uniformly distributed point sequence and integrate the triple tensor
   // for each SH band
#if   defined(USE_FIBONACCI_SEQ)
   const std::vector<Vector> directions = SamplingFibonacci<Vector>(nDirections);
#elif defined(USE_BLUENOISE_SEQ)
   const std::vector<Vector> directions = SamplingBlueNoise<Vector>(nDirections);
#else
   const std::vector<Vector> directions = SamplingRandom<Vector>(nDirections);
#endif

#ifdef USE_THREADS
   std::vector<TTPThread*> threads;
   const unsigned int nthreads = std::thread::hardware_concurrency();
   for(unsigned int i=0; i<nthreads; ++i) {
      const unsigned int block = nDirections / nthreads;
      const unsigned int start = i * block;
      const unsigned int end   = (i<nthreads-1) ? (i+1)*block : nDirections;
      threads.push_back(new TTPThread(directions, start, end, ylms, order));
   }

   for(TTPThread* thread : threads) {
      thread->join();

      for(unsigned int i=0; i<3; ++i) {
         res[i] += thread->res[i];
      }
   }
#else
   Eigen::VectorXf clm(msize);
   const float fact = 4.0f * M_PI / float(nDirections);
   for(auto& w : directions) {
      // Construct the matrix
      SH::FastBasis(w, order, clm);
      const auto matrix = clm * clm.transpose();

      // For each SH vector apply the weight to the matrix and sum it
      for(unsigned int i=0; i<ylms.size(); ++i) {
         res[i] += fact * clm.segment(0, vsize).dot(ylms[i]) * matrix ;
      }
   }
#endif
   return res;
}

template<class SH, class Vector>
Eigen::MatrixXf TripleTensorProductCos(int  order,
                                       int  nDirections=100000) {

   // Compute the max order
   const int msize = SH::Terms(order);

   Eigen::MatrixXf res = Eigen::MatrixXf::Zero(msize, msize);

   // Take a uniformly distributed point sequence and integrate the triple tensor
   // for each SH band
#if   defined(USE_FIBONACCI_SEQ)
   const std::vector<Vector> directions = SamplingFibonacci<Vector>(nDirections);
#elif defined(USE_BLUENOISE_SEQ)
   const std::vector<Vector> directions = SamplingBlueNoise<Vector>(nDirections);
#else
   const std::vector<Vector> directions = SamplingRandom<Vector>(nDirections);
#endif
   const float fact = 4.0f * M_PI / float(nDirections);
   for(auto& w : directions) {
      // Construct the matrix
      const Eigen::VectorXf clm = SH::FastBasis(w, order);
      const auto matrix = clm * clm.transpose();

      // For each SH vector apply the weight to the matrix and sum it
      if(w[2] > 0.0) {
         res += fact * w[2] * matrix;
      }
   }
   return res;
}


#include <iostream>
using namespace std;

#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
// Eigenatrix Exponential
#include <unsupported/Eigen/MatrixFunctions>
using namespace Eigen;

// Declaration
Quaterniond quat_mul(Quaterniond q1,Quaterniond q2);

int main() {
    // Initializae an euler_angle vector
    // euler_angle: Roll-Pitch-Yaw
    //              0 -> -pi/2 -> -pi/4
    Eigen::Vector3d euler_angle(-M_PI/ 4.0, -M_PI/ 2.0, 0.0);
    cout << "euler = " << euler_angle.transpose() << endl;

    Eigen::Matrix3d R;
    R = Eigen::AngleAxisd(euler_angle[0], Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitX());
    cout << "rotation matrix =\n" << R << endl;

    Eigen::Quaterniond q;
    q = Eigen::AngleAxisd(euler_angle[0], Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitX());
    cout << "quaternion = " << q.coeffs().transpose() << endl;

    // w
    Eigen::Vector3d w(0.01,0.02,0.03);
    cout << "w = " << w.transpose() << endl;
    // w^
    Eigen::Matrix3d w_hat;
    w_hat << 0, -w(2), w(1),
            w(2), 0, -w(0),
            -w(1), w(0), 0;
    cout << "w^= \n" << w_hat << endl;

    // R <- Rexp(w^)
    R = R*w_hat.exp();
    cout << "new R = \n" << R << endl;

    // q <- q x [1,w].T
    Eigen::Quaterniond w_mul(1,w(0)/2, w(1)/2, w(2)/2);
    q = quat_mul(q, w_mul);
    cout << "new q = " << q.coeffs().transpose() << endl;
    cout << "new R from q = \n" << q.normalized().toRotationMatrix() << endl;

    return 0;
}

Quaterniond quat_mul(Quaterniond q1,Quaterniond q2) {
    double x =  q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y() + q1.w() * q2.x();
    double y = -q1.x() * q2.z() + q1.y() * q2.w() + q1.z() * q2.x() + q1.w() * q2.y();
    double z =  q1.x() * q2.y() - q1.y() * q2.x() + q1.z() * q2.w() + q1.w() * q2.z();
    double w = -q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z() + q1.w() * q2.w();

    return Quaterniond(w,x,y,z);
}


// error.cpp: numerical error analysis benchmark environment
//
// Copyright (C) 2017-2020 Stillwater Supercomputing, Inc.
//
// This file is part of the HPRBLAS project, which is released under an MIT Open Source license.

// include and configure number systems
// configure the posit number system behavior
#define POSIT_ROUNDING_ERROR_FREE_IO_FORMAT 0
// configure the HPR-BLAS behavior
#define HPRBLAS_TRACE_ROUNDING_EVENTS 0
#include <hprblas>
// Boost arbitrary precision floats
#include <boost/multiprecision/cpp_bin_float.hpp>

// matrix generators
#include <generators/matrix_generators.hpp>

template<typename Scalar, typename Vector>
void GenerateNumericalAnalysisTestCase(const std::string& header, unsigned N, bool verbose = false) {
	using namespace std;
	using namespace mtl;
	using namespace sw::unum;
	using namespace sw::hprblas;

	std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n" << header << std::endl;

	// calculate the numerical error caused by the linear algebra computation
	Vector e(N), eprime(N), eabsolute(N), erelative(N), I(N);
	e = Scalar(1);
	I = Scalar(1);
	// TODO: it is not clear that for posits this would be a fused matrix-vector operation
	matvec(eprime, I, e);
	cout << "reference vector : " << e << '\n';
	cout << "error vector     : " << eprime << '\n';
	// absolute error
	eabsolute = e - eprime;
	cout << "absolute error vector : " << eabsolute << '\n';
	cout << "L1 norm   " << hex_format(l1_norm(eabsolute)) << "  " << l1_norm(eabsolute) << '\n';
	cout << "L2 norm   " << hex_format(l2_norm(eabsolute)) << "  " << l2_norm(eabsolute) << '\n';
	cout << "Linf norm " << hex_format(linf_norm(eabsolute)) << "  " << linf_norm(eabsolute) << '\n';

	// relative error
	cout << "relative error\n";
	Scalar relative_error;
	relative_error = l1_norm(eabsolute) / l1_norm(e);
	cout << "L1 norm   " << hex_format(relative_error) << "  " << relative_error << '\n';
	relative_error = l2_norm(eabsolute) / l2_norm(e);
	cout << "L2 norm   " << hex_format(relative_error) << "  " << relative_error << '\n';
	relative_error = linf_norm(eabsolute) / linf_norm(e);
	cout << "Linf norm " << hex_format(relative_error) << "  " << relative_error << '\n';

	// error volume
	cout << "error bounding box volume\n";
	cout << "Measured in Euclidean distance    : " << error_volume(linf_norm(eabsolute), N, false) << '\n';
	cout << "Measured in ULPs                  : " << error_volume(linf_norm(eabsolute), N, true) << " ulps^" << N << '\n';
	Scalar ulp = numeric_limits<Scalar>::epsilon();
	cout << "L-infinitiy norm measured in ULPs : " << linf_norm(eabsolute) / ulp << " ulps" << '\n';

	cout << endl;
}

// Benchmark Suite runner for numerical error analysis measurements
int main(int argc, char** argv)
try {
	using namespace std;
	using namespace mtl;
	using namespace sw::unum;
	using namespace sw::hprblas;

	int nrOfFailedTestCases = 0;

	// we need to enumerate along the following dimensions
	// 1- number systems: the key here is that when we have posits, we use FDP
	// 2- algorithms: different computational approaches to solve a system of linear equations
	// 3- matrices. easy, difficult, empirical

	// The measurement will always be the error to this equation:
	//    Ax = b, with a b that delivers the solution x = ones()

	// There is another measure and that is driven by the characteristic polynomial of the matrix:
	// If we have very wildy differing eigenvalues, then there are b vectors that can lift up small
	// eigenvalues compared to large eigenvalues. Those are situations in which we want to make
	// certain we don't have cancellation: a big eigenvalue multiplied by a small scaling factor
	// and a small eigenvalue multiplied by a big scaling factor.

	// the benchmark runner is structured as follows:
	// foreach test system
	//     pick a test size N
	//     foreach number system
	//         generate the test matrix A(N,N)
	//         generate the test right hand side: b(N) = A * ones()
	//         foreach algorithm
	//             generate the inverse or decomposition
	//             solve the system of equations: Ax = b
	//             measure the difference between result x and ones()
	using Scalar = sw::unum::posit<32, 2>;
	using Vector = mtl::vec::dense_vector<Scalar>;
	using Matrix = mtl::mat::dense2D<Scalar>;

	int N = 5;
	Matrix H(N, N);
	sw::hprblas::GenerateHilbertMatrix(H, false);
	Matrix Hinv = GaussJordanInversion(H);
	Matrix Href(N, N);
	GenerateHilbertMatrixInverse(Href);
	Matrix I(N, N);  // H * Hinv should yield the identity matrix
	// TODO: this is not clear that for posits this would be a fused matrix multiply
	matmul(I, H, Hinv);

	GenerateNumericalAnalysisTestCase<Scalar, Vector>("testing", 10, true);

	return (nrOfFailedTestCases > 0 ? EXIT_FAILURE : EXIT_SUCCESS);
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


#pragma once

#include <Eigen/Dense>
#include <list>

/**
* A Simple Kalman filter.
*   A - System dynamics matrix
*   C - Output matrix
*   Q - Process noise covariance
*   R - Measurement noise covariance
*   P - Estimate error covariance
* The input is a 3d position vector.
* Outputs are a 3d position, 3d velocity, and 3d acceleration vectors.
*/

class KalmanFilter_Position3D {

public:
	/**
	* Initialize the kalman filter.
	* dt - a filter time step.
	*/
	KalmanFilter_Position3D(const double dt);

	/** Update the estimated state based on measured values. */
	void Update(const Eigen::VectorXd& y);

	/** Returns the filter estimation. */
	inline Eigen::VectorXd GetEstimate() { return x_predicted; }
private:
	static const int n, m;				// The size of the filter output and input vectors.
	const double dt;					// The filter time step size.

	Eigen::VectorXd x_predicted;		// Last predicted value.
	Eigen::MatrixXd P, A, H, R, Q, I;	// Kalman filter matrices.
};


#pragma once

// Eigen includes
#include <Eigen/Core>

// STL includes
#include <cassert>
#include <vector>

#define AXIAL_EPSILON 1.0E-6

/* _Cosine Sum Integral_
 */
inline Eigen::VectorXf CosSumIntegral(float x, float y, float c, int n) {

	const float siny  = sin(y);
	const float sinx  = sin(x);
	const float cosy  = cos(y);
	const float cosx  = cos(x);
	const float cosy2 = cosy*cosy;
	const float cosx2 = cosx*cosx;

   static const Eigen::Vector2i i1 = {1, 1};
   static const Eigen::Vector2i i2 = {2, 2};
   Eigen::Vector2i i = {0, 1};
   Eigen::Vector2f F = {y-x, siny-sinx};
   Eigen::Vector2f S = {0.0f, 0.0f};

   Eigen::VectorXf R = Eigen::VectorXf::Zero(n+2);

   Eigen::Vector2f pow_c    = {1.0, c};
   Eigen::Vector2f pow_cosx = {cosx, cosx2};
   Eigen::Vector2f pow_cosy = {cosy, cosy2};

   while(i[1] <= n) {
      // S <= S + c^{i} F
      S += pow_c.cwiseProduct(F);

      // The resulting vector `R` is shifted of one to the right. This is due to
      // the fact that order `n` moment requires only up to order `n-1` power of
      // cosine integrals.
      R.segment(i[1], 2) = S;

      // T <= cos(y)^{i+1} sin(y) - cos(x)^{i+1} sin(x)
      // F <= (T + i+1) F / (i+2)
      auto T = pow_cosy*siny - pow_cosx*sinx;
      F = (T + (i+i1).cast<float>().cwiseProduct(F)).cwiseQuotient((i+i2).cast<float>());

      // Update temp variable
      i        += i2;
      pow_c    *= c*c;
      pow_cosx *= cosx2;
      pow_cosy *= cosy2;
   }

   return R;
}

/* _sign_
 *
 * Sign function template
 */
template <typename T>
inline int sign(T val) {
    return (T(0) <= val) - (val < T(0));
}

/* _clamp_
 *
 * Clamp function template to restrict a given function to be in between
 * boundaries.
 */
template <typename T>
inline T clamp(T val, T a, T b) {
    return std::max<T>(a, std::min<T>(b, val));
}

/* _Line Integral_
 */
template<class Vector>
inline Eigen::VectorXf LineIntegral(const Vector& A, const Vector& B,
                                    const Vector& w, int n) {
#ifdef ZERO_ORTHOGONAL
   auto wDotA = Vector::Dot(A, w);
   auto wDotB = Vector::Dot(B, w);
   // Zeroth moment and orthogonal directions 'w' to the while edge do not
   // require this part since it will always return '0';
   if(std::abs(wDotA) < AXIAL_EPSILON && std::abs(wDotB) < AXIAL_EPSILON) {
      return Eigen::VectorXf::Zero(n+2);
   }
#endif

   // Note: expanding the (I-ssT)B expression from Arvo's LineIntegral pseudo
   // code to the projection of B on plane with A as normal.
   const auto s = Vector::Normalize(A);
   const auto t = Vector::Normalize(B - Vector::Dot(s, B)*s);

   const auto a = Vector::Dot(w, s);
   const auto b = Vector::Dot(w, t);
   const auto c = sqrt(a*a + b*b);

   // Compute the arc-length on which to compute the integral of the moment
   // function and the shift 'p' that change the integral to the integral of
   // a shifted cosine.
   const auto r = Vector::Dot(s, B) / Vector::Dot(B,B);
   const auto l = acos(clamp<float>(r, -1.0f, 1.0f));
   const auto p = atan2(b, a);

   return CosSumIntegral(-p, l-p, c, n);
}

/* _Boundary Integral_
 *
 * Compute the integral along P egdes of the up to order 'n' axial moment
 * around w. By using 'n' = 'w' you can compute the single axis moment. Double
 * axis moment with second axis begin the normal must use 'v' == 'n' ('n' being
 * the normal).
 */
template<class Polygon, class Vector>
inline Eigen::VectorXf BoundaryIntegral(const Polygon& P, const Vector& w,
                                        const Vector& v, int n) {
   // Init to zero
   Eigen::VectorXf b = Eigen::VectorXf::Zero(n+2);

   for(auto& edge : P) {
      // Compute the edge normal
      const auto normal = Vector::Normalize(Vector::Cross(edge.A, edge.B));

      // Add the egde integral to the total integral
      const auto dotNV   = Vector::Dot(normal, v);
      const auto lineInt = LineIntegral<Vector>(edge.A, edge.B, w, n);
      b += dotNV * lineInt;
   }

   return b;
}

/* _Solid Angle_
 *
 * Compute the solid angle sustained by a `Polygon P`.
 */
template<class Polygon, class Vector>
inline float SolidAngle(const Polygon& P) {
   if(P.size() == 3) {
      // Using the method of Oosterom and Strackee [1983]
      const Vector& A = P[0].A;
      const Vector& B = P[1].A;
      const Vector& C = P[2].A;

      const Vector bc = Vector::Cross(B,C);
      const float num = std::abs(Vector::Dot(bc, A));
      const float al = Vector::Length(A);
      const float bl = Vector::Length(B);
      const float cl = Vector::Length(C);
      const float den = al*bl*cl
                      + Vector::Dot(A, B)*cl
                      + Vector::Dot(A, C)*bl
                      + Vector::Dot(B, C)*al;

      float phi = atan2(num, den);
      if(phi < 0) {
         phi += M_PI;
      }
      return 2.0f * phi;

   } else {
      // Using the algorithm for computing solid angle of polyhedral cones by
      // Mazonka found in http://arxiv.org/pdf/1205.1396v2.pdf
      std::complex<float> z(1, 0);
      for(unsigned int k=0; k<P.size(); ++k) {
         const Vector& A = P[(k > 0) ? k-1 : P.size()-1].A;
         const Vector& B = P[k].A;
         const Vector& C = P[k].B;

         const float ak = Vector::Dot(A, C);
         const float bk = Vector::Dot(A, B);
         const float ck = Vector::Dot(B, C);
         const float dk = Vector::Dot(A, Vector::Cross(B, C));
         const std::complex<float> zk(bk*ck-ak, dk);
         z *= zk;
      }
      const float arg = std::arg(z);
      return arg;
   }
}

/* _Check Polygon_
 *
 * Check if the Poylgon P is well oriented. For a triangle, the centroid of the
 * triangle `D` is computed as `A + B + C / 3` and compared to the normal of
 * the triangle using the orientation. The normal and the centroid must match
 * orientation for the normal of edges to be outwards.
 */
template<class Polygon, class Vector>
inline bool CheckPolygon(const Polygon& P) {

   // A closed Polygon cannot be smaller than 3 Edges.
   if(P.size() < 3) {
      return false;

   // Special case for triangles.
   } else if(P.size() == 3) {
      // Check with respect to centroid
      const auto D = (P[0].A + P[1].A + P[2].A) / 3.0f;
      const auto N = Vector::Cross(P[1].A-P[0].A, P[2].A-P[0].A);
      return Vector::Dot(D, N) <= 0.0f;

   // General computation
   } else {
      // This is a heuristic to select a point on the bounding box of the
      // polygon. The orientation test is then computed on this particular
      // corner.
      unsigned int K = 0;
      const Vector* minX = &P[0].B;
      for(unsigned int k=1; k<P.size(); ++k) {
         const Vector* X = &P[k].B;
         if(X->x < minX->x || (X->x <= minX->x && X->y < minX->y)) {
            minX = X;
            K    = k;
         }
      }

      // Perform the test as defined on the Wikipedia page:
      //   https://en.wikipedia.org/wiki/Curve_orientation
      const int K2 = (K < P.size()-1) ? K+1 : 0;
      const auto D = (P[K].A + P[K].B + P[K2].B) / 3.0f;
      const auto N = Vector::Cross(P[K].B-P[K].A, P[K2].B-P[K].A);
      const bool r = Vector::Dot(D, N) <= 0.0f;
      return r;
   }
}

/* _Axial Moments_
 *
 * input:
 *   + Polygon P: A set of egdes that can be enumerated using iterators.
                  Each edge must enable to access two Vector A and B.
 *   + Vector  w: A 3D vector with elements accessible as x,y,z this
                  vector defines the axis on which to compute moments.
 *   + int     n: The maximum moment order to be computed.
 *
 * output:
 *   + VectorX r: A vector containing all moments up to order 'n'
 */
template<class Polygon, class Vector>
inline Eigen::VectorXf AxialMoment(const Polygon& P, const Vector& w, int n) {

   // Check if the polygon is well oriented
   const bool check = CheckPolygon<Polygon, Vector>(P);
   if(!check) {
      return Eigen::VectorXf::Zero(n+2);
   }

   // Arvo's boundary integral for single vector moment.
   Eigen::VectorXf a = - BoundaryIntegral<Polygon, Vector>(P, w, w, n);

   // Generate the 'b' vector which equals to the Polygon solid angle for
   // even moments and zero for odd moments.
   const int n2 = (n+2)/2;
   auto b = Eigen::Map<Eigen::VectorXf, 0, Eigen::InnerStride<2>>(a.data(), n2);
   b += Eigen::VectorXf::Constant(n2, SolidAngle<Polygon, Vector>(P));

   // 'c' is the vector of linear elements, storing 'i+1' for index 'i'
   auto c = Eigen::VectorXf::LinSpaced(n+2, 1, n+2);

   return a.cwiseQuotient(c);
}

/* _Axial Moments_
 *
 * Compute the axial moments for given set of directions used for lobe sharing
 * the maximum cosine order to compute the integral is a function of the size
 * of the directions list.
 */
template<class Polygon, class Vector>
inline Eigen::VectorXf AxialMoments(const Polygon& P,
                                    const std::vector<Vector>& directions) {

   const int dsize = directions.size();
   const int order = (dsize-1) / 2 + 1;

   Eigen::VectorXf result(dsize*order);

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

      // Get the vector associated to the current row
      const Vector& w = directions[i];

      // Evaluate all the Y_{l,m} for the current vector
      const auto In = AxialMoment<Polygon, Vector>(P, w, order-1);

      const int shift = i*order;
      result.segment(shift, order) = In.segment(0, order);
   }
   return result;
}