#include "trianglepatterngeometry.hpp"
#include "trianglepattterntopology.hpp"
#include <algorithm>
#include <iterator>
#include <numeric>
#include <vcg/complex/algorithms/update/position.h>
#include <vcg/simplex/edge/topology.h>
#include <vcg/space/intersection2.h>
#include <wrap/io_trimesh/export.h>

size_t PatternGeometry::computeTiledValence(
    const size_t &nodeIndex,
    const std::vector<size_t> &numberOfNodesPerSlot) const {
  std::vector<PatternGeometry::EdgeType *> connectedEdges;
  vcg::edge::VEStarVE(&vert[nodeIndex], connectedEdges);
  const size_t nodeValence = connectedEdges.size();
  assert(nodeSlot.count(nodeIndex) != 0);
  const size_t nodeSlotIndex = nodeSlot.at(nodeIndex);
  if (nodeSlotIndex == 0) {
    return nodeValence * fanSize;
  } else if (nodeSlotIndex == 1 || nodeSlotIndex == 2) {
    size_t correspondingNodeIndex;
    if (nodeSlotIndex == 1) {
      correspondingNodeIndex = nodeIndex + 1;
    } else {
      correspondingNodeIndex = nodeIndex - 1;
    }
    std::vector<PatternGeometry::EdgeType *> connectedEdgesCorrespondingNode;
    vcg::edge::VEStarVE(&vert[correspondingNodeIndex],
                        connectedEdgesCorrespondingNode);
    size_t correspondingNodeValence = connectedEdgesCorrespondingNode.size();
    return fanSize / 2 * nodeValence + fanSize / 2 * correspondingNodeValence;
  } else if (nodeSlotIndex == 3 || nodeSlotIndex == 5) {
    size_t correspondingNodeIndex;
    size_t numberOfNodesBefore;
    size_t numberOfNodesAfter;
    if (nodeSlotIndex == 3) {
      numberOfNodesBefore =
          nodeIndex - std::accumulate(numberOfNodesPerSlot.begin(),
                                      numberOfNodesPerSlot.begin() + 3, 0);
      correspondingNodeIndex =

          std::accumulate(numberOfNodesPerSlot.begin(),
                          numberOfNodesPerSlot.begin() + 6, 0) -
          1 - numberOfNodesBefore;
    } else {
      numberOfNodesAfter =
          std::accumulate(numberOfNodesPerSlot.begin(),
                          numberOfNodesPerSlot.begin() + 6, 0) -
          1 - nodeIndex;
      correspondingNodeIndex =
          numberOfNodesAfter + std::accumulate(numberOfNodesPerSlot.begin(),
                                               numberOfNodesPerSlot.begin() + 3,
                                               0);
    }
    assert(correspondingNodeIndex < vn);
    std::vector<PatternGeometry::EdgeType *> connectedEdgesCorrespondingNode;
    vcg::edge::VEStarVE(&vert[correspondingNodeIndex],
                        connectedEdgesCorrespondingNode);
    size_t correspondingNodeValence = connectedEdgesCorrespondingNode.size();
    return nodeValence + correspondingNodeValence;
  } else if (nodeSlotIndex == 4) {
    return 2 * nodeValence;
  } else if (nodeSlotIndex == 6) {
    return nodeValence;
  } else {
    std::cerr << "Error in slot index computation" << std::endl;
  }
  assert(false);
  return 0;
}

size_t PatternGeometry::getFanSize() const { return fanSize; }

double PatternGeometry::getTriangleEdgeSize() const { return triangleEdgeSize; }

PatternGeometry::PatternGeometry() {}

std::vector<vcg::Point3d> PatternGeometry::getVertices() const {
  std::vector<VCGEdgeMesh::CoordType> verts(VN());
  for (size_t vi = 0; vi < VN(); vi++) {
    verts[vi] = vert[vi].cP();
  }
  return verts;
}

