#include <geometrycentral/surface/manifold_surface_mesh.h>
#include <geometrycentral/surface/meshio.h>
#include <geometrycentral/surface/vertex_position_geometry.h>

#include <polyscope/polyscope.h>
#include <polyscope/surface_mesh.h>

using namespace geometrycentral;
using namespace geometrycentral::surface;

// == Geometry-central data
std::unique_ptr<ManifoldSurfaceMesh> mesh;
std::unique_ptr<VertexPositionGeometry> geometry;

/** Gradient of fvals, a scalar quantity on vertices. Outputs a vector quantity on one face */
Vector3 face_gradient_of(Face f, std::vector<double> fvals) {
  Vector3 normal = geometry->faceNormal(f);
  double area = geometry->faceArea(f);

  std::vector<Vertex> v_indices;
  for(Vertex v: f.adjacentVertices()) {
    v_indices.push_back(v);
  }
  
  Vector3 loc_face_gradient = {0, 0, 0};
  for (int i=0; i < 3; i++) {
    Vertex v0 = v_indices.at(i);
    Vertex v1 = v_indices.at((i+1)%3);
    Vertex v2 = v_indices.at((i+2)%3);

    loc_face_gradient += (
      fvals[v0.getIndex()]/(2*area)*
      cross(normal, geometry->vertexPositions[v2]-geometry->vertexPositions[v1])
    );
  }
  return loc_face_gradient;
}

/** Compute the divergence of U (vector on faces), outputs as result a scalar vertices */
std::vector<double> divergence(std::vector<Vector3> U) {
  std::vector<double> out;

  for (Vertex v : mesh->vertices()) {
    out.push_back(0);
  }

  for (Face f : mesh->faces()) {
    Vector3 normal = geometry->faceNormal(f);
    std::vector<Vertex> v_indices;
    for(Vertex v: f.adjacentVertices()) {
      v_indices.push_back(v);
    }

    for (int i=0; i < 3; i++) {
      Vertex v0 = v_indices.at(i);
      Vertex v1 = v_indices.at((i+1)%3);
      Vertex v2 = v_indices.at((i+2)%3);

      out[v0.getIndex()] -= dot(
        U[f.getIndex()],
        cross(normal, geometry->vertexPositions[v2]-geometry->vertexPositions[v1])
      );
    }
  }
  return out;
}

int main(int argc, char **argv) {
  // Initialize polyscope
  polyscope::init();

  // Load mesh
  std::tie(mesh, geometry) = readManifoldSurfaceMesh(argv[1]);

  // Register the mesh with polyscope
  auto sfm = polyscope::registerSurfaceMesh("Input obj",
                                 geometry->inputVertexPositions,
                                 mesh->getFaceVertexList(),
                                 polyscopePermutations(*mesh));
  geometry->requireVertexPositions();
  geometry->requireFaceNormals();
  geometry->requireFaceAreas();

  std::vector<double> sin_coords;
  {//* First question : showing basic quantity
    std::vector<double> snd_coordinate;
    for (auto i=0; i < geometry->inputVertexPositions.size(); i++) {
      auto pos = geometry->inputVertexPositions[i];
      snd_coordinate.push_back((double)pos[1]);
    }
    sfm->addVertexScalarQuantity("Second coordinate", snd_coordinate);

    float lambda = 18.88;
    for (auto i=0; i < geometry->inputVertexPositions.size(); i++) {
      auto pos = geometry->inputVertexPositions[i];
      double posX = cos(PI / 4) * pos.x - sin(PI / 4) * pos.y;
      double posY = sin(PI / 4) * pos.x + cos(PI / 4) * pos.y;
      sin_coords.push_back(sin(lambda * (posX + 0.5)) + sin(lambda * posY));
    }
    sfm->addVertexScalarQuantity("Sinusoid data from coordinates", sin_coords);
  }

  {//* Second question : compute the area and the normal vector for each triangle
    // After doing geometry->requireFaceNormals(),
    // we can access normals with geometry->faceNormal(Face f)

    sfm->addFaceVectorQuantity("Faces normals", geometry->faceNormals);

    // After doing geometry->requireFaceAreas(),
    // we can access normals with geometry->faceArea(Face f)
    sfm->addFaceScalarQuantity("Faces area", geometry->faceAreas);
  }
  
  std::vector<Vector3> face_gradients;
  {//* Third question : compute the per face gradient of f
    for (Face f: mesh->faces()) {
      face_gradients.push_back(face_gradient_of(f, sin_coords));
    }
    sfm->addFaceVectorQuantity("Faces gradients", face_gradients);
  }
  
  {//* Fourth question : Compute and display the divergence of grad(f)
    sfm->addVertexScalarQuantity("Faces div of gradients", divergence(face_gradients));
  }

  //* Laplace-Beltrami

  // Give control to the polyscope gui
  polyscope::show();

  return EXIT_SUCCESS;
}