#include "drmsimulationmodel.hpp"
#include "simulationhistoryplotter.hpp"
#include <Eigen/Geometry>
#include <algorithm>
#include <chrono>
#include <execution>

#ifdef ENABLE_OPENMP
#include <omp.h>
#endif

void DRMSimulationModel::runUnitTests()
{
    const std::filesystem::path groundOfTruthFolder{
        "/home/iason/Coding/Libraries/MySources/formFinder_unitTestFiles"};

    DRMSimulationModel formFinder;

    // First example of the paper
    VCGEdgeMesh beam;
    //  const size_t spanGridSize = 11;
    //  mesh.createSpanGrid(spanGridSize);
    beam.load(std::filesystem::path(groundOfTruthFolder).append("simpleBeam.ply").string());
    std::unordered_map<VertexIndex, std::unordered_set<DoFType>> fixedVertices;
    fixedVertices[0] = std::unordered_set<DoFType>{0, 1, 2};
    fixedVertices[beam.VN() - 1] = std::unordered_set<DoFType>{1, 2};
    std::unordered_map<VertexIndex, Vector6d> nodalForces{
        {beam.VN() / 2, Vector6d({0, 1000, 1000, 0, 0, 0})}};
    // Forced displacements
    std::unordered_map<size_t, Eigen::Vector3d> nodalForcedDisplacements;
    nodalForcedDisplacements.insert({beam.VN() - 1, Eigen::Vector3d(-0.2, 0, 0)});

    SimulationJob beamSimulationJob{std::make_shared<SimulationMesh>(beam),
                                    "First paper example",
                                    //      SimulationJob::constructFixedVerticesSpanGrid(spanGridSize,
                                    //      mesh.VN()),
                                    fixedVertices,
                                    nodalForces,
                                    nodalForcedDisplacements};
    beamSimulationJob.pMesh->setBeamMaterial(0.3, 200 * 1e9);
    assert(CrossSectionType::name == CylindricalBeamDimensions::name);

    beamSimulationJob.pMesh->setBeamCrossSection(CrossSectionType{0.03, 0.026});
    Settings settings;
    settings.Dtini = 0.1;
    settings.xi = 0.9969;
    settings.maxDRMIterations = 0;
    formFinder.mSettings.totalResidualForcesNormThreshold = 1;
    settings.shouldDraw = false;
    settings.beVerbose = true;
    settings.shouldCreatePlots = true;
    SimulationResults simpleBeam_simulationResults
        = formFinder.executeSimulation(std::make_shared<SimulationJob>(beamSimulationJob), settings);
    simpleBeam_simulationResults.save();
    const std::string simpleBeamGroundOfTruthBinaryFilename
        = std::filesystem::path(groundOfTruthFolder)
              .append("SimpleBeam_displacements.eigenBin")
              .string();
    assert(std::filesystem::exists(std::filesystem::path(simpleBeamGroundOfTruthBinaryFilename)));
    Eigen::MatrixXd simpleBeam_groundOfTruthDisplacements;
    Eigen::read_binary(simpleBeamGroundOfTruthBinaryFilename, simpleBeam_groundOfTruthDisplacements);

    double error;
    if (!simpleBeam_simulationResults.isEqual(simpleBeam_groundOfTruthDisplacements, error)) {
        std::cerr << "Error:Beam simulation produces wrong results. Error:" << error << std::endl;
        //    return;
    }
#ifdef POLYSCOPE_DEFINED
    beam.registerForDrawing();
    simpleBeam_simulationResults.registerForDrawing();
    polyscope::show();
    beam.unregister();
    simpleBeam_simulationResults.unregister();
#endif

    // Second example of the paper
    VCGEdgeMesh shortSpanGrid;
    //  const size_t spanGridSize = 11;
    //  mesh.createSpanGrid(spanGridSize);
    shortSpanGrid.load(
        std::filesystem::path(groundOfTruthFolder).append("shortSpanGridshell.ply").string());

    fixedVertices.clear();
    //// Corner nodes
    fixedVertices[0] = std::unordered_set<DoFType>{2};
    fixedVertices[4] = std::unordered_set<DoFType>{2};
    fixedVertices[16] = std::unordered_set<DoFType>{2};
    fixedVertices[20] = std::unordered_set<DoFType>{2};
    //// Center node
    fixedVertices[10] = std::unordered_set<DoFType>{0, 1, 3, 4, 5};

    nodalForcedDisplacements.clear();
    nodalForcedDisplacements.insert({{0, Eigen::Vector3d(0.1, 0.1, 0)},
                                     {4, Eigen::Vector3d(-0.1, 0.1, 0)},
                                     {16, Eigen::Vector3d(0.1, -0.1, 0)},
                                     {20, Eigen::Vector3d(-0.1, -0.1, 0)}});

    SimulationJob shortSpanGridshellSimulationJob{std::make_shared<SimulationMesh>(shortSpanGrid),
                                                  "Short span gridshell",
                                                  fixedVertices,
                                                  {},
                                                  nodalForcedDisplacements};
    shortSpanGridshellSimulationJob.pMesh->setBeamMaterial(0.3, 200 * 1e9);
    assert(typeid(CrossSectionType) == typeid(CylindricalBeamDimensions));
    shortSpanGridshellSimulationJob.pMesh->setBeamCrossSection(CrossSectionType{0.03, 0.026});
    SimulationResults shortSpanGridshellSimulationResults
        = formFinder.executeSimulation(std::make_shared<SimulationJob>(
                                           shortSpanGridshellSimulationJob),
                                       settings);
    shortSpanGridshellSimulationResults.save();

    const std::string groundOfTruthBinaryFilename
        = std::filesystem::path(groundOfTruthFolder)
              .append("ShortSpanGridshell_displacements.eigenBin")
              .string();
    assert(std::filesystem::exists(std::filesystem::path(groundOfTruthBinaryFilename)));
    Eigen::MatrixXd shortSpanGridshell_groundOfTruthDisplacements;
    Eigen::read_binary(groundOfTruthBinaryFilename, shortSpanGridshell_groundOfTruthDisplacements);
    //  shortSpanGridshellSimulationResults.registerForDrawing(
    //      shortSpanGridshellSimulationJob);
    //  polyscope::show();
    assert(shortSpanGridshellSimulationResults.isEqual(shortSpanGridshell_groundOfTruthDisplacements,
                                                       error));
    if (!shortSpanGridshellSimulationResults.isEqual(shortSpanGridshell_groundOfTruthDisplacements,
                                                     error)) {
        std::cerr << "Error:Short span simulation produces wrong results. Error:" << error
                  << std::endl;
        //        return;
    }
#ifdef POLYSCOPE_DEFINED
    shortSpanGrid.registerForDrawing();
    shortSpanGridshellSimulationResults.registerForDrawing();
    polyscope::show();
    shortSpanGrid.unregister();
    shortSpanGridshellSimulationResults.unregister();
#endif

    // Third example of the paper
    VCGEdgeMesh longSpanGrid;
    longSpanGrid.load(
        std::filesystem::path(groundOfTruthFolder).append("longSpanGridshell.ply").string());
    const size_t spanGridSize = 11;

    fixedVertices.clear();
    for (size_t vi = 0; vi < spanGridSize - 1; vi++) {
        fixedVertices[vi] = {0, 2};
    }
    for (size_t vi = longSpanGrid.VN() - 1 - (spanGridSize - 2); vi < longSpanGrid.VN(); vi++) {
        fixedVertices[vi] = {0, 2};
    }
    for (size_t vi = spanGridSize - 1;
         vi < longSpanGrid.VN() - 1 - (spanGridSize - 2) - spanGridSize + 1;
         vi += spanGridSize + 1) {
        fixedVertices[vi] = {1, 2};
        fixedVertices[vi + spanGridSize] = {1, 2};
    }

    nodalForcedDisplacements.clear();
    const size_t horizontalOffset = std::floor((spanGridSize - 2) / 2);
    nodalForcedDisplacements.insert(
        {{horizontalOffset, Eigen::Vector3d(0, 0.3, 0)},
         {horizontalOffset + 1, Eigen::Vector3d(0, 0.3, 0)},
         {spanGridSize * (spanGridSize + 1) - 2 + horizontalOffset, Eigen::Vector3d(0, -0.3, 0)},
         {spanGridSize * (spanGridSize + 1) - 2 + horizontalOffset + 1, Eigen::Vector3d(0, -0.3, 0)},
         {std::floor(spanGridSize / 2) * (spanGridSize + 1) - 2, Eigen::Vector3d(0.3, 0, 0)},
         {(std::floor(spanGridSize / 2) + 1) * (spanGridSize + 1) - 2, Eigen::Vector3d(0.3, 0, 0)},
         {std::floor(spanGridSize / 2) * (spanGridSize + 1) - 2 + spanGridSize,
          Eigen::Vector3d(-0.3, 0, 0)},
         {(std::floor(spanGridSize / 2) + 1) * (spanGridSize + 1) - 2 + spanGridSize,
          Eigen::Vector3d(-0.3, 0, 0)}});

    SimulationJob longSpanGridshell_simulationJob{std::make_shared<SimulationMesh>(longSpanGrid),
                                                  "long span gridshell",
                                                  fixedVertices,
                                                  {},
                                                  nodalForcedDisplacements};
    longSpanGridshell_simulationJob.pMesh->setBeamMaterial(0.3, 200 * 1e9);
    if (typeid(CrossSectionType) != typeid(CylindricalBeamDimensions)) {
        std::cerr << "A cylindrical cross section is expected for running the "
                     "paper examples."
                  << std::endl;
    }
    longSpanGridshell_simulationJob.pMesh->setBeamCrossSection(CrossSectionType{0.03, 0.026});

    SimulationResults longSpanGridshell_simulationResults
        = formFinder.executeSimulation(std::make_shared<SimulationJob>(
                                           longSpanGridshell_simulationJob),
                                       settings);
    longSpanGridshell_simulationResults.save();

    const std::string longSpan_groundOfTruthBinaryFilename
        = std::filesystem::path(groundOfTruthFolder)
              .append("LongSpanGridshell_displacements.eigenBin")
              .string();
    assert(std::filesystem::exists(std::filesystem::path(longSpan_groundOfTruthBinaryFilename)));
    Eigen::MatrixXd longSpanGridshell_groundOfTruthDisplacements;
    Eigen::read_binary(longSpan_groundOfTruthBinaryFilename,
                       longSpanGridshell_groundOfTruthDisplacements);
    assert(longSpanGridshell_simulationResults.isEqual(longSpanGridshell_groundOfTruthDisplacements,
                                                       error));
    if (!longSpanGridshell_simulationResults.isEqual(longSpanGridshell_groundOfTruthDisplacements,
                                                     error)) {
        std::cerr << "Error:Long span simulation produces wrong results. Error:" << error
                  << std::endl;
        //        return;
    }
#ifdef POLYSCOPE_DEFINED
    longSpanGrid.registerForDrawing();
    longSpanGridshell_simulationResults.registerForDrawing();
    polyscope::show();
    longSpanGrid.unregister();
    longSpanGridshell_simulationResults.unregister();
#endif

    std::cout << "Form finder unit tests succesufully passed." << std::endl;

    //  polyscope::show();
}

