#include "reducedmodeloptimizer.hpp"
#include "bobyqa.h"
#include "flatpattern.hpp"
#include "gradientDescent.h"
#include "simulationhistoryplotter.hpp"
#include "trianglepattterntopology.hpp"

const bool gShouldDraw = true;

FormFinder simulator;
Eigen::MatrixX3d g_optimalReducedModelDisplacements;
SimulationJob g_reducedPatternSimulationJob;
std::unordered_map<ReducedPatternVertexIndex, FullPatternVertexIndex>
    g_reducedToFullInterfaceViMap;
matplot::line_handle gPlotHandle;
std::vector<double> gObjectiveValueHistory;
Eigen::Vector4d g_initialX;
std::unordered_set<size_t> g_reducedPatternExludedEdges;
double g_initialParameters;

// struct OptimizationCallback {
//  double operator()(const size_t &iterations, const Eigen::VectorXd &x,
//                    const double &fval, Eigen::VectorXd &gradient) const {
//    // run simulation
//    //    SimulationResults reducedModelResults =
//    //        simulator.executeSimulation(reducedModelSimulationJob);
//    //    reducedModelResults.draw(reducedModelSimulationJob);
//    gObjectiveValueHistory.push_back(fval);
//    auto xPlot = matplot::linspace(0, gObjectiveValueHistory.size(),
//                                   gObjectiveValueHistory.size());
//    gPlotHandle = matplot::scatter(xPlot, gObjectiveValueHistory);
//    //    const std::string plotImageFilename = "objectivePlot.png";
//    //    matplot::save(plotImageFilename);
//    //    if (numberOfOptimizationRounds % 30 == 0) {
//    //      std::filesystem::copy_file(
//    //          std::filesystem::path(plotImageFilename),
//    //          std::filesystem::path("objectivePlot_copy.png"));
//    //    }
//    //    std::stringstream ss;
//    //    ss << x;
//    //    reducedModelResults.simulationLabel = ss.str();
//    //    SimulationResultsReporter resultsReporter;
//    //    resultsReporter.reportResults(
//    //        {reducedModelResults},
//    //        std::filesystem::current_path().append("Results"));

//    return true;
//  }
//};

// struct Objective {
//  double operator()(const Eigen::VectorXd &x, Eigen::VectorXd &) const {
//    assert(x.rows() == 4);

//    //      drawSimulationJob(simulationJob);
//    // Set mesh from x
//    std::shared_ptr<SimulationMesh> reducedModel =
//        g_reducedPatternSimulationJob.mesh;
//    for (EdgeIndex ei = 0; ei < reducedModel->EN(); ei++) {
//      if (g_reducedPatternExludedEdges.contains(ei)) {
//        continue;
//      }
//      Element &e = reducedModel->elements[ei];
//      e.axialConstFactor = g_initialStiffnessFactors(ei, 0) * x(0);
//      e.torsionConstFactor = g_initialStiffnessFactors(ei, 1) * x(1);
//      e.firstBendingConstFactor = g_initialStiffnessFactors(ei, 2) * x(2);
//      e.secondBendingConstFactor = g_initialStiffnessFactors(ei, 3) * x(3);
//    }
//    // run simulation
//    SimulationResults reducedModelResults =
//        simulator.executeSimulation(g_reducedPatternSimulationJob);
//    //    std::stringstream ss;
//    //    ss << x;
//    //    reducedModelResults.simulationLabel = ss.str();
//    //    SimulationResultsReporter resultsReporter;
//    //    resultsReporter.reportResults(
//    //        {reducedModelResults},
//    //        std::filesystem::current_path().append("Results"));
//    // compute error and return it
//    double error = 0;
//    for (const auto reducedFullViPair : g_reducedToFullInterfaceViMap) {
//      VertexIndex reducedModelVi = reducedFullViPair.first;
//      Eigen::Vector3d vertexDisplacement(
//          reducedModelResults.displacements[reducedModelVi][0],
//          reducedModelResults.displacements[reducedModelVi][1],
//          reducedModelResults.displacements[reducedModelVi][2]);
//      Eigen::Vector3d errorVector =
//          Eigen::Vector3d(
//              g_optimalReducedModelDisplacements.row(reducedModelVi)) -
//          vertexDisplacement;
//      error += errorVector.norm();
//    }

