#include "per_corner_normals.h"
#include "triangle_area_normal.h"
// Hint:
#include "vertex_triangle_adjacency.h"

void per_corner_normals(
  const Eigen::MatrixXd & V,
  const Eigen::MatrixXi & F,
  const double corner_threshold,
  Eigen::MatrixXd & N)
{
  N = Eigen::MatrixXd::Zero(F.rows()*3,3);
  Eigen::VectorXi count = Eigen::VectorXi::Zero(F.rows()*3);
  std::vector<std::vector<int>> VF;

  vertex_triangle_adjacency(F, static_cast<int>(V.rows()), VF);

  double cos_threshold = std::cos(corner_threshold * 3.14159265358979323846 / 180.0);

  /* Compute the area-weighted average of normals of faces with normals that deviate less than threshold. */
  for (int i = 0; i < F.rows(); i++) { /* For each face */
    Eigen::RowVector3d face_area_normal = triangle_area_normal(V.row(F(i,0)), V.row(F(i,1)), V.row(F(i,2)));
    for (int j = 0; j < 3; j++) { /* For each corner of the face */
      for (int g = 0; g < VF[F(i,j)].size(); g++) { /* For each face neighboring the corner */
        int face = VF[F(i, j)][g];
        Eigen::RowVector3d neighbor_area_normal = triangle_area_normal(V.row(F(face,0)), V.row(F(face,1)), V.row(F(face,2)));
        double dot = neighbor_area_normal.normalized().dot(face_area_normal.normalized());
        if (std::max(-1.0, std::min(1.0, dot)) > cos_threshold) {
          N.row(i * 3 + j) += neighbor_area_normal;
          count(i * 3 + j)++;
        }
      }
    }
  }
  /* Normalize the sum of the normals by the number of faces neighboring each corner. */
  for (int i = 0; i < N.rows(); i++) {
    N.row(i) = (N.row(i) / count(i)).normalized();
  }
}