void DRMSimulationModel::reset()
{
    mCurrentSimulationStep = 0;
    history.clear();
    constrainedVertices.clear();
    rigidSupports.clear();
    pMesh.reset();
    plotYValues.clear();
    plotHandle.reset();
    checkedForMaximumMoment = false;
    externalMomentsNorm = 0;
    Dt = mSettings.Dtini;
    numOfDampings = 0;
    shouldTemporarilyDampForces = false;
}

VectorType DRMSimulationModel::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 DRMSimulationModel::computeDerivativeOfNormal(const VertexType &v,
                                                         const DifferentiateWithRespectTo &dui) const
{
    const size_t vi = pMesh->getIndex(v);
    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 + 1e-5 >= 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 + 1e-5 >= 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);
    const bool shouldBreak = mCurrentSimulationStep == 118 && vi == 1 && dui.dofi == 3;

    return normalDerivative;
}

double DRMSimulationModel::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 = pMesh->elements[e].length;
    const double L_kDeriv = positionVectorDiff * displacementDiffDeriv / edgeLength;
    return L_kDeriv;
}

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

VectorType DRMSimulationModel::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 DRMSimulationModel::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
                                                ? pMesh->nodes[v_j].derivativeOfNormal[dui.dofi]
                                                : VectorType(0, 0, 0);
    const VectorType derivativeOfNormal_jplus1
        = &v_jplus1 == &dui.v && dui.dofi > 2 ? pMesh->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();
    assert(normOfSumOfNormals != 0);
    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 DRMSimulationModel::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 = pMesh->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 DRMSimulationModel::computeDerivativeT2(const EdgeType &e,
                                                   const DifferentiateWithRespectTo &dui) const
{
    const DoFType dofi = dui.dofi;

    const VertexType &v_j = *e.cV(0);
    const size_t vi_j = pMesh->getIndex(v_j);
    const VertexType &v_jplus1 = *e.cV(1);
    const size_t vi_jplus1 = pMesh->getIndex(v_jplus1);

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

    const VectorType &t1 = pMesh->elements[e].frame.t1;
    const VectorType derivativeT1_j = dofi < 3 && &v_j == &dui.v
                                          ? pMesh->elements[e].derivativeT1_j[dui.dofi]
                                          : VectorType(0, 0, 0);
    const VectorType derivativeT1_jplus1 = dofi < 3 && &v_jplus1 == &dui.v
                                               ? pMesh->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));
    const bool shouldBreak = mCurrentSimulationStep == 118 && (vi_jplus1 == 1 || vi_j == 1)
                             && dofi == 3;
    return t2Deriv;
}

VectorType DRMSimulationModel::computeDerivativeT3(const EdgeType &e,
                                                   const DifferentiateWithRespectTo &dui) const
{
    const Element &element = pMesh->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 size_t vi_j = pMesh->getIndex(v_j);
    const VertexType &v_jplus1 = *e.cV(1);
    const size_t vi_jplus1 = pMesh->getIndex(v_jplus1);
    const VectorType derivativeT1_j = dui.dofi < 3 && &v_j == &dui.v
                                          ? pMesh->elements[e].derivativeT1_j[dui.dofi]
                                          : VectorType(0, 0, 0);
    const VectorType derivativeT1_jplus1 = dui.dofi < 3 && &v_jplus1 == &dui.v
                                               ? pMesh->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 = pMesh->elements[e].derivativeT2_j[dui.dofi];
    } else if (&v_jplus1 == &dui.v) {
        derivativeT2 = pMesh->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 DRMSimulationModel::computeDerivativeTheta1(const EdgeType &e,
                                                   const VertexIndex &evi,
                                                   const VertexIndex &dwrt_evi,
                                                   const DoFType &dwrt_dofi) const
{
    const VertexType &v = *e.cV(evi);
    const size_t vi = pMesh->getIndex(v);
    const Element &element = pMesh->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)
                                                   : pMesh->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));
    const bool shouldBreak = mCurrentSimulationStep == 118 && vi == 1 && dwrt_dofi == 3;

    return theta1Derivative;
}

double DRMSimulationModel::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 = pMesh->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 ? pMesh->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 DRMSimulationModel::computeTheta3(const EdgeType &e, const VertexType &v)
{
    const VertexIndex &vi = pMesh->nodes[v].vi;
    //  assert(e.cV(0) == &v || e.cV(1) == &v);

    const Element &elem = pMesh->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 = pMesh->nodes[v];
    const double &nR = node.nR; // TODO: This makes the function non-const.
                                // Should be refactored in the future

    double theta3;
    const bool shouldBreak = mCurrentSimulationStep == 12970;
    if (&e == node.referenceElement) {
        // Use nR as theta3 only for the first star edge
        return nR;
    }
    std::vector<int> incidentElementsIndices(node.incidentElements.size());
    for (int iei = 0; iei < incidentElementsIndices.size(); iei++) {
        incidentElementsIndices[iei] = pMesh->getIndex(node.incidentElements[iei]);
    }
    assert(pMesh->getIndex(e) == ei);
    assert(node.alphaAngles.contains(ei));
    const double alphaAngle = node.alphaAngles.at(elem.ei);
    const EdgeType &refElem = *node.referenceElement;
    const double rotationAngle = nR + alphaAngle;

    //    const VectorType &t1_k = computeT1Vector(refElem);
    const VectorType &t1_k = pMesh->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 = pMesh->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 DRMSimulationModel::computeDerivativeTheta3(const EdgeType &e,
                                                   const VertexType &v,
                                                   const DifferentiateWithRespectTo &dui) const
{
    const Node &node = pMesh->nodes[v];
    const VertexIndex &vi = pMesh->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 = pMesh->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(pMesh->nodes[vp_j].initialLocation,
                                                      pMesh->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 = pMesh->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
                                                ? pMesh->elements[refElem].derivativeT1_j[dui.dofi]
                                                : VectorType(0, 0, 0);
        const VectorType derivativeT1_k_jplus1
            = refElem.cV(1) == &dui.v ? pMesh->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 DRMSimulationModel::computeTotalInternalPotentialEnergy()
{
    double totalInternalPotentialEnergy = 0;
    for (const SimulationMesh::EdgeType &e : pMesh->edge) {
        const Element &element = pMesh->elements[e];
        const SimulationMesh::VertexType &ev_j = *e.cV(0);
        const SimulationMesh::VertexType &ev_jplus1 = *e.cV(1);
        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);

        const EdgeIndex ei = pMesh->getIndex(e);
        const double e_k = element.length - element.initialLength;
        const double axialPE = pow(e_k, 2) * element.material.youngsModulus * element.A
                               / (2 * element.initialLength);
        const double theta1Diff = theta1_jplus1 - theta1_j;
        const double torsionalPE = element.G * element.J * pow(theta1Diff, 2)
                                   / (2 * element.initialLength);
        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.material.youngsModulus
                                      * element.I2 / (2 * element.initialLength);
        const double secondBendingPE_inBracketsTerm = 4 * pow(theta3_j, 2)
                                                      + 4 * theta3_j * theta3_jplus1
                                                      + 4 * pow(theta3_jplus1, 2);
        const double secondBendingPE = secondBendingPE_inBracketsTerm
                                       * element.material.youngsModulus * element.I3
                                       / (2 * element.initialLength);

        totalInternalPotentialEnergy += axialPE + torsionalPE + firstBendingPE + secondBendingPE;
        int i = 0;
        i++;
    }

    return totalInternalPotentialEnergy;
}

double DRMSimulationModel::computeTotalPotentialEnergy()
{
    double totalExternalPotentialEnergy = 0;
    for (const SimulationMesh::VertexType &v : pMesh->vert) {
        const Node &node = pMesh->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);
    }

    const double totalInternalPotentialEnergy = computeTotalInternalPotentialEnergy();

    return totalInternalPotentialEnergy - totalExternalPotentialEnergy;
}

void DRMSimulationModel::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(
        pMesh->EN(), std::vector<std::pair<int, Vector6d>>(4, {-1, Vector6d()}));
    //  omp_lock_t writelock;
    //  omp_init_lock(&writelock);
#ifdef ENABLE_OPENMP
#pragma omp parallel for schedule(static) num_threads(6)
#endif
    for (int ei = 0; ei < pMesh->EN(); ei++) {
        const EdgeType &e = pMesh->edge[ei];
        const SimulationMesh::VertexType &ev_j = *e.cV(0);
        const SimulationMesh::VertexType &ev_jplus1 = *e.cV(1);
        const Element &element = pMesh->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 bool shouldBreak = mCurrentSimulationStep == 12970;
        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 = pMesh->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 = pMesh->nodes[refElemOtherVertex];
            if (edgeNode.referenceElement != &e) {
                internalForcesContributionFromThisEdge[evi + 2].first = refElemOtherVertexNode.vi;
            }
            const size_t vi = edgeNode.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.rigidity.axial;

                    // 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.rigidity.torsional;

                    // 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.rigidity.firstBending;

                    // 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.rigidity.secondBending;
                    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.rigidity.secondBending;
                        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 < pMesh->VN(); vi++) {
        Node::Forces &force = pMesh->nodes[vi].force;
        //        Vector6d ext = force.external;
        //        if (mCurrentSimulationStep <= 100) {
        //        ext[3] = 0;
        //        ext[4] = 0;
        //        }
        force.residual = force.external;
        force.internal = 0;
    }
    double totalResidualForcesNorm = 0;
    for (size_t ei = 0; ei < pMesh->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 = pMesh->nodes[vi].force;
            force.internal = force.internal + internalForcePair.second;
            force.residual = force.residual + (internalForcePair.second * -1);
        }
    }
    Vector6d sumOfResidualForces(0);
    for (size_t vi = 0; vi < pMesh->VN(); vi++) {
        Node::Forces &force = pMesh->nodes[vi].force;
        const Vector6d &nodeResidualForce = force.residual;
        sumOfResidualForces = sumOfResidualForces + nodeResidualForce;
        //        const double residualForceNorm = nodeResidualForce.norm();
        const double residualForceNorm = nodeResidualForce.getTranslation().norm();
        const bool shouldTrigger = std::isnan(residualForceNorm);
        totalResidualForcesNorm += residualForceNorm;
    }
    pMesh->previousTotalResidualForcesNorm = pMesh->totalResidualForcesNorm;
    pMesh->totalResidualForcesNorm = totalResidualForcesNorm;
    //  mesh->totalResidualForcesNorm = sumOfResidualForces.norm();

    //  plotYValues.push_back(totalResidualForcesNorm);
    //  auto xPlot = matplot::linspace(0, plotYValues.size(), plotYValues.size());
    //  plotHandle = matplot::scatter(xPlot, plotYValues);
}