//    return error;
//  }
//};

double ReducedModelOptimizer::objective(long n, const double *x) {

  for (size_t parameterIndex = 0; parameterIndex < n; parameterIndex++) {
    std::cout << "x[" + std::to_string(parameterIndex) + "]="
              << x[parameterIndex] << std::endl;
  }

  for (EdgeIndex ei = 0; ei < g_reducedPatternSimulationJob.mesh->EN(); ei++) {
    Element &e = g_reducedPatternSimulationJob.mesh->elements[ei];
    //    if (g_reducedPatternExludedEdges.contains(ei)) {
    //      continue;
    //    }
    e.properties.E = g_initialParameters * x[0];
    //    e.properties.G = g_initialParameters(1) * x[1];
    //    e.properties.A = g_initialParameters(0, 0) * x[0];
    //    e.properties.J = g_initialParameters(0, 1) * x[1];
    //    e.properties.I2 = g_initialParameters(0, 2) * x[2];
    //    e.properties.I3 = g_initialParameters(0, 3) * x[3];
    e.properties.G = e.properties.E / (2 * (1 + 0.3));
    e.axialConstFactor = e.properties.E * e.properties.A / e.initialLength;
    e.torsionConstFactor = e.properties.G * e.properties.J / e.initialLength;
    e.firstBendingConstFactor =
        2 * e.properties.E * e.properties.I2 / e.initialLength;
    e.secondBendingConstFactor =
        2 * e.properties.E * e.properties.I3 / e.initialLength;
  }
  // run simulation
  SimulationResults reducedModelResults =
      simulator.executeSimulation(g_reducedPatternSimulationJob, false, false);

  // compute error and return it
  double error = 0;
  for (const auto reducedFullViPair : g_reducedToFullInterfaceViMap) {
    VertexIndex reducedModelVi = reducedFullViPair.first;
    //    const auto pos =
    //        g_reducedPatternSimulationJob.mesh->vert[reducedModelVi].cP();
    //    std::cout << "Interface vi " << reducedModelVi << " is at position "
    //              << pos[0] << " " << pos[1] << " " << pos[2] << std::endl;
    Eigen::Vector3d vertexDisplacement(
        reducedModelResults.displacements[reducedModelVi][0],
        reducedModelResults.displacements[reducedModelVi][1],
        reducedModelResults.displacements[reducedModelVi][2]);
    Eigen::Vector3d errorVector =
        Eigen::Vector3d(
            g_optimalReducedModelDisplacements.row(reducedModelVi)) -
        vertexDisplacement;
    //    error += errorVector.squaredNorm();
    error += errorVector.norm();
  }

  //  gObjectiveValueHistory.push_back(error);
  //  auto xPlot = matplot::linspace(0, gObjectiveValueHistory.size(),
  //                                 gObjectiveValueHistory.size());
  //  gPlotHandle = matplot::scatter(xPlot, gObjectiveValueHistory);

  return error;
}