PatternGeometry PatternGeometry::createTile(PatternGeometry &pattern) {

  const size_t fanSize = PatternGeometry().getFanSize();
  PatternGeometry fan(createFan(pattern));
  PatternGeometry tile(fan);

  if (fanSize % 2 == 1) {
    vcg::Matrix44d R;
    auto rotationAxis = vcg::Point3d(0, 0, 1);
    R.SetRotateDeg(180, rotationAxis);
    vcg::tri::UpdatePosition<PatternGeometry>::Matrix(fan, R);
  }
  vcg::Matrix44d T;
  const double centerAngle = 2 * M_PI / fanSize;
  const double triangleHeight =
      std::sin((M_PI - centerAngle) / 2) * PatternGeometry().triangleEdgeSize;
  T.SetTranslate(0, -2 * triangleHeight, 0);
  vcg::tri::UpdatePosition<PatternGeometry>::Matrix(fan, T);

  PatternGeometry fanOfFan = createFan(fan);
  vcg::tri::Append<PatternGeometry, PatternGeometry>::Mesh(tile, fanOfFan);
  vcg::tri::Clean<PatternGeometry>::MergeCloseVertex(tile, 0.0000005);
  vcg::tri::Allocator<PatternGeometry>::CompactEveryVector(tile);
  vcg::tri::UpdateTopology<PatternGeometry>::VertexEdge(tile);
  vcg::tri::UpdateTopology<PatternGeometry>::EdgeEdge(tile);

  for (size_t vi = 0; vi < pattern.vn; vi++) {
    tile.vert[vi].C() = vcg::Color4b::Blue;
  }

  tile.updateEigenEdgeAndVertices();
  return tile;
}

PatternGeometry PatternGeometry::createFan(PatternGeometry &pattern) {
  const size_t fanSize = PatternGeometry().getFanSize();
  PatternGeometry fan(pattern);
  PatternGeometry rotatedPattern(pattern);
  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<PatternGeometry>::Matrix(rotatedPattern, R);
    vcg::tri::Append<PatternGeometry, PatternGeometry>::Mesh(fan,
                                                             rotatedPattern);
  }
  return fan;
}

PatternGeometry::PatternGeometry(PatternGeometry &other) {
  vcg::tri::Append<PatternGeometry, PatternGeometry>::MeshCopy(*this, other);
  this->vertices = other.getVertices();
  vcg::tri::UpdateTopology<PatternGeometry>::VertexEdge(*this);
  vcg::tri::UpdateTopology<PatternGeometry>::EdgeEdge(*this);
}

bool PatternGeometry::savePly(const std::string plyFilename) {

  int returnValue = vcg::tri::io::ExporterPLY<PatternGeometry>::Save(
      *this, plyFilename.c_str(),
      vcg::tri::io::Mask::IOM_EDGEINDEX | vcg::tri::io::Mask::IOM_VERTCOLOR,
      false);
  if (returnValue != 0) {
    std::cerr << vcg::tri::io::ExporterPLY<PatternGeometry>::ErrorMsg(
                     returnValue)
              << std::endl;
  }

  return static_cast<bool>(returnValue);
}

void PatternGeometry::add(const std::vector<vcg::Point3d> &vertices) {
  this->vertices = vertices;
  std::for_each(vertices.begin(), vertices.end(), [&](const vcg::Point3d &p) {
    vcg::tri::Allocator<PatternGeometry>::AddVertex(*this, p);
  });
  vcg::tri::UpdateTopology<PatternGeometry>::VertexEdge(*this);
  vcg::tri::UpdateTopology<PatternGeometry>::EdgeEdge(*this);
  updateEigenEdgeAndVertices();
}

void PatternGeometry::add(const std::vector<vcg::Point2i> &edges) {
  std::for_each(edges.begin(), edges.end(), [&](const vcg::Point2i &e) {
    vcg::tri::Allocator<PatternGeometry>::AddEdge(*this, e[0], e[1]);
  });
  vcg::tri::UpdateTopology<PatternGeometry>::VertexEdge(*this);
  vcg::tri::UpdateTopology<PatternGeometry>::EdgeEdge(*this);
  updateEigenEdgeAndVertices();
}

void PatternGeometry::add(const std::vector<vcg::Point3d> &vertices,
                          const std::vector<vcg::Point2i> &edges) {
  add(vertices);
  add(edges);
  updateEigenEdgeAndVertices();
}