void DRMSimulationModel::updateNodalExternalForces(
    const std::unordered_map<VertexIndex, Vector6d> &nodalForces,
    const std::unordered_map<VertexIndex, std::unordered_set<DoFType>> &fixedVertices)
{
    externalMomentsNorm = 0;
    for (const std::pair<VertexIndex, Vector6d> &nodalForce : nodalForces) {
        const VertexIndex nodeIndex = nodalForce.first;
        const bool isNodeConstrained = fixedVertices.contains(nodeIndex);
        Node &node = pMesh->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];
        }
        externalMomentsNorm += std::sqrt(pow(nodalExternalForce[3], 2)
                                         + pow(nodalExternalForce[4], 2)
                                         + pow(nodalExternalForce[5], 2));

        /*
     * The external moments are given as a rotation around an axis.
     * In this implementation we model moments as rotation of the normal vector
     * and because of that we need to transform the moments.
     */

        if (externalMomentsNorm != 0) {
            VectorType momentAxis(nodalExternalForce[3],
                                  nodalExternalForce[4],
                                  nodalExternalForce[5]); // rotation around this vector
            VectorType transformedVector = vcg::RotationMatrix(VectorType(0, 0, 1),
                                                               vcg::math::ToRad(-90.0))
                                           * momentAxis;
            nodalExternalForce[3] = transformedVector[0];
            nodalExternalForce[4] = transformedVector[1];
            nodalExternalForce[5] = transformedVector[2];
            //            node.nR = transformedVector[2];
        }

        node.force.external = nodalExternalForce;
    }
}

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

void DRMSimulationModel::computeRigidSupports()
{
    for (const VertexType &v : pMesh->vert) {
        const VertexIndex vi = pMesh->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 DRMSimulationModel::updateNormalDerivatives()
{
    for (const VertexType &v : pMesh->vert) {
        for (DoFType dofi = DoF::Nx; dofi < DoF::NumDoF; dofi++) {
            DifferentiateWithRespectTo dui{v, dofi};
            pMesh->nodes[v].derivativeOfNormal[dofi] = computeDerivativeOfNormal(v, dui);
        }
    }
}

void DRMSimulationModel::updateT1Derivatives()
{
    for (const EdgeType &e : pMesh->edge) {
        for (DoFType dofi = DoF::Ux; dofi < DoF::Nx; dofi++) {
            DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi};
            Element &element = pMesh->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 DRMSimulationModel::updateT2Derivatives()
{
    for (const EdgeType &e : pMesh->edge) {
        for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) {
            DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi};
            pMesh->elements[e].derivativeT2_j[dofi] = computeDerivativeT2(e, dui_v0);
            DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi};
            pMesh->elements[e].derivativeT2_jplus1[dofi] = computeDerivativeT2(e, dui_v1);

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

void DRMSimulationModel::updateT3Derivatives()
{
    for (const EdgeType &e : pMesh->edge) {
        for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) {
            DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi};
            Element &element = pMesh->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 DRMSimulationModel::updateRDerivatives()
{
    for (const EdgeType &e : pMesh->edge) {
        for (DoFType dofi = DoF::Nx; dofi < DoF::NumDoF; dofi++) {
            DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi};
            pMesh->elements[e].derivativeR_j[dofi] = computeDerivativeOfR(e, dui_v0);
            DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi};
            pMesh->elements[e].derivativeR_jplus1[dofi] = computeDerivativeOfR(e, dui_v1);
        }
    }
}

void DRMSimulationModel::updateElementalLengths()
{
    pMesh->updateElementalLengths();
}

DRMSimulationModel::DRMSimulationModel() {}

void DRMSimulationModel::updateNodalMasses()
{
    double gamma = mSettings.gamma;
    const size_t untilStep = 500;
    if (shouldTemporarilyDampForces && mCurrentSimulationStep < untilStep) {
        gamma *= 1e6 * (1 - static_cast<double>(mCurrentSimulationStep) / untilStep);
    }
    if (mCurrentSimulationStep == untilStep && shouldTemporarilyDampForces) {
        Dt = mSettings.Dtini * 0.95;
    }
    for (VertexType &v : pMesh->vert) {
        const size_t vi = pMesh->getIndex(v);
        double translationalSumSk = 0;
        double rotationalSumSk_I2 = 0;
        double rotationalSumSk_I3 = 0;
        double rotationalSumSk_J = 0;
        for (const EdgePointer &ep : pMesh->nodes[v].incidentElements) {
            const size_t ei = pMesh->getIndex(ep);
            const Element &elem = pMesh->elements[ep];
            const double SkTranslational = elem.material.youngsModulus * elem.A / elem.length;
            translationalSumSk += SkTranslational;
            const double lengthToThe3 = std::pow(elem.length, 3);
            const long double SkRotational_I2 = elem.material.youngsModulus * elem.I2
                                                / lengthToThe3;
            const long double SkRotational_I3 = elem.material.youngsModulus * elem.I3
                                                / lengthToThe3;
            const long double SkRotational_J = elem.material.youngsModulus * elem.J / lengthToThe3;
            rotationalSumSk_I2 += SkRotational_I2;
            rotationalSumSk_I3 += SkRotational_I3;
            rotationalSumSk_J += SkRotational_J;
            assert(rotationalSumSk_I2 != 0);
            assert(rotationalSumSk_I3 != 0);
            assert(rotationalSumSk_J != 0);
        }
        pMesh->nodes[v].mass.translational = gamma * pow(mSettings.Dtini, 2) * 2
                                             * translationalSumSk;
        pMesh->nodes[v].mass.rotationalI2 = gamma * pow(mSettings.Dtini, 2) * 8
                                            * rotationalSumSk_I2;
        pMesh->nodes[v].mass.rotationalI3 = gamma * pow(mSettings.Dtini, 2) * 8
                                            * rotationalSumSk_I3;
        pMesh->nodes[v].mass.rotationalJ = gamma * pow(mSettings.Dtini, 2) * 8 * rotationalSumSk_J;

        assert(std::pow(mSettings.Dtini, 2.0) * translationalSumSk
                   / pMesh->nodes[v].mass.translational
               < 2);
        assert(std::pow(mSettings.Dtini, 2.0) * rotationalSumSk_I2
                   / pMesh->nodes[v].mass.rotationalI2
               < 2);
        assert(std::pow(mSettings.Dtini, 2.0) * rotationalSumSk_I3
                   / pMesh->nodes[v].mass.rotationalI3
               < 2);
        assert(std::pow(mSettings.Dtini, 2.0) * rotationalSumSk_J / pMesh->nodes[v].mass.rotationalJ
               < 2);
    }
}

void DRMSimulationModel::updateNodalAccelerations()
{
    for (VertexType &v : pMesh->vert) {
        Node &node = pMesh->nodes[v];
        const VertexIndex vi = pMesh->getIndex(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.mass.translational;
            } else if (dofi == DoF::Nx) {
                node.acceleration[dofi] = node.force.residual[dofi] / node.mass.rotationalJ;
            } else if (dofi == DoF::Ny) {
                node.acceleration[dofi] = node.force.residual[dofi] / node.mass.rotationalI3;
            } else if (dofi == DoF::Nr) {
                node.acceleration[dofi] = node.force.residual[dofi] / node.mass.rotationalI2;
            }
        }
        //        if (vi == 10) {
        //            std::cout << "Acceleration:" << node.acceleration[0] << " " << node.acceleration[1]
        //                      << " " << node.acceleration[2] << std::endl;
        //        }

        //        if (shouldTemporarilyDampForces && mCurrentSimulationStep < 700) {
        //            node.acceleration = node.acceleration * 1e-2;
        //        }
    }
}

void DRMSimulationModel::updateNodalVelocities()
{
    for (VertexType &v : pMesh->vert) {
        const VertexIndex vi = pMesh->getIndex(v);
        Node &node = pMesh->nodes[v];
        node.velocity = node.velocity + node.acceleration * Dt;
        //        if (vi == 10) {
        //            std::cout << "Velocity:" << node.velocity[0] << " " << node.velocity[1] << " "
        //                      << node.velocity[2] << std::endl;
        //        }
    }
    updateKineticEnergy();
}

void DRMSimulationModel::updateNodalDisplacements()
{
    const bool shouldCapDisplacements = mSettings.displacementCap.has_value();
    for (VertexType &v : pMesh->vert) {
        Node &node = pMesh->nodes[v];
        Vector6d stepDisplacement = node.velocity * Dt;
        if (shouldCapDisplacements
            && stepDisplacement.getTranslation().norm() > mSettings.displacementCap) {
            stepDisplacement = stepDisplacement
                               * (*mSettings.displacementCap
                                  / stepDisplacement.getTranslation().norm());
            std::cout << "Displacement capped" << std::endl;
        }
        node.displacements = node.displacements + stepDisplacement;
        //        if (mSettings.isDebugMode && mSettings.beVerbose && pMesh->getIndex(v) == 40) {
        //            std::cout << "Node " << node.vi << ":" << endl;
        //            std::cout << node.displacements[0] << " " << node.displacements[0] << " "
        //                      << node.displacements[1] << " " << node.displacements[2] << " "
        //                      << node.displacements[3] << " " << node.displacements[4] << " "
        //                      << node.displacements[5] << std::endl;
        //        }
    }
}

void DRMSimulationModel::updateNodePosition(
    VertexType &v, const std::unordered_map<VertexIndex, std::unordered_set<DoFType>> &fixedVertices)
{
    Node &node = pMesh->nodes[v];
    const VertexIndex &vi = pMesh->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]);
    }
    v.P() = node.initialLocation + displacementVector;
    if (shouldApplyInitialDistortion && mCurrentSimulationStep < 100) {
        //TODO:The initial displacement should depend on the model and should only be applied if the forced displacements applied are out of plane
        VectorType desiredInitialDisplacement(0.0015, 0.0015, 0.0015);
        v.P() = v.P() + desiredInitialDisplacement;
    }
}