void ReducedModelOptimizer::computeMaps(
    FlatPattern &fullPattern, FlatPattern &reducedPattern,
    const std::unordered_set<size_t> &reducedModelExcludedEges) {
  // Compute the offset between the interface nodes
  const size_t interfaceSlotIndex = 4; // bottom edge
  assert(slotToNode.find(interfaceSlotIndex) != slotToNode.end() &&
         slotToNode.find(interfaceSlotIndex)->second.size() == 1);
  // Assuming that in the bottom edge there is only one vertex which is also the
  // interface
  const size_t baseTriangleInterfaceVi =
      *(slotToNode.find(interfaceSlotIndex)->second.begin());

  vcg::tri::Allocator<FlatPattern>::PointerUpdater<FlatPattern::VertexPointer>
      pu_fullModel;
  fullPattern.deleteDanglingVertices(pu_fullModel);
  const size_t fullModelBaseTriangleInterfaceVi =
      pu_fullModel.remap.empty() ? baseTriangleInterfaceVi
                                 : pu_fullModel.remap[baseTriangleInterfaceVi];
  const size_t fullModelBaseTriangleVN = fullPattern.VN();
  fullPattern.createFan();
  const size_t duplicateVerticesPerFanPair =
      fullModelBaseTriangleVN - fullPattern.VN() / 6;
  const size_t fullPatternInterfaceVertexOffset =
      fullModelBaseTriangleVN - duplicateVerticesPerFanPair;
  //  std::cout << "Dups in fan pair:" << duplicateVerticesPerFanPair <<
  //  std::endl;

  // Save excluded edges
  g_reducedPatternExludedEdges.clear();
  const size_t fanSize = 6;
  const size_t reducedBaseTriangleNumberOfEdges = reducedPattern.EN();
  for (size_t fanIndex = 0; fanIndex < fanSize; fanIndex++) {
    for (const size_t ei : reducedModelExcludedEges) {
      g_reducedPatternExludedEdges.insert(
          fanIndex * reducedBaseTriangleNumberOfEdges + ei);
    }
  }

  // Construct reduced->full and full->reduced interface vi map
  g_reducedToFullInterfaceViMap.clear();
  vcg::tri::Allocator<FlatPattern>::PointerUpdater<FlatPattern::VertexPointer>
      pu_reducedModel;
  reducedPattern.deleteDanglingVertices(pu_reducedModel);
  const size_t reducedModelBaseTriangleInterfaceVi =
      pu_reducedModel.remap[baseTriangleInterfaceVi];
  const size_t reducedModelInterfaceVertexOffset =
      reducedPattern.VN() - 1 /*- reducedModelBaseTriangleInterfaceVi*/;
  reducedPattern.createFan();
  for (size_t fanIndex = 0; fanIndex < fanSize; fanIndex++) {
    g_reducedToFullInterfaceViMap[reducedModelInterfaceVertexOffset * fanIndex +
                                  reducedModelBaseTriangleInterfaceVi] =
        fullModelBaseTriangleInterfaceVi +
        fanIndex * fullPatternInterfaceVertexOffset;
  }
  m_fullToReducedInterfaceViMap.clear();
  constructInverseMap(g_reducedToFullInterfaceViMap,
                      m_fullToReducedInterfaceViMap);

  //  fullPattern.setLabel("FullPattern");
  //  reducedPattern.setLabel("ReducedPattern");
  // Create opposite vertex map
  m_fullPatternOppositeInterfaceViMap.clear();
  for (size_t fanIndex = 0; fanIndex < fanSize / 2; fanIndex++) {
    const size_t vi0 = fullModelBaseTriangleInterfaceVi +
                       fanIndex * fullPatternInterfaceVertexOffset;
    const size_t vi1 = vi0 + (fanSize / 2) * fullPatternInterfaceVertexOffset;
    assert(vi0 < fullPattern.VN() && vi1 < fullPattern.VN());
    m_fullPatternOppositeInterfaceViMap[vi0] = vi1;
  }

  const bool debugMapping = false;
  if (debugMapping) {
    reducedPattern.registerForDrawing();
    std::vector<glm::vec3> colors_reducedPatternExcludedEdges(
        reducedPattern.EN(), glm::vec3(0, 0, 0));
    for (const size_t ei : g_reducedPatternExludedEdges) {
      colors_reducedPatternExcludedEdges[ei] = glm::vec3(1, 0, 0);
    }
    const std::string label = reducedPattern.getLabel();
    polyscope::getCurveNetwork(label)
        ->addEdgeColorQuantity("Excluded edges",
                               colors_reducedPatternExcludedEdges)
        ->setEnabled(true);
    polyscope::show();

    std::vector<glm::vec3> nodeColorsOpposite(fullPattern.VN(),
                                              glm::vec3(0, 0, 0));
    for (const std::pair<size_t, size_t> oppositeVerts :
         m_fullPatternOppositeInterfaceViMap) {
      auto color = polyscope::getNextUniqueColor();
      nodeColorsOpposite[oppositeVerts.first] = color;
      nodeColorsOpposite[oppositeVerts.second] = color;
    }
    fullPattern.registerForDrawing();
    polyscope::getCurveNetwork(fullPattern.getLabel())
        ->addNodeColorQuantity("oppositeMap", nodeColorsOpposite)
        ->setEnabled(true);
    polyscope::show();

    std::vector<glm::vec3> nodeColorsReducedToFull_reduced(reducedPattern.VN(),
                                                           glm::vec3(0, 0, 0));
    std::vector<glm::vec3> nodeColorsReducedToFull_full(fullPattern.VN(),
                                                        glm::vec3(0, 0, 0));
    for (size_t vi = 0; vi < reducedPattern.VN(); vi++) {
      if (g_reducedToFullInterfaceViMap.contains(vi)) {

        auto color = polyscope::getNextUniqueColor();
        nodeColorsReducedToFull_reduced[vi] = color;
        nodeColorsReducedToFull_full[g_reducedToFullInterfaceViMap[vi]] = color;
      }
    }
    polyscope::getCurveNetwork(reducedPattern.getLabel())
        ->addNodeColorQuantity("reducedToFull_reduced",
                               nodeColorsReducedToFull_reduced)
        ->setEnabled(true);
    polyscope::getCurveNetwork(fullPattern.getLabel())
        ->addNodeColorQuantity("reducedToFull_full",
                               nodeColorsReducedToFull_full)
        ->setEnabled(true);
    polyscope::show();
  }
}

