#include "beamformfinder.hpp"
#include "polyscope/curve_network.h"
#include "polyscope/histogram.h"
#include "polyscope/polyscope.h"
#include "simulationhistoryplotter.hpp"
#include <algorithm>
#include <chrono>
#include <execution>
#include <omp.h>

void FormFinder::reset() {
  Dt = Dtini;
  t = 0;
  currentSimulationStep = 0;
  history.clear();
  //  theta3Derivatives =
  //      Eigen::Tensor<double, 4>(DoF::NumDoFType, mesh->VN(), mesh->EN(),
  //      mesh->VN());
  //  theta3Derivatives.setZero();
}

VectorType FormFinder::computeDisplacementDifferenceDerivative(
    const EdgeType &e, const DifferentiateWithRespectTo &dui) const {
  VectorType displacementDiffDeriv(0, 0, 0);
  const DoFType &dofi = dui.dofi;
  const bool differentiateWithRespectToANonEdgeNode =
      e.cV(0) != &dui.v && e.cV(1) != &dui.v;
  if (differentiateWithRespectToANonEdgeNode || dofi > 2) {
    return displacementDiffDeriv;
  }

  if (e.cV(0) == &dui.v) {
    displacementDiffDeriv[dofi] = -1;
  } else if (e.cV(1) == &dui.v) {
    displacementDiffDeriv[dofi] = 1;
  }

  return displacementDiffDeriv;
}

VectorType FormFinder::computeDerivativeOfNormal(
    const VertexType &v, const DifferentiateWithRespectTo &dui) const {
  VectorType normalDerivative(0, 0, 0);
  if (&dui.v != &v ||
      (dui.dofi == 0 || dui.dofi == 1 || dui.dofi == 2 || dui.dofi == 5)) {
    return normalDerivative;
  }
  const VectorType &n = v.cN();
  const double &nx = n[0], ny = n[1];
  const double nxnyMagnitude = std::pow(nx, 2) + std::pow(ny, 2);

  if (dui.dofi == 3) {
    if (nxnyMagnitude >= 1) {
      const double normalDerivativeX =
          1 / sqrt(nxnyMagnitude) -
          std::pow(nx, 2) / std::pow(nxnyMagnitude, 1.5);
      const double normalDerivativeY = -nx * ny / std::pow(nxnyMagnitude, 1.5);
      const double normalDerivativeZ = 0;
      normalDerivative =
          VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ);
    } else {
      const double normalDerivativeX = 1;
      const double normalDerivativeY = 0;
      const double normalDerivativeZ = -nx / std::sqrt(1 - nxnyMagnitude);
      normalDerivative =
          VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ);
    }
  } else if (dui.dofi == 4) {
    if (nxnyMagnitude >= 1) {
      const double normalDerivativeX = -nx * ny / std::pow(nxnyMagnitude, 1.5);
      const double normalDerivativeY =
          1 / sqrt(nxnyMagnitude) -
          std::pow(ny, 2) / std::pow(nxnyMagnitude, 1.5);
      const double normalDerivativeZ = 0;
      normalDerivative =
          VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ);
    } else {
      const double normalDerivativeX = 0;
      const double normalDerivativeY = 1;
      const double normalDerivativeZ = -ny / std::sqrt(1 - nxnyMagnitude);
      normalDerivative =
          VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ);
    }
  }

  const bool normalDerivativeIsFinite = std::isfinite(normalDerivative[0]) &&
                                        std::isfinite(normalDerivative[1]) &&
                                        std::isfinite(normalDerivative[2]);
  assert(normalDerivativeIsFinite);

  return normalDerivative;
}

double FormFinder::computeDerivativeElementLength(
    const EdgeType &e, const DifferentiateWithRespectTo &dui) const {
  if (e.cV(0) != &dui.v && e.cV(1) != &dui.v) {
    return 0;
  }

  const VectorType &X_j = e.cP(0);
  const VectorType &X_jplus1 = e.cP(1);
  const VectorType positionVectorDiff = X_jplus1 - X_j;
  const VectorType displacementDiffDeriv =
      computeDisplacementDifferenceDerivative(e, dui);
  const double edgeLength = mesh->elements[e].length;
  const double L_kDeriv =
      positionVectorDiff * displacementDiffDeriv / edgeLength;
  return L_kDeriv;
}

double FormFinder::computeDerivativeOfNorm(const VectorType &x,
                                           const VectorType &derivativeOfX) {
  return x.dot(derivativeOfX) / x.Norm();
}

VectorType FormFinder::computeDerivativeOfCrossProduct(
    const VectorType &a, const VectorType &derivativeOfA, const VectorType &b,
    const VectorType &derivativeOfB) {
  const auto firstTerm = Cross(derivativeOfA, b);
  const auto secondTerm = Cross(a, derivativeOfB);
  return firstTerm + secondTerm;
}

VectorType
FormFinder::computeDerivativeOfR(const EdgeType &e,
                                 const DifferentiateWithRespectTo &dui) const {

  const VertexType &v_j = *e.cV(0);
  const VertexType &v_jplus1 = *e.cV(1);
  const VectorType normal_j = v_j.cN();
  const VectorType normal_jplus1 = v_jplus1.cN();
  const VectorType derivativeOfNormal_j =
      &v_j == &dui.v && dui.dofi > 2
          ? mesh->nodes[v_j].derivativeOfNormal[dui.dofi]
          : VectorType(0, 0, 0);
  const VectorType derivativeOfNormal_jplus1 =
      &v_jplus1 == &dui.v && dui.dofi > 2
          ? mesh->nodes[v_jplus1].derivativeOfNormal[dui.dofi]
          : VectorType(0, 0, 0);

  const VectorType derivativeOfSumOfNormals =
      derivativeOfNormal_j + derivativeOfNormal_jplus1;
  const VectorType sumOfNormals = normal_j + normal_jplus1;
  const double normOfSumOfNormals = sumOfNormals.Norm();
  const double derivativeOfNormOfSumOfNormals =
      computeDerivativeOfNorm(sumOfNormals, derivativeOfSumOfNormals);

  const VectorType derivativeOfR_firstTerm = -sumOfNormals *
                                             derivativeOfNormOfSumOfNormals /
                                             std::pow(normOfSumOfNormals, 2);
  const VectorType derivativeOfR_secondTerm =
      derivativeOfSumOfNormals / normOfSumOfNormals;
  const VectorType derivativeOfR =
      derivativeOfR_firstTerm + derivativeOfR_secondTerm;

  assert(std::isfinite(derivativeOfR[0]) && std::isfinite(derivativeOfR[1]) &&
         std::isfinite(derivativeOfR[2]));

  return derivativeOfR;
}

VectorType
FormFinder::computeDerivativeT1(const EdgeType &e,
                                const DifferentiateWithRespectTo &dui) const {

  const VectorType &X_j = e.cP(0);
  const VectorType &X_jplus1 = e.cP(1);
  const VectorType edgeVector = X_jplus1 - X_j;
  const double L_kDerivative = computeDerivativeElementLength(e, dui);
  const double edgeLength = mesh->elements[e].length;
  const VectorType firstTerm =
      -edgeVector * L_kDerivative / std::pow(edgeLength, 2);
  const VectorType secondTerm =
      computeDisplacementDifferenceDerivative(e, dui) / edgeLength;
  const VectorType t1Derivative = firstTerm + secondTerm;

  return t1Derivative;
}

VectorType
FormFinder::computeDerivativeT2(const EdgeType &e,
                                const DifferentiateWithRespectTo &dui) const {

  const DoFType dofi = dui.dofi;

  const VertexType &v_j = *e.cV(0);
  const VertexType &v_jplus1 = *e.cV(1);

  const VectorType r = (v_j.cN() + v_jplus1.cN()).Normalize();
  const VectorType derivativeR_j =
      dofi > 2 && &v_j == &dui.v ? mesh->elements[e].derivativeR_j[dui.dofi]
                                 : VectorType(0, 0, 0);
  const VectorType derivativeR_jplus1 =
      dofi > 2 && &v_jplus1 == &dui.v
          ? mesh->elements[e].derivativeR_jplus1[dui.dofi]
          : VectorType(0, 0, 0);
  const VectorType derivativeR = derivativeR_j + derivativeR_jplus1;

  const VectorType &t1 = mesh->elements[e].frame.t1;
  const VectorType derivativeT1_j =
      dofi < 3 && &v_j == &dui.v ? mesh->elements[e].derivativeT1_j[dui.dofi]
                                 : VectorType(0, 0, 0);
  const VectorType derivativeT1_jplus1 =
      dofi < 3 && &v_jplus1 == &dui.v
          ? mesh->elements[e].derivativeT1_jplus1[dui.dofi]
          : VectorType(0, 0, 0);
  const VectorType derivativeT1 = derivativeT1_j + derivativeT1_jplus1;

  const VectorType derivativeOfRCrossT1 =
      computeDerivativeOfCrossProduct(r, derivativeR, t1, derivativeT1);
  const VectorType rCrossT1 = Cross(r, t1);
  const double normOfRCrossT1 = rCrossT1.Norm();
  const double derivativeNormRCrossT1 =
      computeDerivativeOfNorm(rCrossT1, derivativeOfRCrossT1);

  const VectorType t2Deriv_firstTerm =
      -(rCrossT1 * derivativeNormRCrossT1) / std::pow(normOfRCrossT1, 2);
  const VectorType t2Deriv_secondTerm = derivativeOfRCrossT1 / normOfRCrossT1;
  const VectorType t2Deriv = t2Deriv_firstTerm + t2Deriv_secondTerm;

  const double t2DerivNorm = t2Deriv.Norm();
  assert(std::isfinite(t2DerivNorm));
  return t2Deriv;
}