void DRMSimulationModel::updateNodeNr(VertexType &v)
{
    const VertexIndex &vi = pMesh->nodes[v].vi;
    Node &node = pMesh->nodes[v];
    if (!rigidSupports.contains(vi)) {
        node.nR = node.displacements[5];
    } else {
        const EdgePointer &refElem = node.referenceElement;
        const VectorType &refT1 = pMesh->elements[refElem].frame.t1;

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

void DRMSimulationModel::updateNodeNormal(
    VertexType &v, const std::unordered_map<VertexIndex, std::unordered_set<DoFType>> &fixedVertices)
{
    Node &node = pMesh->nodes[v];
    const VertexIndex &vi = node.vi;
    //  if (vi == 1) {
    //    std::cout << "PRE:" << mesh->vert[1].N()[0] << " " <<
    //    mesh->vert[1].N()[1]
    //              << " " << mesh->vert[1].N()[2] << std::endl;
    //  }
    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);
    }
    const double nxnyMagnitudePre = std::pow(v.N()[0], 2) + std::pow(v.N()[1], 2);
    v.N() = node.initialNormal + normalDisplacementVector;
    const double &nx = v.N()[0], ny = v.N()[1], nz = v.N()[2];
    const double nxnyMagnitude = std::pow(nx, 2) + std::pow(ny, 2);
    VectorType n = v.N();
    const bool shouldBreak = mCurrentSimulationStep == 118 && vi == 3;
    if (nxnyMagnitude > 1) {
        VectorType newNormal(nx / std::sqrt(nxnyMagnitude), ny / std::sqrt(nxnyMagnitude), 0);
        v.N() = newNormal;

        /*If an external moment caused the normal to lay on the xy plane this means
     * that in order to disable its effect a greater internal force is needed
     * than what is possible (the constraint on the z of the normals imposes a
     * constraint on the maximum internal force). Because of that the
     * totalResidualForcesNorm can't drop below the magnitude of external moment
     * applied on vertex vi. In order to allow termination of the simulation
     * when the described phenomenon happens we allow the termination of the
     * algorithm if the kinetic energy of the system drops below the set
     * threshold.
     * */
        const bool viHasMoments = node.force.external[3] != 0 || node.force.external[4] != 0;
        if (!checkedForMaximumMoment && viHasMoments) {
            mSettings.shouldUseTranslationalKineticEnergyThreshold = true;
#ifdef POLYSCOPE_DEFINED
            std::cout << "Maximum moment reached.The Kinetic energy of the system will "
                         "be used as a convergence criterion"
                      << std::endl;
#endif
            checkedForMaximumMoment = true;
        }

    } else {
        const double nzSquared = 1.0 - nxnyMagnitude;
        const double nz = std::sqrt(nzSquared);
        VectorType newNormal(nx, ny, nz);
        v.N() = newNormal;
    }

    //    if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(DoF::Nr)) {
    //  if (vi == 1) {
    //    std::cout << "AFTER:" << mesh->vert[1].N()[0] << " " <<
    //    mesh->vert[1].N()[1]
    //              << " " << mesh->vert[1].N()[2] << std::endl;
    //  }
}

void DRMSimulationModel::applyDisplacements(
    const std::unordered_map<VertexIndex, std::unordered_set<DoFType>> &fixedVertices)
{
    for (VertexType &v : pMesh->vert) {
        updateNodePosition(v, fixedVertices);
        updateNodeNormal(v, fixedVertices);
        updateNodeNr(v);
    }
    updateElementalFrames();
    if (mSettings.shouldDraw) {
        pMesh->updateEigenEdgeAndVertices();
    }
}

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

void DRMSimulationModel::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 = pMesh->nodes[vi];
        VectorType displacementVector(vertexDisplacement(0),
                                      vertexDisplacement(1),
                                      vertexDisplacement(2));
        if (mCurrentSimulationStep < mSettings.gradualForcedDisplacementSteps) {
            displacementVector *= mCurrentSimulationStep
                                  / static_cast<double>(mSettings.gradualForcedDisplacementSteps);
        }
        pMesh->vert[vi].P() = node.initialLocation + displacementVector;
        node.displacements = Vector6d({displacementVector[0],
                                       displacementVector[1],
                                       displacementVector[2],
                                       node.displacements[3],
                                       node.displacements[4],
                                       node.displacements[5]});
    }

    if (mSettings.shouldDraw) {
        pMesh->updateEigenEdgeAndVertices();
    }
}

void DRMSimulationModel::applyForcedNormals(
    const std::unordered_map<VertexIndex, VectorType> nodalForcedRotations)
{
    for (const std::pair<VertexIndex, VectorType> vertexIndexDesiredNormalPair :
         nodalForcedRotations) {
        const VertexIndex vi = vertexIndexDesiredNormalPair.first;

        Node &node = pMesh->nodes[vi];
        pMesh->vert[vi].N() = vertexIndexDesiredNormalPair.second;
        node.displacements = Vector6d(
            {node.displacements[0],
             node.displacements[1],
             node.displacements[2],
             vertexIndexDesiredNormalPair.second[0] - node.initialNormal[0],
             vertexIndexDesiredNormalPair.second[1] - node.initialNormal[1],
             node.displacements[5]});
    }
}

// NOTE: Is this correct? Should the kinetic energy be computed like that?
void DRMSimulationModel::updateKineticEnergy()
{
    pMesh->previousTotalKineticEnergy = pMesh->currentTotalKineticEnergy;
    pMesh->currentTotalKineticEnergy = 0;
    pMesh->currentTotalTranslationalKineticEnergy = 0;
    for (const VertexType &v : pMesh->vert) {
        Node &node = pMesh->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));
        const double nodeTranslationalKineticEnergy = 0.5 * node.mass.translational
                                                      * pow(translationalVelocityNorm, 2);

        const double nodeRotationalKineticEnergy
            = 0.5
              * (node.mass.rotationalJ * pow(node.velocity[3], 2)
                 + node.mass.rotationalI3 * pow(node.velocity[4], 2)
                 + node.mass.rotationalI2 * pow(node.velocity[5], 2));

        node.kineticEnergy += nodeTranslationalKineticEnergy /*+ nodeRotationalKineticEnergy*/;
        assert(node.kineticEnergy < 1e15);

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

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