void ReducedModelOptimizer::createSimulationMeshes(FlatPattern &fullModel,
                                                   FlatPattern &reducedModel) {
  m_pReducedPatternElementalMesh =
      std::make_shared<SimulationMesh>(reducedModel);
  m_pReducedPatternElementalMesh->setBeamCrossSection(
      CrossSectionType{0.002, 0.002});
  m_pReducedPatternElementalMesh->setBeamMaterial(0.3, 1);
  m_pFullModelElementalMesh = std::make_shared<SimulationMesh>(fullModel);
  m_pFullModelElementalMesh->setBeamCrossSection(
      CrossSectionType{0.002, 0.002});
  m_pFullModelElementalMesh->setBeamMaterial(0.3, 1);
}

ReducedModelOptimizer::ReducedModelOptimizer(
    const std::vector<size_t> &numberOfNodesPerSlot) {
  FlatPatternTopology::constructNodeToSlotMap(numberOfNodesPerSlot, nodeToSlot);
  FlatPatternTopology::constructSlotToNodeMap(nodeToSlot, slotToNode);
}

void ReducedModelOptimizer::initialize(
    FlatPattern &fullPattern, FlatPattern &reducedPattern,
    const std::unordered_set<size_t> &reducedModelExcludedEdges) {
  assert(fullPattern.VN() == reducedPattern.VN() &&
         fullPattern.EN() >= reducedPattern.EN());
  polyscope::removeAllStructures();
  // Create copies of the input models
  FlatPattern copyFullPattern;
  FlatPattern copyReducedPattern;
  copyFullPattern.copy(fullPattern);
  copyReducedPattern.copy(reducedPattern);
  computeMaps(copyFullPattern, copyReducedPattern, reducedModelExcludedEdges);
  createSimulationMeshes(copyFullPattern, copyReducedPattern);
  initializeStiffnesses();
}