void PatternGeometry::add(const std::vector<size_t> &numberOfNodesPerSlot,
                          const std::vector<vcg::Point2i> &edges) {
  assert(numberOfNodesPerSlot.size() == 7);
  auto vertices =
      constructVertexVector(numberOfNodesPerSlot, fanSize, triangleEdgeSize);
  add(vertices, edges);
  vcg::tri::UpdateTopology<PatternGeometry>::VertexEdge(*this);
  vcg::tri::UpdateTopology<PatternGeometry>::EdgeEdge(*this);
  updateEigenEdgeAndVertices();
}

std::vector<vcg::Point3d> PatternGeometry::constructVertexVector(
    const std::vector<size_t> &numberOfNodesPerSlot, const size_t &fanSize,
    const double &triangleEdgeSize) {

  std::vector<vcg::Point3d> vertices;
  const double centerAngle = 2 * M_PI / fanSize;
  const double triangleHeight =
      std::sin((M_PI - centerAngle) / 2) * triangleEdgeSize;
  const double baseEdgeSize =
      2 * triangleEdgeSize * std::cos((M_PI - centerAngle) / 2);
  const vcg::Point3d triangleV0 = vcg::Point3d(0, 0, 0);
  const vcg::Point3d triangleV1 =
      vcg::Point3d(-baseEdgeSize / 2, -triangleHeight, 0);
  const vcg::Point3d triangleV2 =
      vcg::Point3d(baseEdgeSize / 2, -triangleHeight, 0);

  // Nodes
  if (numberOfNodesPerSlot[0] == 1) {
    //    vertices[0] = triangleV0;
    vertices.push_back(triangleV0);
  }
  if (numberOfNodesPerSlot[1] == 1) {
    //    vertices[1] = triangleV1;
    vertices.push_back(triangleV1);
  }
  if (numberOfNodesPerSlot[2] == 1) {
    //    vertices[2] = triangleV2;
    vertices.push_back(triangleV2);
  }

  // Edges
  if (numberOfNodesPerSlot[3] != 0) {
    const double offset0 = 1.0 / (numberOfNodesPerSlot[3] + 1);
    const vcg::Point3d edgeVector0(triangleV1 - triangleV0);
    for (size_t vertexIndex = 0; vertexIndex < numberOfNodesPerSlot[3];
         vertexIndex++) {

      //      vertices[std::accumulate(numberOfNodesPerSlot.begin(),
      //                               numberOfNodesPerSlot.begin() + 2, 0) +
      //               vertexIndex] =
      vertices.push_back(triangleV0 +
                         edgeVector0.operator*((vertexIndex + 1) * offset0));
    }
  }

  if (numberOfNodesPerSlot[4] == 1) {
    vertices.push_back(vcg::Point3d(0, -triangleHeight, 0));
  } else if (numberOfNodesPerSlot[4] != 0) {
    const double offset1 = 1.0 / (numberOfNodesPerSlot[4] + 1);
    const vcg::Point3d edgeVector1(triangleV2 - triangleV1);
    for (size_t vertexIndex = 0; vertexIndex < numberOfNodesPerSlot[4];
         vertexIndex++) {

      //      vertices[std::accumulate(numberOfNodesPerSlot.begin(),
      //                               numberOfNodesPerSlot.begin() + 3, 0) +
      //               vertexIndex] =
      vertices.push_back(triangleV1 +
                         edgeVector1.operator*((vertexIndex + 1) * offset1));
    }
  }

  if (numberOfNodesPerSlot[5] != 0) {
    const double offset2 = 1.0 / (numberOfNodesPerSlot[5] + 1);
    const vcg::Point3d edgeVector2(triangleV0 - triangleV2);
    for (size_t vertexIndex = 0; vertexIndex < numberOfNodesPerSlot[5];
         vertexIndex++) {
      //      vertices[std::accumulate(numberOfNodesPerSlot.begin(),
      //                               numberOfNodesPerSlot.begin() + 4, 0) +
      //               vertexIndex] =
      vertices.push_back(triangleV2 +
                         edgeVector2.operator*((vertexIndex + 1) * offset2));
    }
  }

  // Face
  if (numberOfNodesPerSlot[6] != 0) {
    const vcg::Point3d triangleCenter((triangleV0 + triangleV1 + triangleV2) /
                                      3);
    const double radius = 0.1;
    const double offsetRad = 2 * M_PI / numberOfNodesPerSlot[6];
    for (size_t vertexIndex = 0; vertexIndex < numberOfNodesPerSlot[6];
         vertexIndex++) {
      const double pX =
          triangleCenter[0] + radius * std::cos(vertexIndex * offsetRad);
      const double pY =
          triangleCenter[1] + radius * std::sin(vertexIndex * offsetRad);
      /*vertices[std::accumulate(numberOfNodesPerSlot.begin(),
                               numberOfNodesPerSlot.begin() + 5, 0) +
               vertexIndex] =*/
      vertices.push_back(vcg::Point3d(pX, pY, 0));
    }
  }
  return vertices;
}