void DRMSimulationModel::updatePositionsOnTheFly(
    const std::unordered_map<VertexIndex, std::unordered_set<DoFType>> &fixedVertices)
{
    const double gamma = 0.8;
    for (VertexType &v : pMesh->vert) {
        double translationalSumSk = 0;
        double rotationalSumSk_I2 = 0;
        double rotationalSumSk_I3 = 0;
        double rotationalSumSk_J = 0;
        for (const EdgePointer &ep : pMesh->nodes[v].incidentElements) {
            const Element &elem = pMesh->elements[ep];
            const double SkTranslational = elem.material.youngsModulus * elem.A / elem.length;
            translationalSumSk += SkTranslational;
            const double lengthToThe3 = std::pow(elem.length, 3);
            const double SkRotational_I2 = elem.material.youngsModulus * elem.I2
                                           / lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia
            const double SkRotational_I3 = elem.material.youngsModulus * elem.I3
                                           / lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia
            const double SkRotational_J = elem.material.youngsModulus * elem.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);
        }
        pMesh->nodes[v].mass.translational = gamma * pow(mSettings.Dtini, 2) * 2
                                             * translationalSumSk;
        pMesh->nodes[v].mass.rotationalI2 = gamma * pow(mSettings.Dtini, 2) * 8
                                            * rotationalSumSk_I2;
        pMesh->nodes[v].mass.rotationalI3 = gamma * pow(mSettings.Dtini, 2) * 8
                                            * rotationalSumSk_I3;
        pMesh->nodes[v].mass.rotationalJ = gamma * pow(mSettings.Dtini, 2) * 8 * rotationalSumSk_J;

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

    for (VertexType &v : pMesh->vert) {
        Node &node = pMesh->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.mass.translational;
            } else if (dofi == DoF::Nx) {
                node.acceleration[dofi] = node.force.residual[dofi] / node.mass.rotationalJ;
            } else if (dofi == DoF::Ny) {
                node.acceleration[dofi] = node.force.residual[dofi] / node.mass.rotationalI3;
            } else if (dofi == DoF::Nr) {
                node.acceleration[dofi] = node.force.residual[dofi] / node.mass.rotationalI2;
            }
        }
    }

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

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

    for (VertexType &v : pMesh->vert) {
        updateNodePosition(v, fixedVertices);
        Node &node = pMesh->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 DRMSimulationModel::computeResults(const std::shared_ptr<SimulationJob> &pJob)
{
    std::vector<Vector6d> displacements(pMesh->VN());
    for (size_t vi = 0; vi < pMesh->VN(); vi++) {
        displacements[vi] = pMesh->nodes[vi].displacements;
    }
    history.numberOfSteps = mCurrentSimulationStep;
    SimulationResults results;
    results.converged = true;
    results.job = pJob;
    results.history = history;
    results.displacements = displacements;
    return results;
}

void DRMSimulationModel::printCurrentState() const
{
    std::cout << "Simulation steps executed:" << mCurrentSimulationStep << std::endl;
    std::cout << "Residual forces norm: " << pMesh->totalResidualForcesNorm << std::endl;
    std::cout << "Kinetic energy:" << pMesh->currentTotalKineticEnergy << std::endl;
    static std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
    const auto timePerNodePerIteration = std::chrono::duration_cast<std::chrono::microseconds>(
                                             std::chrono::steady_clock::now() - begin)
                                             .count()
                                         * 1e-6 / (mCurrentSimulationStep * pMesh->VN());
    std::cout << "Total potential:" << pMesh->currentTotalPotentialEnergykN << " kNm" << std::endl;
    std::cout << "time(s)/(iterations*node) = " << timePerNodePerIteration << std::endl;
    std::cout << "Mov aver deriv norm:" << pMesh->residualForcesMovingAverageDerivativeNorm
              << std::endl;
    std::cout << "xi:" << mSettings.xi << std::endl;
}

void DRMSimulationModel::printDebugInfo() const
{
    std::cout << pMesh->elements[0].rigidity.toString() << std::endl;
    std::cout << "Number of dampings:" << numOfDampings << endl;
    printCurrentState();
}

#ifdef POLYSCOPE_DEFINED
void DRMSimulationModel::draw(const std::string &screenshotsFolder)
{
    // update positions
    //  polyscope::getCurveNetwork("Undeformed edge mesh")->setEnabled(false);
    polyscope::CurveNetwork *meshPolyscopeHandle = polyscope::getCurveNetwork(meshPolyscopeLabel);
    meshPolyscopeHandle->updateNodePositions(pMesh->getEigenVertices());

    // Vertex quantities
    std::vector<double> kineticEnergies(pMesh->VN());
    std::vector<std::array<double, 3>> nodeNormals(pMesh->VN());
    std::vector<std::array<double, 3>> internalForces(pMesh->VN());
    std::vector<std::array<double, 3>> externalForces(pMesh->VN());
    std::vector<std::array<double, 3>> externalMoments(pMesh->VN());
    std::vector<double> internalForcesNorm(pMesh->VN());
    std::vector<std::array<double, 3>> internalAxialForces(pMesh->VN());
    std::vector<std::array<double, 3>> internalFirstBendingTranslationForces(
        pMesh->VN());
    std::vector<std::array<double, 3>> internalFirstBendingRotationForces(
        pMesh->VN());
    std::vector<std::array<double, 3>> internalSecondBendingTranslationForces(
        pMesh->VN());
    std::vector<std::array<double, 3>> internalSecondBendingRotationForces(
        pMesh->VN());
    std::vector<double> nRs(pMesh->VN());
    std::vector<double> theta2(pMesh->VN());
    std::vector<double> theta3(pMesh->VN());
    std::vector<std::array<double, 3>> residualForces(pMesh->VN());
    std::vector<double> residualForcesNorm(pMesh->VN());
    std::vector<double> accelerationX(pMesh->VN());
    for (const VertexType &v : pMesh->vert) {
        kineticEnergies[pMesh->getIndex(v)] = pMesh->nodes[v].kineticEnergy;
        const VectorType n = v.cN();
        nodeNormals[pMesh->getIndex(v)] = {n[0], n[1], n[2]};
        // per node internal forces
        const Vector6d nodeforce = pMesh->nodes[v].force.internal * (-1);
        internalForces[pMesh->getIndex(v)] = {nodeforce[0], nodeforce[1],
                                              nodeforce[2]};
        internalForcesNorm[pMesh->getIndex(v)] = nodeforce.norm();
        // External force
        const Vector6d nodeExternalForce = pMesh->nodes[v].force.external;
        externalForces[pMesh->getIndex(v)] = {
            nodeExternalForce[0], nodeExternalForce[1], nodeExternalForce[2]};
        externalMoments[pMesh->getIndex(v)] = {nodeExternalForce[3],
                                               nodeExternalForce[4], 0};
        internalAxialForces[pMesh->getIndex(v)] = {nodeforce[0], nodeforce[1],
                                                   nodeforce[2]};
        const Node &node = pMesh->nodes[v];
        const Vector6d nodeForceFirst = node.force.internalFirstBending * (-1);
        internalFirstBendingTranslationForces[pMesh->getIndex(v)] = {
            nodeForceFirst[0], nodeForceFirst[1], nodeForceFirst[2]};
        internalFirstBendingRotationForces[pMesh->getIndex(v)] = {
            nodeForceFirst[3], nodeForceFirst[4], 0};

        const Vector6d nodeForceSecond = node.force.internalSecondBending * (-1);
        internalSecondBendingTranslationForces[pMesh->getIndex(v)] = {
            nodeForceSecond[0], nodeForceSecond[1], nodeForceSecond[2]};
        internalSecondBendingRotationForces[pMesh->getIndex(v)] = {
            nodeForceSecond[3], nodeForceSecond[4], 0};
        nRs[pMesh->getIndex(v)] = node.nR;
        const Vector6d nodeResidualForce = pMesh->nodes[v].force.residual;
        residualForces[pMesh->getIndex(v)] = {
            nodeResidualForce[0], nodeResidualForce[1], nodeResidualForce[2]};
        residualForcesNorm[pMesh->getIndex(v)] = nodeResidualForce.norm();
        accelerationX[pMesh->getIndex(v)] = pMesh->nodes[v].acceleration[0];
    }
    meshPolyscopeHandle->addNodeScalarQuantity("Kinetic Energy", kineticEnergies)->setEnabled(false);
    meshPolyscopeHandle->addNodeVectorQuantity("Node normals", nodeNormals)->setEnabled(false);
    meshPolyscopeHandle->addNodeVectorQuantity("Internal force", internalForces)->setEnabled(false);
    meshPolyscopeHandle->addNodeVectorQuantity("External force", externalForces)->setEnabled(false);
    meshPolyscopeHandle->addNodeVectorQuantity("External Moments", externalMoments)->setEnabled(true);
    meshPolyscopeHandle->addNodeScalarQuantity("Internal force norm", internalForcesNorm)
        ->setEnabled(true);
    meshPolyscopeHandle->setRadius(0.002);
    //  polyscope::getCurveNetwork(meshPolyscopeLabel)
    //      ->addNodeVectorQuantity("Internal Axial force", internalAxialForces)
    //      ->setEnabled(false);
    //  polyscope::getCurveNetwork(meshPolyscopeLabel)
    //      ->addNodeVectorQuantity("First bending force-Translation",
    //                              internalFirstBendingTranslationForces)
    //      ->setEnabled(false);
    //  polyscope::getCurveNetwork(meshPolyscopeLabel)
    //      ->addNodeVectorQuantity("First bending force-Rotation",
    //                              internalFirstBendingRotationForces)
    //      ->setEnabled(false);

    //  polyscope::getCurveNetwork(meshPolyscopeLabel)
    //      ->addNodeVectorQuantity("Second bending force-Translation",
    //                              internalSecondBendingTranslationForces)
    //      ->setEnabled(false);
    //  polyscope::getCurveNetwork(meshPolyscopeLabel)
    //      ->addNodeVectorQuantity("Second bending force-Rotation",
    //                              internalSecondBendingRotationForces)
    //      ->setEnabled(false);
    polyscope::getCurveNetwork(meshPolyscopeLabel)
        ->addNodeScalarQuantity("nR", nRs)
        ->setEnabled(false);
    //  polyscope::getCurveNetwork(meshPolyscopeLabel)
    //      ->addNodeScalarQuantity("theta3", theta3)
    //      ->setEnabled(false);
    //  polyscope::getCurveNetwork(meshPolyscopeLabel)
    //      ->addNodeScalarQuantity("theta2", theta2)
    //      ->setEnabled(false);
    polyscope::getCurveNetwork(meshPolyscopeLabel)
        ->addNodeVectorQuantity("Residual force", residualForces)
        ->setEnabled(false);
    polyscope::getCurveNetwork(meshPolyscopeLabel)
        ->addNodeScalarQuantity("Residual force norm", residualForcesNorm)
        ->setEnabled(false);
    //  polyscope::getCurveNetwork(meshPolyscopeLabel)
    //      ->addNodeScalarQuantity("Node acceleration x", accelerationX);

    // Edge quantities
    std::vector<double> A(pMesh->EN());
    std::vector<double> J(pMesh->EN());
    std::vector<double> I2(pMesh->EN());
    std::vector<double> I3(pMesh->EN());
    for (const EdgeType &e : pMesh->edge) {
        const size_t ei = pMesh->getIndex(e);
        A[ei] = pMesh->elements[e].A;
        J[ei] = pMesh->elements[e].J;
        I2[ei] = pMesh->elements[e].I2;
        I3[ei] = pMesh->elements[e].I3;
    }

    polyscope::getCurveNetwork(meshPolyscopeLabel)->addEdgeScalarQuantity("A", A);
    polyscope::getCurveNetwork(meshPolyscopeLabel)->addEdgeScalarQuantity("J", J);
    polyscope::getCurveNetwork(meshPolyscopeLabel)
        ->addEdgeScalarQuantity("I2", I2);
    polyscope::getCurveNetwork(meshPolyscopeLabel)
        ->addEdgeScalarQuantity("I3", I3);

    // Specify the callback
    static bool calledOnce = false;
    if (!calledOnce) {
        PolyscopeInterface::addUserCallback([&]() {
            // 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 debug step",
                            &mSettings.debugModeStep); // set a int variable
            mSettings.drawingStep = mSettings.debugModeStep;
            ImGui::Checkbox("Enable drawing",
                            &mSettings.shouldDraw); // set a int variable
            ImGui::Text("Number of simulation steps: %zu", mCurrentSimulationStep);

            ImGui::PopItemWidth();
        });
        calledOnce = true;
    }

    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(mCurrentSimulationStep) + ".png")
                .string(),
            false);
    } else {
        polyscope::show();
    }
}
#endif