void ReducedModelOptimizer::initializeStiffnesses() {
  //  g_initialParameters.resize(1, 2);
  // Save save the beam stiffnesses
  //  for (size_t ei = 0; ei < pReducedModelElementalMesh->EN(); ei++) {
  //    Element &e = pReducedModelElementalMesh->elements[ei];
  //    if (g_reducedPatternExludedEdges.contains(ei)) {
  //      const double stiffnessFactor = 5;
  //      e.axialConstFactor *= stiffnessFactor;
  //      e.torsionConstFactor *= stiffnessFactor;
  //      e.firstBendingConstFactor *= stiffnessFactor;
  //      e.secondBendingConstFactor *= stiffnessFactor;
  //    }
  g_initialParameters =
      m_pReducedPatternElementalMesh->elements[0].properties.E;
  //  g_initialParameters(1) =
  //  pReducedModelElementalMesh->elements[0].properties.G;
  //  g_initialParameters(0, 0) =
  //      pReducedModelElementalMesh->elements[0].properties.A;
  //  g_initialParameters(0, 1) =
  //      pReducedModelElementalMesh->elements[0].properties.J;
  //  g_initialParameters(0, 2) =
  //      pReducedModelElementalMesh->elements[0].properties.I2;
  //  g_initialParameters(0, 3) =
  //      pReducedModelElementalMesh->elements[0].properties.I3;
  //  }
}

void ReducedModelOptimizer::computeReducedModelSimulationJob(
    const SimulationJob &simulationJobOfFullModel,
    SimulationJob &simulationJobOfReducedModel) {
  std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
      reducedModelFixedVertices;
  for (auto fullModelFixedVertex : simulationJobOfFullModel.fixedVertices) {
    reducedModelFixedVertices[m_fullToReducedInterfaceViMap.at(
        fullModelFixedVertex.first)] = fullModelFixedVertex.second;
  }

  std::unordered_map<VertexIndex, Vector6d> reducedModelNodalForces;
  for (auto fullModelNodalForce :
       simulationJobOfFullModel.nodalExternalForces) {
    reducedModelNodalForces[m_fullToReducedInterfaceViMap.at(
        fullModelNodalForce.first)] = fullModelNodalForce.second;
  }

  std::unordered_map<VertexIndex, VectorType> reducedModelNodalForcedNormals;
  for (auto fullModelNodalForcedRotation :
       simulationJobOfFullModel.nodalForcedNormals) {
    reducedModelNodalForcedNormals[m_fullToReducedInterfaceViMap.at(
        fullModelNodalForcedRotation.first)] =
        fullModelNodalForcedRotation.second;
  }

  simulationJobOfReducedModel = SimulationJob{m_pReducedPatternElementalMesh,
                                              reducedModelFixedVertices,
                                              reducedModelNodalForces,
                                              {},
                                              reducedModelNodalForcedNormals};
}

SimulationJob ReducedModelOptimizer::getReducedSimulationJob(
    const SimulationJob &fullModelSimulationJob) {
  SimulationJob reducedModelSimulationJob;
  computeReducedModelSimulationJob(fullModelSimulationJob,
                                   reducedModelSimulationJob);
  return reducedModelSimulationJob;
}

void ReducedModelOptimizer::computeDesiredReducedModelDisplacements(
    const SimulationResults &fullModelResults,
    Eigen::MatrixX3d &optimalDisplacementsOfReducedModel) {

  optimalDisplacementsOfReducedModel.resize(0, 3);
  optimalDisplacementsOfReducedModel.resize(
      m_pReducedPatternElementalMesh->VN(), 3);
  optimalDisplacementsOfReducedModel.setZero(
      optimalDisplacementsOfReducedModel.rows(),
      optimalDisplacementsOfReducedModel.cols());

  for (auto reducedFullViPair : g_reducedToFullInterfaceViMap) {
    const VertexIndex fullModelVi = reducedFullViPair.second;
    const Vector6d fullModelViDisplacements =
        fullModelResults.displacements[fullModelVi];
    optimalDisplacementsOfReducedModel.row(reducedFullViPair.first) =
        Eigen::Vector3d(fullModelViDisplacements[0],
                        fullModelViDisplacements[1],
                        fullModelViDisplacements[2]);
  }
}