void PatternGeometry::constructNodeToTiledValenceMap(
    const std::vector<size_t> &numberOfNodesPerSlot) {
  for (size_t nodeIndex = 0; nodeIndex < vn; nodeIndex++) {
    const size_t tiledValence =
        computeTiledValence(nodeIndex, numberOfNodesPerSlot);
    nodeTiledValence[nodeIndex] = tiledValence;
  }
}

bool PatternGeometry::hasDanglingEdges(
    const std::vector<size_t> &numberOfNodesPerSlot) {
  if (nodeSlot.empty()) {
    FlatPatternTopology::constructNodeToSlotMap(numberOfNodesPerSlot, nodeSlot);
  }
  if (correspondingNode.empty()) {
    constructCorresponginNodeMap(numberOfNodesPerSlot);
  }
  if (nodeTiledValence.empty()) {
    constructNodeToTiledValenceMap(numberOfNodesPerSlot);
  }
  bool hasDanglingEdges = false;
  for (size_t nodeIndex = 0; nodeIndex < vn; nodeIndex++) {
    const size_t tiledValence = nodeTiledValence[nodeIndex];
    if (tiledValence == 1) {
      vert[nodeIndex].C() = vcg::Color4b::Red;
      hasDanglingEdges = true;
    }
  }
  return hasDanglingEdges;
}

bool PatternGeometry::hasUntiledDanglingEdges() {
  //  vcg::tri::Clean<TrianglePatternGeometry>::MergeCloseVertex(*this,
  //  0.0000005);
  //  vcg::tri::Allocator<TrianglePatternGeometry>::CompactEveryVector(*this);
  //  vcg::tri::UpdateTopology<TrianglePatternGeometry>::VertexEdge(*this);
  //  vcg::tri::UpdateTopology<TrianglePatternGeometry>::EdgeEdge(*this);
  bool hasDanglingEdges = false;
  for (size_t vi = 0; vi < vn; vi++) {
    std::vector<PatternGeometry::EdgeType *> connectedEdges;
    vcg::edge::VEStarVE(&vert[vi], connectedEdges);
    const size_t nodeValence = connectedEdges.size();
    if (nodeValence == 1) {
      if (vert[vi].C().operator==(vcg::Color4b(vcg::Color4b::Red))) {

      } else {
        vert[vi].C() = vcg::Color4b::Blue;
      }
      hasDanglingEdges = true;
    }
  }
  return hasDanglingEdges;
}