VectorType
FormFinder::computeDerivativeT3(const EdgeType &e,
                                const DifferentiateWithRespectTo &dui) const {
  const Element &element = mesh->elements[e];
  const VectorType &t1 = element.frame.t1;
  const VectorType &t2 = element.frame.t2;
  const VectorType t1CrossT2 = Cross(t1, t2);
  const VertexType &v_j = *e.cV(0);
  const VertexType &v_jplus1 = *e.cV(1);
  const VectorType derivativeT1_j =
      dui.dofi < 3 && &v_j == &dui.v
          ? mesh->elements[e].derivativeT1_j[dui.dofi]
          : VectorType(0, 0, 0);
  const VectorType derivativeT1_jplus1 =
      dui.dofi < 3 && &v_jplus1 == &dui.v
          ? mesh->elements[e].derivativeT1_jplus1[dui.dofi]
          : VectorType(0, 0, 0);
  const VectorType derivativeT1 = derivativeT1_j + derivativeT1_jplus1;

  //  const VectorType derivativeOfT2 = computeDerivativeT2(e, dui);
  //  const VectorType derivativeT2_j =
  //      &v_j == &dui.v
  //          ? mesh->elements[e].derivativeT2_j[dui.dofi]
  //          : VectorType(0, 0, 0);
  //  const VectorType derivativeT2_jplus1 =
  //       &v_jplus1 == &dui.v
  //          ? mesh->elements[e].derivativeT2_jplus1[dui.dofi]
  //          : VectorType(0, 0, 0);
  VectorType derivativeT2(0, 0, 0);
  if (&v_j == &dui.v) {
    derivativeT2 = mesh->elements[e].derivativeT2_j[dui.dofi];
  } else if (&v_jplus1 == &dui.v) {
    derivativeT2 = mesh->elements[e].derivativeT2_jplus1[dui.dofi];
  }

  const VectorType derivativeT1CrossT2 =
      computeDerivativeOfCrossProduct(t1, derivativeT1, t2, derivativeT2);
  const double derivativeOfNormT1CrossT2 =
      computeDerivativeOfNorm(t1CrossT2, derivativeT1CrossT2);
  const double normT1CrossT2 = t1CrossT2.Norm();

  const VectorType t3Deriv_firstTerm =
      -(t1CrossT2 * derivativeOfNormT1CrossT2) / std::pow(normT1CrossT2, 2);
  const VectorType t3Deriv_secondTerm = derivativeT1CrossT2 / normT1CrossT2;
  const VectorType t3Deriv = t3Deriv_firstTerm + t3Deriv_secondTerm;

  assert(std::isfinite(t3Deriv[0]) && std::isfinite(t3Deriv[1]) &&
         std::isfinite(t3Deriv[2]));
  return t3Deriv;
}

double FormFinder::computeDerivativeTheta1(const EdgeType &e,
                                           const VertexIndex &evi,
                                           const VertexIndex &dwrt_evi,
                                           const DoFType &dwrt_dofi) const {
  const VertexType &v = *e.cV(evi);
  const Element &element = mesh->elements[e];
  const VectorType derivativeT1 = element.derivativeT1[dwrt_evi][dwrt_dofi];
  const VectorType derivativeT3 = element.derivativeT3[dwrt_evi][dwrt_dofi];
  const VectorType nDerivative =
      evi != dwrt_evi ? VectorType(0, 0, 0)
                      : mesh->nodes[v].derivativeOfNormal[dwrt_dofi];
  const VectorType n = v.cN();
  const VectorType &t1 = element.frame.t1;
  const VectorType &t3 = element.frame.t3;
  const double theta1Derivative =
      derivativeT1 * Cross(t3, n) +
      t1 * (Cross(derivativeT3, n) + Cross(t3, nDerivative));

  return theta1Derivative;
}

double FormFinder::computeDerivativeTheta2(const EdgeType &e,
                                           const VertexIndex &evi,
                                           const VertexIndex &dwrt_evi,
                                           const DoFType &dwrt_dofi) const {
  const VertexType &v = *e.cV(evi);

  const Element &element = mesh->elements[e];
  const VectorType derivativeT2 = element.derivativeT2[dwrt_evi][dwrt_dofi];
  const VectorType derivativeT3 = element.derivativeT3[dwrt_evi][dwrt_dofi];

  const VectorType n = v.cN();
  const VectorType &t2 = element.frame.t2;
  const VectorType &t3 = element.frame.t3;
  const VectorType nDerivative =
      dwrt_evi == evi ? mesh->nodes[v].derivativeOfNormal[dwrt_dofi]
                      : VectorType(0, 0, 0);
  const double theta2Derivative =
      derivativeT2 * Cross(t3, n) +
      t2 * (Cross(derivativeT3, n) + Cross(t3, nDerivative));

  return theta2Derivative;
}

double FormFinder::computeTheta3(const EdgeType &e, const VertexType &v) {
  const VertexIndex &vi = mesh->nodes[v].vi;
  //  assert(e.cV(0) == &v || e.cV(1) == &v);

  const Element &elem = mesh->elements[e];
  const EdgeIndex &ei = elem.ei;
  const Element::LocalFrame &ef = elem.frame;
  const VectorType &t1 = ef.t1;
  const VectorType &n = v.cN();
  const Node &node = mesh->nodes[v];
  const double &nR = node.nR; // TODO: This makes the function non-const.
                              // Should be refactored in the future

  double theta3;
  if (&e == node.referenceElement) {
    // Use nR as theta3 only for the first star edge
    return nR;
  }
  //    assert(node.alphaAngles.contains(mesh->getIndex(e)));
  double alphaAngle = node.alphaAngles.find(elem.ei)->second;
  const EdgeType &refElem = *node.referenceElement;
  const double rotationAngle = nR + alphaAngle;

  //    const VectorType &t1_k = computeT1Vector(refElem);
  const VectorType &t1_k = mesh->elements[refElem].frame.t1;
  const VectorType f1 = (t1_k - (n * (t1_k * n))).Normalize();
  vcg::Matrix44<double> rotationMatrix;
  rotationMatrix.SetRotateRad(rotationAngle, n);
  const double cosRotationAngle = cos(rotationAngle);
  const double sinRotationAngle = sin(rotationAngle);
  const VectorType f2 =
      (f1 * cosRotationAngle + Cross(n, f1) * sinRotationAngle).Normalize();
  const VectorType &t1Current = t1;
  const VectorType f3 = Cross(t1Current, f2);

  Element &element = mesh->elements[e];
  // Save for computing theta3Derivative
  if (&v == e.cV(0)) {
    element.f1_j = f1;
    element.f2_j = f2;
    element.f3_j = f3;
    element.cosRotationAngle_j = cosRotationAngle;
    element.sinRotationAngle_j = sinRotationAngle;
  } else {
    element.f1_jplus1 = f1;
    element.f2_jplus1 = f2;
    element.f3_jplus1 = f3;
    element.cosRotationAngle_jplus1 = cosRotationAngle;
    element.sinRotationAngle_jplus1 = sinRotationAngle;
  }
  theta3 = f3.dot(n);

  return theta3;
}