Eigen::VectorXd ReducedModelOptimizer::optimizeForSimulationJob(
    const SimulationJob &fullPatternSimulationJob) {

  //  fullPatternSimulationJob.registerForDrawing();
  //  polyscope::show();
  gObjectiveValueHistory.clear();
  fullPatternSimulationJob.mesh->savePly(
      "Fanned_" + m_pFullModelElementalMesh->getLabel() + ".ply");
  SimulationResults fullModelResults =
      simulator.executeSimulation(fullPatternSimulationJob, false, true);
  fullModelResults.simulationLabel = "fullModel";
  fullModelResults.registerForDrawing(fullPatternSimulationJob);
  registerWorldAxes();
  polyscope::show();
  computeDesiredReducedModelDisplacements(fullModelResults,
                                          g_optimalReducedModelDisplacements);
  computeReducedModelSimulationJob(fullPatternSimulationJob,
                                   g_reducedPatternSimulationJob);
  //  gReducedPatternSimulationJob.registerForDrawing();
  //  polyscope::show();
  fullModelResults.registerForDrawing(fullPatternSimulationJob);
  polyscope::show();

  double (*pObjectiveFunction)(long, const double *) = &objective;
  const size_t n = 1;
  //      g_initialParameters.rows();
  const size_t npt = ((n + 2) + ((n + 1) * (n + 2) / 2)) / 2;
  assert(npt <= (n + 1) * (n + 2) / 2 && npt >= n + 2);
  assert(npt <= 2 * n + 1 && "The choice of the number of interpolation "
                             "conditions is not recommended.");
  // Set initial guess of solution
  const double initialGuess = 1;
  std::vector<double> x(n, initialGuess);
  //      {1, 5.9277};
  //      {initialGuess(0), initialGuess(1), initialGuess(2),
  //      initialGuess(3)};
  const double xMin = 1;
  const double xMax = 100;
  //  assert(x.end() == find_if(x.begin(), x.end(), [&](const double &d) {
  //           return d >= xMax || d <= xMin;
  //         }));
  std::vector<double> xLow(x.size(), xMin);
  std::vector<double> xUpper(x.size(), xMax);
  const double maxX = *std::max_element(
      x.begin(), x.end(),
      [](const double &a, const double &b) { return abs(a) < abs(b); });
  const double rhobeg = std::min(0.95, 0.2 * maxX);
  //  const double rhobeg = 10;
  const double rhoend = rhobeg * 1e-6;
  const size_t wSize = (npt + 5) * (npt + n) + 3 * n * (n + 5) / 2;
  std::vector<double> w(wSize);
  //  const size_t maxFun = 10 * (x.size() ^ 2);
  const size_t maxFun = 120;
  bobyqa(pObjectiveFunction, n, npt, x.data(), xLow.data(), xUpper.data(),
         rhobeg, rhoend, maxFun, w.data());

  SimulationResults reducedModelOptimizedResults =
      simulator.executeSimulation(g_reducedPatternSimulationJob);

  double error = 0;
  for (const auto reducedToFullViPair : g_reducedToFullInterfaceViMap) {
    const size_t reducedInterfaceVi = reducedToFullViPair.first;
    error +=
        (Eigen::Vector3d(
             g_optimalReducedModelDisplacements.row(reducedInterfaceVi)) -
         Eigen::Vector3d(
             reducedModelOptimizedResults.displacements[reducedInterfaceVi][0],
             reducedModelOptimizedResults.displacements[reducedInterfaceVi][1],
             reducedModelOptimizedResults.displacements[reducedInterfaceVi][2]))
            .norm();
  }

  std::cout << "Final objective value:" << error << std::endl;

  reducedModelOptimizedResults.simulationLabel = "reducedModel";
  reducedModelOptimizedResults.registerForDrawing(
      g_reducedPatternSimulationJob);
  polyscope::show();

  Eigen::VectorXd eigenX(x.size(), 1);
  for (size_t xi = 0; xi < x.size(); xi++) {
    eigenX(xi) = x[xi];
  }
  return eigenX;
}

