#include "flatpattern.hpp"
#include "trianglepatterngeometry.hpp"
#include <filesystem>

FlatPattern::FlatPattern() {}

FlatPattern::FlatPattern(const string &filename, bool addNormalsIfAbsent) {
  assert(std::filesystem::exists(std::filesystem::path(filename)));
  loadFromPly(filename);
  if (addNormalsIfAbsent) {
    bool normalsAreAbsent = vert[0].cN().Norm() < 0.000001;
    if (normalsAreAbsent) {
      for (auto &v : vert) {
        v.N() = CoordType(0, 0, 1);
      }
    }
  }

  vcg::tri::UpdateTopology<FlatPattern>::VertexEdge(*this);
  scale();

  updateEigenEdgeAndVertices();
}

FlatPattern::FlatPattern(const std::vector<size_t> &numberOfNodesPerSlot,
                         const std::vector<vcg::Point2i> &edges) {
  add(numberOfNodesPerSlot, edges);
  // add normals
  bool normalsAreAbsent = vert[0].cN().Norm() < 0.000001;
  if (normalsAreAbsent) {
    for (auto &v : vert) {
      v.N() = CoordType(0, 0, 1);
    }
  }
  scale();
  updateEigenEdgeAndVertices();
}

bool FlatPattern::createHoneycombAtom() {
  VCGEdgeMesh honeycombQuarter;
  const VCGEdgeMesh::CoordType n(0, 0, 1);
  const double H = 0.2;
  const double height = 1.5 * H;
  const double width = 0.2;
  const double theta = 70;
  const double dy = tan(vcg::math::ToRad(90 - theta)) * width / 2;
  vcg::tri::Allocator<VCGEdgeMesh>::AddVertex(
      honeycombQuarter, VCGEdgeMesh::CoordType(0, height / 2, 0), n);
  vcg::tri::Allocator<VCGEdgeMesh>::AddVertex(
      honeycombQuarter, VCGEdgeMesh::CoordType(0, H / 2 - dy, 0), n);
  vcg::tri::Allocator<VCGEdgeMesh>::AddVertex(
      honeycombQuarter, VCGEdgeMesh::CoordType(width / 2, H / 2, 0), n);
  vcg::tri::Allocator<VCGEdgeMesh>::AddVertex(
      honeycombQuarter, VCGEdgeMesh::CoordType(width / 2, 0, 0), n);
  vcg::tri::Allocator<VCGEdgeMesh>::AddEdge(honeycombQuarter, 0, 1);
  vcg::tri::Allocator<VCGEdgeMesh>::AddEdge(honeycombQuarter, 1, 2);
  vcg::tri::Allocator<VCGEdgeMesh>::AddEdge(honeycombQuarter, 2, 3);

  VCGEdgeMesh honeycombAtom;
  // Top right
  vcg::tri::Append<VCGEdgeMesh, VCGEdgeMesh>::MeshCopy(honeycombAtom,
                                                       honeycombQuarter);
  // Bottom right
  vcg::Matrix44d rotM;
  rotM.SetRotateDeg(180, vcg::Point3d(1, 0, 0));
  vcg::tri::UpdatePosition<VCGEdgeMesh>::Matrix(honeycombQuarter, rotM);
  vcg::tri::Append<VCGEdgeMesh, VCGEdgeMesh>::Mesh(honeycombAtom,
                                                   honeycombQuarter);
  // Bottom left
  rotM.SetRotateDeg(180, vcg::Point3d(0, 1, 0));
  vcg::tri::UpdatePosition<VCGEdgeMesh>::Matrix(honeycombQuarter, rotM);
  vcg::tri::Append<VCGEdgeMesh, VCGEdgeMesh>::Mesh(honeycombAtom,
                                                   honeycombQuarter);
  // Top left
  rotM.SetRotateDeg(180, vcg::Point3d(1, 0, 0));
  vcg::tri::UpdatePosition<VCGEdgeMesh>::Matrix(honeycombQuarter, rotM);
  vcg::tri::Append<VCGEdgeMesh, VCGEdgeMesh>::Mesh(honeycombAtom,
                                                   honeycombQuarter);

  for (VertexType &v : honeycombAtom.vert) {
    v.P()[2] = 0;
  }

  return true;
}