double FormFinder::computeDerivativeTheta3(
    const EdgeType &e, const VertexType &v,
    const DifferentiateWithRespectTo &dui) const {
  const Node &node = mesh->nodes[v];
  const VertexIndex &vi = mesh->nodes[v].vi;
  const bool isRigidSupport = rigidSupports.contains(vi);
  if (&e == node.referenceElement && !isRigidSupport) {
    if (dui.dofi == DoF::Nr && &dui.v == &v) {
      return 1;
    } else {
      return 0;
    }
  }
  //  assert(e.cV(0) == &v || e.cV(1) == &v);

  const Element &element = mesh->elements[e];
  const Element::LocalFrame &ef = element.frame;
  const VectorType &t1 = ef.t1;
  const VectorType &n = v.cN();
  const DoFType &dofi = dui.dofi;
  const VertexPointer &vp_j = e.cV(0);
  const VertexPointer &vp_jplus1 = e.cV(1);

  double derivativeTheta3_dofi = 0;
  if (isRigidSupport) {
    const VectorType &t1Initial =
        computeT1Vector(mesh->nodes[vp_j].initialLocation,
                        mesh->nodes[vp_jplus1].initialLocation);
    VectorType g1 = Cross(t1, t1Initial);

    const VectorType derivativeInitialT1_dofi(0, 0, 0);
    const VectorType derivativeT1_j = dui.dofi < 3 && vp_j == &dui.v
                                          ? element.derivativeT1_j[dui.dofi]
                                          : VectorType(0, 0, 0);
    const VectorType derivativeT1_jplus1 =
        dui.dofi < 3 && vp_jplus1 == &dui.v
            ? element.derivativeT1_jplus1[dui.dofi]
            : VectorType(0, 0, 0);
    const VectorType derivativeT1_dofi = derivativeT1_j + derivativeT1_jplus1;
    //    VectorType derivativeT1_dofi(0, 0, 0);
    //    if (dui.dofi < 3) {
    //      if (&v_j == &dui.v) {
    //        derivativeT1_dofi = mesh->elements[e].derivativeT1_j[dui.dofi];
    //      } else if (&v_jplus1 == &dui.v) {
    //        derivativeT1_dofi =
    //        mesh->elements[e].derivativeT1_jplus1[dui.dofi];
    //      }
    //    }

    const VectorType derivativeG1_firstTerm =
        Cross(derivativeT1_dofi, t1Initial);
    const VectorType derivativeG1_secondTerm =
        Cross(t1, derivativeInitialT1_dofi);
    const VectorType derivativeG1 =
        derivativeG1_firstTerm + derivativeG1_secondTerm;
    const VectorType derivativeNormal = &v == &dui.v && dui.dofi > 2
                                            ? node.derivativeOfNormal[dui.dofi]
                                            : VectorType(0, 0, 0);
    derivativeTheta3_dofi = derivativeG1 * n + g1 * derivativeNormal;
    return derivativeTheta3_dofi;
  }
  EdgeType &refElem = *node.referenceElement;
  const VectorType &t1_k = mesh->elements[refElem].frame.t1;
  VectorType f1, f2, f3;
  double cosRotationAngle, sinRotationAngle;
  if (e.cV(0) == &v) {
    f1 = element.f1_j;
    cosRotationAngle = element.cosRotationAngle_j;
    sinRotationAngle = element.sinRotationAngle_j;
    f2 = element.f2_j;
    f3 = element.f3_j;
  } else {
    f1 = element.f1_jplus1;
    cosRotationAngle = element.cosRotationAngle_jplus1;
    sinRotationAngle = element.sinRotationAngle_jplus1;
    f2 = element.f2_jplus1;
    f3 = element.f3_jplus1;
  }
  const VectorType &t1_kplus1 = t1;
  const VectorType f1Normalized = f1 / f1.Norm();

  VectorType derivativeF1(0, 0, 0);
  VectorType derivativeF2(0, 0, 0);
  VectorType derivativeF3(0, 0, 0);
  if (dui.dofi < 3) {

    const VectorType derivativeT1_kplus1_j =
        vp_j == &dui.v ? element.derivativeT1_j[dui.dofi] : VectorType(0, 0, 0);
    const VectorType derivativeT1_kplus1_jplus1 =
        vp_jplus1 == &dui.v ? element.derivativeT1_jplus1[dui.dofi]
                            : VectorType(0, 0, 0);
    const VectorType derivativeT1_kplus1 =
        derivativeT1_kplus1_j + derivativeT1_kplus1_jplus1;

    const VectorType derivativeT1_k_j =
        refElem.cV(0) == &dui.v
            ? mesh->elements[refElem].derivativeT1_j[dui.dofi]
            : VectorType(0, 0, 0);
    const VectorType derivativeT1_k_jplus1 =
        refElem.cV(1) == &dui.v
            ? mesh->elements[refElem].derivativeT1_jplus1[dui.dofi]
            : VectorType(0, 0, 0);
    const VectorType derivativeT1_k = derivativeT1_k_j + derivativeT1_k_jplus1;

    derivativeF1 = derivativeT1_k - (n * (derivativeT1_k * n));

    const double f1Norm = f1.Norm();
    const VectorType derivativeF1Normalized =
        -f1 * (f1 * derivativeF1) / (f1Norm * f1Norm * f1Norm) +
        derivativeF1 / f1Norm;

    derivativeF2 = derivativeF1Normalized * cosRotationAngle +
                   Cross(n, derivativeF1Normalized) * sinRotationAngle;
    const VectorType derivativeF3_firstTerm = Cross(derivativeT1_kplus1, f2);
    const VectorType derivativeF3_secondTerm = Cross(t1_kplus1, derivativeF2);
    derivativeF3 = derivativeF3_firstTerm + derivativeF3_secondTerm;
    derivativeTheta3_dofi = derivativeF3 * n;

  } else if (dui.dofi == DoF::Nr && &dui.v == &v) {
    derivativeF2 = f1Normalized * (-sinRotationAngle) +
                   Cross(n, f1Normalized) * cosRotationAngle;
    derivativeF3 = Cross(t1_kplus1, derivativeF2);
    derivativeTheta3_dofi = derivativeF3 * n;
  } else { // 2<dofi<5
    if (&v == &dui.v) {
      const VectorType &derivativeOfNormal = node.derivativeOfNormal[dofi];
      derivativeF1 =
          -(n * (t1_k * derivativeOfNormal) + derivativeOfNormal * (t1_k * n));
      const double f1Norm = f1.Norm();
      const VectorType derivativeF1Normalized =
          -f1 * (f1 * derivativeF1) / (f1Norm * f1Norm * f1Norm) +
          derivativeF1 / f1Norm;

      derivativeF2 = derivativeF1Normalized * cosRotationAngle +

                     (Cross(derivativeOfNormal, f1Normalized) +
                      Cross(n, derivativeF1Normalized)) *
                         sinRotationAngle;
      derivativeF3 = Cross(t1_kplus1, derivativeF2);
      derivativeTheta3_dofi = derivativeF3 * n + f3 * derivativeOfNormal;
    }
  }
  return derivativeTheta3_dofi;
}

double FormFinder::computeTotalPotentialEnergy() const {

  double totalExternalPotentialEnergy = 0;
  for (const SimulationMesh::VertexType &v : mesh->vert) {
    const Node &node = mesh->nodes[v];
    if (!node.force.hasExternalForce()) {
      continue;
    }
    const auto nodeDisplacement = v.cP() - node.initialLocation;
    const SimulationMesh::CoordType externalForce(
        node.force.external[0], node.force.external[1], node.force.external[2]);
    totalExternalPotentialEnergy += externalForce.dot(nodeDisplacement);
  }

  double totalInternalPotentialEnergy = 0;
  for (const SimulationMesh::EdgeType &e : mesh->edge) {

    const Element &element = mesh->elements[e];
    const EdgeIndex ei = mesh->getIndex(e);
    const double e_k = element.length - element.initialLength;
    const double axialPE = pow(e_k, 2) * element.properties.E *
                           element.properties.A / (2 * element.initialLength);
    const double theta1Diff = element.rotationalDisplacements_jplus1.theta1 -
                              element.rotationalDisplacements_j.theta1;
    const double torsionalPE = element.properties.G * element.properties.J *
                               pow(theta1Diff, 2) / (2 * element.initialLength);
    const double &theta2_j = element.rotationalDisplacements_j.theta2;
    const double &theta2_jplus1 = element.rotationalDisplacements_jplus1.theta2;
    const double firstBendingPE_inBracketsTerm = 4 * pow(theta2_j, 2) +
                                                 4 * theta2_j * theta2_jplus1 +
                                                 4 * pow(theta2_jplus1, 2);
    const double firstBendingPE = firstBendingPE_inBracketsTerm *
                                  element.properties.E * element.properties.I2 /
                                  (2 * element.initialLength);
    const double &theta3_j = element.rotationalDisplacements_j.theta3;
    const double &theta3_jplus1 = element.rotationalDisplacements_jplus1.theta3;
    const double secondBendingPE_inBracketsTerm = 4 * pow(theta3_j, 2) +
                                                  4 * theta3_j * theta3_jplus1 +
                                                  4 * pow(theta3_jplus1, 2);
    const double secondBendingPE =
        secondBendingPE_inBracketsTerm * 2 * element.properties.E *
        element.properties.I3 / element.initialLength;

    totalInternalPotentialEnergy +=
        axialPE + torsionalPE + firstBendingPE + secondBendingPE;
  }
  return totalInternalPotentialEnergy - totalExternalPotentialEnergy;
}

