#include "elementalmesh.hpp"

SimulationMesh::SimulationMesh() {
  elements = vcg::tri::Allocator<VCGEdgeMesh>::GetPerEdgeAttribute<Element>(
      *this, std::string("Elements"));
  nodes = vcg::tri::Allocator<VCGEdgeMesh>::GetPerVertexAttribute<Node>(
      *this, std::string("Nodes"));
}

SimulationMesh::SimulationMesh(VCGEdgeMesh &mesh) {
  vcg::tri::MeshAssert<VCGEdgeMesh>::VertexNormalNormalized(mesh);
  //  bool containsNormals = true;
  //  for (VertexIterator vi = mesh.vert.begin(); vi != mesh.vert.end(); ++vi)
  //    if (!vi->IsD()) {
  //      if (fabs(vi->cN().Norm() - 1.0) > 0.000001) {
  //        containsNormals = false;
  //        break;
  //      }
  //    }

  //  if (!containsNormals) {
  //    for (VertexIterator vi = mesh.vert.begin(); vi != mesh.vert.end(); ++vi)
  //      if (!vi->IsD()) {
  //        vi->N() = CoordType(1, 0, 0);
  //      }
  //  }

  vcg::tri::Append<VCGEdgeMesh, ConstVCGEdgeMesh>::MeshCopy(*this, mesh);
  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 = mesh.getLabel();
  eigenEdges = mesh.getEigenEdges();
  eigenVertices = mesh.getEigenVertices();
}

SimulationMesh::~SimulationMesh()
{
    vcg::tri::Allocator<VCGEdgeMesh>::DeletePerEdgeAttribute<Element>(*this, elements);
    vcg::tri::Allocator<VCGEdgeMesh>::DeletePerVertexAttribute<Node>(*this,nodes);
}

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 &mesh) {
  vcg::tri::Append<VCGEdgeMesh, ConstVCGEdgeMesh>::MeshCopy(*this, mesh);
  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] = mesh.elements[ei];
  }

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