void DRMSimulationModel::applySolutionGuess(const SimulationResults &solutionGuess,
                                            const std::shared_ptr<SimulationJob> &pJob)
{
    assert(solutionGuess.displacements.size() == pMesh->VN()
           && solutionGuess.rotationalDisplacementQuaternion.size() == pMesh->VN());

    for (size_t vi = 0; vi < pMesh->VN(); vi++) {
        Node &node = pMesh->nodes[vi];
        Eigen::Vector3d translationalDisplacements(solutionGuess.displacements[vi].getTranslation());
        if (!pJob->constrainedVertices.contains(vi)
            || !pJob->constrainedVertices.at(vi).contains(0)) {
            node.displacements[0] = translationalDisplacements[0];
        }
        if (!pJob->constrainedVertices.contains(vi)
            || !pJob->constrainedVertices.at(vi).contains(1)) {
            node.displacements[1] = translationalDisplacements[1];
        }
        if (!pJob->constrainedVertices.contains(vi)
            || !pJob->constrainedVertices.at(vi).contains(2)) {
            node.displacements[2] = translationalDisplacements[2];
        }

        updateNodePosition(pMesh->vert[vi], pJob->constrainedVertices);
    }

    for (size_t vi = 0; vi < pMesh->VN(); vi++) {
        Node &node = pMesh->nodes[vi];
        const Eigen::Vector3d nInitial_eigen = node.initialNormal.ToEigenVector<Eigen::Vector3d>();
        Eigen::Quaternion<double> q;
        Eigen::Vector3d eulerAngles = solutionGuess.displacements[vi].getRotation();
        q = Eigen::AngleAxisd(eulerAngles[0], Eigen::Vector3d::UnitX())
            * Eigen::AngleAxisd(eulerAngles[1], Eigen::Vector3d::UnitY())
            * Eigen::AngleAxisd(eulerAngles[2], Eigen::Vector3d::UnitZ());

        Eigen::Vector3d nDeformed_eigen = (q * nInitial_eigen) /*.normalized()*/;
        nDeformed_eigen.normalized();
        //        Eigen::Vector3d n_groundOfTruth = solutionGuess.debug_q_normal[vi] * nInitial_eigen;
        //        n_groundOfTruth.normalized();
        if (!pJob->constrainedVertices.contains(vi)
            || !pJob->constrainedVertices.at(vi).contains(3)) {
            node.displacements[3] = (nDeformed_eigen - nInitial_eigen)[0];
        }
        if (!pJob->constrainedVertices.contains(vi)
            || !pJob->constrainedVertices.at(vi).contains(4)) {
            node.displacements[4] = (nDeformed_eigen - nInitial_eigen)[1];
        }
        updateNodeNormal(pMesh->vert[vi], pJob->constrainedVertices);
        //        const auto debug_rightNy = solutionGuess.debug_drmDisplacements[vi][4];

        Eigen::Vector3d referenceT1_deformed = computeT1Vector(node.referenceElement->cP(0),
                                                               node.referenceElement->cP(1))
                                                   .ToEigenVector<Eigen::Vector3d>();
        const Eigen::Vector3d referenceF1_deformed
            = (referenceT1_deformed - (nInitial_eigen * (referenceT1_deformed.dot(nInitial_eigen))))
                  .normalized();

        const Eigen::Vector3d referenceT1_initial
            = computeT1Vector(pMesh->nodes[node.referenceElement->cV(0)].initialLocation,
                              pMesh->nodes[node.referenceElement->cV(1)].initialLocation)
                  .ToEigenVector<Eigen::Vector3d>();
        //          const VectorType &n_initial = node.initialNormal;
        const Eigen::Vector3d referenceF1_initial
            = (referenceT1_initial - (nInitial_eigen * (referenceT1_initial.dot(nInitial_eigen))))
                  .normalized();
        Eigen::Quaternion<double> q_f1; //nr is with respect to f1
        q_f1.setFromTwoVectors(referenceF1_initial, referenceF1_deformed);
        Eigen::Quaternion<double> q_normal;
        q_normal.setFromTwoVectors(nInitial_eigen, nDeformed_eigen);
        Eigen::Quaternion<double> q_nr = q_f1.inverse() * q_normal.inverse() * q;
        const double nr_0To2pi_pos = 2 * std::acos(q_nr.w());
        //        const double nr_0To2pi_groundOfTruth = 2 * std::acos(solutionGuess.debug_q_nr[vi].w());
        const double nr_0To2pi = nr_0To2pi_pos <= M_PI ? nr_0To2pi_pos : (nr_0To2pi_pos - 2 * M_PI);
        Eigen::Vector3d deformedNormal_debug(q_nr.x() * sin(nr_0To2pi_pos / 2),
                                             q_nr.y() * sin(nr_0To2pi_pos / 2),
                                             q_nr.z() * sin(nr_0To2pi_pos / 2));
        /*deformedNormal_debug =*/deformedNormal_debug.normalize();
        const double nr = deformedNormal_debug.dot(nDeformed_eigen) > 0 ? nr_0To2pi : -nr_0To2pi;
        if (!pJob->constrainedVertices.contains(vi)
            || !pJob->constrainedVertices.at(vi).contains(5)) {
            node.displacements[5] = nr;
        }
        //        const double nr_asin = q_nr.x()
        if (rigidSupports.contains(vi)) {
            const EdgePointer &refElem = node.referenceElement;
            const VectorType &refT1 = computeT1Vector(refElem->cP(0), refElem->cP(1));

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

        } else {
            node.displacements[5] = nr;
        }
    }
    updateElementalFrames();

    applyDisplacements(constrainedVertices);
    if (!pJob->nodalForcedDisplacements.empty()) {
        applyForcedDisplacements(
            pJob->nodalForcedDisplacements);
    }
    updateElementalLengths();

    //    //    registerWorldAxes();
    //    Eigen::MatrixX3d framesX(pMesh->VN(), 3);
    //    Eigen::MatrixX3d framesY(pMesh->VN(), 3);
    //    Eigen::MatrixX3d framesZ(pMesh->VN(), 3);
    //    for (VertexIndex vi = 0; vi < pMesh->VN(); vi++) {
    //        Node &node = pMesh->nodes[vi];
    //        Eigen::Vector3d translationalDisplacements(solutionGuess.displacements[vi].getTranslation());
    //        node.displacements[0] = translationalDisplacements[0];
    //        node.displacements[1] = translationalDisplacements[1];
    //        node.displacements[2] = translationalDisplacements[2];
    //        Eigen::Quaternion<double> q;
    //        Eigen::Vector3d eulerAngles = solutionGuess.displacements[vi].getRotation();
    //        q = Eigen::AngleAxisd(eulerAngles[0], Eigen::Vector3d::UnitX())
    //            * Eigen::AngleAxisd(eulerAngles[1], Eigen::Vector3d::UnitY())
    //            * Eigen::AngleAxisd(eulerAngles[2], Eigen::Vector3d::UnitZ());

    //        auto deformedNormal = q * Eigen::Vector3d(0, 0, 1);
    //        auto deformedFrameY = q * Eigen::Vector3d(0, 1, 0);
    //        auto deformedFrameX = q * Eigen::Vector3d(1, 0, 0);
    //        framesX.row(vi) = Eigen::Vector3d(deformedFrameX[0], deformedFrameX[1], deformedFrameX[2]);
    //        framesY.row(vi) = Eigen::Vector3d(deformedFrameY[0], deformedFrameY[1], deformedFrameY[2]);
    //        framesZ.row(vi) = Eigen::Vector3d(deformedNormal[0], deformedNormal[1], deformedNormal[2]);
    //    }
    //    polyscope::CurveNetwork *meshPolyscopeHandle = polyscope::getCurveNetwork(meshPolyscopeLabel);
    //    meshPolyscopeHandle->updateNodePositions(pMesh->getEigenVertices());

    //    //if(showFramesOn.contains(vi)){
    //    auto polyscopeHandle_frameX = meshPolyscopeHandle->addNodeVectorQuantity("FrameX", framesX);
    //    polyscopeHandle_frameX->setVectorLengthScale(0.01);
    //    polyscopeHandle_frameX->setVectorRadius(0.01);
    //    polyscopeHandle_frameX->setVectorColor(
    //        /*polyscope::getNextUniqueColor()*/ glm::vec3(1, 0, 0));
    //    auto polyscopeHandle_frameY = meshPolyscopeHandle->addNodeVectorQuantity("FrameY", framesY);
    //    polyscopeHandle_frameY->setVectorLengthScale(0.01);
    //    polyscopeHandle_frameY->setVectorRadius(0.01);
    //    polyscopeHandle_frameY->setVectorColor(
    //        /*polyscope::getNextUniqueColor()*/ glm::vec3(0, 1, 0));
    //    auto polyscopeHandle_frameZ = meshPolyscopeHandle->addNodeVectorQuantity("FrameZ", framesZ);
    //    polyscopeHandle_frameZ->setVectorLengthScale(0.01);
    //    polyscopeHandle_frameZ->setVectorRadius(0.01);
    //    polyscopeHandle_frameZ->setVectorColor(
    //        /*polyscope::getNextUniqueColor()*/ glm::vec3(0, 0, 1));
    //    //}
    //    polyscope::show();
}