void FlatPattern::deleteDanglingEdges() {
  for (VertexType &v : vert) {
    std::vector<VCGEdgeMesh::EdgePointer> incidentElements;
    vcg::edge::VEStarVE(&v, incidentElements);
    if (incidentElements.size() == 1) {
      vcg::tri::Allocator<VCGEdgeMesh>::DeleteEdge(*this, *incidentElements[0]);
    }
    if (incidentElements.size() == 1) {
      vcg::tri::Allocator<VCGEdgeMesh>::DeleteVertex(*this, v);
    }
  }

  vcg::tri::Clean<VCGEdgeMesh>::RemoveDegenerateVertex(*this);
  vcg::tri::Clean<VCGEdgeMesh>::RemoveDegenerateEdge(*this);
  vcg::tri::Allocator<VCGEdgeMesh>::CompactEveryVector(*this);
}

void FlatPattern::scale() {
  const double baseTriangleCentralEdgeSize =
      (vert[0].cP() - vert[3].cP()).Norm();
  const double scaleRatio =
      desiredBaseTriangleCentralEdgeSize / baseTriangleCentralEdgeSize;

  vcg::tri::UpdatePosition<VCGEdgeMesh>::Scale(*this, scaleRatio);
}

void FlatPattern::deleteDanglingVertices() {
  vcg::tri::Allocator<FlatPattern>::PointerUpdater<VertexPointer> pu;
  deleteDanglingVertices(pu);
}

void FlatPattern::deleteDanglingVertices(
    vcg::tri::Allocator<FlatPattern>::PointerUpdater<VertexPointer> &pu) {
  for (VertexType &v : vert) {
    std::vector<FlatPattern::EdgePointer> incidentElements;
    vcg::edge::VEStarVE(&v, incidentElements);
    if (incidentElements.size() == 0) {
      vcg::tri::Allocator<FlatPattern>::DeleteVertex(*this, v);
    }
  }

  vcg::tri::Allocator<FlatPattern>::CompactVertexVector(*this, pu);
  vcg::tri::Allocator<FlatPattern>::CompactEdgeVector(*this);

  updateEigenEdgeAndVertices();
}