void SimulationMesh::computeElementalProperties() {
  const std::vector<CrossSectionType> 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].setDimensions(elementalDimensions[ei]);
    elements[e].setMaterial(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.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 = 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::reset() {
  for (const EdgeType &e : edge) {
    Element &element = elements[e];
    element.ei = getIndex(e);
    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;
    element.updateRigidity();
  }

  for (const VertexType &v : vert) {
    Node &node = nodes[v];
    node.vi = getIndex(v);
    node.initialLocation = 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.

    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 dimensions
    element.dimensions = CrossSectionType();
    // Initialize material
    element.material = ElementMaterial();
    // 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.updateRigidity();
    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++;
  }
}

void SimulationMesh::setBeamCrossSection(
    const CrossSectionType &beamDimensions) {
  for (size_t ei = 0; ei < EN(); ei++) {
    elements[ei].dimensions = beamDimensions;
    elements[ei].computeDimensionsProperties(beamDimensions);
    elements[ei].updateRigidity();
  }
}

void SimulationMesh::setBeamMaterial(const double &pr, const double &ym) {
  for (size_t ei = 0; ei < EN(); ei++) {
    elements[ei].setMaterial(ElementMaterial{pr, ym});
    elements[ei].updateRigidity();
  }
}

std::vector<CrossSectionType> SimulationMesh::getBeamDimensions() {
  std::vector<CrossSectionType> beamDimensions(EN());
  for (size_t ei = 0; ei < EN(); ei++) {
    beamDimensions[ei] = elements[ei].dimensions;
  }
  return beamDimensions;
}

std::vector<ElementMaterial> SimulationMesh::getBeamMaterial() {
  std::vector<ElementMaterial> beamMaterial(EN());
  for (size_t ei = 0; ei < EN(); ei++) {
    beamMaterial[ei] = elements[ei].material;
  }
  return beamMaterial;
}

bool SimulationMesh::loadPly(const string &plyFilename) {
  this->Clear();
  //  assert(plyFileHasAllRequiredFields(plyFilename));
  // Load the ply file
  VCGEdgeMesh::PerEdgeAttributeHandle<CrossSectionType> handleBeamDimensions =
      vcg::tri::Allocator<SimulationMesh>::AddPerEdgeAttribute<
          CrossSectionType>(*this, plyPropertyBeamDimensionsID);
  VCGEdgeMesh::PerEdgeAttributeHandle<ElementMaterial> handleBeamMaterial =
      vcg::tri::Allocator<SimulationMesh>::AddPerEdgeAttribute<ElementMaterial>(
          *this, plyPropertyBeamMaterialID);
  nanoply::NanoPlyWrapper<SimulationMesh>::CustomAttributeDescriptor
      customAttrib;
  customAttrib.GetMeshAttrib(plyFilename);
  customAttrib.AddEdgeAttribDescriptor<CrossSectionType, double, 2>(
      plyPropertyBeamDimensionsID, nanoply::NNP_LIST_INT8_FLOAT64, nullptr);
  /*FIXME: Since I allow CrossSectionType to take two types I should export the
   * type as well such that that when loaded the correct type of cross section
   * is used.
   */
  customAttrib.AddEdgeAttribDescriptor<vcg::Point2d, double, 2>(
      plyPropertyBeamMaterialID, nanoply::NNP_LIST_INT8_FLOAT64, nullptr);

  //  VCGEdgeMesh::PerEdgeAttributeHandle<std::array<double, 6>>
  //      handleBeamProperties =
  //          vcg::tri::Allocator<SimulationMesh>::AddPerEdgeAttribute<
  //              std::array<double, 6>>(*this, plyPropertyBeamProperties);
  //  customAttrib.AddEdgeAttribDescriptor<std::array<double, 6>, double, 6>(
  //      plyPropertyBeamProperties, nanoply::NNP_LIST_INT8_FLOAT64, nullptr);

  //  VCGEdgeMesh::PerEdgeAttributeHandle<ElementMaterial>
  //  handleBeamRigidityContants;
  //  customAttrib.AddEdgeAttribDescriptor<vcg::Point4f, float, 4>(
  //      plyPropertyBeamRigidityConstantsID, nanoply::NNP_LIST_INT8_FLOAT32,
  //      nullptr);
  unsigned int mask = 0;
  mask |= nanoply::NanoPlyWrapper<VCGEdgeMesh>::IO_VERTCOORD;
  mask |= nanoply::NanoPlyWrapper<VCGEdgeMesh>::IO_VERTNORMAL;
  mask |= nanoply::NanoPlyWrapper<VCGEdgeMesh>::IO_EDGEINDEX;
  mask |= nanoply::NanoPlyWrapper<VCGEdgeMesh>::IO_EDGEATTRIB;
  if (nanoply::NanoPlyWrapper<SimulationMesh>::LoadModel(
          plyFilename.c_str(), *this, mask, customAttrib) != 0) {
    return false;
  }

  elements = vcg::tri::Allocator<SimulationMesh>::GetPerEdgeAttribute<Element>(
      *this, std::string("Elements"));
  nodes = vcg::tri::Allocator<SimulationMesh>::GetPerVertexAttribute<Node>(
      *this, std::string("Nodes"));
  vcg::tri::UpdateTopology<SimulationMesh>::VertexEdge(*this);
  initializeNodes();
  initializeElements();
  updateEigenEdgeAndVertices();

  //  if (!handleBeamProperties._handle->data.empty()) {
  //    for (size_t ei = 0; ei < EN(); ei++) {
  //      elements[ei] =
  //      Element::Properties(handleBeamProperties[ei]);
  //      elements[ei].updateRigidity();
  //    }
  //  }
  for (size_t ei = 0; ei < EN(); ei++) {
    elements[ei].setDimensions(handleBeamDimensions[ei]);
    elements[ei].setMaterial(handleBeamMaterial[ei]);
    elements[ei].updateRigidity();
  }

  bool normalsAreAbsent = vert[0].cN().Norm() < 0.000001;
  if (normalsAreAbsent) {
    CoordType normalVector(0, 0, 1);
    std::cout << "Warning: Normals are missing from " << plyFilename
              << ". Added normal vector:" << toString(normalVector)
              << std::endl;
    for (auto &v : vert) {
      v.N() = normalVector;
    }
  }

  return true;
}

bool SimulationMesh::savePly(const std::string &plyFilename) {
  nanoply::NanoPlyWrapper<VCGEdgeMesh>::CustomAttributeDescriptor customAttrib;
  customAttrib.GetMeshAttrib(plyFilename);

  std::vector<CrossSectionType> dimensions = getBeamDimensions();
  customAttrib.AddEdgeAttribDescriptor<CrossSectionType, double, 2>(
      plyPropertyBeamDimensionsID, nanoply::NNP_LIST_INT8_FLOAT64,
      dimensions.data());
  std::vector<ElementMaterial> material = getBeamMaterial();
  customAttrib.AddEdgeAttribDescriptor<vcg::Point2d, double, 2>(
      plyPropertyBeamMaterialID, nanoply::NNP_LIST_INT8_FLOAT64,
      material.data());
  //  std::vector<std::array<double, 6>> beamProperties(EN());
  //  for (size_t ei = 0; ei < EN(); ei++) {
  //    auto props = elements[ei].toArray();
  //    for (auto p : props) {
  //      std::cout << p << " ";
  //    }
  //    std::cout << std::endl;
  //    beamProperties[ei] = props;
  //  }
  //  customAttrib.AddEdgeAttribDescriptor<std::array<double, 6>, double, 6>(
  //      plyPropertyBeamProperties, nanoply::NNP_LIST_INT8_FLOAT64,
  //      beamProperties.data());
  // Load the ply file
  unsigned int mask = 0;
  mask |= nanoply::NanoPlyWrapper<VCGEdgeMesh>::IO_VERTCOORD;
  mask |= nanoply::NanoPlyWrapper<VCGEdgeMesh>::IO_EDGEINDEX;
  mask |= nanoply::NanoPlyWrapper<VCGEdgeMesh>::IO_EDGEATTRIB;
  mask |= nanoply::NanoPlyWrapper<VCGEdgeMesh>::IO_VERTNORMAL;
  if (nanoply::NanoPlyWrapper<VCGEdgeMesh>::SaveModel(
          plyFilename.c_str(), *this, mask, customAttrib, false) != 1) {
    return false;
  }
  return true;
}

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;
}

