#include "elementalmesh.hpp"

SimulationMesh::SimulationMesh(FlatPattern &pattern) {
  vcg::tri::MeshAssert<FlatPattern>::VertexNormalNormalized(pattern);

  vcg::tri::Append<VCGEdgeMesh, ConstVCGEdgeMesh>::MeshCopy(*this, pattern);
  elements = vcg::tri::Allocator<VCGEdgeMesh>::GetPerEdgeAttribute<Element>(
      *this, std::string("Elements"));
  nodes = vcg::tri::Allocator<VCGEdgeMesh>::GetPerVertexAttribute<Node>(
      *this, std::string("Nodes"));
  vcg::tri::UpdateTopology<VCGEdgeMesh>::VertexEdge(*this);
  initializeNodes();
  initializeElements();
  label = pattern.getLabel();
  eigenEdges = pattern.getEigenEdges();
  eigenVertices = pattern.getEigenVertices();
}

SimulationMesh::SimulationMesh(SimulationMesh &elementalMesh) {
  vcg::tri::Append<VCGEdgeMesh, ConstVCGEdgeMesh>::MeshCopy(*this,
                                                            elementalMesh);
  elements = vcg::tri::Allocator<VCGEdgeMesh>::GetPerEdgeAttribute<Element>(
      *this, std::string("Elements"));
  nodes = vcg::tri::Allocator<VCGEdgeMesh>::GetPerVertexAttribute<Node>(
      *this, std::string("Nodes"));
  vcg::tri::UpdateTopology<VCGEdgeMesh>::VertexEdge(*this);
  initializeNodes();

  for (size_t ei = 0; ei < EN(); ei++) {
    elements[ei] = elementalMesh.elements[ei];
  }

  label = label;
  eigenEdges = elementalMesh.getEigenEdges();
  eigenVertices = elementalMesh.getEigenVertices();
}

void SimulationMesh::computeElementalProperties() {
  const std::vector<CylindricalElementDimensions> elementalDimensions =
      getBeamDimensions();
  const std::vector<ElementMaterial> elementalMaterials = getBeamMaterial();
  assert(EN() == elementalDimensions.size() &&
         elementalDimensions.size() == elementalMaterials.size());

  for (const EdgeType &e : edge) {
    const EdgeIndex ei = getIndex(e);
    elements[e].properties =
        Element::Properties{elementalDimensions[ei], elementalMaterials[ei]};
  }
}

void SimulationMesh::initializeNodes() {
  // set initial and previous locations,
  for (const VertexType &v : vert) {
    const VertexIndex vi = getIndex(v);
    Node &node = nodes[v];
    node.vi = vi;
    node.initialLocation = v.cP();
    node.previousLocation = v.cP();
    node.initialNormal = v.cN();
    node.derivativeOfNormal.resize(6, VectorType(0, 0, 0));

    node.displacements[3] =
        v.cN()[0]; // initialize nx diplacement with vertex normal x
                   //        component
    node.displacements[4] =
        v.cN()[1]; // initialize ny(in the paper) diplacement with vertex
    //        normal
    // y component.

    // Initialize incident elements
    std::vector<VCGEdgeMesh::EdgePointer> incidentElements;
    vcg::edge::VEStarVE(&v, incidentElements);
    assert(
        vcg::tri::IsValidPointer<SimulationMesh>(*this, incidentElements[0]) &&
        incidentElements.size() > 0);
    nodes[v].incidentElements = std::move(incidentElements);
    node.referenceElement = getReferenceElement(v);
    // Initialze alpha angles

    const EdgeType &referenceElement = *getReferenceElement(v);
    const VectorType t01 =
        computeT1Vector(referenceElement.cP(0), referenceElement.cP(1));
    const VectorType f01 = (t01 - (v.cN() * (t01.dot(v.cN())))).Normalize();

    for (const VCGEdgeMesh::EdgePointer &ep : nodes[v].incidentElements) {
      assert(referenceElement.cV(0) == ep->cV(0) ||
             referenceElement.cV(0) == ep->cV(1) ||
             referenceElement.cV(1) == ep->cV(0) ||
             referenceElement.cV(1) == ep->cV(1));
      const VectorType t1 = computeT1Vector(*ep);
      const VectorType f1 = t1 - (v.cN() * (t1.dot(v.cN()))).Normalize();
      const EdgeIndex ei = getIndex(ep);
      const double alphaAngle = computeAngle(f01, f1, v.cN());
      node.alphaAngles[ei] = alphaAngle;
    }
  }
}

