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

const bool gShouldDraw = true;
size_t g_numberOfOptimizationRounds{0};
FormFinder simulator;
Eigen::MatrixX3d g_optimalReducedModelDisplacements;
SimulationJob gReducedPatternSimulationJob;
std::unordered_map<ReducedModelVertexIndex, FullModelVertexIndex>
    g_reducedToFullInterfaceViMap;
matplot::line_handle gPlotHandle;
std::vector<double> gObjectiveValueHistory;
Eigen::Vector4d g_initialX;
std::unordered_set<size_t> g_reducedPatternExludedEdges;
Eigen::MatrixX4d g_initialStiffnessFactors;

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 =
        gReducedPatternSimulationJob.mesh;
    for (EdgeIndex ei = 0; ei < reducedModel->EN(); ei++) {
      if (g_reducedPatternExludedEdges.contains(ei)) {
        continue;
      }
      Element &e = reducedModel->elements[ei];
      e.axialConstFactor *= x(0);
      e.torsionConstFactor *= x(1);
      e.firstBendingConstFactor *= x(2);
      e.secondBendingConstFactor *= x(3);
    }
    // run simulation
    SimulationResults reducedModelResults =
        simulator.executeSimulation(gReducedPatternSimulationJob);
    //    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 objective(long n, const double *x) {
  Eigen::VectorXd eigenX(n, 1);
  for (size_t xi = 0; xi < n; xi++) {
    eigenX(xi) = x[xi];
  }

  std::shared_ptr<SimulationMesh> reducedPattern =
      gReducedPatternSimulationJob.mesh;
  for (EdgeIndex ei = 0; ei < reducedPattern->EN(); ei++) {
    Element &e = reducedPattern->elements[ei];
    if (g_reducedPatternExludedEdges.contains(ei)) {
      continue;
    }
    e.axialConstFactor = g_initialStiffnessFactors(ei, 0) * eigenX(0);
    e.torsionConstFactor = g_initialStiffnessFactors(ei, 1) * eigenX(1);
    e.firstBendingConstFactor = g_initialStiffnessFactors(ei, 2) * eigenX(2);
    e.secondBendingConstFactor = g_initialStiffnessFactors(ei, 3) * eigenX(3);
  }
  // run simulation
  SimulationResults reducedModelResults =
      simulator.executeSimulation(gReducedPatternSimulationJob, false, false);

  // 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();
  }

  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) {
  pReducedModelElementalMesh = std::make_shared<SimulationMesh>(reducedModel);
  pFullModelElementalMesh = std::make_shared<SimulationMesh>(fullModel);
}

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();

  int i = 0;
  i++;
}