void Element::computeMaterialProperties(const ElementMaterial &material) {
  this->G = material.youngsModulus / (2 * (1 + material.poissonsRatio));
}

void Element::computeDimensionsProperties(
    const RectangularBeamDimensions &dimensions) {
  assert(typeid(CrossSectionType) == typeid(RectangularBeamDimensions));
  A = (dimensions.b * dimensions.h);
  I2 = dimensions.b * std::pow(dimensions.h, 3) / 12;
  I3 = dimensions.h * std::pow(dimensions.b, 3) / 12;
  J = I2 + I3;
}

void Element::computeDimensionsProperties(
    const CylindricalBeamDimensions &dimensions) {
  assert(typeid(CrossSectionType) == typeid(CylindricalBeamDimensions));
  A = M_PI * (std::pow(dimensions.od / 2, 2) - std::pow(dimensions.id / 2, 2));
  I2 = M_PI * (std::pow(dimensions.od, 4) - std::pow(dimensions.id, 4)) / 64;
  I3 = I2;
  J = I2 + I3;
}

void Element::setDimensions(const CrossSectionType &dimensions) {
  this->dimensions = dimensions;
  computeDimensionsProperties(dimensions);
  updateRigidity();
}

void Element::setMaterial(const ElementMaterial &material) {
  this->material = material;
  computeMaterialProperties(material);
  updateRigidity();
}

void Element::updateRigidity() {
  rigidity.axial = material.youngsModulus * A / initialLength;
  rigidity.torsional = G * J / initialLength;
  rigidity.firstBending = 2 * material.youngsModulus * I2 / initialLength;
  rigidity.secondBending = 2 * material.youngsModulus * I3 / initialLength;
}