void SimulationMesh::initializeElements() {
  computeElementalProperties();
  for (const EdgeType &e : edge) {
    Element &element = elements[e];
    element.ei = getIndex(e);
    // Initialize lengths
    const VCGEdgeMesh::CoordType p0 = e.cP(0);
    const VCGEdgeMesh::CoordType p1 = e.cP(1);
    const vcg::Segment3<double> s(p0, p1);
    element.initialLength = s.Length();
    element.length = element.initialLength;
    // Initialize const factors
    element.axialConstFactor =
        element.properties.E * element.properties.A / element.initialLength;
    element.torsionConstFactor =
        element.properties.G * element.properties.J / element.initialLength;
    element.firstBendingConstFactor = 2 * element.properties.E *
                                      element.properties.I2 /
                                      element.initialLength;
    element.secondBendingConstFactor = 2 * element.properties.E *
                                       element.properties.I3 /
                                       element.initialLength;
    element.derivativeT1.resize(
        2, std::vector<VectorType>(6, VectorType(0, 0, 0)));
    element.derivativeT2.resize(
        2, std::vector<VectorType>(6, VectorType(0, 0, 0)));
    element.derivativeT3.resize(
        2, std::vector<VectorType>(6, VectorType(0, 0, 0)));
    element.derivativeT1_jplus1.resize(6);
    element.derivativeT1_j.resize(6);
    element.derivativeT1_jplus1.resize(6);
    element.derivativeT2_j.resize(6);
    element.derivativeT2_jplus1.resize(6);
    element.derivativeT3_j.resize(6);
    element.derivativeT3_jplus1.resize(6);
    element.derivativeR_j.resize(6);
    element.derivativeR_jplus1.resize(6);
  }
}

void SimulationMesh::updateElementalLengths() {
  for (const EdgeType &e : edge) {
    const EdgeIndex ei = getIndex(e);
    const VertexIndex vi0 = getIndex(e.cV(0));
    const VCGEdgeMesh::CoordType p0 = e.cP(0);
    const VertexIndex vi1 = getIndex(e.cV(1));
    const VCGEdgeMesh::CoordType p1 = e.cP(1);
    const vcg::Segment3<double> s(p0, p1);
    const double elementLength = s.Length();
    elements[e].length = elementLength;
    int i = 0;
    i++;
  }
}

SimulationMesh::EdgePointer
SimulationMesh::getReferenceElement(const VCGEdgeMesh::VertexType &v) {
  const VertexIndex vi = getIndex(v);
  //  return nodes[v].incidentElements[0];
  //  if (vi == 0 || vi == 1) {
  //    return nodes[0].incidentElements[0];
  //  }

  return nodes[v].incidentElements[0];
}

VectorType computeT1Vector(const SimulationMesh::EdgeType &e) {
  return computeT1Vector(e.cP(0), e.cP(1));
}

VectorType computeT1Vector(const CoordType &p0, const CoordType &p1) {
  const VectorType t1 = (p1 - p0).Normalize();
  return t1;
}

Element::LocalFrame computeElementFrame(const CoordType &p0,
                                        const CoordType &p1,
                                        const VectorType &elementNormal) {
  const VectorType t1 = computeT1Vector(p0, p1);
  const VectorType t2 = (elementNormal ^ t1).Normalize();
  const VectorType t3 = (t1 ^ t2).Normalize();

  return Element::LocalFrame{t1, t2, t3};
}

double computeAngle(const VectorType &vector0, const VectorType &vector1,
                    const VectorType &normalVector) {
  double cosAngle = vector0.dot(vector1);
  const double epsilon = std::pow(10, -6);
  if (abs(cosAngle) > 1 && abs(cosAngle) < 1 + epsilon) {
    if (cosAngle > 0) {
      cosAngle = 1;

    } else {
      cosAngle = -1;
    }
  }
  assert(abs(cosAngle) <= 1);
  const double angle =
      acos(cosAngle); // NOTE: I compute the alpha angle not between
                      // two consecutive elements but rather between
                      // the first and the ith. Is this correct?
  assert(!std::isnan(angle));

  const VectorType cp = vector0 ^ vector1;
  if (cp.dot(normalVector) < 0) {
    return -angle;
  }
  return angle;
}