SimulationResults DRMSimulationModel::executeSimulation(const std::shared_ptr<SimulationJob> &pJob,
                                                        const Settings &settings,
                                                        const SimulationResults &solutionGuess)
{
    assert(pJob->pMesh.operator bool());
    auto t1 = std::chrono::high_resolution_clock::now();
    mSettings = settings;
    reset();
    //    double totalExternalForcesNorm = 0;
    //    for (const std::pair<VertexIndex, Vector6d> &externalForce : pJob->nodalExternalForces) {
    //        totalExternalForcesNorm += externalForce.second.norm();
    //    }

    //    if (!pJob->nodalExternalForces.empty()) {
    //        //        totalResidualForcesNormThreshold = settings.totalExternalForcesNormPercentageTermination
    //        //                                           * totalExternalForcesNorm;
    //    } else {
    //        totalResidualForcesNormThreshold = 1;
    //        std::cout << "No forces setted default residual forces norm threshold" << std::endl;
    //    }

    if (mSettings.beVerbose) {
        std::cout << "totalResidualForcesNormThreshold:"
                  << mSettings.totalResidualForcesNormThreshold << std::endl;
    }
    history.label = pJob->pMesh->getLabel() + "_" + pJob->getLabel();

    //      if (!pJob->nodalExternalForces.empty()) {
    //        double externalForcesNorm = 0;
    //        for (const auto &externalForce : pJob->nodalExternalForces) {
    //          externalForcesNorm += externalForce.second.norm();
    //        }
    //        mSettings.totalResidualForcesNormThreshold = externalForcesNorm * 1e-2;
    //      }

    constrainedVertices = pJob->constrainedVertices;
    if (!pJob->nodalForcedDisplacements.empty()) {
        for (std::pair<VertexIndex, Eigen::Vector3d> viDisplPair : pJob->nodalForcedDisplacements) {
            const VertexIndex vi = viDisplPair.first;
            constrainedVertices[vi].insert({0, 1, 2});
        }
    }
    //  if (!pJob->nodalForcedNormals.empty()) {
    //    for (std::pair<VertexIndex, Eigen::Vector3d> viNormalPair :
    //         pJob->nodalForcedDisplacements) {
    //      assert(viNormalPair3second[2] >= 0);
    //    }
    //  }

    pMesh.reset();
    pMesh = std::make_unique<SimulationMesh>(*pJob->pMesh);
    if (mSettings.beVerbose) {
        std::cout << "Executing simulation for mesh with:" << pMesh->VN() << " nodes and "
                  << pMesh->EN() << " elements." << std::endl;
    }
    vcg::tri::UpdateBounding<SimulationMesh>::Box(*pMesh);
    computeRigidSupports();
    for (auto fixedVertex : pJob->constrainedVertices) {
        assert(fixedVertex.first < pMesh->VN());
    }

#ifdef POLYSCOPE_DEFINED
    if (mSettings.shouldDraw || mSettings.isDebugMode) {
        PolyscopeInterface::init();
        polyscope::registerCurveNetwork(meshPolyscopeLabel,
                                        pMesh->getEigenVertices(),
                                        pMesh->getEigenEdges());
        //    registerWorldAxes();
    }
#endif

    if (!pJob->nodalForcedDisplacements.empty() && pJob->nodalExternalForces.empty()) {
        shouldApplyInitialDistortion = true;
    }
    updateElementalFrames();
    if (!solutionGuess.displacements.empty()) {
        //#ifdef POLYSCOPE_DEFINED
        //        if (mSettings.shouldDraw || mSettings.isDebugMode) {
        //            solutionGuess.registerForDrawing();
        //            polyscope::show();
        //            solutionGuess.unregister();
        //        }
        //#endif

        applySolutionGuess(solutionGuess, pJob);
        shouldTemporarilyDampForces = true;
        //        Dt = mSettings.Dtini * 0.9;
    }

    updateNodalMasses();
    updateNodalExternalForces(pJob->nodalExternalForces, constrainedVertices);
    const size_t movingAverageSampleSize = 200;
    std::queue<double> residualForcesMovingAverageHistorySample;
    std::vector<double> percentageOfConvergence;

    double residualForcesMovingAverageDerivativeMax = 0;
    while (settings.maxDRMIterations == 0 || mCurrentSimulationStep < settings.maxDRMIterations) {
        if ((mSettings.isDebugMode && mCurrentSimulationStep == 50000)) {
            //            std::filesystem::create_directory("./PatternOptimizationNonConv");
            //            pJob->save("./PatternOptimizationNonConv");
            //            Dt = mSettings.Dtini;
        }
        if (mCurrentSimulationStep == 500 && shouldTemporarilyDampForces) {
            Dt = mSettings.Dtini;
        }
        //  while (true) {
        updateNormalDerivatives();
        updateT1Derivatives();
        updateRDerivatives();
        updateT2Derivatives();
        updateT3Derivatives();
        const bool shouldBreak = mCurrentSimulationStep == 12970;
        updateResidualForcesOnTheFly(constrainedVertices);

        // 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 (!pJob->nodalForcedDisplacements.empty()) {
            applyForcedDisplacements(

                pJob->nodalForcedDisplacements);
        }
        //    if (!pJob->nodalForcedNormals.empty()) {
        //      applyForcedNormals(pJob->nodalForcedNormals);
        //    }
        updateElementalLengths();
        mCurrentSimulationStep++;

        double sumOfDisplacementsNorm = 0;
        for (size_t vi = 0; vi < pMesh->VN(); vi++) {
            sumOfDisplacementsNorm += pMesh->nodes[vi].displacements.getTranslation().norm();
        }
        sumOfDisplacementsNorm /= pMesh->bbox.Diag();
        pMesh->sumOfNormalizedDisplacementNorms = sumOfDisplacementsNorm;
        //      pMesh->residualForcesMovingAverage = (pMesh->totalResidualForcesNorm
        //                                            + mCurrentSimulationStep
        //                                                  * pMesh->residualForcesMovingAverage)
        //                                           / (1 + mCurrentSimulationStep);
        pMesh->residualForcesMovingAverage += pMesh->totalResidualForcesNorm
                                              / movingAverageSampleSize;
        residualForcesMovingAverageHistorySample.push(pMesh->residualForcesMovingAverage);
        if (movingAverageSampleSize < residualForcesMovingAverageHistorySample.size()) {
            const double firstElementValue = residualForcesMovingAverageHistorySample.front();
            pMesh->residualForcesMovingAverage -= firstElementValue / movingAverageSampleSize;
            residualForcesMovingAverageHistorySample.pop();

            pMesh->residualForcesMovingAverageDerivativeNorm
                = std::abs((residualForcesMovingAverageHistorySample.back()
                            - residualForcesMovingAverageHistorySample.front()))
                  / (movingAverageSampleSize);
            residualForcesMovingAverageDerivativeMax
                = std::max(pMesh->residualForcesMovingAverageDerivativeNorm,
                           residualForcesMovingAverageDerivativeMax);
            pMesh->residualForcesMovingAverageDerivativeNorm
                /= residualForcesMovingAverageDerivativeMax;
            //          std::cout << "Normalized derivative:"
            //                    << residualForcesMovingAverageDerivativeNorm
            //                    << std::endl;
        }

        //    pMesh->previousTotalPotentialEnergykN =
        //        pMesh->currentTotalPotentialEnergykN;
        //    pMesh->currentTotalPotentialEnergykN = computeTotalPotentialEnergy() / 1000;
        //    timePerNodePerIterationHistor.push_back(timePerNodePerIteration);
        if (mSettings.beVerbose && mSettings.isDebugMode
            && mCurrentSimulationStep % mSettings.debugModeStep == 0) {
            printCurrentState();
            std::cout << "Percentage of target:"
                      << 100 * mSettings.totalResidualForcesNormThreshold
                             / pMesh->totalResidualForcesNorm
                      << "%" << std::endl;
            //          SimulationResultsReporter::createPlot("Number of Steps",
            //                                                "Residual Forces mov aver",
            //                                                movingAverages);
            //          SimulationResultsReporter::createPlot("Number of Steps",
            //                                                "Residual Forces mov aver deriv",
            //                                                movingAveragesDerivatives);
            //          draw();
            //        SimulationResulnodalForcedDisplacementstsReporter::createPlot("Number of Steps",
            //                                              "Time/(#nodes*#iterations)",
            //                                              timePerNodePerIterationHistory);
        }

        if ((mSettings.shouldCreatePlots || mSettings.isDebugMode) && mCurrentSimulationStep != 0) {
            history.stepPulse(*pMesh);
            percentageOfConvergence.push_back(100 * mSettings.totalResidualForcesNormThreshold
                                              / pMesh->totalResidualForcesNorm);
        }

        if (std::isnan(pMesh->currentTotalKineticEnergy)) {
            if (mSettings.beVerbose) {
                std::cout << "Infinite kinetic energy detected.Exiting.." << std::endl;
            }
            break;
        }

        if (mSettings.shouldCreatePlots && mSettings.isDebugMode
            && mCurrentSimulationStep % mSettings.debugModeStep == 0) {
            //            SimulationResultsReporter::createPlot("Number of Steps",
            //                                                  "Residual Forces mov aver deriv",
            //                                                  movingAveragesDerivatives_norm);
            //          SimulationResultsReporter::createPlot("Number of Steps",
            //                                                "Residual Forces mov aver",
            //                                                          history.residualForcesMovingAverage);
            //            SimulationResultsReporter::createPlot("Number of Steps",
            //                                                  "Log of Residual Forces",
            //                                                  history.logResidualForces,
            //                                                  {},
            //                                                  history.redMarks);
            //            SimulationResultsReporter::createPlot("Number of Steps",
            //                                                  "Log of Kinetic energy",
            //                                                  history.kineticEnergy,
            //                                                  {},
            //                                                  history.redMarks);
            SimulationResultsReporter reporter;
            reporter.reportHistory(history, "IntermediateResults", pJob->pMesh->getLabel());
            //            SimulationResultsReporter::createPlot("Number of Iterations",
            //                                                  "Sum of normalized displacement norms",
            //                                                  history.sumOfNormalizedDisplacementNorms /*,
            //                   std::filesystem::path("./")
            //                       .append("SumOfNormalizedDisplacementNorms_" + graphSuffix + ".png")
            //                       .string()*/
            //                                                  ,
            //                                                  {},
            //                                                  history.redMarks);
            //          SimulationResultsReporter::createPlot("Number of Steps",
            //                                                "Percentage of convergence",
            //                                                percentageOfConvergence,
            //                                                {},
            //                                                history.redMarks);
        }

#ifdef POLYSCOPE_DEFINED
        if (mSettings.shouldDraw && mSettings.isDebugMode
            && mCurrentSimulationStep % mSettings.drawingStep == 0) /* &&
          
currentSimulationStep > maxDRMIterations*/
        {
            //      std::string saveTo = std::filesystem::current_path()
            //                               .append("Debugging_files")
            //                               .append("Screenshots")
            //                               .string();
            draw();
            //      yValues = history.kineticEnergy;
            //      plotHandle = matplot::scatter(xPlot, yValues);
            //      label = "Log of Kinetic energy";
            //      plotHandle->legend_string(label);

            //      shouldUseKineticEnergyThreshold = true;
        }
#endif

        //    t = t + Dt;
        //    std::cout << "Kinetic energy:" << mesh.currentTotalKineticEnergy
        //              << std::endl;
        //    std::cout << "Residual forces norm:" << mesh.totalResidualForcesNorm
        //              << std::endl;
        // Residual forces norm convergence
        if ((pMesh->previousTotalKineticEnergy > pMesh->currentTotalKineticEnergy
             //          && mCurrentSimulationStep > movingAverageSampleSize
             && (pJob->nodalForcedDisplacements.empty()
                 || mCurrentSimulationStep > mSettings.gradualForcedDisplacementSteps))
            /* || (pMesh->totalResidualForcesNorm / mSettings.totalResidualForcesNormThreshold <= 1
              && mCurrentSimulationStep > 1)*/
            /*||
mesh->previousTotalPotentialEnergykN >
mesh->currentTotalPotentialEnergykN*/
            /*|| mesh->currentTotalPotentialEnergykN < minPotentialEnergy*/) {
            //        if (pMesh->totalResidualForcesNorm < totalResidualForcesNormThreshold) {
            const bool fullfillsKineticEnergyTerminationCriterion
                = mSettings.shouldUseTranslationalKineticEnergyThreshold
                  && pMesh->currentTotalTranslationalKineticEnergy
                         < mSettings.totalTranslationalKineticEnergyThreshold
                  && mCurrentSimulationStep > 20 && numOfDampings > 0;
            const bool fullfillsResidualForcesNormTerminationCriterion
                = pMesh->totalResidualForcesNorm < mSettings.totalResidualForcesNormThreshold;
            const bool fullfillsMovingAverageNormTerminationCriterion
                = pMesh->residualForcesMovingAverage
                  < mSettings.residualForcesMovingAverageNormThreshold;
            /*pMesh->residualForcesMovingAverage < totalResidualForcesNormThreshold*/
            const bool fullfillsMovingAverageDerivativeNormTerminationCriterion
                = pMesh->residualForcesMovingAverageDerivativeNorm < 1e-4;
            const bool shouldTerminate
                = fullfillsKineticEnergyTerminationCriterion
                  //                                                       || fullfillsMovingAverageNormTerminationCriterion
                  //                || fullfillsMovingAverageDerivativeNormTerminationCriterion
                  || fullfillsResidualForcesNormTerminationCriterion;
            if (shouldTerminate) {
                if (mSettings.beVerbose && !mSettings.isDebugMode) {
                    std::cout << "Simulation converged." << std::endl;
                    printCurrentState();
                    if (fullfillsResidualForcesNormTerminationCriterion) {
                        std::cout << "Converged using residual forces norm threshold criterion"
                                  << std::endl;
                    } else if (fullfillsKineticEnergyTerminationCriterion) {
                        std::cout << "Warning: The kinetic energy of the system was "
                                     " used as a convergence criterion"
                                  << std::endl;
                    } else if (fullfillsMovingAverageNormTerminationCriterion) {
                        std::cout
                            << "Converged using residual forces moving average derivative norm "
                               "threshold criterion"
                            << std::endl;
                    }
                }
                break;
                //        }
            }

            //            const bool shouldCapDisplacements = mSettings.displacementCap.has_value();
            //            for (VertexType &v : pMesh->vert) {
            //                Node &node = pMesh->nodes[v];
            //                Vector6d stepDisplacement = node.velocity * Dt;
            //                if (shouldCapDisplacements
            //                    && stepDisplacement.getTranslation().norm() > mSettings.displacementCap) {
            //                    stepDisplacement = stepDisplacement
            //                                       * (*mSettings.displacementCap
            //                                          / stepDisplacement.getTranslation().norm());
            //                }
            //                node.displacements = node.displacements - stepDisplacement;
            //            }
            //            applyDisplacements(constrainedVertices);
            //            if (!pJob->nodalForcedDisplacements.empty()) {
            //                applyForcedDisplacements(

            //                    pJob->nodalForcedDisplacements);
            //            }
            //            updateElementalLengths();

            //            const double triggerPercentage = 0.01;
            //            const double xi_min = 0.55;
            //            const double xi_init = 0.9969;
            //            if (mSettings.totalResidualForcesNormThreshold / pMesh->totalResidualForcesNorm
            //                > triggerPercentage) {
            //                mSettings.xi = ((xi_min - xi_init)
            //                                    * (mSettings.totalResidualForcesNormThreshold
            //                                       / pMesh->totalResidualForcesNorm)
            //                                + xi_init - triggerPercentage * xi_min)
            //                               / (1 - triggerPercentage);
            //            }
            Dt *= mSettings.xi;
            //            if (mSettings.isDebugMode) {
            //                std::cout << Dt << std::endl;
            //            }
            resetVelocities();
            ++numOfDampings;
            if (mSettings.shouldCreatePlots) {
                history.redMarks.push_back(mCurrentSimulationStep);
            }
            //          std::cout << "Number of dampings:" << numOfDampings << endl;
            //          draw();
        }
    }

    SimulationResults results = computeResults(pJob);

    if (mCurrentSimulationStep == settings.maxDRMIterations && mCurrentSimulationStep != 0) {
        if (mSettings.beVerbose) {
            std::cout << "Did not reach equilibrium before reaching the maximum number "
                         "of DRM steps ("
                      << settings.maxDRMIterations << "). Breaking simulation" << std::endl;
        }
        results.converged = false;
    } else if (std::isnan(pMesh->currentTotalKineticEnergy)) {
        if (mSettings.beVerbose) {
            std::cerr << "Simulation did not converge due to infinite kinetic energy." << std::endl;
        }
        results.converged = false;
    }
    //    mesh.printVertexCoordinates(mesh.VN() / 2);