std::vector<SimulationJob> ReducedModelOptimizer::createScenarios(
    const std::shared_ptr<SimulationMesh> &pMesh) {
  std::vector<SimulationJob> scenarios;
  std::unordered_map<VertexIndex, std::unordered_set<DoFType>> fixedVertices;
  std::unordered_map<VertexIndex, Vector6d> nodalForces;
  const double forceMagnitude = 1;
  // Assuming the patterns lays on the x-y plane
  const CoordType patternPlaneNormal(0, 0, 1);
  // Make the first interface node lay on the x axis
  const size_t fullPatternFirstInterfaceNodeIndex =
      m_fullPatternOppositeInterfaceViMap.begin()->second;
  CoordType fullPatternFirstInterfaceNodePosition =
      m_pFullModelElementalMesh->vert[fullPatternFirstInterfaceNodeIndex].cP();
  const vcg::Matrix33d R = vcg::RotationMatrix(
      fullPatternFirstInterfaceNodePosition,
      CoordType(fullPatternFirstInterfaceNodePosition.Norm(), 0, 0), false);
  std::for_each(m_pFullModelElementalMesh->vert.begin(),
                m_pFullModelElementalMesh->vert.end(), [&](auto &v) {
                  v.P() = R * v.P();
                  v.N() = R * v.N();
                });
  std::for_each(m_pReducedPatternElementalMesh->vert.begin(),
                m_pReducedPatternElementalMesh->vert.end(), [&](auto &v) {
                  v.P() = R * v.P();
                  v.N() = R * v.N();
                });
  m_pFullModelElementalMesh->updateEigenEdgeAndVertices();
  m_pReducedPatternElementalMesh->updateEigenEdgeAndVertices();

  //  //// Axial
  //  for (const auto &viPair : m_fullPatternOppositeInterfaceViMap) {
  //    CoordType forceDirection =
  //        (pMesh->vert[viPair.first].cP() - pMesh->vert[viPair.second].cP())
  //            .Normalize();
  //    nodalForces[viPair.first] = Vector6d({forceDirection[0],
  //    forceDirection[1],
  //                                          forceDirection[2], 0, 0, 0}) *
  //                                forceMagnitude * 10;
  //    fixedVertices[viPair.second] =
  //        std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
  //  }
  //  scenarios.push_back({pMesh, fixedVertices, nodalForces, {}});

  //  //// In-plane Bending
  //  fixedVertices.clear();
  //  nodalForces.clear();
  //  for (const auto &viPair : m_fullPatternOppositeInterfaceViMap) {
  //    CoordType v =
  //        (pMesh->vert[viPair.first].cP() - pMesh->vert[viPair.second].cP())
  //            .Normalize();
  //    CoordType forceDirection = (v ^ patternPlaneNormal).Normalize();
  //    nodalForces[viPair.first] = Vector6d({forceDirection[0],
  //    forceDirection[1],
  //                                          forceDirection[2], 0, 0, 0}) *
  //                                0.40 * forceMagnitude;
  //    fixedVertices[viPair.second] =
  //        std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
  //  }
  //  scenarios.push_back({pMesh, fixedVertices, nodalForces, {}});

  //  //// Torsion
  //  fixedVertices.clear();
  //  nodalForces.clear();
  //  for (auto viPairIt = m_fullPatternOppositeInterfaceViMap.begin();
  //       viPairIt != m_fullPatternOppositeInterfaceViMap.end(); viPairIt++) {
  //    const auto &viPair = *viPairIt;
  //    if (viPairIt == m_fullPatternOppositeInterfaceViMap.begin()) {
  //      CoordType v =
  //          (pMesh->vert[viPair.first].cP() - pMesh->vert[viPair.second].cP())
  //              .Normalize();
  //      CoordType normalVDerivativeDir = (v ^ patternPlaneNormal).Normalize();
  //      nodalForces[viPair.first] = Vector6d{
  //          0, 0, 0, normalVDerivativeDir[0], normalVDerivativeDir[1], 0};
  //      fixedVertices[viPair.second] =
  //          std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
  //      fixedVertices[viPair.first] = std::unordered_set<DoFType>{0, 1, 2};
  //    } else {
  //      fixedVertices[viPair.first] = std::unordered_set<DoFType>{0, 1, 2};
  //      fixedVertices[viPair.second] = std::unordered_set<DoFType>{0, 1, 2};
  //    }
  //  }
  //  scenarios.push_back({pMesh, fixedVertices, nodalForces});

  //  //  Out - of - plane bending.Pull towards Z
  //  fixedVertices.clear();
  //  nodalForces.clear();
  //  for (const auto &viPair : m_fullPatternOppositeInterfaceViMap) {
  //    nodalForces[viPair.first] = Vector6d({0, 0, forceMagnitude, 0, 0, 0}) *
  //    1; fixedVertices[viPair.second] =
  //        std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
  //  }
  //  scenarios.push_back({pMesh, fixedVertices, nodalForces, {}});

  //  //// Double using moments
  //  fixedVertices.clear();
  //  nodalForces.clear();
  //  for (auto viPairIt = m_fullPatternOppositeInterfaceViMap.begin();
  //       viPairIt != m_fullPatternOppositeInterfaceViMap.end(); viPairIt++) {
  //    const auto viPair = *viPairIt;
  //    if (viPairIt == m_fullPatternOppositeInterfaceViMap.begin()) {
  //      fixedVertices[viPair.first] = std::unordered_set<DoFType>{0, 1, 2};
  //      fixedVertices[viPair.second] = std::unordered_set<DoFType>{1, 2};
  //    } else {
  //      fixedVertices[viPair.first] = std::unordered_set<DoFType>{2};
  //      fixedVertices[viPair.second] = std::unordered_set<DoFType>{2};
  //    }
  //    CoordType v =
  //        (pMesh->vert[viPair.first].cP() - pMesh->vert[viPair.second].cP())
  //            .Normalize();
  //    nodalForces[viPair.first] =
  //        Vector6d({0, 0, 0, v[0], v[1], 0}) * forceMagnitude * 0.1;
  //    nodalForces[viPair.second] =
  //        Vector6d({0, 0, 0, -v[0], -v[1], 0}) * forceMagnitude * 0.1;
  //  }
  //  scenarios.push_back({pMesh, fixedVertices, nodalForces, {}});

  //// Saddle
  fixedVertices.clear();
  nodalForces.clear();
  for (auto viPairIt = m_fullPatternOppositeInterfaceViMap.begin();
       viPairIt != m_fullPatternOppositeInterfaceViMap.end(); viPairIt++) {
    const auto &viPair = *viPairIt;
    CoordType v =
        (pMesh->vert[viPair.first].cP() - pMesh->vert[viPair.second].cP())
            .Normalize();
    if (viPairIt == m_fullPatternOppositeInterfaceViMap.begin()) {
      nodalForces[viPair.first] =
          Vector6d({0, 0, 0, v[0], v[1], 0}) * 0.02 * forceMagnitude;
      nodalForces[viPair.second] =
          Vector6d({0, 0, 0, -v[0], -v[1], 0}) * 0.02 * forceMagnitude;
    } else {
      fixedVertices[viPair.first] = std::unordered_set<DoFType>{2};
      fixedVertices[viPair.second] = std::unordered_set<DoFType>{0, 1, 2};

      nodalForces[viPair.first] =
          Vector6d({0, 0, 0, -v[0], -v[1], 0}) * 0.01 * forceMagnitude;
      nodalForces[viPair.second] =
          Vector6d({0, 0, 0, v[0], v[1], 0}) * 0.01 * forceMagnitude;
    }
  }
  scenarios.push_back({pMesh, fixedVertices, nodalForces, {}});

  return scenarios;
}

Eigen::VectorXd ReducedModelOptimizer::optimize() {
  std::vector<SimulationJob> simulationJobs =
      createScenarios(m_pFullModelElementalMesh);
  std::vector<Eigen::VectorXd> results;
  for (const SimulationJob &job : simulationJobs) {
    polyscope::removeAllStructures();
    auto result = optimizeForSimulationJob(job);
    results.push_back(result);
  }

  if (results.empty()) {
    return Eigen::VectorXd();
  }

  return results[0];
}