void FormFinder::updateResidualForcesOnTheFly(
    const std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
        &fixedVertices) {

  //  std::vector<Vector6d> internalForcesParallel(mesh->vert.size());

  std::vector<std::vector<std::pair<int, Vector6d>>>
      internalForcesContributionsFromEachEdge(
          mesh->EN(),
          std::vector<std::pair<int, Vector6d>>(4, {-1, Vector6d()}));
  //  omp_lock_t writelock;
  //  omp_init_lock(&writelock);
#pragma omp parallel for schedule(static) num_threads(8)
  for (size_t ei = 0; ei < mesh->EN(); ei++) {
    const EdgeType &e = mesh->edge[ei];
    const SimulationMesh::VertexType &ev_j = *e.cV(0);
    const SimulationMesh::VertexType &ev_jplus1 = *e.cV(1);
    const Element &element = mesh->elements[e];
    const Element::LocalFrame &ef = element.frame;
    const VectorType t3CrossN_j = Cross(ef.t3, ev_j.cN());
    const VectorType t3CrossN_jplus1 = Cross(ef.t3, ev_jplus1.cN());
    const double theta1_j = ef.t1.dot(t3CrossN_j);
    const double theta1_jplus1 = ef.t1.dot(t3CrossN_jplus1);
    const double theta2_j = ef.t2.dot(t3CrossN_j);
    const double theta2_jplus1 = ef.t2.dot(t3CrossN_jplus1);
    const double theta3_j = computeTheta3(e, ev_j);
    const double theta3_jplus1 = computeTheta3(e, ev_jplus1);
    //    element.rotationalDisplacements_j.theta1 = theta1_j;
    //    element.rotationalDisplacements_jplus1.theta1 = theta1_jplus1;
    //    element.rotationalDisplacements_j.theta2 = theta2_j;
    //    element.rotationalDisplacements_jplus1.theta2 = theta2_jplus1;
    //    element.rotationalDisplacements_j.theta3 = theta3_j;
    //    element.rotationalDisplacements_jplus1.theta3 = theta3_jplus1;
    std::vector<std::pair<int, Vector6d>>
        internalForcesContributionFromThisEdge(4, {-1, Vector6d()});
    for (VertexIndex evi = 0; evi < 2; evi++) {
      const SimulationMesh::VertexType &ev = *e.cV(evi);
      const Node &edgeNode = mesh->nodes[ev];
      internalForcesContributionFromThisEdge[evi].first = edgeNode.vi;

      const VertexPointer &rev_j = edgeNode.referenceElement->cV(0);
      const VertexPointer &rev_jplus1 = edgeNode.referenceElement->cV(1);
      // refElemOtherVertex can be precomputed
      const VertexPointer &refElemOtherVertex =
          rev_j == &ev ? rev_jplus1 : rev_j;
      const Node &refElemOtherVertexNode = mesh->nodes[refElemOtherVertex];
      if (edgeNode.referenceElement != &e) {
        internalForcesContributionFromThisEdge[evi + 2].first =
            refElemOtherVertexNode.vi;
      }
      //      #pragma omp parallel for schedule(static) num_threads(6)
      for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) {
        const bool isDofConstrainedFor_ev =
            fixedVertices.contains(edgeNode.vi) &&
            fixedVertices.at(edgeNode.vi).contains(dofi);
        if (!isDofConstrainedFor_ev) {
          DifferentiateWithRespectTo dui{ev, dofi};
          // Axial force computation
          const double e_k = element.length - element.initialLength;
          const double e_kDeriv = computeDerivativeElementLength(e, dui);
          const double axialForce_dofi =
              e_kDeriv * e_k * element.axialConstFactor;

          // Torsional force computation
          const double theta1_j_deriv =
              computeDerivativeTheta1(e, 0, evi, dofi);
          const double theta1_jplus1_deriv =
              computeDerivativeTheta1(e, 1, evi, dofi);
          const double theta1Diff = theta1_jplus1 - theta1_j;
          const double theta1DiffDerivative =
              theta1_jplus1_deriv - theta1_j_deriv;
          const double torsionalForce_dofi =
              theta1DiffDerivative * theta1Diff * element.torsionConstFactor;

          // First bending force computation
          ////theta2_j derivative
          const double theta2_j_deriv =
              computeDerivativeTheta2(e, 0, evi, dofi);
          ////theta2_jplus1 derivative
          const double theta2_jplus1_deriv =
              computeDerivativeTheta2(e, 1, evi, dofi);
          ////1st in bracket term
          const double firstBendingForce_inBracketsTerm_0 =
              theta2_j_deriv * 2 * theta2_j;
          ////2nd in bracket term
          const double firstBendingForce_inBracketsTerm_1 =
              theta2_jplus1_deriv * theta2_j;
          ////3rd in bracket term
          const double firstBendingForce_inBracketsTerm_2 =
              theta2_j_deriv * theta2_jplus1;
          ////4th in bracket term
          const double firstBendingForce_inBracketsTerm_3 =
              2 * theta2_jplus1_deriv * theta2_jplus1;
          // 3rd term computation
          const double firstBendingForce_inBracketsTerm =
              firstBendingForce_inBracketsTerm_0 +
              firstBendingForce_inBracketsTerm_1 +
              firstBendingForce_inBracketsTerm_2 +
              firstBendingForce_inBracketsTerm_3;
          const double firstBendingForce_dofi =
              firstBendingForce_inBracketsTerm *
              element.firstBendingConstFactor;

          // Second bending force computation
          ////theta2_j derivative
          const double theta3_j_deriv = computeDerivativeTheta3(e, ev_j, dui);
          ////theta2_jplus1 derivative
          const double theta3_jplus1_deriv =
              computeDerivativeTheta3(e, ev_jplus1, dui);
          ////1st in bracket term
          const double secondBendingForce_inBracketsTerm_0 =
              theta3_j_deriv * 2 * theta3_j;
          ////2nd in bracket term
          const double secondBendingForce_inBracketsTerm_1 =
              theta3_jplus1_deriv * theta3_j;
          ////3rd in bracket term
          const double secondBendingForce_inBracketsTerm_2 =
              theta3_j_deriv * theta3_jplus1;
          ////4th in bracket term
          const double secondBendingForce_inBracketsTerm_3 =
              2 * theta3_jplus1_deriv * theta3_jplus1;
          // 3rd term computation
          const double secondBendingForce_inBracketsTerm =
              secondBendingForce_inBracketsTerm_0 +
              secondBendingForce_inBracketsTerm_1 +
              secondBendingForce_inBracketsTerm_2 +
              secondBendingForce_inBracketsTerm_3;
          double secondBendingForce_dofi = secondBendingForce_inBracketsTerm *
                                           element.secondBendingConstFactor;
          internalForcesContributionFromThisEdge[evi].second[dofi] =
              axialForce_dofi + firstBendingForce_dofi +
              secondBendingForce_dofi + torsionalForce_dofi;
        }
        if (edgeNode.referenceElement != &e) {
          const bool isDofConstrainedFor_refElemOtherVertex =
              fixedVertices.contains(refElemOtherVertexNode.vi) &&
              fixedVertices.at(refElemOtherVertexNode.vi).contains(dofi);
          if (!isDofConstrainedFor_refElemOtherVertex) {
            DifferentiateWithRespectTo dui{*refElemOtherVertex, dofi};
            ////theta3_j derivative
            const double theta3_j_deriv = computeDerivativeTheta3(e, ev_j, dui);
            ////theta3_jplus1 derivative
            const double theta3_jplus1_deriv =
                computeDerivativeTheta3(e, ev_jplus1, dui);
            ////1st in bracket term
            const double secondBendingForce_inBracketsTerm_0 =
                theta3_j_deriv * 2 * theta3_j;
            ////2nd in bracket term
            const double secondBendingForce_inBracketsTerm_1 =
                theta3_jplus1_deriv * theta3_j;
            ////3rd in bracket term
            const double secondBendingForce_inBracketsTerm_2 =
                theta3_j_deriv * theta3_jplus1;
            ////4th in bracket term
            const double secondBendingForce_inBracketsTerm_3 =
                theta3_jplus1_deriv * 2 * theta3_jplus1;

            // 4th term computation
            const double secondBendingForce_inBracketsTerm =
                secondBendingForce_inBracketsTerm_0 +
                secondBendingForce_inBracketsTerm_1 +
                secondBendingForce_inBracketsTerm_2 +
                secondBendingForce_inBracketsTerm_3;
            const double secondBendingForce_dofi =
                secondBendingForce_inBracketsTerm *
                element.secondBendingConstFactor;
            internalForcesContributionFromThisEdge[evi + 2].second[dofi] =
                secondBendingForce_dofi;
          }
        }
      }
    }
    internalForcesContributionsFromEachEdge[ei] =
        internalForcesContributionFromThisEdge;
  }

  //#pragma omp parallel for schedule(static) num_threads(8)

  for (size_t vi = 0; vi < mesh->VN(); vi++) {
    Node::Forces &force = mesh->nodes[vi].force;
    force.residual = force.external;
    force.internal = 0;
  }
  double totalResidualForcesNorm = 0;
  for (size_t ei = 0; ei < mesh->EN(); ei++) {
    for (int i = 0; i < 4; i++) {
      std::pair<int, Vector6d> internalForcePair =
          internalForcesContributionsFromEachEdge[ei][i];
      int vi = internalForcePair.first;
      if (i > 1 && vi == -1) {
        continue;
      }
      Node::Forces &force = mesh->nodes[vi].force;
      force.internal = force.internal + internalForcePair.second;
      force.residual = force.residual + (internalForcePair.second * -1);
    }
  }
  for (size_t vi = 0; vi < mesh->VN(); vi++) {
    const double residualForceNorm = (mesh->nodes[vi].force.residual).norm();
    totalResidualForcesNorm += residualForceNorm;
  }
  mesh->previousTotalResidualForcesNorm = mesh->totalResidualForcesNorm;
  mesh->totalResidualForcesNorm = totalResidualForcesNorm;
}

void FormFinder::updateNodalExternalForces(
    const std::unordered_map<VertexIndex, Vector6d> &nodalForces,
    const std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
        &fixedVertices) {

  for (const std::pair<VertexIndex, Vector6d> &nodalForce : nodalForces) {
    const VertexIndex nodeIndex = nodalForce.first;
    const bool isNodeConstrained = fixedVertices.contains(nodeIndex);
    Node &node = mesh->nodes[nodeIndex];
    Vector6d nodalExternalForce(0);
    for (DoFType dofi = 0; dofi < 6; dofi++) {
      const bool isDofConstrained =
          isNodeConstrained && fixedVertices.at(nodeIndex).contains(dofi);
      if (isDofConstrained) {
        continue;
      }
      nodalExternalForce[dofi] = nodalForce.second[dofi];
    }
    node.force.external = nodalExternalForce;
  }
}

void FormFinder::updateResidualForces() {

  mesh->totalResidualForcesNorm = 0;
  for (const VertexType &v : mesh->vert) {
    Node &node = mesh->nodes[v];
    node.force.residual = node.force.external - node.force.internal;
    const double residualForceNorm = (node.force.residual).norm();
    mesh->totalResidualForcesNorm += residualForceNorm;
  }
}

void FormFinder::computeRigidSupports() {

  for (const VertexType &v : mesh->vert) {
    const VertexIndex vi = mesh->nodes[v].vi;
    const bool isVertexConstrained = constrainedVertices.contains(vi);
    if (isVertexConstrained) {
      auto constrainedDoFType = constrainedVertices.at(vi);
      const bool hasAllDoFTypeConstrained =
          constrainedDoFType.contains(DoF::Ux) &&
          constrainedDoFType.contains(DoF::Uy) &&
          constrainedDoFType.contains(DoF::Uz) &&
          constrainedDoFType.contains(DoF::Nx) &&
          constrainedDoFType.contains(DoF::Ny) &&
          constrainedDoFType.contains(DoF::Nr);
      if (hasAllDoFTypeConstrained) {
        rigidSupports.insert(vi);
      }
    }
  }
}

void FormFinder::updateNormalDerivatives() {
  for (const VertexType &v : mesh->vert) {
    for (DoFType dofi = DoF::Nx; dofi < DoF::NumDoF; dofi++) {
      DifferentiateWithRespectTo dui{v, dofi};
      mesh->nodes[v].derivativeOfNormal[dofi] =
          computeDerivativeOfNormal(v, dui);
    }
  }
}