#ifdef POLYSCOPE_DEFINED
    if (mSettings.shouldDraw && !mSettings.isDebugMode) {
        draw();
    }
#endif
    if (!std::isnan(pMesh->currentTotalKineticEnergy)) {
        results.debug_drmDisplacements = results.displacements;
        results.internalPotentialEnergy = computeTotalInternalPotentialEnergy();

        results.rotationalDisplacementQuaternion.resize(pMesh->VN());
        results.debug_q_f1.resize(pMesh->VN());
        results.debug_q_normal.resize(pMesh->VN());
        results.debug_q_nr.resize(pMesh->VN());
        for (int vi = 0; vi < pMesh->VN(); vi++) {
            const Node &node = pMesh->nodes[vi];
            const Eigen::Vector3d nInitial_eigen = node.initialNormal
                                                       .ToEigenVector<Eigen::Vector3d>();
            const Eigen::Vector3d nDeformed_eigen
                = pMesh->vert[vi].cN().ToEigenVector<Eigen::Vector3d>();

            Eigen::Quaternion<double> q_normal;
            q_normal.setFromTwoVectors(nInitial_eigen, nDeformed_eigen);
            Eigen::Quaternion<double> q_nr_nDeformed;
            q_nr_nDeformed = Eigen::AngleAxis<double>(pMesh->nodes[vi].nR, nDeformed_eigen);
            Eigen::Quaternion<double> q_nr_nInit;
            q_nr_nInit = Eigen::AngleAxis<double>(pMesh->nodes[vi].nR, nInitial_eigen);
            const auto w = q_nr_nDeformed.w();
            const auto theta = 2 * acos(q_nr_nDeformed.w());
            const auto nr = pMesh->nodes[vi].nR;
            Eigen::Vector3d deformedNormal_debug(q_nr_nDeformed.x() * sin(theta / 2),
                                                 q_nr_nDeformed.y() * sin(theta / 2),
                                                 q_nr_nDeformed.z() * sin(theta / 2));
            deformedNormal_debug.normalize();
            //        const double nr = nr_0To2pi <= M_PI ? nr_0To2pi : (nr_0To2pi - 2 * M_PI);
            const double nr_debug = deformedNormal_debug.dot(nDeformed_eigen) > 0 ? theta : -theta;
            assert(pMesh->nodes[vi].nR - nr_debug < 1e-6);
            VectorType referenceT1_deformed = pMesh->elements[node.referenceElement].frame.t1;
            const VectorType &nDeformed = pMesh->vert[vi].cN();
            const VectorType referenceF1_deformed
                = (referenceT1_deformed
                   - (node.initialNormal * (referenceT1_deformed * node.initialNormal)))
                      .Normalize();

            const VectorType referenceT1_initial
                = computeT1Vector(pMesh->nodes[node.referenceElement->cV(0)].initialLocation,
                                  pMesh->nodes[node.referenceElement->cV(1)].initialLocation);
            //          const VectorType &n_initial = node.initialNormal;
            const VectorType referenceF1_initial = (referenceT1_initial
                                                    - (node.initialNormal
                                                       * (referenceT1_initial * node.initialNormal)))
                                                       .Normalize();
            Eigen::Quaternion<double> q_f1_nInit; //nr is with respect to f1
            q_f1_nInit.setFromTwoVectors(referenceF1_initial.ToEigenVector<Eigen::Vector3d>(),
                                         referenceF1_deformed.ToEigenVector<Eigen::Vector3d>());

            Eigen::Quaternion<double> q_f1_nDeformed; //nr is with respect to f1
            //          const VectorType &n_initial = node.initialNormal;
            const VectorType referenceF1_initial_def
                = (referenceT1_initial - (nDeformed * (referenceT1_initial * nDeformed))).Normalize();
            const VectorType referenceF1_deformed_def = (referenceT1_deformed
                                                         - (nDeformed
                                                            * (referenceT1_deformed * nDeformed)))
                                                            .Normalize();
            q_f1_nDeformed
                .setFromTwoVectors(referenceF1_initial_def.ToEigenVector<Eigen::Vector3d>(),
                                   referenceF1_deformed_def.ToEigenVector<Eigen::Vector3d>());
            const auto debug_qf1_nDef = (q_f1_nDeformed * q_nr_nDeformed) * nDeformed_eigen;
            const auto debug_qf1_nInit = (q_f1_nInit * q_nr_nInit) * nInitial_eigen;
            const auto debug_deformedNormal_f1Init = (q_normal * (q_f1_nInit * q_nr_nInit))
                                                     * nInitial_eigen;
            const auto debug_deformedNormal_f1Def = ((q_nr_nDeformed * q_f1_nDeformed) * q_normal)
                                                    * nInitial_eigen;
            //          Eigen::Quaternion<double> q_t1;
            //          q_t1.setFromTwoVectors(referenceT1_initial.ToEigenVector<Eigen::Vector3d>(),
            //                                 referenceT1_deformed.ToEigenVector<Eigen::Vector3d>());

            results.debug_q_f1[vi] = q_f1_nInit;
            results.debug_q_normal[vi] = q_normal;
            results.debug_q_nr[vi] = q_nr_nInit;
            results.rotationalDisplacementQuaternion[vi]
                = (q_normal
                   * (q_f1_nInit * q_nr_nInit)); //q_f1_nDeformed * q_nr_nDeformed * q_normal;
            //Update the displacement vector to contain the euler angles
            const Eigen::Vector3d eulerAngles = results.rotationalDisplacementQuaternion[vi]
                                                    .toRotationMatrix()
                                                    .eulerAngles(0, 1, 2);
            results.displacements[vi][3] = eulerAngles[0];
            results.displacements[vi][4] = eulerAngles[1];
            results.displacements[vi][5] = eulerAngles[2];

            //          Eigen::Quaterniond q_test = Eigen::AngleAxisd(eulerAngles[0], Eigen::Vector3d::UnitX())
            //                                      * Eigen::AngleAxisd(eulerAngles[1], Eigen::Vector3d::UnitY())
            //                                      * Eigen::AngleAxisd(eulerAngles[2], Eigen::Vector3d::UnitZ());

            //          const double dot_test = results.rotationalDisplacementQuaternion[vi].dot(q_test);
            //          assert(dot_test > 1 - 1e5);

            int i = 0;
            i++;
        }
    }

    if (!mSettings.isDebugMode && mSettings.shouldCreatePlots) {
        SimulationResultsReporter reporter;
        reporter.reportResults({results}, "Results", pJob->pMesh->getLabel());
    }

  return results;
}