// TODO: The function expects that the numberOfNodesPerSlot follows a specific
// format and that the vertex container was populated in a particular order.
void PatternGeometry::constructCorresponginNodeMap(
    const std::vector<size_t> &numberOfNodesPerSlot) {
  assert(vn != 0 && !nodeSlot.empty() && correspondingNode.empty() &&
         numberOfNodesPerSlot.size() == 7);

  for (size_t nodeIndex = 0; nodeIndex < vn; nodeIndex++) {
    const size_t slotIndex = nodeSlot[nodeIndex];
    if (slotIndex == 1) {
      correspondingNode[nodeIndex] = nodeIndex + 1;
    } else if (slotIndex == 2) {
      correspondingNode[nodeIndex] = nodeIndex - 1;
    } else if (slotIndex == 3) {
      const size_t numberOfNodesBefore =
          nodeIndex - std::accumulate(numberOfNodesPerSlot.begin(),
                                      numberOfNodesPerSlot.begin() + 3, 0);
      correspondingNode[nodeIndex] =
          std::accumulate(numberOfNodesPerSlot.begin(),
                          numberOfNodesPerSlot.begin() + 6, 0) -
          1 - numberOfNodesBefore;
    } else if (slotIndex == 5) {
      const size_t numberOfNodesAfter =
          std::accumulate(numberOfNodesPerSlot.begin(),
                          numberOfNodesPerSlot.begin() + 6, 0) -
          1 - nodeIndex;
      correspondingNode[nodeIndex] =
          numberOfNodesAfter + std::accumulate(numberOfNodesPerSlot.begin(),
                                               numberOfNodesPerSlot.begin() + 3,
                                               0);
    }
  }
}

bool PatternGeometry::isFullyConnectedWhenTiled() {
  assert(vn != 0 /* && !correspondingNode.empty()*/);
  //  TrianglePatternGeometry copyOfPattern(*this);

  // If bottom interface nodes have a valence of zero there definetely more than
  // a single cc
  bool bottomInterfaceIsConnected = false;
  for (size_t nodeIndex = 0; nodeIndex < vn; nodeIndex++) {
    if (nodeSlot[nodeIndex] == 1 || nodeSlot[nodeIndex] == 4 ||
        nodeSlot[nodeIndex] == 2) {
      std::vector<PatternGeometry::EdgeType *> connectedEdges;
      vcg::edge::VEStarVE(&vert[nodeIndex], connectedEdges);
      const size_t nodeValence = connectedEdges.size();
      if (nodeValence != 0) {
        bottomInterfaceIsConnected = true;
        break;
      }
    }
  }
  if (!bottomInterfaceIsConnected) {
    return false;
  }

  PatternGeometry fanedPattern = createFan(*this);
  vcg::tri::Clean<PatternGeometry>::MergeCloseVertex(fanedPattern, 0.000000005);
  vcg::tri::Allocator<PatternGeometry>::CompactEveryVector(fanedPattern);
  vcg::tri::UpdateTopology<PatternGeometry>::VertexEdge(fanedPattern);
  vcg::tri::UpdateTopology<PatternGeometry>::EdgeEdge(fanedPattern);
  std::vector<std::pair<int, PatternGeometry::EdgePointer>> eCC;
  vcg::tri::Clean<PatternGeometry>::edgeMeshConnectedComponents(fanedPattern,
                                                                eCC);

  const bool sideInterfaceIsConnected = 1 == eCC.size();
  if (!sideInterfaceIsConnected) {
    return false;
  }

  //  for (size_t nodeIndex = 0; nodeIndex < vn; nodeIndex++) {
  //    if (nodeTiledValence[nodeIndex] == 0)
  //      continue;
  //    const size_t slotIndex = nodeSlot[nodeIndex];
  //    // connect nodes belonging to first or third slot(nodes on the first
  //    // triangle edge)
  //    //      if (nodeTiledValence[nodeIndex] == 0 && slotIndex == 3) {
  //    //        continue;
  //    //      }
  //    if (slotIndex == 1 || slotIndex == 3) {
  //      assert(correspondingNode.count(nodeIndex) != 0);
  //      const size_t correspondingNodeIndex =
  //      correspondingNode[nodeIndex];
  //      std::vector<TrianglePatternGeometry::EdgeType *> starEdges;
  //      vcg::edge::VEStarVE(&vert[nodeIndex], starEdges);
  //      bool containsEdge = false;
  //      for (TrianglePatternGeometry::EdgeType *e : starEdges) {
  //        assert(vcg::tri::Index(*this, e->V(0)) == nodeIndex ||
  //                vcg::tri::Index(*this, e->V(1)) == nodeIndex);
  //        if (vcg::tri::Index(*this, e->V(0)) == nodeIndex) {
  //          if (vcg::tri::Index(*this, e->V(1)) == correspondingNodeIndex) {
  //            containsEdge = true;
  //            break;
  //          }
  //        } else if (vcg::tri::Index(*this, e->V(1)) == nodeIndex) {
  //          if (vcg::tri::Index(*this, e->V(0)) == correspondingNodeIndex) {
  //            containsEdge = true;
  //            break;
  //          }
  //        }
  //      }
  //      if (!containsEdge) {
  //        vcg::tri::Allocator<TrianglePatternGeometry>::AddEdge(
  //            copyOfPattern, nodeIndex, correspondingNodeIndex);
  //      }
  //    } else if (slotIndex == 2 || slotIndex == 5) {
  //      assert(correspondingNode.count(nodeIndex) != 0);
  //    } else {
  //      assert(correspondingNode.count(nodeIndex) == 0);
  //    }
  //  }

  //  std::vector<std::pair<int, TrianglePatternGeometry::EdgePointer>> eCC;
  //  vcg::tri::Clean<TrianglePatternGeometry>::edgeMeshConnectedComponents(
  //      copyOfPattern, eCC);
  //  size_t numberOfCC_edgeBased = eCC.size();
  //  size_t numberOfCC_vertexBased = numberOfCC_edgeBased;
  //  if (numberOfCC_edgeBased == 1) {
  //    vcg::tri::UpdateTopology<TrianglePatternGeometry>::VertexEdge(
  //        copyOfPattern);
  //    vcg::tri::UpdateTopology<TrianglePatternGeometry>::EdgeEdge(copyOfPattern);
  //    vcg::tri::UpdateFlags<TrianglePatternGeometry>::VertexSetV(copyOfPattern);
  //    vcg::tri::UpdateFlags<TrianglePatternGeometry>::VertexClear(copyOfPattern);
  //    for (size_t ei = 0; ei < copyOfPattern.EN(); ei++) {
  //      copyOfPattern.edge[ei].V(0)->SetV();
  //      copyOfPattern.edge[ei].V(1)->SetV();
  //    }

  //    for (size_t vi = 0; vi < copyOfPattern.VN(); vi++) {
  //      if (!copyOfPattern.vert[vi].IsV()) {
  //        numberOfCC_vertexBased++;
  //      }
  //    }
  //    return numberOfCC_vertexBased;
  //  }

  //  return numberOfCC_edgeBased == 1; // TODO: not good
  return true;
}