void FlatPattern::tilePattern(VCGEdgeMesh &pattern, VCGPolyMesh &tileInto,
                              const bool &shouldDeleteDanglingEdges) {
  const size_t middleIndex = 3;
  double xOffset =
      vcg::Distance(pattern.vert[0].cP(), pattern.vert[middleIndex].cP()) /
      std::tan(M_PI / 3);
  CoordType patternCoord0 = pattern.vert[0].cP();
  CoordType patternBottomRight =
      pattern.vert[middleIndex].cP() + CoordType(xOffset, 0, 0);
  CoordType patternBottomLeft =
      pattern.vert[middleIndex].cP() - CoordType(xOffset, 0, 0);
  std::vector<vcg::Point3d> patternTrianglePoints{
      patternCoord0, patternBottomRight, patternBottomLeft};
  CoordType pointOnPattern =
      patternCoord0 + (patternBottomLeft - patternCoord0) ^
      (patternBottomRight - patternCoord0);

  std::vector<CoordType> faceCenters(FN());
  VCGTriMesh tileIntoEdgeMesh;

  for (VCGPolyMesh::FaceType &f : tileInto.face) {
    std::vector<VCGPolyMesh::VertexType *> incidentVertices;
    vcg::face::VFIterator<PFace> vfi(
        &f, 0); // initialize the iterator to the first face
                //    vcg::face::Pos p(vfi.F(), f.cV(0));
                //    vcg::face::VVOrderedStarFF(p, incidentVertices);
    size_t numberOfNodes = 0;
    CoordType centerOfFace(0, 0, 0);
    for (size_t vi = 0; vi < f.VN(); vi++) {
      numberOfNodes++;
      centerOfFace = centerOfFace + f.cP(vi);
    }
    centerOfFace /= f.VN();
    vcg::tri::Allocator<VCGTriMesh>::AddVertex(tileIntoEdgeMesh, centerOfFace,
                                               vcg::Color4b::Yellow);

    //    const size_t vi = vcg::tri::Index<VCGPolyMesh>(tileInto, v);
    //    std::cout << "vertex " << vi << " has incident vertices:" <<
    //    std::endl;
    for (size_t vi = 0; vi < f.VN(); vi++) {
      //      size_t f = 0;
      //                std::cout << vcg::tri::Index<VCGTriMesh>(tileInto,
      //  / incidentVertices[f]) /                          << std::endl;

      // Compute transformation matrix M
      //        vcg::Matrix44d M;
      std::vector<vcg::Point3d> meshTrianglePoints{
          centerOfFace, f.cP(vi), vi + 1 == f.VN() ? f.cP(0) : f.cP(vi + 1)};
      CoordType faceNormal = ((meshTrianglePoints[1] - meshTrianglePoints[0]) ^
                              (meshTrianglePoints[2] - meshTrianglePoints[0]))
                                 .Normalize();
      auto fit = vcg::tri::Allocator<VCGTriMesh>::AddFace(
          tileIntoEdgeMesh, meshTrianglePoints[0], meshTrianglePoints[1],
          meshTrianglePoints[2]);
      fit->N() = faceNormal;
      CoordType pointOnTriMesh =
          meshTrianglePoints[0] +
              (meshTrianglePoints[1] - meshTrianglePoints[0]) ^
          (meshTrianglePoints[2] - meshTrianglePoints[0]);
      vcg::Matrix44d M;
      //        vcg::ComputeRigidMatchMatrix(meshTrianglePoints,
      //        patternTrianglePoints,
      //                                     M);
      vcg::Matrix44d A_prime;
      A_prime[0][0] = meshTrianglePoints[0][0];
      A_prime[1][0] = meshTrianglePoints[0][1];
      A_prime[2][0] = meshTrianglePoints[0][2];
      A_prime[3][0] = 1;
      A_prime[0][1] = meshTrianglePoints[1][0];
      A_prime[1][1] = meshTrianglePoints[1][1];
      A_prime[2][1] = meshTrianglePoints[1][2];
      A_prime[3][1] = 1;
      A_prime[0][2] = meshTrianglePoints[2][0];
      A_prime[1][2] = meshTrianglePoints[2][1];
      A_prime[2][2] = meshTrianglePoints[2][2];
      A_prime[3][2] = 1;
      A_prime[0][3] = pointOnTriMesh[0];
      A_prime[1][3] = pointOnTriMesh[1];
      A_prime[2][3] = pointOnTriMesh[2];
      A_prime[3][3] = 1;
      vcg::Matrix44d A;
      A[0][0] = patternTrianglePoints[0][0];
      A[1][0] = patternTrianglePoints[0][1];
      A[2][0] = patternTrianglePoints[0][2];
      A[3][0] = 1;
      A[0][1] = patternTrianglePoints[1][0];
      A[1][1] = patternTrianglePoints[1][1];
      A[2][1] = patternTrianglePoints[1][2];
      A[3][1] = 1;
      A[0][2] = patternTrianglePoints[2][0];
      A[1][2] = patternTrianglePoints[2][1];
      A[2][2] = patternTrianglePoints[2][2];
      A[3][2] = 1;
      A[0][3] = pointOnPattern[0];
      A[1][3] = pointOnPattern[1];
      A[2][3] = pointOnPattern[2];
      A[3][3] = 1;
      M = A_prime * vcg::Inverse(A);

      VCGEdgeMesh transformedPattern;
      vcg::tri::Append<VCGEdgeMesh, VCGEdgeMesh>::MeshCopy(transformedPattern,
                                                           pattern);
      vcg::tri::UpdatePosition<VCGEdgeMesh>::Matrix(transformedPattern, M);
      for (VertexType &v : transformedPattern.vert) {
        v.N() = faceNormal;
      }

      vcg::tri::Append<VCGEdgeMesh, VCGEdgeMesh>::Mesh(*this,
                                                       transformedPattern);
    }
  }

  //  vcg::tri::Clean<VCGEdgeMesh>::MergeCloseVertex(*this, 0.0000000001);
  //  vcg::tri::Clean<VCGEdgeMesh>::RemoveDegenerateVertex(*this);
  //  vcg::tri::Clean<VCGEdgeMesh>::RemoveDegenerateEdge(*this);
  //  vcg::tri::Allocator<VCGEdgeMesh>::CompactEveryVector(*this);

  vcg::tri::UpdateTopology<VCGEdgeMesh>::VertexEdge(*this);

  vcg::tri::Clean<VCGEdgeMesh>::MergeCloseVertex(*this, 0.0000000001);
  deleteDanglingVertices();
  deleteDanglingEdges();
  vcg::tri::Clean<VCGEdgeMesh>::RemoveDegenerateVertex(*this);
  vcg::tri::Clean<VCGEdgeMesh>::RemoveDegenerateEdge(*this);
  vcg::tri::Allocator<VCGEdgeMesh>::CompactEveryVector(*this);
  updateEigenEdgeAndVertices();
  savePly("tiledPattern.ply");

  vcg::tri::Clean<VCGTriMesh>::MergeCloseVertex(tileIntoEdgeMesh, 0.0000000001);
  vcg::tri::Clean<VCGTriMesh>::RemoveDegenerateVertex(tileIntoEdgeMesh);
  vcg::tri::Clean<VCGTriMesh>::RemoveDegenerateEdge(tileIntoEdgeMesh);
  tileIntoEdgeMesh.savePly("tileIntoTriMesh.ply");
}