void FormFinder::updateT1Derivatives() {
  for (const EdgeType &e : mesh->edge) {
    for (DoFType dofi = DoF::Ux; dofi < DoF::Nx; dofi++) {
      DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi};
      Element &element = mesh->elements[e];
      element.derivativeT1_j[dofi] = computeDerivativeT1(e, dui_v0);
      DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi};
      element.derivativeT1_jplus1[dofi] = computeDerivativeT1(e, dui_v1);

      element.derivativeT1[0][dofi] = element.derivativeT1_j[dofi];
      element.derivativeT1[1][dofi] = element.derivativeT1_jplus1[dofi];
    }
  }
}

void FormFinder::updateT2Derivatives() {
  for (const EdgeType &e : mesh->edge) {
    for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) {
      DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi};
      mesh->elements[e].derivativeT2_j[dofi] = computeDerivativeT2(e, dui_v0);
      DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi};
      mesh->elements[e].derivativeT2_jplus1[dofi] =
          computeDerivativeT2(e, dui_v1);

      Element &element = mesh->elements[e];
      element.derivativeT2[0][dofi] = element.derivativeT2_j[dofi];
      element.derivativeT2[1][dofi] = element.derivativeT2_jplus1[dofi];
    }
  }
}

void FormFinder::updateT3Derivatives() {
  for (const EdgeType &e : mesh->edge) {
    for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) {
      DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi};
      Element &element = mesh->elements[e];
      element.derivativeT3_j[dofi] = computeDerivativeT3(e, dui_v0);
      DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi};
      element.derivativeT3_jplus1[dofi] = computeDerivativeT3(e, dui_v1);

      element.derivativeT3[0][dofi] = element.derivativeT3_j[dofi];
      element.derivativeT3[1][dofi] = element.derivativeT3_jplus1[dofi];
    }
  }
}

void FormFinder::updateRDerivatives() {
  for (const EdgeType &e : mesh->edge) {
    for (DoFType dofi = DoF::Nx; dofi < DoF::NumDoF; dofi++) {
      DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi};
      mesh->elements[e].derivativeR_j[dofi] = computeDerivativeOfR(e, dui_v0);
      DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi};
      mesh->elements[e].derivativeR_jplus1[dofi] =
          computeDerivativeOfR(e, dui_v1);
    }
  }
}

void FormFinder::updateElementalLengths() { mesh->updateElementalLengths(); }

FormFinder::FormFinder() {}

void FormFinder::updateNodalMasses() {
  const double gamma = 0.8;
  for (VertexType &v : mesh->vert) {
    double translationalSumSk = 0;
    double rotationalSumSk_I2 = 0;
    double rotationalSumSk_I3 = 0;
    double rotationalSumSk_J = 0;
    for (const EdgePointer &ep : mesh->nodes[v].incidentElements) {
      const size_t ei = mesh->getIndex(ep);
      const Element &elem = mesh->elements[ep];
      const double SkTranslational =
          elem.properties.E * elem.properties.A / elem.length;
      translationalSumSk += SkTranslational;
      const double lengthToThe3 = std::pow(elem.length, 3);
      const double SkRotational_I2 =
          elem.properties.E * elem.properties.I2 /
          lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia
      const double SkRotational_I3 =
          elem.properties.E * elem.properties.I3 /
          lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia
      const double SkRotational_J =
          elem.properties.E * elem.properties.J /
          lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia
      rotationalSumSk_I2 += SkRotational_I2;
      rotationalSumSk_I3 += SkRotational_I3;
      rotationalSumSk_J += SkRotational_J;
      assert(rotationalSumSk_I2 != 0);
      assert(rotationalSumSk_I3 != 0);
      assert(rotationalSumSk_J != 0);
    }
    mesh->nodes[v].translationalMass =
        gamma * pow(Dtini, 2) * 2 * translationalSumSk;
    mesh->nodes[v].rotationalMass_I2 =
        gamma * pow(Dtini, 2) * 8 * rotationalSumSk_I2;
    mesh->nodes[v].rotationalMass_I3 =
        gamma * pow(Dtini, 2) * 8 * rotationalSumSk_I3;
    mesh->nodes[v].rotationalMass_J =
        gamma * pow(Dtini, 2) * 8 * rotationalSumSk_J;

    assert(std::pow(Dtini, 2.0) * translationalSumSk /
               mesh->nodes[v].translationalMass <
           2);
    assert(std::pow(Dtini, 2.0) * rotationalSumSk_I2 /
               mesh->nodes[v].rotationalMass_I2 <
           2);
    assert(std::pow(Dtini, 2.0) * rotationalSumSk_I3 /
               mesh->nodes[v].rotationalMass_I3 <
           2);
    assert(std::pow(Dtini, 2.0) * rotationalSumSk_J /
               mesh->nodes[v].rotationalMass_J <
           2);
  }
}

void FormFinder::updateNodalAccelerations() {
  for (VertexType &v : mesh->vert) {
    Node &node = mesh->nodes[v];
    for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) {
      if (dofi == DoF::Ux || dofi == DoF::Uy || dofi == DoF::Uz) {
        node.acceleration[dofi] =
            node.force.residual[dofi] / node.translationalMass;
      } else if (dofi == DoF::Nx) {
        node.acceleration[dofi] =
            node.force.residual[dofi] / node.rotationalMass_J;
      } else if (dofi == DoF::Ny) {
        node.acceleration[dofi] =
            node.force.residual[dofi] / node.rotationalMass_I2;
      } else if (dofi == DoF::Nr) {
        node.acceleration[dofi] =
            node.force.residual[dofi] / node.rotationalMass_I3;
      }
    }
  }
}

void FormFinder::updateNodalVelocities() {
  for (VertexType &v : mesh->vert) {
    Node &node = mesh->nodes[v];
    node.velocity = node.velocity + node.acceleration * Dt;
  }
  updateKineticEnergy();
}

void FormFinder::updateNodalDisplacements() {
  for (VertexType &v : mesh->vert) {
    Node &node = mesh->nodes[v];
    node.displacements = node.displacements + node.velocity * Dt;
  }
}

void FormFinder::updateNodePosition(
    VertexType &v,
    const std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
        &fixedVertices) {
  Node &node = mesh->nodes[v];
  CoordType previousLocation = v.cP();
  const VertexIndex &vi = mesh->nodes[v].vi;

  VectorType displacementVector(0, 0, 0);
  if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(0)) {
    displacementVector += VectorType(node.displacements[0], 0, 0);
  }
  if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(1)) {
    displacementVector += VectorType(0, node.displacements[1], 0);
  }
  if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(2)) {
    displacementVector += VectorType(0, 0, node.displacements[2]);
  }
  node.previousLocation = previousLocation;
  v.P() = node.initialLocation + displacementVector;
  if (shouldApplyInitialDistortion && currentSimulationStep < 40) {
    VectorType desiredInitialDisplacement(0, 0, 0.1);
    v.P() = v.P() + desiredInitialDisplacement;
  }
}

void FormFinder::applyDisplacements(
    const std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
        &fixedVertices) {
  for (VertexType &v : mesh->vert) {
    updateNodePosition(v, fixedVertices);
    Node &node = mesh->nodes[v];
    const VertexIndex &vi = node.vi;
    VectorType normalDisplacementVector(0, 0, 0);
    if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(3)) {
      normalDisplacementVector += VectorType(node.displacements[3], 0, 0);
    }
    if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(4)) {
      normalDisplacementVector += VectorType(0, node.displacements[4], 0);
    }
    v.N() = node.initialNormal + normalDisplacementVector;
    const double &nx = v.N()[0], ny = v.N()[1];
    const double nxnyMagnitude = std::pow(nx, 2) + std::pow(ny, 2);
    if (nxnyMagnitude > 1) {
      v.N() = VectorType(nx / std::sqrt(nxnyMagnitude),
                         ny / std::sqrt(nxnyMagnitude), 0);
    } else {
      const double nzSquared = 1.0 - nxnyMagnitude;
      const double nz = std::sqrt(nzSquared);
      VectorType newNormal(nx, ny, nz);
      v.N() = newNormal;
    }
    if (!rigidSupports.contains(vi)) {
      node.nR = node.displacements[5];
    } else {
      const EdgePointer &refElem = node.referenceElement;
      const VectorType &refT1 = mesh->elements[refElem].frame.t1;

      const VectorType &t1Initial =
          computeT1Vector(mesh->nodes[refElem->cV(0)].initialLocation,
                          mesh->nodes[refElem->cV(1)].initialLocation);
      VectorType g1 = Cross(refT1, t1Initial);
      node.nR = g1.dot(v.cN());
    }
  }
  updateElementalFrames();
}

void FormFinder::updateElementalFrames() {
  for (const EdgeType &e : mesh->edge) {
    const VectorType elementNormal =
        (e.cV(0)->cN() + e.cV(1)->cN()).Normalize();
    mesh->elements[e].frame =
        computeElementFrame(e.cP(0), e.cP(1), elementNormal);
  }
}

void FormFinder::applyForcedDisplacements(
    const std::unordered_map<VertexIndex, Eigen::Vector3d>
        nodalForcedDisplacements) {
  for (const std::pair<VertexIndex, Eigen::Vector3d>
           vertexIndexDisplacementPair : nodalForcedDisplacements) {
    const VertexIndex vi = vertexIndexDisplacementPair.first;
    const Eigen::Vector3d vertexDisplacement =
        vertexIndexDisplacementPair.second;
    Node &node = mesh->nodes[vi];
    VectorType displacementVector(vertexDisplacement(0), vertexDisplacement(1),
                                  vertexDisplacement(2));
    mesh->vert[vi].P() = node.initialLocation + displacementVector;
  }
}