bool PatternGeometry::hasIntersectingEdges(
    const std::string &patternBinaryRepresentation,
    const std::unordered_map<size_t, std::unordered_set<size_t>>
        &intersectingEdges) {
  bool containsIntersectingEdges = false;
  for (size_t validEdgeIndex = 0;
       validEdgeIndex < patternBinaryRepresentation.size(); validEdgeIndex++) {
    if (patternBinaryRepresentation[validEdgeIndex] == '1' &&
        intersectingEdges.count(validEdgeIndex) != 0) {
      for (auto edgeIndexIt =
               intersectingEdges.find(validEdgeIndex)->second.begin();
           edgeIndexIt != intersectingEdges.find(validEdgeIndex)->second.end();
           edgeIndexIt++) {
        if (patternBinaryRepresentation[*edgeIndexIt] == '1') {
          containsIntersectingEdges = true;
          //            statistics.numberOfIntersectingEdgesOverAllPatterns++;
          break; // should be uncommented in order to improve
          //            performance
        }
      }
      if (containsIntersectingEdges) {
        break; // should be uncommented in order to improve performance
      }
    }
  }

  return containsIntersectingEdges;
}

std::unordered_map<size_t, std::unordered_set<size_t>>
PatternGeometry::getIntersectingEdges(
    size_t &numberOfIntersectingEdgePairs) const {
  std::unordered_map<size_t, std::unordered_set<size_t>> intersectingEdges;
  numberOfIntersectingEdgePairs = 0;
  size_t numberOfEdgePairs;
  if (en == 0 || en == 1) {
    numberOfEdgePairs = 0;
  } else {
    numberOfEdgePairs = binomialCoefficient(en, 2);
  }

  for (size_t edgePairIndex = 0; edgePairIndex < numberOfEdgePairs;
       edgePairIndex++) {
    size_t ei0 =
        en - 2 -
        floor(sqrt(-8 * edgePairIndex + 4 * en * (en - 1) - 7) / 2.0 - 0.5);
    size_t ei1 = edgePairIndex + ei0 + 1 - en * (en - 1) / 2 +
                 (en - ei0) * ((en - ei0) - 1) / 2;
    const vcg::Point2d p0(edge[ei0].cP(0)[0], edge[ei0].cP(0)[1]);
    const float epsilon = 0.000005;
    vcg::Box2d bb0(p0 - vcg::Point2d(epsilon, epsilon),
                   p0 + vcg::Point2d(epsilon, epsilon));
    const vcg::Point2d p1(edge[ei0].cP(1)[0], edge[ei0].cP(1)[1]);
    vcg::Box2d bb1(p1 - vcg::Point2d(epsilon, epsilon),
                   p1 + vcg::Point2d(epsilon, epsilon));
    const vcg::Point2d p2(edge[ei1].cP(0)[0], edge[ei1].cP(0)[1]);
    vcg::Box2d bb2(p2 - vcg::Point2d(epsilon, epsilon),
                   p2 + vcg::Point2d(epsilon, epsilon));
    if (bb2.Collide(bb1) || bb2.Collide(bb0))
      continue;
    const vcg::Point2d p3(edge[ei1].cP(1)[0], edge[ei1].cP(1)[1]);
    vcg::Box2d bb3(p3 - vcg::Point2d(epsilon, epsilon),
                   p3 + vcg::Point2d(epsilon, epsilon));
    if (bb3.Collide(bb1) || bb3.Collide(bb0))
      continue;
    const vcg::Segment2d s0(p0, p1);
    const vcg::Segment2d s1(p2, p3);

    vcg::Point2d intersectionPoint;
    const bool edgesIntersect =
        vcg::SegmentSegmentIntersection(s0, s1, intersectionPoint);
    if (edgesIntersect) {
      numberOfIntersectingEdgePairs++;
      intersectingEdges[ei0].insert(ei1);
      intersectingEdges[ei1].insert(ei0);
    }
  }
  return intersectingEdges;
}

