#include "grad.h"
#include "doublearea.h"
#include "per_face_normals.h"

double compute_lscm_energy(
  const Eigen::MatrixXd& V,
  const Eigen::MatrixXi& F,
  const Eigen::VectorXd& W_flat)
{
  const int nV = V.rows();
  const int nF = F.rows();

  // Compute gradient
  Eigen::SparseMatrix<double> G;
  igl::grad(V, F, G);

  Eigen::VectorXd nablaU_flat = G * W_flat.head(nV);
  Eigen::VectorXd nablaV_flat = G * W_flat.tail(nV);

  Eigen::MatrixXd nablaU(nF, 3);
  Eigen::MatrixXd nablaV(nF, 3);
  nablaU << nablaU_flat.segment(0, nF), nablaU_flat.segment(nF, nF), nablaU_flat.segment(2 * nF, nF);
  nablaV << nablaV_flat.segment(0, nF), nablaV_flat.segment(nF, nF), nablaV_flat.segment(2 * nF, nF);

  // Compute face normal
  Eigen::MatrixXd N;
  igl::per_face_normals(V, F, N);

  // Compute area
  Eigen::VectorXd dblA;
  igl::doublearea(V, F, dblA);

  // Sum up
  double sum = 0;

  for (int i = 0; i < nF; ++i) {
    Eigen::Vector3d N_i = N.row(i);
    Eigen::Vector3d nablaU_i = nablaU.row(i);
    Eigen::Vector3d nablaV_i = nablaV.row(i);

    // Rotate nablaU_i about N_i by 90 degrees ccw
    nablaU_i = N_i.cross(nablaU_i);

    sum += 0.5 * dblA[i] * (nablaU_i - nablaV_i).squaredNorm();
  }

  return 0.5 * sum;
}

  std::cout << std::setprecision(20) << "LSCM energy 1: " << compute_lscm_energy(V, F, W_flat) << std::endl;
  std::cout << std::setprecision(20) << "LSCM energy 2: " << (0.5 * W_flat.transpose() * Q * W_flat) << std::endl;