// NOTE: Is this correct? Should the kinetic energy be computed like that?
void FormFinder::updateKineticEnergy() {
  mesh->previousTotalKineticEnergy = mesh->currentTotalKineticEnergy;
  mesh->currentTotalKineticEnergy = 0;
  for (const VertexType &v : mesh->vert) {
    Node &node = mesh->nodes[v];
    node.kineticEnergy = 0;

    const double translationalVelocityNorm = std::sqrt(
        std::pow(node.velocity[0], 2) + std::pow(node.velocity[1], 2) +
        std::pow(node.velocity[2], 2));
    node.kineticEnergy +=
        0.5 * node.translationalMass * pow(translationalVelocityNorm, 2);

    assert(node.kineticEnergy < 10000000000000000000);
    //    const double rotationalVelocityNorm = std::sqrt(
    //        std::pow(node.velocity[3], 2) + std::pow(node.velocity[4], 2) +
    //        std::pow(node.velocity[5], 2));
    node.kineticEnergy +=
        0.5 * node.rotationalMass_J * pow(node.velocity[3], 2) +
        0.5 * node.rotationalMass_I3 * pow(node.velocity[4], 2) +
        0.5 * node.rotationalMass_I2 * pow(node.velocity[5], 2);

    mesh->currentTotalKineticEnergy += node.kineticEnergy;
  }
  //  assert(mesh->currentTotalKineticEnergy < 100000000000000);
}

void FormFinder::resetVelocities() {
  for (const VertexType &v : mesh->vert) {
    mesh->nodes[v].velocity = 0;
    //            mesh->nodes[v].force.residual * 0.5 * Dt /
    //            mesh->nodes[v].mass; // NOTE: Do I reset the previous
    //            velocity?
    //        reset
    // current to 0 or this?
  }
  updateKineticEnergy();
}

void FormFinder::updatePositionsOnTheFly(
    const std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
        &fixedVertices) {
  const double gamma = 0.8;
  for (VertexType &v : mesh->vert) {
    double translationalSumSk = 0;
    double rotationalSumSk_I2 = 0;
    double rotationalSumSk_I3 = 0;
    double rotationalSumSk_J = 0;
    for (const EdgePointer &ep : mesh->nodes[v].incidentElements) {
      const Element &elem = mesh->elements[ep];
      const double SkTranslational =
          elem.properties.E * elem.properties.A / elem.length;
      translationalSumSk += SkTranslational;
      const double lengthToThe3 = std::pow(elem.length, 3);
      const double SkRotational_I2 =
          elem.properties.E * elem.properties.I2 /
          lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia
      const double SkRotational_I3 =
          elem.properties.E * elem.properties.I3 /
          lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia
      const double SkRotational_J =
          elem.properties.E * elem.properties.J /
          lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia
      rotationalSumSk_I2 += SkRotational_I2;
      rotationalSumSk_I3 += SkRotational_I3;
      rotationalSumSk_J += SkRotational_J;
      //      assert(rotationalSumSk_I2 != 0);
      //      assert(rotationalSumSk_I3 != 0);
      //      assert(rotationalSumSk_J != 0);
    }
    mesh->nodes[v].translationalMass =
        gamma * pow(Dtini, 2) * 2 * translationalSumSk;
    mesh->nodes[v].rotationalMass_I2 =
        gamma * pow(Dtini, 2) * 8 * rotationalSumSk_I2;
    mesh->nodes[v].rotationalMass_I3 =
        gamma * pow(Dtini, 2) * 8 * rotationalSumSk_I3;
    mesh->nodes[v].rotationalMass_J =
        gamma * pow(Dtini, 2) * 8 * rotationalSumSk_J;

    //    assert(std::pow(Dtini, 2.0) * translationalSumSk /
    //               mesh->nodes[v].translationalMass <
    //           2);
    //    assert(std::pow(Dtini, 2.0) * rotationalSumSk_I2 /
    //               mesh->nodes[v].rotationalMass_I2 <
    //           2);
    //    assert(std::pow(Dtini, 2.0) * rotationalSumSk_I3 /
    //               mesh->nodes[v].rotationalMass_I3 <
    //           2);
    //    assert(std::pow(Dtini, 2.0) * rotationalSumSk_J /
    //               mesh->nodes[v].rotationalMass_J <
    //           2);
  }

  for (VertexType &v : mesh->vert) {
    Node &node = mesh->nodes[v];
    for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) {
      if (dofi == DoF::Ux || dofi == DoF::Uy || dofi == DoF::Uz) {
        node.acceleration[dofi] =
            node.force.residual[dofi] / node.translationalMass;
      } else if (dofi == DoF::Nx) {
        node.acceleration[dofi] =
            node.force.residual[dofi] / node.rotationalMass_J;
      } else if (dofi == DoF::Ny) {
        node.acceleration[dofi] =
            node.force.residual[dofi] / node.rotationalMass_I3;
      } else if (dofi == DoF::Nr) {
        node.acceleration[dofi] =
            node.force.residual[dofi] / node.rotationalMass_I2;
      }
    }
  }

  for (VertexType &v : mesh->vert) {
    Node &node = mesh->nodes[v];
    node.velocity = node.velocity + node.acceleration * Dt;
  }
  updateKineticEnergy();

  for (VertexType &v : mesh->vert) {
    Node &node = mesh->nodes[v];
    node.displacements = node.displacements + node.velocity * Dt;
  }

  for (VertexType &v : mesh->vert) {
    updateNodePosition(v, fixedVertices);
    Node &node = mesh->nodes[v];
    const VertexIndex &vi = node.vi;
    VectorType normalDisplacementVector(0, 0, 0);
    if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(3)) {
      normalDisplacementVector += VectorType(node.displacements[3], 0, 0);
    }
    if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(4)) {
      normalDisplacementVector += VectorType(0, node.displacements[4], 0);
    }
    v.N() = node.initialNormal + normalDisplacementVector;
    const double &nx = v.N()[0], ny = v.N()[1];
    const double nxnyMagnitude = std::pow(nx, 2) + std::pow(ny, 2);
    if (nxnyMagnitude > 1) {
      v.N() = VectorType(nx / std::sqrt(nxnyMagnitude),
                         ny / std::sqrt(nxnyMagnitude), 0);
    } else {
      const double nzSquared = 1.0 - nxnyMagnitude;
      const double nz = std::sqrt(nzSquared);
      VectorType newNormal(nx, ny, nz);
      v.N() = newNormal;
    }
    if (!rigidSupports.contains(vi)) {
      node.nR = node.displacements[5];
    } else {
    }
  }
  updateElementalFrames();
}

SimulationResults
FormFinder::computeResults(const SimulationMesh &initialMesh) {
  const size_t numberOfVertices = initialMesh.VN();
  std::vector<Vector6d> displacements(numberOfVertices);

  for (size_t vi = 0; vi < numberOfVertices; vi++) {
    displacements[vi] = mesh->nodes[vi].displacements;
  }

  history.numberOfSteps = currentSimulationStep;

  return SimulationResults{history, displacements};
}