void FlatPattern::createFan(const size_t &fanSize) {
  FlatPattern rotatedPattern;
  vcg::tri::Append<FlatPattern, FlatPattern>::MeshCopy(rotatedPattern, *this);
  for (int rotationCounter = 1; rotationCounter < fanSize; rotationCounter++) {
    vcg::Matrix44d R;
    auto rotationAxis = vcg::Point3d(0, 0, 1);
    R.SetRotateDeg(360 / fanSize, rotationAxis);
    vcg::tri::UpdatePosition<FlatPattern>::Matrix(rotatedPattern, R);
    vcg::tri::Append<FlatPattern, FlatPattern>::Mesh(*this, rotatedPattern);
  }

  removeDuplicateVertices();
  updateEigenEdgeAndVertices();
}

void FlatPattern::removeDuplicateVertices() {
  vcg::tri::Clean<VCGEdgeMesh>::MergeCloseVertex(*this, 0.0000000001);
  vcg::tri::Clean<VCGEdgeMesh>::RemoveDegenerateVertex(*this);
  vcg::tri::Clean<VCGEdgeMesh>::RemoveDegenerateEdge(*this);
  vcg::tri::Allocator<VCGEdgeMesh>::CompactEveryVector(*this);
  vcg::tri::UpdateTopology<FlatPattern>::VertexEdge(*this);
}