PatternGeometry::PatternGeometry(const std::string &filename,
                                 bool addNormalsIfAbsent) {
  if (!std::filesystem::exists(std::filesystem::path(filename))) {
    assert(false);
    std::cerr << "No flat pattern with name " << filename << std::endl;
    return;
  }
  if (!load(filename)) {
    assert(false);
    std::cerr << "File could not be loaded  " << filename << std::endl;
    return;
  }
  if (addNormalsIfAbsent) {
    addNormals();
  }

  vcg::tri::UpdateTopology<PatternGeometry>::VertexEdge(*this);
  baseTriangleHeight = computeBaseTriangleHeight();

  updateEigenEdgeAndVertices();
}

double PatternGeometry::computeBaseTriangleHeight() const {
  return (vert[0].cP() - vert[3].cP()).Norm();
}

void PatternGeometry::addNormals() {
  bool normalsAreAbsent = vert[0].cN().Norm() < 0.000001;
  if (normalsAreAbsent) {
    for (auto &v : vert) {
      v.N() = CoordType(0, 0, 1);
    }
  }
}

PatternGeometry::PatternGeometry(
    const std::vector<size_t> &numberOfNodesPerSlot,
    const std::vector<vcg::Point2i> &edges) {
  add(numberOfNodesPerSlot, edges);
  addNormals();
  baseTriangleHeight = computeBaseTriangleHeight();
  updateEigenEdgeAndVertices();
}

bool PatternGeometry::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 PatternGeometry::copy(PatternGeometry &copyFrom) {
  VCGEdgeMesh::copy(copyFrom);
  baseTriangleHeight = copyFrom.getBaseTriangleHeight();
}