void FormFinder::draw(const std::string &screenshotsFolder = {}) {
  // update positions
  //  polyscope::getCurveNetwork("Undeformed edge mesh")->setEnabled(false);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->updateNodePositions(mesh->getEigenVertices());
  // Per node kinetic energies
  std::vector<double> kineticEnergies(mesh->VN());
  for (const VertexType &v : mesh->vert) {
    kineticEnergies[mesh->getIndex(v)] = mesh->nodes[v].kineticEnergy;
  }
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeScalarQuantity("Kinetic Energy", kineticEnergies);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("Kinetic Energy")
      ->setEnabled(false);

  // Per node normals
  std::vector<std::array<double, 3>> nodeNormals(mesh->VN());
  for (const VertexType &v : mesh->vert) {
    const VectorType n = v.cN();
    nodeNormals[mesh->getIndex(v)] = {n[0], n[1], n[2]};
  }
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeVectorQuantity("Node normals", nodeNormals)
      ->setEnabled(true);

  // per node external forces
  //  std::vector<std::array<double, 3>> externalForces(mesh->VN());
  //  for (const VertexType &v : mesh->vert) {
  //    const Vector6d nodeForce =
  //        mesh->nodes[v].force.external.value_or(Vector6d(0));
  //    externalForces[mesh->getIndex(v)] = {nodeForce[0], nodeForce[1],
  //                                         nodeForce[2]};
  //  }
  //  polyscope::getCurveNetwork("Deformed edge mesh")
  //      ->addNodeVectorQuantity("External force", externalForces);
  //  polyscope::getCurveNetwork("Deformed edge mesh")
  //      ->getQuantity("External force")
  //      ->setEnabled(true);

  std::vector<std::array<double, 3>> internalForces(mesh->VN());
  std::vector<std::array<double, 3>> externalForces(mesh->VN());
  std::vector<double> internalForcesNorm(mesh->VN());
  for (const VertexType &v : mesh->vert) {
    // per node internal forces
    const Vector6d nodeforce = mesh->nodes[v].force.internal * (-1);
    internalForces[mesh->getIndex(v)] = {nodeforce[0], nodeforce[1],
                                         nodeforce[2]};
    internalForcesNorm[mesh->getIndex(v)] = nodeforce.norm();
    // External force
    const Vector6d nodeExternalForce = mesh->nodes[v].force.external;
    externalForces[mesh->getIndex(v)] = {
        nodeExternalForce[0], nodeExternalForce[1], nodeExternalForce[2]};
  }
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeVectorQuantity("Internal force", internalForces);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("Internal force")
      ->setEnabled(false);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeVectorQuantity("External force", externalForces);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("External force")
      ->setEnabled(true);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeScalarQuantity("Internal force norm", internalForcesNorm);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("Internal force norm")
      ->setEnabled(true);
  // per node internal forces
  std::vector<std::array<double, 3>> internalAxialForces(mesh->VN());
  for (const VertexType &v : mesh->vert) {
    const Vector6d nodeforce = mesh->nodes[v].force.internalAxial * (-1);
    internalAxialForces[mesh->getIndex(v)] = {nodeforce[0], nodeforce[1],
                                              nodeforce[2]};
  }
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeVectorQuantity("Internal Axial force", internalAxialForces);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("Internal Axial force")
      ->setEnabled(false);
  // per node internal first bending force
  std::vector<std::array<double, 3>> internalFirstBendingTranslationForces(
      mesh->VN());
  std::vector<std::array<double, 3>> internalFirstBendingRotationForces(
      mesh->VN());
  std::vector<std::array<double, 3>> internalSecondBendingTranslationForces(
      mesh->VN());
  std::vector<std::array<double, 3>> internalSecondBendingRotationForces(
      mesh->VN());
  std::vector<double> nRs(mesh->VN());
  std::vector<double> theta2(mesh->VN());
  std::vector<double> theta3(mesh->VN());
  for (const VertexType &v : mesh->vert) {
    const Node &node = mesh->nodes[v];
    const Vector6d nodeForceFirst = node.force.internalFirstBending * (-1);
    internalFirstBendingTranslationForces[mesh->getIndex(v)] = {
        nodeForceFirst[0], nodeForceFirst[1], nodeForceFirst[2]};
    internalFirstBendingRotationForces[mesh->getIndex(v)] = {
        nodeForceFirst[3], nodeForceFirst[4], 0};

    const Vector6d nodeForceSecond = node.force.internalSecondBending * (-1);
    internalSecondBendingTranslationForces[mesh->getIndex(v)] = {
        nodeForceSecond[0], nodeForceSecond[1], nodeForceSecond[2]};
    internalSecondBendingRotationForces[mesh->getIndex(v)] = {
        nodeForceSecond[3], nodeForceSecond[4], 0};
    nRs[mesh->getIndex(v)] = node.nR;
    const std::vector<EdgePointer> incidentEdges = node.incidentElements;
    const EdgeIndex ei = mesh->getIndex(incidentEdges.back());
    //    theta2[mesh->getIndex(v)] =
    //    node.rotationalDisplacements.at(ei).theta2;
    //    theta3[mesh->getIndex(v)] =
    //    node.rotationalDisplacements.at(ei).theta3;
  }
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeVectorQuantity("First bending force-Translation",
                              internalFirstBendingTranslationForces);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("First bending force-Translation")
      ->setEnabled(false);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeVectorQuantity("First bending force-Rotation",
                              internalFirstBendingRotationForces);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("First bending force-Rotation")
      ->setEnabled(false);

  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeVectorQuantity("Second bending force-Translation",
                              internalSecondBendingTranslationForces);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("Second bending force-Translation")
      ->setEnabled(false);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeVectorQuantity("Second bending force-Rotation",
                              internalSecondBendingRotationForces);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("Second bending force-Rotation")
      ->setEnabled(false);

  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeScalarQuantity("nR", nRs);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("nR")
      ->setEnabled(false);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeScalarQuantity("theta3", theta3);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("theta3")
      ->setEnabled(false);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeScalarQuantity("theta2", theta2);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("theta2")
      ->setEnabled(false);

  // per node residual forces
  std::vector<std::array<double, 3>> residualForces(mesh->VN());
  std::vector<double> residualForcesNorm(mesh->VN());
  for (const VertexType &v : mesh->vert) {
    const Vector6d nodeResidualForce = mesh->nodes[v].force.residual;
    residualForces[mesh->getIndex(v)] = {
        nodeResidualForce[0], nodeResidualForce[1], nodeResidualForce[2]};
    residualForcesNorm[mesh->getIndex(v)] = nodeResidualForce.norm();
  }
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeVectorQuantity("Residual force", residualForces);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("Residual force")
      ->setEnabled(false);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeScalarQuantity("Residual force norm", residualForcesNorm);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("Residual force norm")
      ->setEnabled(false);

  // per node x acceleration
  std::vector<double> accelerationX(mesh->VN());
  for (const VertexType &v : mesh->vert) {
    accelerationX[mesh->getIndex(v)] = mesh->nodes[v].acceleration[0];
  }
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addNodeScalarQuantity("Node acceleration x", accelerationX);

  // per element t1
  std::vector<std::array<double, 3>> t1s(mesh->EN());
  for (const EdgeType &e : mesh->edge) {
    const VectorType &t1 = mesh->elements[e].frame.t1;
    t1s[mesh->getIndex(e)] = {t1[0], t1[1], t1[2]};
  }
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addEdgeVectorQuantity("t1Check", t1s);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("t1Check")
      ->setEnabled(false);
  // per element t2
  std::vector<std::array<double, 3>> t2s(mesh->EN());
  for (const EdgeType &e : mesh->edge) {
    const VectorType &t2 = mesh->elements[e].frame.t2;
    t2s[mesh->getIndex(e)] = {t2[0], t2[1], t2[2]};
  }
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addEdgeVectorQuantity("t2", t2s);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("t2")
      ->setEnabled(false);
  // per element t3
  std::vector<std::array<double, 3>> t3s(mesh->EN());
  for (const EdgeType &e : mesh->edge) {
    const VectorType &t3 = mesh->elements[e].frame.t3;
    t3s[mesh->getIndex(e)] = {t3[0], t3[1], t3[2]};
  }
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->addEdgeVectorQuantity("t3", t3s);
  polyscope::getCurveNetwork("Deformed edge mesh")
      ->getQuantity("t3")
      ->setEnabled(false);

  // Specify the callback
  polyscope::state::userCallback = [&]() {
    // Since options::openImGuiWindowForUserCallback == true by default,
    // we can immediately start using ImGui commands to build a UI

    ImGui::PushItemWidth(100); // Make ui elements 100 pixels wide,
                               // instead of full width. Must have
                               // matching PopItemWidth() below.

    ImGui::InputInt("Simulation drawing step",
                    &mDrawingStep); // set a int variable

    ImGui::PopItemWidth();
  };

  if (!screenshotsFolder.empty()) {
    static bool firstDraw = true;
    if (firstDraw) {
      for (const auto &entry :
           std::filesystem::directory_iterator(screenshotsFolder))
        std::filesystem::remove_all(entry.path());
      //      polyscope::view::processZoom(5);
      firstDraw = false;
    }
    polyscope::screenshot(
        std::filesystem::path(screenshotsFolder)
            .append(std::to_string(currentSimulationStep) + ".png")
            .string(),
        false);
  } else {
    polyscope::show();
  }
}