void FlatPattern::tilePattern(VCGEdgeMesh &pattern, VCGTriMesh &tileInto) {

  const size_t middleIndex = 3;
  double xOffset =
      vcg::Distance(pattern.vert[0].cP(), pattern.vert[middleIndex].cP()) /
      std::tan(M_PI / 3);
  CoordType patternCoord0 = pattern.vert[0].cP();
  CoordType patternBottomRight =
      pattern.vert[middleIndex].cP() + CoordType(xOffset, 0, 0);
  CoordType patternBottomLeft =
      pattern.vert[middleIndex].cP() - CoordType(xOffset, 0, 0);
  std::vector<vcg::Point3d> patternTrianglePoints{
      patternCoord0, patternBottomRight, patternBottomLeft};
  CoordType pointOnPattern =
      patternCoord0 + (patternBottomLeft - patternCoord0) ^
      (patternBottomRight - patternCoord0);

  for (VCGTriMesh::VertexType &v : tileInto.vert) {
    const auto centralNodeColor = vcg::Color4<unsigned char>(64, 64, 64, 255);
    const bool isCentralNode = v.cC() == centralNodeColor;
    if (isCentralNode) {
      std::vector<VCGTriMesh::VertexType *> incidentVertices;
      vcg::face::VFIterator<VCGTriMeshFace> vfi(
          &v); // initialize the iterator tohe first face
      vcg::face::Pos p(vfi.F(), &v);
      vcg::face::VVOrderedStarFF(p, incidentVertices);

      const size_t vi = vcg::tri::Index<VCGTriMesh>(tileInto, v);
      std::cout << "vertex " << vi << " has incident vertices:" << std::endl;
      for (size_t f = 0; f < incidentVertices.size(); f++) {
        //      size_t f = 0;
        std::cout << vcg::tri::Index<VCGTriMesh>(tileInto, incidentVertices[f])
                  << std::endl;

        // Compute transformation matrix M
        //        vcg::Matrix44d M;
        std::vector<vcg::Point3d> meshTrianglePoints{
            v.cP(), incidentVertices[f]->cP(),
            f + 1 == incidentVertices.size() ? incidentVertices[0]->cP()
                                             : incidentVertices[f + 1]->cP()};
        CoordType faceNormal =
            ((meshTrianglePoints[1] - meshTrianglePoints[0]) ^
             (meshTrianglePoints[2] - meshTrianglePoints[0]))
                .Normalize();
        CoordType pointOnTriMesh =
            meshTrianglePoints[0] +
                (meshTrianglePoints[1] - meshTrianglePoints[0]) ^
            (meshTrianglePoints[2] - meshTrianglePoints[0]);
        vcg::Matrix44d M;
        //        vcg::ComputeRigidMatchMatrix(meshTrianglePoints,
        //        patternTrianglePoints,
        //                                     M);
        vcg::Matrix44d A_prime;
        A_prime[0][0] = meshTrianglePoints[0][0];
        A_prime[1][0] = meshTrianglePoints[0][1];
        A_prime[2][0] = meshTrianglePoints[0][2];
        A_prime[3][0] = 1;
        A_prime[0][1] = meshTrianglePoints[1][0];
        A_prime[1][1] = meshTrianglePoints[1][1];
        A_prime[2][1] = meshTrianglePoints[1][2];
        A_prime[3][1] = 1;
        A_prime[0][2] = meshTrianglePoints[2][0];
        A_prime[1][2] = meshTrianglePoints[2][1];
        A_prime[2][2] = meshTrianglePoints[2][2];
        A_prime[3][2] = 1;
        A_prime[0][3] = pointOnTriMesh[0];
        A_prime[1][3] = pointOnTriMesh[1];
        A_prime[2][3] = pointOnTriMesh[2];
        A_prime[3][3] = 1;
        vcg::Matrix44d A;
        A[0][0] = patternTrianglePoints[0][0];
        A[1][0] = patternTrianglePoints[0][1];
        A[2][0] = patternTrianglePoints[0][2];
        A[3][0] = 1;
        A[0][1] = patternTrianglePoints[1][0];
        A[1][1] = patternTrianglePoints[1][1];
        A[2][1] = patternTrianglePoints[1][2];
        A[3][1] = 1;
        A[0][2] = patternTrianglePoints[2][0];
        A[1][2] = patternTrianglePoints[2][1];
        A[2][2] = patternTrianglePoints[2][2];
        A[3][2] = 1;
        A[0][3] = pointOnPattern[0];
        A[1][3] = pointOnPattern[1];
        A[2][3] = pointOnPattern[2];
        A[3][3] = 1;
        M = A_prime * vcg::Inverse(A);

        VCGEdgeMesh transformedPattern;
        vcg::tri::Append<VCGEdgeMesh, VCGEdgeMesh>::MeshCopy(transformedPattern,
                                                             pattern);
        vcg::tri::UpdatePosition<VCGEdgeMesh>::Matrix(transformedPattern, M);
        for (VertexType &v : transformedPattern.vert) {
          v.N() = faceNormal;
        }

        vcg::tri::Append<VCGEdgeMesh, VCGEdgeMesh>::Mesh(*this,
                                                         transformedPattern);
      }
    }
  }

  vcg::tri::UpdateTopology<VCGEdgeMesh>::VertexEdge(*this);
  deleteDanglingVertices();
  deleteDanglingEdges();
  vcg::tri::Allocator<VCGEdgeMesh>::CompactEveryVector(*this);

  updateEigenEdgeAndVertices();
}