void ReducedModelOptimizer::initializeStiffnesses() {
  g_initialStiffnessFactors.resize(pReducedModelElementalMesh->EN(), 4);
  // 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 = 1;
    //      e.axialConstFactor *= stiffnessFactor;
    //      e.torsionConstFactor *= stiffnessFactor;
    //      e.firstBendingConstFactor *= stiffnessFactor;
    //      e.secondBendingConstFactor *= stiffnessFactor;
    //    }
    g_initialStiffnessFactors(ei, 0) = e.axialConstFactor;
    g_initialStiffnessFactors(ei, 1) = e.torsionConstFactor;
    g_initialStiffnessFactors(ei, 2) = e.firstBendingConstFactor;
    g_initialStiffnessFactors(ei, 3) = e.secondBendingConstFactor;
  }
}

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

  simulationJobOfReducedModel = SimulationJob{pReducedModelElementalMesh,
                                              reducedModelFixedVertices,
                                              reducedModelNodalForces,
                                              {}};
}

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

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

  optimalDisplacementsOfReducedModel.resize(pReducedModelElementalMesh->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 &fullModelSimulationJob) {

  gObjectiveValueHistory.clear();
  SimulationResults fullModelResults =
      simulator.executeSimulation(fullModelSimulationJob, false, false);
  fullModelResults.simulationLabel = "fullModel";
  computeDesiredReducedModelDisplacements(fullModelResults,
                                          g_optimalReducedModelDisplacements);
  computeReducedModelSimulationJob(fullModelSimulationJob,
                                   gReducedPatternSimulationJob);
  //  fullModelSimulationJob.registerForDrawing();
  //  polyscope::show();
  //  gReducedPatternSimulationJob.registerForDrawing();
  //  polyscope::show();
  fullModelResults.registerForDrawing(fullModelSimulationJob);
  polyscope::show();

  // Set initial guess of solution
  Eigen::VectorXd initialGuess(4);
  const double stifnessFactor = 1;
  initialGuess(0) = stifnessFactor;
  initialGuess(1) = stifnessFactor;
  initialGuess(2) = stifnessFactor;
  initialGuess(3) = stifnessFactor;

  const bool useGradientDescent = false;
  if (useGradientDescent) {

    //  gdc::GradientDescent<double, Objective,
    //  gdc::DecreaseBacktracking<double>,
    //                       OptimizationCallback>
    gdc::GradientDescent<double, Objective, gdc::BarzilaiBorwein<double>,
                         OptimizationCallback>
        //  gdc::GradientDescent<double, Objective,
        //  gdc::DecreaseBacktracking<double>,
        //                       OptimizationCallback>
        //  gdc::GradientDescent<double, Objective,
        //  gdc::DecreaseBacktracking<double>,
        //                       OptimizationCallback>
        optimizer;

    // Turn verbosity on, so the optimizer prints status updates after each
    // iteration.
    optimizer.setVerbosity(1);

    // Set initial guess.
    matplot::xlabel("Optimization iterations");
    matplot::ylabel("Objective value");
    //  matplot::figure(false);
    matplot::grid(matplot::on);
    // Start the optimization
    auto result = optimizer.minimize(initialGuess);

    std::cout << "Done! Converged: " << (result.converged ? "true" : "false")
              << " Iterations: " << result.iterations << std::endl;

    // do something with final function value
    std::cout << "Final fval: " << result.fval << std::endl;

    // do something with final x-value
    std::cout << "Final xval: " << result.xval.transpose() << std::endl;

    SimulationResults reducedModelOptimizedResults =
        simulator.executeSimulation(gReducedPatternSimulationJob);
    reducedModelOptimizedResults.simulationLabel = "reducedModel";
    reducedModelOptimizedResults.registerForDrawing(
        gReducedPatternSimulationJob);

    return result.xval;
  } else { // use bobyqa
    double (*pObjectiveFunction)(long, const double *) = &objective;
    const size_t n = 4;
    const size_t npt = 8;
    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.");
    std::vector<double> x
        //        {1.03424, 0.998456, 0.619916, -0.202997};
        {initialGuess(0), initialGuess(1), initialGuess(2), initialGuess(3)};
    std::vector<double> xLow(x.size(), -100);
    std::vector<double> xUpper(x.size(), 100);
    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 rhoend = rhobeg * 1e-6;
    const size_t wSize = (npt + 5) * (npt + n) + 3 * n * (n + 5) / 2;
    std::vector<double> w(wSize);
    bobyqa(pObjectiveFunction, n, npt, x.data(), xLow.data(), xUpper.data(),
           rhobeg, rhoend, 100, w.data());

    std::cout << "Final objective value:" << objective(n, x.data())
              << std::endl;

    Eigen::VectorXd eigenX(x.size(), 1);

    for (size_t xi = 0; xi < x.size(); xi++) {
      eigenX(xi) = x[xi];
    }

    SimulationResults reducedModelOptimizedResults =
        simulator.executeSimulation(gReducedPatternSimulationJob);
    reducedModelOptimizedResults.simulationLabel = "reducedModel";
    reducedModelOptimizedResults.registerForDrawing(
        gReducedPatternSimulationJob);
    polyscope::show();

    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 = 250;

  //  // 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;
  //    fixedVertices[viPair.second] =
  //        std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
  //  }
  //  scenarios.push_back({pMesh, fixedVertices, nodalForces, {}});

  //  // In-plane Bending
  //  // Assuming the patterns lay on the x-y plane
  //  const CoordType patternPlane(0, 0, 1);
  //  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 = patternPlane ^ v;
  //    nodalForces[viPair.first] = Vector6d({forceDirection[0],
  //    forceDirection[1],
  //                                          forceDirection[2], 0, 0, 0}) *
  //                                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();
  //        nodalForces[viPair.first] =
  //            Vector6d({0, 0, 0, v[1], v[0], 0}) * forceMagnitude;
  //      } else {
  //        fixedVertices[viPair.first] =
  //            std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
  //      }
  //      fixedVertices[viPair.second] =
  //          std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
  //    }
  //    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});
    fixedVertices[viPair.second] =
        std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
  }
  scenarios.push_back({pMesh, fixedVertices, nodalForces, {}});

  //  // Dou??
  //  fixedVertices.clear();
  //  nodalForces.clear();
  //  for (const auto &viPair : m_fullPatternOppositeInterfaceViMap) {
  //    CoordType v =
  //        (pMesh->vert[viPair.first].cP() - pMesh->vert[viPair.second].cP())
  //            .Normalize();
  //    CoordType momentDirection = patternPlane ^ v;
  //    nodalForces[viPair.first] =
  //        Vector6d({0, 0, 0, momentDirection[0], momentDirection[1], 0}) *
  //        forceMagnitude;
  //    fixedVertices[viPair.first] = std::unordered_set<DoFType>{2};
  //    fixedVertices[viPair.second] = std::unordered_set<DoFType>{0, 1, 2};
  //  }
  //  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();
  //    CoordType momentDirection = patternPlane ^ v;
  //    if (viPairIt == m_fullPatternOppositeInterfaceViMap.begin()) {
  //      nodalForces[viPair.first] =
  //          Vector6d({0, 0, 0, momentDirection[0], momentDirection[1], 0}) * 3
  //          * forceMagnitude;
  //      nodalForces[viPair.second] =
  //          Vector6d({0, 0, 0, momentDirection[0], momentDirection[1], 0}) * 3
  //          *
  //          (-forceMagnitude);
  //    } else {
  //      fixedVertices[viPair.first] = std::unordered_set<DoFType>{2};
  //      nodalForces[viPair.first] =
  //          Vector6d({0, 0, 0, momentDirection[0], momentDirection[1], 0}) *
  //          (-forceMagnitude);
  //      fixedVertices[viPair.second] = std::unordered_set<DoFType>{0, 1, 2};
  //    }
  //  }
  //  scenarios.push_back({pMesh, fixedVertices, nodalForces, {}});

  //  std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
  //      saddleFixedVertices;
  //  //  saddle_fixedVertices[3] = std::unordered_set<DoFType>{0, 1, 2};
  //  saddleFixedVertices[7] = std::unordered_set<DoFType>{0, 1, 2};
  //  saddleFixedVertices[11] = std::unordered_set<DoFType>{0, 1, 2};
  //  //  saddle_fixedVertices[15] = std::unordered_set<DoFType>{0, 1, 2};
  //  //  saddle_fixedVertices[19] = std::unordered_set<DoFType>{0, 1, 2};
  //  //  saddle_fixedVertices[23] = std::unordered_set<DoFType>{0, 1, 2};
  //  std::unordered_map<VertexIndex, Vector6d> saddleNodalForces{
  //      {15, {0, 0, 0, 0, -4 * 90, 0}}, {3, {0, 0, 0, 0, 4 * 90, 0}},
  //      {7, {0, 0, 0, 4 * 70, 0, 0}},   {11, {0, 0, 0, 4 * 70, 0, 0}},
  //      {19, {0, 0, 0, -4 * 70, 0, 0}}, {23, {0, 0, 0, -4 * 70, 0, 0}}};
  //  scenarios.push_back({pMesh, saddleFixedVertices, saddleNodalForces, {}});

  return scenarios;
}

Eigen::VectorXd ReducedModelOptimizer::optimize() {
  std::vector<SimulationJob> simulationJobs =
      createScenarios(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];
}