SimulationResults
FormFinder::executeSimulation(const SimulationJob &job, const bool &beVerbose,
                              const bool &shouldDraw,
                              const size_t &maxDRMIterations) {
  assert(job.mesh.operator bool());
  auto t1 = std::chrono::high_resolution_clock::now();

  constrainedVertices = job.fixedVertices;
  if (!job.nodalForcedDisplacements.empty()) {
    for (std::pair<VertexIndex, Eigen::Vector3d> viDisplPair :
         job.nodalForcedDisplacements) {
      const VertexIndex vi = viDisplPair.first;
      constrainedVertices[vi].insert({0, 1, 2});
    }
  }

  //  VCGEdgeMesh *jobEdgeMesh = job.mesh.get();
  mesh = std::make_unique<SimulationMesh>(*job.mesh);
  //  ElementalMesh *jobElementalMesh = job.mesh.get();
  //  vcg::tri::Append<ElementalMesh, ConstElementalMesh>::MeshCopy(
  //      *(this->mesh), *jobElementalMesh);
  if (beVerbose) {
    std::cout << "Executing simulation for mesh with:" << mesh->VN()
              << " nodes and " << mesh->EN() << " elements." << std::endl;
  }
  reset();
  computeRigidSupports();
  if (shouldDraw) {
    if (!polyscope::state::initialized) {
      initPolyscope();
    }
    const std::string deformedMeshName = "Deformed edge mesh";
    if (!polyscope::hasCurveNetwork(deformedMeshName)) {
      polyscope::registerCurveNetwork(
          deformedMeshName, mesh->getEigenVertices(), mesh->getEigenEdges());
    }

    registerWorldAxes();
  }
  //      const double beamRadius = mesh.getBeamDimensions()[0].od / 2;
  //  std::cout << "BeamRadius:" << beamRadius << std::endl;
  //  polyscope::getCurveNetwork("Deformed edge mesh")
  //      //
  //      //      ->setRadius(beamRadius);
  //      ->setRadius(0.01);

  //  const bool hasExternalLoads =
  //      !job.nodalExternalForces.empty() ||
  //      !job.nodalForcedDisplacements.empty();
  //  assert(hasExternalLoads);
  for (auto fixedVertex : job.fixedVertices) {
    assert(fixedVertex.first < mesh->VN());
  }

  updateElementalFrames();
  updateNodalMasses();
  if (job.nodalExternalForces.empty()) {
    shouldApplyInitialDistortion = true;
  }
  //    std::unordered_map<VertexIndex, Eigen::Vector3d> temporaryForces{
  //        //        //        {2, Eigen::Vector3d(0, 0, 1000)},
  //        //        //        {12, Eigen::Vector3d(0, 0, 1000)},
  //        //        //        {18, Eigen::Vector3d(0, 0, 1000)},
  //        //        //        {8, Eigen::Vector3d(0, 0, 1000)}};
  //        {10, Eigen::Vector3d(0, 0, 10000)}};
  //    std::unordered_map<VertexIndex, Eigen::Vector3d> temporaryForces;
  //    for (VertexIndex vi = 0; vi < mesh.VN(); vi++) {
  //    for (VertexType &v : mesh.vert) {
  //      const VertexIndex vi = mesh.getIndex(v);
  //      VectorType allowedDoFType(1, 1, 1);
  //      if (constrainedVertices.contains(vi)) {
  //        std::unordered_set<gsl::index> constrainedDof =
  //            constrainedVertices.at(vi);
  //        if (constrainedDof.contains(0)) {
  //          allowedDoFType[0] = 0;
  //        } else if (constrainedDof.contains(1)) {
  //          allowedDoFType[1] = 0;
  //        } else if (constrainedDof.contains(2)) {
  //          allowedDoFType[2] = 0;
  //        }
  //      }
  //      VectorType desiredDisplacement = VectorType(0, 0, 0.2);
  //      VectorType displacement(allowedDoFType[0] * desiredDisplacement[0],
  //                              allowedDoFType[1] * desiredDisplacement[1],
  //                              allowedDoFType[2] * desiredDisplacement[2]);
  //      v.P() = v.P() + displacement;
  //      temporaryForces.insert({vi, Eigen::Vector3d(0, 0, 100000)});
  //    }
  //    updateNodalExternalForces(temporaryForces, job.fixedVertices);
  //    appliedTemporaryForce = true;
  //  } else {
  //  std::unordered_map<VertexIndex, Eigen::Vector3d> appliedLoad =
  //      job.nodalExternalForces;
  //  const size_t numberOfStepsForApplyingExternalLoads = 10;
  //  int externalLoadStep = 1;
  //  std::for_each(appliedLoad.begin(), appliedLoad.end(), [](auto &pair) {
  //    pair.second /= numberOfStepsForApplyingExternalLoads;
  //  });
  //  updateNodalExternalForces(appliedLoad, constrainedVertices);
  updateNodalExternalForces(job.nodalExternalForces, constrainedVertices);
  //  }

  while (currentSimulationStep < maxDRMIterations) {
    //  while (true) {
    updateNormalDerivatives();
    updateT1Derivatives();
    updateRDerivatives();
    updateT2Derivatives();
    updateT3Derivatives();
    //    updateRotationalDisplacements();
    //    if (currentSimulationStep > lastPulseStepIndex) {
    //    const std::vector<Vector6d> internalForces =
    updateResidualForcesOnTheFly(constrainedVertices);
    //#pragma omp parallel for schedule(static) num_threads(8)
    //    double totalResidualForcesNorm = 0;
    //    for (size_t vi = 0; vi < mesh.VN(); vi++) {
    //      Node::Forces &force = mesh.nodes[vi].force;
    //      //    const Vector6d &parallelForce = internalForcesParallel[vi];
    //      //    const Vector6d &serialForce = internalForces[vi];
    //      //    const double normOfDifference = (serialForce +
    //      (parallelForce
    //      *
    //      //    -1)).norm(); assert(normOfDifference < 0.000001);
    //      force.residual = force.external - internalForces[vi];
    //      const double residualForceNorm = (force.residual).norm();
    //      totalResidualForcesNorm += residualForceNorm;
    //      //    assert(residualForceNorm < std::pow(10, 20));
    //    }
    //    mesh.totalResidualForcesNorm = totalResidualForcesNorm;
    //    } else {
    //      if (currentSimulationStep == lastPulseStepIndex &&
    //          appliedTemporaryForce) {
    //        for (const VertexType &v : mesh.vert) {
    //          Node &node = mesh.nodes[v];
    //          node.force.external = std::optional<Vector6d>();
    //        }
    //      }
    //      updateNodalInternalForces(job.fixedVertices);
    //    }

    // TODO: write parallel function for updating positions
    // TODO: Make a single function out of updateResidualForcesOnTheFly
    // updatePositionsOnTheFly
    //    updatePositionsOnTheFly(constrainedVertices);
    updateNodalMasses();
    updateNodalAccelerations();
    updateNodalVelocities();
    updateNodalDisplacements();
    applyDisplacements(constrainedVertices);
    if (!job.nodalForcedDisplacements.empty()) {
      applyForcedDisplacements(

          job.nodalForcedDisplacements);
    }
    updateElementalLengths();

    //    if (!std::isfinite(mesh.currentTotalKineticEnergy)) {
    //      std::cerr << "Infinite kinetic energy. Breaking simulation.."
    //                << std::endl;
    //      break;
    //    }
    //    assert(std::isfinite(mesh.currentTotalKineticEnergy));
    //    mesh.printVertexCoordinates(mesh.VN() / 2);
    //    draw();
    if (shouldDraw &&
        currentSimulationStep % mDrawingStep == 0 /* &&
currentSimulationStep > maxDRMIterations*/) {
      //      std::string saveTo = std::filesystem::current_path()
      //                               .append("Debugging_files")
      //                               .append("Screenshots")
      //                               .string();
      //      draw(saveTo);
      //      SimulationResultsReporter::createPlot(
      //          "Number of iterations", "Log of Kinetic Energy",
      //          history.kineticEnergy,
      //          std::filesystem::path(saveTo).append(
      //              std::to_string(currentSimulationStep) + "_kin.png"));

      //      draw();
      //      auto t2 = std::chrono::high_resolution_clock::now();
      //      auto timePerNodePerIteration =
      //          std::chrono::duration_cast<std::chrono::microseconds>(t2 -
      //          t1)
      //              .count() /
      //          (mesh.VN() * (currentSimulationStep + 1));
      //      std::cout << "Time/(node*iteration) "
      //                << timePerNodePerIteration * std::pow(10, -6) << "s"
      //                << std::endl;
      draw();
    }
    if (currentSimulationStep != 0) {
      history.stepPulse(*mesh);
    }
    //    t = t + Dt;
    currentSimulationStep++;
    //    std::cout << "Kinetic energy:" << mesh.currentTotalKineticEnergy
    //              << std::endl;
    //    std::cout << "Residual forces norm:" << mesh.totalResidualForcesNorm
    //              << std::endl;
    if (mesh->previousTotalKineticEnergy > mesh->currentTotalKineticEnergy) {
      if (/*mesh.totalPotentialEnergykN < 10 ||*/
          mesh->totalResidualForcesNorm < totalResidualForcesNormThreshold) {
        if (beVerbose) {
          std::cout << "Convergence after " << currentSimulationStep << " steps"
                    << std::endl;
          std::cout << "Residual force of magnitude:"
                    << mesh->previousTotalResidualForcesNorm << std::endl;
          std::cout << "Kinetic energy:" << mesh->currentTotalKineticEnergy
                    << std::endl;
          mesh->totalPotentialEnergykN = computeTotalPotentialEnergy() / 1000;
          std::cout << "Total potential:" << mesh->totalPotentialEnergykN
                    << " kNm" << std::endl;
        }
        //        if (externalLoadStep !=
        //        numberOfStepsForApplyingExternalLoads)
        //        {
        //          std::for_each(
        //              appliedLoad.begin(), appliedLoad.end(), [&](auto
        //              &pair)
        //              {
        //                pair.second +=
        //                job.nodalExternalForces.at(pair.first)
        //                /
        //                               numberOfStepsForApplyingExternalLoads;
        //              });
        //          updateNodalExternalForces(appliedLoad,
        //          constrainedVertices);
        //        } else {
        break;
        //        }
      }
      //        history.markRed(currentSimulationStep);
      //        std::cout << "Total potential:" << mesh.totalPotentialEnergykN
      //        << " kNm"
      //                  << std::endl;
      // reset displacements to previous position where the peak occured
      for (VertexType &v : mesh->vert) {
        Node &node = mesh->nodes[v];
        node.displacements = node.displacements - node.velocity * Dt;
      }
      applyDisplacements(constrainedVertices);
      if (!job.nodalForcedDisplacements.empty()) {
        applyForcedDisplacements(

            job.nodalForcedDisplacements);
      }
      updateElementalLengths();
      resetVelocities();
      Dt = Dt * xi;
      //        std::cout << "Residual forces norm:" <<
      //        mesh.totalResidualForcesNorm
      //                  << std::endl;
    }
  }
  if (currentSimulationStep == maxDRMIterations) {
    std::cout << "Did not reach equilibrium before reaching the maximum number "
                 "of DRM steps ("
              << maxDRMIterations << "). Breaking simulation" << std::endl;
  } else if (beVerbose) {
    auto t2 = std::chrono::high_resolution_clock::now();
    double simulationDuration =
        std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count();
    simulationDuration /= 1000;
    std::cout << "Simulation converged after " << simulationDuration << "s"
              << std::endl;
    std::cout << "Time/(node*iteration) "
              << simulationDuration /
                     (static_cast<double>(currentSimulationStep) * mesh->VN())
              << "s" << std::endl;
  }
  //    mesh.printVertexCoordinates(mesh.VN() / 2);
  if (shouldDraw) {
    draw();
  }
  //  if (!polyscope::hasCurveNetwork(deformedMeshName)) {
  //    polyscope::removeCurveNetwork(deformedMeshName);
  //  }
  //  debugger.createPlots();
  SimulationResults results = computeResults(*job.mesh);
  //  Eigen::write_binary("optimizedResult.eigenBin",
  //  results.nodalDisplacements);

  //  const std::string groundOfTruthBinaryFilename =
  //      "../../groundOfTruths/grid6_groundOfTruth.eigenBin";
  //  //      "../../groundOfTruths/longSpanGridshell_groundOfTruth.eigenBin";
  //  assert(std::filesystem::exists(
  //      std::filesystem::path(groundOfTruthBinaryFilename)));
  //  Eigen::MatrixX3d groundOfTruthMatrix;
  //  Eigen::read_binary(groundOfTruthBinaryFilename, groundOfTruthMatrix);
  //  assert(results.nodalDisplacements.isApprox(groundOfTruthMatrix));
  return results;
  //  return history;
}