void PatternGeometry::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 PatternGeometry::scale(const double &desiredBaseTriangleCentralEdgeSize) {
  this->baseTriangleHeight = desiredBaseTriangleCentralEdgeSize;
  const double baseTriangleCentralEdgeSize =
      (vert[0].cP() - vert[3].cP()).Norm();
  const double scaleRatio =
      desiredBaseTriangleCentralEdgeSize / baseTriangleCentralEdgeSize;

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

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

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

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

  updateEigenEdgeAndVertices();
}

void PatternGeometry::tilePattern(VCGEdgeMesh& pattern, VCGPolyMesh &tileInto,const int& interfaceNodeIndex,
                                  const bool &shouldDeleteDanglingEdges) {
  double xOffset =
      vcg::Distance(pattern.vert[0].cP(), pattern.vert[interfaceNodeIndex].cP()) /
      std::tan(M_PI / 3);
  CoordType patternCoord0 = pattern.vert[0].cP();
  CoordType patternBottomRight =
      pattern.vert[interfaceNodeIndex].cP() + CoordType(xOffset, 0, 0);
  CoordType patternBottomLeft =
      pattern.vert[interfaceNodeIndex].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::Clean<VCGEdgeMesh>::RemoveDuplicateVertex(*this);
  vcg::tri::Clean<VCGEdgeMesh>::MergeCloseVertex(*this, 0.000061524);
  vcg::tri::UpdateTopology<VCGEdgeMesh>::VertexEdge(*this);

  //  vcg::tri::Clean<VCGEdgeMesh>::RemoveUnreferencedVertex(*this);
  //  vcg::tri::UpdateTopology<VCGEdgeMesh>::VertexEdge(*this);
  //    vcg::tri::Clean<VCGEdgeMesh>::RemoveDegenerateVertex(*this);
  //  vcg::tri::Clean<VCGEdgeMesh>::RemoveDegenerateEdge(*this);
  //  vcg::tri::Allocator<VCGEdgeMesh>::CompactEveryVector(*this);
  deleteDanglingVertices();
  vcg::tri::Allocator<VCGEdgeMesh>::CompactEveryVector(*this);
  //  vcg::tri::Clean<VCGEdgeMesh>::RemoveDegenerateVertex(*this);
  //  vcg::tri::Clean<VCGEdgeMesh>::RemoveUnreferencedVertex(*this);
  //  vcg::tri::UpdateTopology<VCGEdgeMesh>::VertexEdge(*this);
  //  deleteDanglingEdges();
  //  vcg::tri::Clean<VCGEdgeMesh>::RemoveUnreferencedVertex(*this);
  //  vcg::tri::Clean<VCGEdgeMesh>::RemoveDegenerateVertex(*this);
  //  vcg::tri::Clean<VCGEdgeMesh>::RemoveDegenerateEdge(*this);
  //  vcg::tri::Allocator<VCGEdgeMesh>::CompactEveryVector(*this);
  updateEigenEdgeAndVertices();
  savePly("tiledPattern.ply");
}

void PatternGeometry::createFan(const size_t &fanSize) {
  PatternGeometry rotatedPattern;
  vcg::tri::Append<PatternGeometry, PatternGeometry>::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<PatternGeometry>::Matrix(rotatedPattern, R);
    vcg::tri::Append<PatternGeometry, PatternGeometry>::Mesh(*this,
                                                             rotatedPattern);
  }

  removeDuplicateVertices();
  //  const double precision = 1e-4;
  //    for (size_t vi = 0; vi < VN(); vi++) {
  //      vert[vi].P()[0] = std::round(vert[vi].P()[0] * (1 / precision)) *
  //      precision; vert[vi].P()[1] = std::round(vert[vi].P()[1] * (1 /
  //      precision)) * precision; vert[vi].P()[2] = std::round(vert[vi].P()[2]
  //      * (1 / precision)) * precision;
  //    }

  updateEigenEdgeAndVertices();
}

void PatternGeometry::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<PatternGeometry>::VertexEdge(*this);
}

double PatternGeometry::getBaseTriangleHeight() const {
  return baseTriangleHeight;
}

void PatternGeometry::tilePattern(PatternGeometry &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();
}