#include "reducedmodeloptimizer.hpp"
#include "chronoseulersimulationmodel.hpp"
#include "chronosigasimulationmodel.hpp"
#include "hexagonremesher.hpp"
#include "linearsimulationmodel.hpp"
#include "simulationhistoryplotter.hpp"
#include "simulationmodelfactory.hpp"
#include "trianglepatterngeometry.hpp"
#include "trianglepattterntopology.hpp"

#ifdef USE_ENSMALLEN
#include "ensmallen.hpp"
#endif
#include <atomic>
#include <chrono>
#include <execution>
#include <experimental/numeric>
#include <functional>
//#define USE_SCENARIO_WEIGHTS
#include <armadillo>

using namespace ReducedModelOptimization;

struct BaseScenarioMaxDisplacementHelperData {
  std::function<void(
      const double&,
      const std::vector<std::pair<PatternVertexIndex, PatternVertexIndex>>&,
      SimulationJob&)>
      constructScenarioFunction;
  double desiredMaxDisplacementValue;
  double desiredMaxRotationAngle;
  PatternVertexIndex interfaceViForComputingScenarioError;
  std::string currentScenarioName;
  std::shared_ptr<SimulationEdgeMesh> pFullPatternSimulationEdgeMesh;

  std::vector<std::pair<PatternVertexIndex, PatternVertexIndex>>
      fullPatternOppositeInterfaceViPairs;
} g_baseScenarioMaxDisplacementHelperData;

ReducedModelOptimizer::OptimizationState
    g_optimizationState;  // TODO: instead of having this global I could
                          // encapsulate it into a struct as I did for ensmallen

double ReducedModelOptimizer::computeDisplacementError(
    const std::vector<Vector6d>& fullPatternDisplacements,
    const std::vector<Vector6d>& reducedPatternDisplacements,
    const std::unordered_map<ReducedModelVertexIndex, PatternVertexIndex>&
        reducedToFullInterfaceViMap,
    const double& normalizationFactor) {
  const double rawError = computeRawTranslationalError(
      fullPatternDisplacements, reducedPatternDisplacements,
      reducedToFullInterfaceViMap);
  //    std::cout << "raw trans error:" << rawError << std::endl;
  //    std::cout << "raw trans error:" << normalizationFactor << std::endl;
  return rawError / normalizationFactor;
}

double ReducedModelOptimizer::computeRawTranslationalError(
    const std::vector<Vector6d>& fullPatternDisplacements,
    const std::vector<Vector6d>& reducedPatternDisplacements,
    const std::unordered_map<ReducedModelVertexIndex, PatternVertexIndex>&
        reducedToFullInterfaceViMap) {
  double error = 0;
  for (const auto reducedFullViPair : reducedToFullInterfaceViMap) {
    const VertexIndex reducedModelVi = reducedFullViPair.first;
    const VertexIndex fullModelVi = reducedFullViPair.second;
    const Eigen::Vector3d fullPatternVertexDiplacement =
        fullPatternDisplacements[fullModelVi].getTranslation();
    const Eigen::Vector3d reducedPatternVertexDiplacement =
        reducedPatternDisplacements[reducedModelVi].getTranslation();
    const double vertexError =
        (fullPatternVertexDiplacement - reducedPatternVertexDiplacement).norm();
    error += vertexError;
  }

  return error;
}

double ReducedModelOptimizer::computeRawRotationalError(
    const std::vector<Eigen::Quaternion<double>>& rotatedQuaternion_fullPattern,
    const std::vector<Eigen::Quaternion<double>>&
        rotatedQuaternion_reducedPattern,
    const std::unordered_map<ReducedModelVertexIndex, PatternVertexIndex>&
        reducedToFullInterfaceViMap) {
  double rawRotationalError = 0;
  for (const auto& reducedToFullInterfaceViPair : reducedToFullInterfaceViMap) {
    const double vertexRotationalError = std::abs(
        rotatedQuaternion_fullPattern[reducedToFullInterfaceViPair.second]
            .angularDistance(
                rotatedQuaternion_reducedPattern[reducedToFullInterfaceViPair
                                                     .first]));
    rawRotationalError += vertexRotationalError;
  }

  return rawRotationalError;
}

double ReducedModelOptimizer::computeRotationalError(
    const std::vector<Eigen::Quaternion<double>>& rotatedQuaternion_fullPattern,
    const std::vector<Eigen::Quaternion<double>>&
        rotatedQuaternion_reducedPattern,
    const std::unordered_map<ReducedModelVertexIndex, PatternVertexIndex>&
        reducedToFullInterfaceViMap,
    const double& normalizationFactor) {
  const double rawRotationalError = computeRawRotationalError(
      rotatedQuaternion_fullPattern, rotatedQuaternion_reducedPattern,
      reducedToFullInterfaceViMap);
  return rawRotationalError / normalizationFactor;
}

double ReducedModelOptimizer::computeError(
    const SimulationResults& simulationResults_fullPattern,
    const SimulationResults& simulationResults_reducedPattern,
    const std::unordered_map<ReducedModelVertexIndex, PatternVertexIndex>&
        reducedToFullInterfaceViMap,
    const double& normalizationFactor_translationalDisplacement,
    const double& normalizationFactor_rotationalDisplacement,
    const double& scenarioWeight,
    const Settings::ObjectiveWeights& objectiveWeights) {
  const double translationalError =
      computeDisplacementError(simulationResults_fullPattern.displacements,
                               simulationResults_reducedPattern.displacements,
                               reducedToFullInterfaceViMap,
                               normalizationFactor_translationalDisplacement);
  //    const double translationalError
  //        =
  //        computeRawTranslationalError(simulationResults_fullPattern.displacements,
  //                                       simulationResults_reducedPattern.displacements,
  //                                       reducedToFullInterfaceViMap);

  //    std::cout << "normalization factor:" <<
  //    normalizationFactor_rotationalDisplacement << std::endl;
  //        std::cout << "trans error:" << translationalError << std::endl;
  const double rotationalError = computeRotationalError(
      simulationResults_fullPattern.rotationalDisplacementQuaternion,
      simulationResults_reducedPattern.rotationalDisplacementQuaternion,
      reducedToFullInterfaceViMap, normalizationFactor_rotationalDisplacement);
  //    const double rotationalError
  //        =
  //        computeRawRotationalError(simulationResults_fullPattern.rotationalDisplacementQuaternion,
  //                                    simulationResults_reducedPattern.rotationalDisplacementQuaternion,
  //                                    reducedToFullInterfaceViMap);
  //        std::cout << "rot error:" << rotationalError<< std::endl;
  const double simulationError =
      objectiveWeights.translational * translationalError +
      objectiveWeights.rotational * rotationalError;
  assert(!std::isnan(simulationError));
  return simulationError * simulationError * scenarioWeight * scenarioWeight;
}

// double ReducedModelOptimizer::objective(double E,double A,double J,double
// I2,double I3,
//                                        double innerHexagonSize,
//                                        double innerHexagonRotationAngle) {
//    std::vector<double> x{E,A,J,I2,I3, innerHexagonSize,
//    innerHexagonRotationAngle}; return
//    ReducedModelOptimizer::objective(x.size(), x.data());
//}

#ifdef USE_ENSMALLEN
struct EnsmallenReducedModelOptimizationObjective {
  EnsmallenReducedModelOptimizationObjective(
      ReducedModelOptimizer::OptimizationState& optimizationState)
      : optimizationState(optimizationState) {}
  ReducedModelOptimizer::OptimizationState& optimizationState;
  double Evaluate(const arma::mat& x_arma) {
    for (int xi = 0; xi < x_arma.size(); xi++) {
      if (x_arma[xi] > optimizationState.xMax[xi]) {
        //#ifdef POLYSCOPE_DEFINED
        //                std::cout << "Out of range" << std::endl;
        //#endif
        return std::numeric_limits<double>::max();
        //                                return 1e10;
        //                x[xi] = optimizationState.xMax[xi];
      } else if (x_arma[xi] < optimizationState.xMin[xi]) {
        //#ifdef POLYSCOPE_DEFINED
        //                std::cout << "Out of range" << std::endl;
        //#endif
        return std::numeric_limits<double>::max();
        //                                return 1e10;
        //                x[xi] = optimizationState.xMin[xi];
      }
    }
    std::vector<double> x(x_arma.begin(), x_arma.end());
    return ReducedModelOptimizer::objective(x, optimizationState);
  }
};
#elif DLIB_DEFINED
double ReducedModelOptimizer::objective(const dlib::matrix<double, 0, 1>& x) {
  return objective(std::vector<double>(x.begin(), x.end()),
                   g_optimizationState);
}
#endif

// double ReducedModelOptimizer::objective(const double &xValue)
//{
//    return objective(std::vector<double>({xValue}));
//}

double ReducedModelOptimizer::objective(
    const std::vector<double>& x,
    ReducedModelOptimizer::OptimizationState& optimizationState) {
  //  std::cout.precision(17);
  //  for (int i = 0; i < x.size(); i++) {
  //    std::cout << x[i] << " ";
  //  }
  //  std::cout << std::endl;

  //    std::cout << x(x.size() - 2) << " " << x(x.size() - 1) << std::endl;
  //  const Element &e =
  //  optimizationState.reducedPatternSimulationJobs[0]->pMesh->elements[0];
  //  std::cout << e.axialConstFactor << " " << e.torsionConstFactor << " "
  //            << e.firstBendingConstFactor << " " <<
  //            e.secondBendingConstFactor
  //            << std::endl;
  //    const int n = x.size();
  std::shared_ptr<SimulationEdgeMesh>& pReducedPatternSimulationEdgeMesh =
      optimizationState
          .simulationJobs_reducedModel[optimizationState
                                           .simulationScenarioIndices[0]]
          ->pMesh;
  function_updateReducedPattern(x, pReducedPatternSimulationEdgeMesh);
  //    optimizationState.reducedPatternSimulationJobs[0]->pMesh->registerForDrawing();
  //    optimizationState.fullPatternSimulationJobs[0]->pMesh->registerForDrawing();
  //    polyscope::show();
  //    optimizationState.reducedPatternSimulationJobs[0]->pMesh->unregister();

  // run simulations
  std::vector<double> simulationErrorsPerScenario(
      totalNumberOfSimulationScenarios, 0);
  //    LinearSimulationModel simulator;
  //    simulator.setStructure(pReducedPatternSimulationEdgeMesh);
  //    simulator.initialize();
  //    FormFinder simulator;
  std::unique_ptr<SimulationModel> pReducedModelSimulator = nullptr;
  if (optimizationState.simulationModelLabel_reducedModel ==
      LinearSimulationModel::label) {
    pReducedModelSimulator = SimulationModelFactory::create(
        optimizationState.simulationModelLabel_reducedModel);
    pReducedModelSimulator->setStructure(pReducedPatternSimulationEdgeMesh);
  }

  std::for_each(
      std::execution::par_unseq,
      optimizationState.simulationScenarioIndices.begin(),
      optimizationState.simulationScenarioIndices.end(),
      [&](const int& simulationScenarioIndex) {
        //    for (const int simulationScenarioIndex :
        //    optimizationState.simulationScenarioIndices) {
        const std::shared_ptr<SimulationJob>& pSimulationJob_reducedModel =
            optimizationState
                .simulationJobs_reducedModel[simulationScenarioIndex];
        assert(pSimulationJob_reducedModel != nullptr);
        assert(pSimulationJob_reducedModel->pMesh != nullptr);
        assert(pSimulationJob_reducedModel->pMesh->VN() != 0);
        SimulationResults reducedModelResults;
        if (pReducedModelSimulator == nullptr) {
          std::unique_ptr<SimulationModel> pReducedModelSimulator =
              SimulationModelFactory::create(
                  optimizationState.simulationModelLabel_reducedModel);
          pReducedModelSimulator->setStructure(
              pSimulationJob_reducedModel->pMesh);

          reducedModelResults = pReducedModelSimulator->executeSimulation(
              pSimulationJob_reducedModel);
        } else {
          reducedModelResults = pReducedModelSimulator->executeSimulation(
              pSimulationJob_reducedModel);
        }

        //    std::string filename;
        if (!reducedModelResults.converged) {
          simulationErrorsPerScenario[simulationScenarioIndex] =
              std::numeric_limits<double>::max();
          optimizationState.numOfSimulationCrashes++;
#ifdef POLYSCOPE_DEFINED
          std::cout << "Failed simulation with x:" << std::endl;
          std::cout.precision(17);
          for (int i = 0; i < x.size(); i++) {
            std::cout << x[i] << " ";
          }
          std::cout << std::endl;
      //                pReducedPatternSimulationEdgeMesh->registerForDrawing();
      //                polyscope::show();
      //                pReducedPatternSimulationEdgeMesh->unregister();
      //                std::filesystem::create_directories("failedJob");
      //                reducedJob->save("failedJob");
      //                            Results debugRes;

#endif
        } else {
//                const double simulationScenarioError_PE = std::abs(
//                    (reducedModelResults.internalPotentialEnergy
//                     -
//                     optimizationState.fullPatternResults[simulationScenarioIndex].internalPotentialEnergy)
//                    /
//                    optimizationState.fullPatternResults[simulationScenarioIndex].internalPotentialEnergy);
//             else {
#ifdef POLYSCOPE_DEFINED
#ifdef USE_SCENARIO_WEIGHTS
          const double scenarioWeight =
              optimizationState.scenarioWeights[simulationScenarioIndex];
#else
          const double scenarioWeight = 1;
#endif
#else
          const double scenarioWeight = 1;
#endif
          const double simulationScenarioError_displacements = computeError(
              optimizationState.fullPatternResults[simulationScenarioIndex],
              reducedModelResults,
              optimizationState.reducedToFullInterfaceViMap,
              optimizationState.translationalDisplacementNormalizationValues
                  [simulationScenarioIndex],
              optimizationState.rotationalDisplacementNormalizationValues
                  [simulationScenarioIndex],
              scenarioWeight,
              optimizationState.objectiveWeights[simulationScenarioIndex]);

          simulationErrorsPerScenario[simulationScenarioIndex] =
              simulationScenarioError_displacements /*+
                                                       simulationScenarioError_PE*/
              ;
          //            }
          //                #ifdef POLYSCOPE_DEFINED
          //                            reducedJob->pMesh->registerForDrawing(Colors::reducedInitial);
          //                            reducedModelResults.registerForDrawing(Colors::reducedDeformed);
          //                            optimizationState.pFullPatternSimulationEdgeMesh->registerForDrawing(Colors::fullDeformed);
          //                            optimizationState.fullPatternResults[simulationScenarioIndex].registerForDrawing(
          //                                Colors::fullDeformed);
          //                            polyscope::show();
          //                            reducedModelResults.unregister();
          //                            optimizationState.pFullPatternSimulationEdgeMesh->unregister();
          //                            optimizationState.fullPatternResults[simulationScenarioIndex].unregister();
          //                #endif
        }
      });
  const double totalError = std::reduce(std::execution::par_unseq,
                                        simulationErrorsPerScenario.begin(),
                                        simulationErrorsPerScenario.end());
  //  std::cout << totalError << std::endl;
  //    optimizationState.objectiveValueHistory.push_back(totalError);
  //    optimizationState.plotColors.push_back(10);
  ++optimizationState.numberOfFunctionCalls;
  if (totalError < optimizationState.minY) {
    optimizationState.minY = totalError;
    //        optimizationState.objectiveValueHistory.push_back(totalError);
    //        optimizationState.objectiveValueHistory_iteration.push_back(optimizationState.numberOfFunctionCalls);
    optimizationState.minX.assign(x.begin(), x.end());
#ifdef POLYSCOPE_DEFINED
    std::cout << "New best:" << totalError << std::endl;
    std::cout.precision(17);
    for (int i = 0; i < x.size(); i++) {
      std::cout << x[i] << " ";
    }
    std::cout << std::endl;
#endif
    //        optimizationState.objectiveValueHistoryY.push_back(std::log(totalError));
    //        optimizationState.objectiveValueHistoryX.push_back(optimizationState.numberOfFunctionCalls);
    //        optimizationState.plotColors.push_back(0.1);
    //        auto xPlot = matplot::linspace(0,
    //                                       optimizationState.objectiveValueHistoryY.size(),
    //                                       optimizationState.objectiveValueHistoryY.size());
    //        optimizationState.gPlotHandle =
    //        matplot::scatter(optimizationState.objectiveValueHistoryX,
    //                                              optimizationState.objectiveValueHistoryY,
    //                                              4,
    //                                              optimizationState.plotColors);
    //        matplot::show();
    //                SimulationResultsReporter::createPlot("Number of Steps",
    //                                              "Objective value",
    //                                              optimizationState.objectiveValueHistoryY);
  }
#ifdef POLYSCOPE_DEFINED

#ifdef USE_ENSMALLEN
  if (optimizationState.numberOfFunctionCalls % 1000 == 0) {
    std::cout << "Number of function calls:"
              << optimizationState.numberOfFunctionCalls << std::endl;
  }
#else
  if (optimizationState.optimizationSettings.dlib.numberOfFunctionCalls >=
          100 &&
      optimizationState.numberOfFunctionCalls %
              (optimizationState.optimizationSettings.dlib
                   .numberOfFunctionCalls /
               100) ==
          0) {
    std::cout << "Number of function calls:"
              << optimizationState.numberOfFunctionCalls << std::endl;
  }

#endif
#endif
  //     compute error and return it
  return totalError;
}

void ReducedModelOptimizer::createSimulationEdgeMeshes(
    PatternGeometry& pattern,
    PatternGeometry& reducedModel,
    const Settings& optimizationSettings,
    std::shared_ptr<SimulationEdgeMesh>& pFullPatternSimulationEdgeMesh,
    std::shared_ptr<SimulationEdgeMesh>& pReducedPatternSimulationEdgeMesh) {
  if (typeid(CrossSectionType) != typeid(RectangularBeamDimensions)) {
    std::cerr << "Error: A rectangular cross section is expected." << std::endl;
    std::terminate();
  }

  // Full pattern
  pFullPatternSimulationEdgeMesh =
      std::make_shared<SimulationEdgeMesh>(pattern);
  pFullPatternSimulationEdgeMesh->setBeamCrossSection(
      optimizationSettings.beamDimensions_pattern);
  pFullPatternSimulationEdgeMesh->setBeamMaterial(
      0.3, optimizationSettings.youngsModulus_pattern);
  pFullPatternSimulationEdgeMesh->reset();

  // Reduced pattern
  pReducedPatternSimulationEdgeMesh =
      std::make_shared<SimulationEdgeMesh>(reducedModel);
  pReducedPatternSimulationEdgeMesh->setBeamCrossSection(
      optimizationSettings.beamDimensions_pattern);
  pReducedPatternSimulationEdgeMesh->setBeamMaterial(
      0.3, optimizationSettings.youngsModulus_pattern);
  pReducedPatternSimulationEdgeMesh->reset();
}

void ReducedModelOptimizer::createSimulationEdgeMeshes(
    PatternGeometry& fullModel,
    PatternGeometry& reducedModel,
    const Settings& optimizationSettings) {
  ReducedModelOptimizer::createSimulationEdgeMeshes(
      fullModel, reducedModel, optimizationSettings,
      m_pSimulationEdgeMesh_pattern, m_pReducedModelSimulationEdgeMesh);
}

void ReducedModelOptimizer::computeMaps(
    const std::unordered_map<size_t, std::unordered_set<size_t>>& slotToNode,
    PatternGeometry& fullPattern,
    ReducedModel& reducedModel,
    std::unordered_map<ReducedModelVertexIndex, PatternVertexIndex>&
        reducedToFullInterfaceViMap,
    std::unordered_map<PatternVertexIndex, ReducedModelVertexIndex>&
        fullToReducedInterfaceViMap,
    std::vector<std::pair<PatternVertexIndex, ReducedModelVertexIndex>>&
        fullPatternOppositeInterfaceViPairs) {
  // 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<VCGEdgeMesh>::PointerUpdater<
      PatternGeometry::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 TODO:this changes the optimizationState object. Should
  // this be a function parameter?
  //  optimizationState.reducedPatternExludedEdges.clear();
  //  const size_t reducedBaseTriangleNumberOfEdges = reducedPattern.EN();
  //  for (size_t fanIndex = 0; fanIndex < fanSize; fanIndex++) {
  //    for (const size_t ei : reducedModelExcludedEdges) {
  //      optimizationState.reducedPatternExludedEdges.insert(
  //          fanIndex * reducedBaseTriangleNumberOfEdges + ei);
  //    }
  //  }

  // Construct reduced->full and full->reduced interface vi map
  reducedToFullInterfaceViMap.clear();
  vcg::tri::Allocator<VCGEdgeMesh>::PointerUpdater<
      PatternGeometry::VertexPointer>
      pu_reducedModel;
  reducedModel.deleteDanglingVertices(pu_reducedModel);
  const size_t reducedModelBaseTriangleInterfaceVi =
      pu_reducedModel.remap[baseTriangleInterfaceVi];
  const size_t reducedModelInterfaceVertexOffset =
      reducedModel.VN() /*- 1*/ /*- reducedModelBaseTriangleInterfaceVi*/;
  Results::applyOptimizationResults_reducedModel_nonFanned(
      initialValue_R, initialValue_theta, baseTriangle, reducedModel);
  reducedModel.createFan();  // TODO: should be an input parameter from main
  for (size_t fanIndex = 0; fanIndex < fanCardinality; fanIndex++) {
    reducedToFullInterfaceViMap[reducedModelInterfaceVertexOffset * fanIndex +
                                reducedModelBaseTriangleInterfaceVi] =
        fullModelBaseTriangleInterfaceVi +
        fanIndex * fullPatternInterfaceVertexOffset;
  }
  fullToReducedInterfaceViMap.clear();
  constructInverseMap(reducedToFullInterfaceViMap, fullToReducedInterfaceViMap);

  // Create opposite vertex map
  fullPatternOppositeInterfaceViPairs.clear();
  fullPatternOppositeInterfaceViPairs.reserve(fanCardinality / 2);
  //    for (int fanIndex = fanSize / 2 - 1; fanIndex >= 0; fanIndex--) {
  for (int fanIndex = 0; fanIndex < fanCardinality / 2; fanIndex++) {
    const size_t vi0 = fullModelBaseTriangleInterfaceVi +
                       fanIndex * fullPatternInterfaceVertexOffset;
    const size_t vi1 =
        vi0 + (fanCardinality / 2) * fullPatternInterfaceVertexOffset;
    assert(vi0 < fullPattern.VN() && vi1 < fullPattern.VN());
    fullPatternOppositeInterfaceViPairs.emplace_back(std::make_pair(vi0, vi1));
  }

#if POLYSCOPE_DEFINED
  const bool debugMapping = false;
  if (debugMapping) {
    reducedModel.registerForDrawing();

    //    std::vector<glm::vec3> colors_reducedPatternExcludedEdges(
    //        reducedPattern.EN(), glm::vec3(0, 0, 0));
    //    for (const size_t ei : optimizationState.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 :
         fullPatternOppositeInterfaceViPairs) {
      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(reducedModel.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 < reducedModel.VN(); vi++) {
      if (optimizationState.reducedToFullInterfaceViMap.contains(vi)) {
        auto color = polyscope::getNextUniqueColor();
        nodeColorsReducedToFull_reduced[vi] = color;
        nodeColorsReducedToFull_full[optimizationState
                                         .reducedToFullInterfaceViMap[vi]] =
            color;
      }
    }
    polyscope::getCurveNetwork(reducedModel.getLabel())
        ->addNodeColorQuantity("reducedToFull_reduced",
                               nodeColorsReducedToFull_reduced)
        ->setEnabled(true);
    polyscope::getCurveNetwork(fullPattern.getLabel())
        ->addNodeColorQuantity("reducedToFull_full",
                               nodeColorsReducedToFull_full)
        ->setEnabled(true);
    polyscope::show();
  }
#endif
}

void ReducedModelOptimizer::computeMaps(PatternGeometry& fullPattern,
                                        ReducedModel& reducedModel) {
  ReducedModelOptimizer::computeMaps(
      slotToNode, fullPattern, reducedModel,
      optimizationState.reducedToFullInterfaceViMap,
      m_fullToReducedInterfaceViMap, m_fullPatternOppositeInterfaceViPairs);
}

ReducedModelOptimizer::ReducedModelOptimizer() {
  const std::vector<size_t> numberOfNodesPerSlot{1, 0, 0, 2, 1, 2, 1};
  FlatPatternTopology::constructNodeToSlotMap(numberOfNodesPerSlot, nodeToSlot);
  FlatPatternTopology::constructSlotToNodeMap(nodeToSlot, slotToNode);
  interfaceNodeIndex = numberOfNodesPerSlot[0] + numberOfNodesPerSlot[3];
  //    constructBaseScenarioFunctions.resize(BaseSimulationScenario::NumberOfBaseSimulationScenarios);
  scenarioIsSymmetrical.resize(
      BaseSimulationScenario::NumberOfBaseSimulationScenarios);
  constructBaseScenarioFunctions[BaseSimulationScenario::Axial] =
      &constructAxialSimulationScenario;
  //    constructBaseScenarioFunctions[BaseSimulationScenario::Axial] =
  //    &constructSSimulationScenario;
  scenarioIsSymmetrical[BaseSimulationScenario::Axial] = false;
  constructBaseScenarioFunctions[BaseSimulationScenario::Shear] =
      &constructShearSimulationScenario;
  scenarioIsSymmetrical[BaseSimulationScenario::Shear] = false;
  constructBaseScenarioFunctions[BaseSimulationScenario::Bending] =
      &constructBendingSimulationScenario;
  scenarioIsSymmetrical[BaseSimulationScenario::Bending] = true;
  constructBaseScenarioFunctions[BaseSimulationScenario::Dome] =
      &constructDomeSimulationScenario;
  scenarioIsSymmetrical[BaseSimulationScenario::Dome] = true;
  constructBaseScenarioFunctions[BaseSimulationScenario::Saddle] =
      &constructSaddleSimulationScenario;
  scenarioIsSymmetrical[BaseSimulationScenario::Saddle] = true;
  constructBaseScenarioFunctions[BaseSimulationScenario::S] =
      &constructSSimulationScenario;
  scenarioIsSymmetrical[BaseSimulationScenario::S] = true;
}

void ReducedModelOptimizer::initializePatterns(
    PatternGeometry& fullPattern,
    ReducedModel& reducedModel,
    const ReducedModelOptimization::Settings& optimizationSettings) {
  assert(fullPattern.VN() == reducedModel.VN() &&
         fullPattern.EN() >= reducedModel.EN());
  const std::array<xRange, NumberOfOptimizationVariables>&
      optimizationParameters = optimizationSettings.variablesRanges;

#ifdef POLYSCOPE_DEFINED
  polyscope::removeAllStructures();
#endif
  // Create copies of the input models
  PatternGeometry copyFullPattern;
  ReducedModel copyReducedModel;
  copyFullPattern.copy(fullPattern);
  copyReducedModel.copy(reducedModel);
#ifdef POLYSCOPE_DEFINED
  copyFullPattern.updateEigenEdgeAndVertices();
#endif
  copyReducedModel.updateEigenEdgeAndVertices();
  baseTriangle = copyReducedModel.getBaseTriangle();

  computeMaps(copyFullPattern, copyReducedModel);
  optimizationState.fullPatternOppositeInterfaceViPairs =
      m_fullPatternOppositeInterfaceViPairs;
  g_baseScenarioMaxDisplacementHelperData.fullPatternOppositeInterfaceViPairs =
      m_fullPatternOppositeInterfaceViPairs;
  createSimulationEdgeMeshes(copyFullPattern, copyReducedModel,
                             optimizationSettings);
  initializeUpdateReducedPatternFunctions();
  initializeOptimizationParameters(m_pSimulationEdgeMesh_pattern,
                                   optimizationParameters);
}

void ReducedModelOptimizer::optimize(
    ConstPatternGeometry& fullPattern,
    ReducedModelOptimization::Settings& optimizationSettings,
    ReducedModelOptimization::Results& optimizationResults) {
  // scale pattern and reduced model
  fullPattern.scale(optimizationSettings.targetBaseTriangleSize);
  ReducedModel reducedModel;
  reducedModel.scale(optimizationSettings.targetBaseTriangleSize);
  auto start = std::chrono::system_clock::now();
  optimizationResults.label = fullPattern.getLabel();
  optimizationResults.baseTrianglePattern.copy(fullPattern);
  initializePatterns(fullPattern, reducedModel, optimizationSettings);
  optimize(optimizationSettings, optimizationResults);
  optimizationResults.settings =
      optimizationSettings;  // NOTE: currently I set the max force base magn in
                             // optimize thus this must be called after the
                             // optimize function
  auto end = std::chrono::system_clock::now();
  auto elapsed =
      std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
  optimizationResults.time = elapsed.count() / 1000.0;
}

void ReducedModelOptimizer::initializeUpdateReducedPatternFunctions() {
  const vcg::Triangle3<double>& baseTriangle = this->baseTriangle;
  optimizationState.functions_updateReducedPatternParameter[R] =
      [=](const double& newR, std::shared_ptr<SimulationEdgeMesh>&
                                  pReducedModelSimulationEdgeMesh) {
        //        std::shared_ptr<VCGEdgeMesh> pReducedModel_edgeMesh =
        //        std::dynamic_pointer_cast<VCGEdgeMesh>(
        //            pReducedModelSimulationEdgeMesh);
        //        ReducedModel::updateFannedGeometry_R(newR,
        //        pReducedModel_edgeMesh);
        const CoordType barycentricCoordinates_hexagonBaseTriangleVertex(
            1 - newR, newR / 2, newR / 2);
        //            const vcg::Triangle3<double> &baseTriangle =
        //            getBaseTriangle();
        const CoordType hexagonBaseTriangleVertexPosition =
            baseTriangle.cP(0) *
                barycentricCoordinates_hexagonBaseTriangleVertex[0] +
            baseTriangle.cP(1) *
                barycentricCoordinates_hexagonBaseTriangleVertex[1] +
            baseTriangle.cP(2) *
                barycentricCoordinates_hexagonBaseTriangleVertex[2];

        //            constexpr int fanSize = 6;
        for (int rotationCounter = 0;
             rotationCounter < ReducedModelOptimizer::fanCardinality;
             rotationCounter++) {
          pReducedModelSimulationEdgeMesh->vert[2 * rotationCounter].P() =
              vcg::RotationMatrix(PatternGeometry::DefaultNormal,
                                  vcg::math::ToRad(60.0 * rotationCounter)) *
              hexagonBaseTriangleVertexPosition;
        }
      };

  optimizationState.functions_updateReducedPatternParameter[Theta] =
      [](const double& newTheta_degrees,
         std::shared_ptr<SimulationEdgeMesh>& pReducedModelSimulationEdgeMesh) {
        //            std::shared_ptr<VCGEdgeMesh> pReducedModel_edgeMesh
        //                =
        //                std::dynamic_pointer_cast<VCGEdgeMesh>(pReducedModelSimulationEdgeMesh);
        //            ReducedModel::updateFannedGeometry_theta(newTheta_degrees,
        //            pReducedModel_edgeMesh);

        const CoordType baseTriangleHexagonVertexPosition =
            pReducedModelSimulationEdgeMesh->vert[0].cP();
        const CoordType thetaRotatedHexagonBaseTriangleVertexPosition =
            vcg::RotationMatrix(PatternGeometry::DefaultNormal,
                                vcg::math::ToRad(newTheta_degrees)) *
            baseTriangleHexagonVertexPosition;
        for (int rotationCounter = 0;
             rotationCounter < ReducedModelOptimizer::fanCardinality;
             rotationCounter++) {
          pReducedModelSimulationEdgeMesh->vert[2 * rotationCounter].P() =
              vcg::RotationMatrix(PatternGeometry::DefaultNormal,
                                  vcg::math::ToRad(60.0 * rotationCounter)) *
              thetaRotatedHexagonBaseTriangleVertexPosition;
        }
      };

  optimizationState.functions_updateReducedPatternParameter[E] =
      [](const double& newE, std::shared_ptr<SimulationEdgeMesh>&
                                 pReducedPatternSimulationEdgeMesh) {
        for (EdgeIndex ei = 0; ei < pReducedPatternSimulationEdgeMesh->EN();
             ei++) {
          Element& e = pReducedPatternSimulationEdgeMesh->elements[ei];
          e.setMaterial(ElementMaterial(e.material.poissonsRatio, newE));
        }
      };
  optimizationState.functions_updateReducedPatternParameter[A] =
      [](const double& newA, std::shared_ptr<SimulationEdgeMesh>&
                                 pReducedPatternSimulationEdgeMesh) {
        assert(pReducedPatternSimulationEdgeMesh != nullptr);
        const double beamWidth = std::sqrt(newA);
        const double beamHeight = beamWidth;
        for (EdgeIndex ei = 0; ei < pReducedPatternSimulationEdgeMesh->EN();
             ei++) {
          Element& e = pReducedPatternSimulationEdgeMesh->elements[ei];
          e.setDimensions(CrossSectionType(beamWidth, beamHeight));
        }
      };

  optimizationState.functions_updateReducedPatternParameter[I2] =
      [](const double& newI2, std::shared_ptr<SimulationEdgeMesh>&
                                  pReducedPatternSimulationEdgeMesh) {
        for (EdgeIndex ei = 0; ei < pReducedPatternSimulationEdgeMesh->EN();
             ei++) {
          Element& e = pReducedPatternSimulationEdgeMesh->elements[ei];
          e.dimensions.inertia.I2 = newI2;
          e.updateRigidity();
        }
      };
  optimizationState.functions_updateReducedPatternParameter[I3] =
      [](const double& newI3, std::shared_ptr<SimulationEdgeMesh>&
                                  pReducedPatternSimulationEdgeMesh) {
        for (EdgeIndex ei = 0; ei < pReducedPatternSimulationEdgeMesh->EN();
             ei++) {
          Element& e = pReducedPatternSimulationEdgeMesh->elements[ei];
          e.dimensions.inertia.I3 = newI3;
          e.updateRigidity();
        }
      };
  optimizationState.functions_updateReducedPatternParameter[J] =
      [](const double& newJ, std::shared_ptr<SimulationEdgeMesh>&
                                 pReducedPatternSimulationEdgeMesh) {
        for (EdgeIndex ei = 0; ei < pReducedPatternSimulationEdgeMesh->EN();
             ei++) {
          Element& e = pReducedPatternSimulationEdgeMesh->elements[ei];
          e.dimensions.inertia.J = newJ;
          e.updateRigidity();
        }
      };
}

void ReducedModelOptimizer::initializeOptimizationParameters(
    const std::shared_ptr<SimulationEdgeMesh>& mesh,
    const std::array<ReducedModelOptimization::xRange,
                     ReducedModelOptimization::NumberOfOptimizationVariables>&
        optimizationParameters) {
  for (int optimizationParameterIndex = 0;
       optimizationParameterIndex < optimizationParameters.size();
       optimizationParameterIndex++) {
    for (int optimizationParameterIndex = E;
         optimizationParameterIndex != NumberOfOptimizationVariables;
         optimizationParameterIndex++) {
      //            const xRange &xOptimizationParameter =
      //            optimizationParameters[optimizationParameterIndex];
      switch (optimizationParameterIndex) {
        case E:
          optimizationState.parametersInitialValue[optimizationParameterIndex] =
              mesh->elements[0].material.youngsModulus;
          optimizationState
              .optimizationInitialValue[optimizationParameterIndex] = 1;
          break;
        case A:
          optimizationState.parametersInitialValue[optimizationParameterIndex] =
              mesh->elements[0].dimensions.A;
          optimizationState
              .optimizationInitialValue[optimizationParameterIndex] = 1;
          break;
        case I2:
          optimizationState.parametersInitialValue[optimizationParameterIndex] =
              mesh->elements[0].dimensions.inertia.I2;
          optimizationState
              .optimizationInitialValue[optimizationParameterIndex] = 1;
          break;
        case I3:
          optimizationState.parametersInitialValue[optimizationParameterIndex] =
              mesh->elements[0].dimensions.inertia.I3;
          optimizationState
              .optimizationInitialValue[optimizationParameterIndex] = 1;
          break;
        case J:
          optimizationState.parametersInitialValue[optimizationParameterIndex] =
              mesh->elements[0].dimensions.inertia.J;
          optimizationState
              .optimizationInitialValue[optimizationParameterIndex] = 1;
          break;
        case R:
          optimizationState.parametersInitialValue[optimizationParameterIndex] =
              0;
          optimizationState
              .optimizationInitialValue[optimizationParameterIndex] = 0.5;
          break;
        case Theta:
          optimizationState.parametersInitialValue[optimizationParameterIndex] =
              0;
          optimizationState
              .optimizationInitialValue[optimizationParameterIndex] = 0;
          break;
      }
    }
  }
}

void ReducedModelOptimizer::computeReducedModelSimulationJob(
    const SimulationJob& simulationJobOfFullModel,
    const std::unordered_map<size_t, size_t>& fullToReducedMap,
    SimulationJob& simulationJobOfReducedModel) {
  assert(simulationJobOfReducedModel.pMesh->VN() != 0);
  std::unordered_map<VertexIndex,
                     std::unordered_set<DRMSimulationModel::DoFType>>
      reducedModelFixedVertices;
  for (auto fullModelFixedVertex :
       simulationJobOfFullModel.constrainedVertices) {
    reducedModelFixedVertices[fullToReducedMap.at(fullModelFixedVertex.first)] =
        fullModelFixedVertex.second;
  }

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

  std::unordered_map<VertexIndex, Eigen::Vector3d>
      reducedNodalForcedDisplacements;
  for (auto fullForcedDisplacement :
       simulationJobOfFullModel.nodalForcedDisplacements) {
    reducedNodalForcedDisplacements[fullToReducedMap.at(
        fullForcedDisplacement.first)] = fullForcedDisplacement.second;
  }

  simulationJobOfReducedModel.constrainedVertices = reducedModelFixedVertices;
  simulationJobOfReducedModel.nodalExternalForces = reducedModelNodalForces;
  simulationJobOfReducedModel.label = simulationJobOfFullModel.getLabel();
  simulationJobOfReducedModel.nodalForcedDisplacements =
      reducedNodalForcedDisplacements;
}

void ReducedModelOptimizer::computeDesiredReducedModelDisplacements(
    const SimulationResults& fullModelResults,
    const std::unordered_map<size_t, size_t>& displacementsReducedToFullMap,
    Eigen::MatrixX3d& optimalDisplacementsOfReducedModel) {
  assert(optimalDisplacementsOfReducedModel.rows() != 0 &&
         optimalDisplacementsOfReducedModel.cols() == 3);
  optimalDisplacementsOfReducedModel.setZero(
      optimalDisplacementsOfReducedModel.rows(),
      optimalDisplacementsOfReducedModel.cols());

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

void ReducedModelOptimizer::getResults(
    const FunctionEvaluation& optimalObjective,
    const Settings& settings,
    ReducedModelOptimization::Results& results) {
  //    std::shared_ptr<SimulationEdgeMesh> &pReducedPatternSimulationEdgeMesh
  //        =
  //        g_optimizationState.reducedPatternSimulationJobs[global.simulationScenarioIndices[0]]
  //              ->pMesh;
  // Number of crashes
  //    results.numberOfSimulationCrashes =
  //    optimizationState.numOfSimulationCrashes;
  // Value of optimal objective Y
  results.objectiveValue.total = optimalObjective.y;
  //    results.objectiveValue.total = 0;
  if (std::abs(optimalObjective.y - optimizationState.minY) > 1e-1) {
    std::cout << "Different optimal y:" << optimalObjective.y << " "
              << optimizationState.minY << std::endl;
  }
  // Optimal X values
  results.optimalXNameValuePairs.resize(NumberOfOptimizationVariables);
  for (int optimizationParameterIndex = E;
       optimizationParameterIndex != NumberOfOptimizationVariables;
       optimizationParameterIndex++) {
    results.optimalXNameValuePairs[optimizationParameterIndex] = std::make_pair(
        settings.variablesRanges[optimizationParameterIndex].label,
        optimalObjective.x[optimizationParameterIndex]);
  }

  std::unordered_set<OptimizationParameterIndex> optimizationParametersUsed;
  for (const std::vector<OptimizationParameterIndex>& parameterGroup :
       settings.optimizationStrategy) {
    optimizationParametersUsed.insert(parameterGroup.begin(),
                                      parameterGroup.end());
  }

  for (int optimizationParameterIndex = E;
       optimizationParameterIndex != NumberOfOptimizationVariables;
       optimizationParameterIndex++) {
    if (optimizationParameterIndex != OptimizationParameterIndex::R &&
        optimizationParameterIndex != OptimizationParameterIndex::Theta) {
      results.optimalXNameValuePairs[optimizationParameterIndex] =
          std::make_pair(
              settings.variablesRanges[optimizationParameterIndex].label,
              optimalObjective.x[optimizationParameterIndex] *
                  optimizationState
                      .parametersInitialValue[optimizationParameterIndex]);
    } else {
      // Hex size and angle are pure values (not multipliers of the initial
      // values)
      results.optimalXNameValuePairs[optimizationParameterIndex] =
          std::make_pair(
              settings.variablesRanges[optimizationParameterIndex].label,
              optimalObjective.x[optimizationParameterIndex]);
    }

    optimizationState
        .functions_updateReducedPatternParameter[optimizationParameterIndex](
            results.optimalXNameValuePairs[optimizationParameterIndex].second,
            m_pReducedModelSimulationEdgeMesh);  // NOTE:due to this call this
                                                 // function is not const
#ifdef POLYSCOPE_DEFINED
    std::cout
        << results.optimalXNameValuePairs[optimizationParameterIndex].first
        << ":"
        << results.optimalXNameValuePairs[optimizationParameterIndex].second
        << " ";
#endif
  }
#ifdef POLYSCOPE_DEFINED
  std::cout << std::endl;
  m_pReducedModelSimulationEdgeMesh->updateEigenEdgeAndVertices();
#endif
  m_pReducedModelSimulationEdgeMesh->reset();
  // Compute obj value per simulation scenario and the raw objective value
  //    updateMeshFunction(optimalX);
  //    results.objectiveValue.totalPerSimulationScenario.resize(totalNumberOfSimulationScenarios);
  // TODO:use push_back it will make the code more readable
  results.objectiveValue.totalRaw = 0;
  results.objectiveValue.perSimulationScenario_translational.resize(
      optimizationState.simulationScenarioIndices.size());
  results.objectiveValue.perSimulationScenario_rawTranslational.resize(
      optimizationState.simulationScenarioIndices.size());
  results.objectiveValue.perSimulationScenario_rotational.resize(
      optimizationState.simulationScenarioIndices.size());
  results.objectiveValue.perSimulationScenario_rawRotational.resize(
      optimizationState.simulationScenarioIndices.size());
  results.objectiveValue.perSimulationScenario_total.resize(
      optimizationState.simulationScenarioIndices.size());
  results.objectiveValue.perSimulationScenario_total_unweighted.resize(
      optimizationState.simulationScenarioIndices.size());
  //#ifdef POLYSCOPE_DEFINED
  //    optimizationState.pFullPatternSimulationEdgeMesh->registerForDrawing(Colors::fullDeformed);
  //#endif

  results.perScenario_fullPatternPotentialEnergy.resize(
      optimizationState.simulationScenarioIndices.size());
  //    reducedModelSimulator.setStructure(m_pReducedModelSimulationEdgeMesh);
  for (int i = 0; i < optimizationState.simulationScenarioIndices.size(); i++) {
    const int simulationScenarioIndex =
        optimizationState.simulationScenarioIndices[i];
    std::shared_ptr<SimulationJob>& pSimulationJob_reducedModel =
        optimizationState.simulationJobs_reducedModel[simulationScenarioIndex];
    //    ChronosEulerSimulationModel reducedModelSimulator;
    //    reducedModelSimulator.settings.analysisType =
    //        ChronosEulerSimulationModel::Settings::AnalysisType::Linear;
    //    SimulationResults reducedModelResults =
    //        reducedModelSimulator.executeSimulation(pReducedJob);
    std::unique_ptr<SimulationModel> pReducedModelSimulator =
        SimulationModelFactory::create(
            optimizationState.simulationModelLabel_reducedModel);
    pReducedModelSimulator->setStructure(pSimulationJob_reducedModel->pMesh);
    SimulationResults reducedModelResults =
        pReducedModelSimulator->executeSimulation(pSimulationJob_reducedModel);

#ifdef POLYSCOPE_DEFINED
#ifdef USE_SCENARIO_WEIGHTS
    const double scenarioWeight =
        optimizationState.scenarioWeights[simulationScenarioIndex];
#else
    const double scenarioWeight = 1;
#endif
#else
    const double scenarioWeight = 1;
#endif
    results.objectiveValue.perSimulationScenario_total[i] = computeError(
        optimizationState.fullPatternResults[simulationScenarioIndex],
        reducedModelResults, optimizationState.reducedToFullInterfaceViMap,
        optimizationState.translationalDisplacementNormalizationValues
            [simulationScenarioIndex],
        optimizationState
            .rotationalDisplacementNormalizationValues[simulationScenarioIndex],
        scenarioWeight,
        optimizationState.objectiveWeights[simulationScenarioIndex]);

    results.objectiveValue
        .perSimulationScenario_total_unweighted[i] = computeError(
        optimizationState.fullPatternResults[simulationScenarioIndex],
        reducedModelResults, optimizationState.reducedToFullInterfaceViMap,
        optimizationState.translationalDisplacementNormalizationValues
            [simulationScenarioIndex],
        optimizationState
            .rotationalDisplacementNormalizationValues[simulationScenarioIndex],
        1, optimizationState.objectiveWeights[simulationScenarioIndex]);

    //        results.objectiveValue.total +=
    //        results.objectiveValue.perSimulationScenario_total[i];
    // Raw translational
    const double rawTranslationalError = computeRawTranslationalError(
        optimizationState.fullPatternResults[simulationScenarioIndex]
            .displacements,
        reducedModelResults.displacements,
        optimizationState.reducedToFullInterfaceViMap);
    results.objectiveValue.perSimulationScenario_rawTranslational[i] =
        rawTranslationalError;
    // Raw rotational
    const double rawRotationalError = computeRawRotationalError(
        optimizationState.fullPatternResults[simulationScenarioIndex]
            .rotationalDisplacementQuaternion,
        reducedModelResults.rotationalDisplacementQuaternion,
        optimizationState.reducedToFullInterfaceViMap);
    results.objectiveValue.perSimulationScenario_rawRotational[i] =
        rawRotationalError;

    // Normalized translational
    const double normalizedTranslationalError = computeDisplacementError(
        optimizationState.fullPatternResults[simulationScenarioIndex]
            .displacements,
        reducedModelResults.displacements,
        optimizationState.reducedToFullInterfaceViMap,
        optimizationState.translationalDisplacementNormalizationValues
            [simulationScenarioIndex]);
    results.objectiveValue.perSimulationScenario_translational[i] =
        normalizedTranslationalError;
    //            const double test_normalizedTranslationError =
    //            computeDisplacementError(
    //                optimizationState.fullPatternResults[simulationScenarioIndex].displacements,
    //                reducedModelResults.displacements,
    //                optimizationState.reducedToFullInterfaceViMap,
    //                optimizationState.translationalDisplacementNormalizationValues[simulationScenarioIndex]);
    // Normalized rotational
    const double normalizedRotationalError = computeRotationalError(
        optimizationState.fullPatternResults[simulationScenarioIndex]
            .rotationalDisplacementQuaternion,
        reducedModelResults.rotationalDisplacementQuaternion,
        optimizationState.reducedToFullInterfaceViMap,
        optimizationState.rotationalDisplacementNormalizationValues
            [simulationScenarioIndex]);
    results.objectiveValue.perSimulationScenario_rotational[i] =
        normalizedRotationalError;
    //            const double test_normalizedRotationalError =
    //            computeRotationalError(
    //                optimizationState.fullPatternResults[simulationScenarioIndex].rotationalDisplacementQuaternion,
    //                reducedModelResults.rotationalDisplacementQuaternion,
    //                optimizationState.reducedToFullInterfaceViMap,
    //                optimizationState.rotationalDisplacementNormalizationValues[simulationScenarioIndex]);
    //            assert(test_normalizedTranslationError ==
    //            normalizedTranslationalError);
    //            assert(test_normalizedRotationalError ==
    //            normalizedRotationalError);
    results.objectiveValue.totalRaw +=
        rawTranslationalError + rawRotationalError;
    results.perScenario_fullPatternPotentialEnergy[i] =
        optimizationState.fullPatternResults[simulationScenarioIndex]
            .internalPotentialEnergy;
#ifdef POLYSCOPE_DEFINED_
    std::cout << "Simulation scenario:"
              << optimizationState
                     .simulationJobs_reducedModel[simulationScenarioIndex]
                     ->getLabel()
              << std::endl;
    std::cout << "raw translational error:" << rawTranslationalError
              << std::endl;
    std::cout << "translation normalization value:"
              << optimizationState.translationalDisplacementNormalizationValues
                     [simulationScenarioIndex]
              << std::endl;
    std::cout << "raw Rotational error:" << rawRotationalError << std::endl;
    std::cout << "rotational normalization value:"
              << optimizationState.rotationalDisplacementNormalizationValues
                     [simulationScenarioIndex]
              << std::endl;
    std::cout << "Translational error:" << normalizedTranslationalError
              << std::endl;
    std::cout << "Rotational error:" << normalizedRotationalError << std::endl;
    //        results.objectiveValuePerSimulationScenario[simulationScenarioIndex]
    //            = normalizedTranslationalError + normalizedRotationalError;
    std::cout << "Total Error value:"
              << results.objectiveValue.perSimulationScenario_total[i]
              << std::endl;
    std::cout << std::endl;

    reducedModelResults.registerForDrawing(Colors::reducedDeformed, true, 0.01);
    optimizationState.fullPatternResults[simulationScenarioIndex]
        .registerForDrawing(Colors::patternDeformed, true, 0.01);
    ChronosEulerNonLinearSimulationModel debug_chronosModel;
    auto debug_chronosResults = debug_chronosModel.executeSimulation(
        optimizationState.pSimulationJobs_pattern[simulationScenarioIndex]);
    debug_chronosResults.registerForDrawing();
    //        debug_chronosResults.setLabelPrefix("chron");
    polyscope::show();
    debug_chronosResults.unregister();
    reducedModelResults.unregister();
    optimizationState.fullPatternResults[simulationScenarioIndex].unregister();

#endif
  }

  for (int simulationScenarioIndex :
       optimizationState.simulationScenarioIndices) {
    results.pSimulationJobs_pattern.push_back(
        optimizationState.pSimulationJobs_pattern[simulationScenarioIndex]);
    results.pSimulationJobs_reducedModel.push_back(
        optimizationState.simulationJobs_reducedModel[simulationScenarioIndex]);
    //        const std::string temp =
    //        optimizationState.reducedPatternSimulationJobs[simulationScenarioIndex]
    //                                     ->pMesh->getLabel();
    //        optimizationState.reducedPatternSimulationJobs[simulationScenarioIndex]->pMesh->setLabel("temp");
    //        optimizationState.reducedPatternSimulationJobs[simulationScenarioIndex]->pMesh->registerForDrawing();
    //        optimizationState.reducedPatternSimulationJobs[simulationScenarioIndex]->pMesh->setLabel(temp);
  }
  results.objectiveValueHistory = optimizationState.objectiveValueHistory;
  results.objectiveValueHistory_iteration =
      optimizationState.objectiveValueHistory_iteration;
  //    results.draw();
}

#ifdef DLIB_DEFINED
std::array<double, NumberOfBaseSimulationScenarios>
ReducedModelOptimizer::computeFullPatternMaxSimulationForces(
    const std::vector<BaseSimulationScenario>& desiredBaseSimulationScenario)
    const {
  std::array<double, NumberOfBaseSimulationScenarios>
      fullPatternMaxSimulationForces{0};
  for (const BaseSimulationScenario& scenario : desiredBaseSimulationScenario) {
    const double maxForce = computeFullPatternMaxSimulationForce(scenario);
    fullPatternMaxSimulationForces[scenario] = maxForce;
  }

  return fullPatternMaxSimulationForces;
}

std::array<double, NumberOfBaseSimulationScenarios>
ReducedModelOptimizer::getFullPatternMaxSimulationForces(
    const std::vector<BaseSimulationScenario>&
        desiredBaseSimulationScenarioIndices,
    const std::filesystem::path& intermediateResultsDirectoryPath,
    const bool& recomputeForceMagnitudes) {
  std::array<double, NumberOfBaseSimulationScenarios>
      fullPatternSimulationScenarioMaxMagnitudes;
  //#ifdef POLYSCOPE_DEFINED
  const std::filesystem::path forceMagnitudesDirectoryPath(
      std::filesystem::path(intermediateResultsDirectoryPath)
          .append("ForceMagnitudes"));
  std::filesystem::path patternMaxForceMagnitudesFilePath(
      std::filesystem::path(forceMagnitudesDirectoryPath)
          .append(m_pFullPatternSimulationEdgeMesh->getLabel() + ".json"));
  const bool fullPatternScenarioMagnitudesExist =
      std::filesystem::exists(patternMaxForceMagnitudesFilePath);
  if (fullPatternScenarioMagnitudesExist && !recomputeForceMagnitudes) {
    nlohmann::json json;
    std::ifstream ifs(patternMaxForceMagnitudesFilePath.string());
    ifs >> json;
    fullPatternSimulationScenarioMaxMagnitudes =
        static_cast<std::array<double, NumberOfBaseSimulationScenarios>>(
            json.at("maxMagn"));
    return fullPatternSimulationScenarioMaxMagnitudes;
  }

  //#endif
  fullPatternSimulationScenarioMaxMagnitudes =
      computeFullPatternMaxSimulationForces(
          desiredBaseSimulationScenarioIndices);

  //#ifdef POLYSCOPE_DEFINED
  nlohmann::json json;
  json["maxMagn"] = fullPatternSimulationScenarioMaxMagnitudes;

  std::filesystem::create_directories(forceMagnitudesDirectoryPath);
  std::ofstream jsonFile(patternMaxForceMagnitudesFilePath.string());
  jsonFile << json;

#ifdef POLYSCOPE_DEFINED
  std::cout << "Saved base scenario max magnitudes to:"
            << patternMaxForceMagnitudesFilePath << std::endl;
#endif

  //#endif
  assert(fullPatternSimulationScenarioMaxMagnitudes.size() ==
         desiredBaseSimulationScenarioIndices.size());

  return fullPatternSimulationScenarioMaxMagnitudes;
}
#endif

void ReducedModelOptimizer::runOptimization(
    const Settings& settings,
    ReducedModelOptimization::Results& results) {
  optimizationState.simulationModelLabel_reducedModel =
      settings.simulationModelLabel_reducedModel;
  optimizationState.objectiveValueHistory.clear();
  optimizationState.objectiveValueHistory_iteration.clear();
  optimizationState.objectiveValueHistory.reserve(
      settings.dlib.numberOfFunctionCalls / 100);
  optimizationState.objectiveValueHistory_iteration.reserve(
      settings.dlib.numberOfFunctionCalls / 100);
  optimizationState.minY = std::numeric_limits<double>::max();
  optimizationState.numOfSimulationCrashes = 0;
  optimizationState.numberOfFunctionCalls = 0;
  optimizationState.pFullPatternSimulationEdgeMesh =
      m_pSimulationEdgeMesh_pattern;

#if POLYSCOPE_DEFINED
//    optimizationState.plotColors.reserve(settings.numberOfFunctionCalls);
#endif
  assert(!settings.optimizationStrategy.empty());
  const std::vector<std::vector<OptimizationParameterIndex>>&
      optimizationParametersGroups = settings.optimizationStrategy;

#ifndef USE_ENSMALLEN
  const int totalNumberOfOptimizationParameters = std::accumulate(
      optimizationParametersGroups.begin(), optimizationParametersGroups.end(),
      0,
      [](const int& sum,
         const std::vector<OptimizationParameterIndex>& parameterGroup) {
        return sum + parameterGroup.size();
      });
#endif

  FunctionEvaluation optimization_optimalResult;
  optimization_optimalResult.x.resize(NumberOfOptimizationVariables, 0);
  for (int optimizationParameterIndex = E;
       optimizationParameterIndex != NumberOfOptimizationVariables;
       optimizationParameterIndex++) {
    optimization_optimalResult.x[optimizationParameterIndex] =
        optimizationState.optimizationInitialValue[optimizationParameterIndex];
  }

  for (int parameterGroupIndex = 0;
       parameterGroupIndex < optimizationParametersGroups.size();
       parameterGroupIndex++) {
    const std::vector<OptimizationParameterIndex>& parameterGroup =
        optimizationParametersGroups[parameterGroupIndex];
    FunctionEvaluation parameterGroup_optimalResult;
    // Set update function. TODO: Make this function immutable by defining it
    // once and using the optimizationState variable to set parameterGroup
    function_updateReducedPattern = [&](const std::vector<double>& x,
                                        std::shared_ptr<SimulationEdgeMesh>&
                                            pMesh) {
      for (int xIndex = 0; xIndex < parameterGroup.size(); xIndex++) {
        const OptimizationParameterIndex parameterIndex =
            parameterGroup[xIndex];
        //                const double
        //                parameterInitialValue=optimizationSettings.parameterRanges[parameterIndex].initialValue;
        const double parameterNewValue = [&]() {
          if (parameterIndex == R || parameterIndex == Theta) {
            return x[xIndex] /*+ parameterInitialValue*/;
          }
          // and the material parameters exponentially(?).TODO: Check what
          // happens if I make all linear
          const double parameterInitialValue =
              optimizationState.parametersInitialValue[parameterIndex];
          return x[xIndex] * parameterInitialValue;
        }();
        //                    std::cout << "Optimization parameter:" <<
        //                    parameterIndex << std::endl; std::cout << "New
        //                    value:" << parameterNewValue << std::endl;
        optimizationState
            .functions_updateReducedPatternParameter[parameterIndex](
                parameterNewValue, pMesh);
      }
      pMesh->reset();  // NOTE: I could put this code into each updateParameter
                       // function for avoiding unessecary calculations
    };

    std::vector<double> xMin;
    std::vector<double> xMax;
    xMin.resize(parameterGroup.size());
    xMax.resize(parameterGroup.size());
    for (int xIndex = 0; xIndex < parameterGroup.size(); xIndex++) {
      const OptimizationParameterIndex parameterIndex = parameterGroup[xIndex];

      xMin[xIndex] = settings.variablesRanges[parameterIndex].min;
      xMax[xIndex] = settings.variablesRanges[parameterIndex].max;
    }
#ifdef USE_ENSMALLEN
#ifdef POLYSCOPE_DEFINED
    std::cout << "Optimizing using ensmallen" << std::endl;
#endif
    arma::mat x(parameterGroup.size(), 1);
    for (int xIndex = 0; xIndex < parameterGroup.size(); xIndex++) {
      const OptimizationParameterIndex parameterIndex = parameterGroup[xIndex];

      x(xIndex, 0) = optimizationState.optimizationInitialValue[parameterIndex];
    }

    optimizationState.xMin = xMin;
    optimizationState.xMax = xMax;

    EnsmallenReducedModelOptimizationObjective optimizationFunction(
        optimizationState);
//            optimizationFunction.optimizationState = optimizationState;
// Set min max values
//            ens::CNE optimizer;
//            ens::DE optimizer;
//            ens::SPSA optimizer;
#ifdef USE_PSO
    arma::mat xMin_arma(optimizationState.xMin);
    arma::mat xMax_arma(optimizationState.xMax);
    //            ens::LBestPSO optimizer(64, xMin_arma, xMax_arma, 1000);
    ens::LBestPSO optimizer(settings.pso.numberOfParticles, xMin_arma,
                            xMax_arma, 1e5, 500, settings.solverAccuracy, 2.05,
                            2.35);
#else
    //            ens::SA optimizer;
    ens::SA<> optimizer(ens::ExponentialSchedule(), 100000, 10000., 1000, 100,
                        settings.solverAccuracy, 3, 20, 0.3, 0.3);
#endif
    //        ens::LBestPSO optimizer;
    parameterGroup_optimalResult.y =
        optimizer.Optimize(optimizationFunction, x);
    //        for (int xIndex = 0; xIndex < parameterGroup.size(); xIndex++) {
    //            if (x[xIndex] > optimizationState.xMax[xIndex]) {
    //                x[xIndex] = optimizationState.xMax[xIndex];
    //            } else if (x[xIndex] < optimizationState.xMin[xIndex]) {
    //                x[xIndex] = optimizationState.xMin[xIndex];
    //            }
    //        }

#ifdef POLYSCOPE_DEFINED
    std::cout << "optimal x:"
              << "\n"
              << x << std::endl;
    std::cout << "optimal objective:" << optimizationFunction.Evaluate(x)
              << std::endl;
    std::cout << "optimal objective using state var:"
              << optimizationFunction.Evaluate(optimizationState.minX)
              << std::endl;
#endif
    parameterGroup_optimalResult.x.clear();
    parameterGroup_optimalResult.x.resize(parameterGroup.size());
    //        std::copy(x.begin(), x.end(),
    //        parameterGroup_optimalResult.x.begin());
    std::copy(optimizationState.minX.begin(), optimizationState.minX.end(),
              parameterGroup_optimalResult.x.begin());
#else
    g_optimizationState = optimizationState;
#ifdef POLYSCOPE_DEFINED
    std::cout << "Optimizing using dlib" << std::endl;
#endif
    // Set min max values
    dlib::matrix<double, 0, 1> xMin_dlib(parameterGroup.size());
    dlib::matrix<double, 0, 1> xMax_dlib(parameterGroup.size());
    for (int xIndex = 0; xIndex < parameterGroup.size(); xIndex++) {
      const OptimizationParameterIndex parameterIndex = parameterGroup[xIndex];

      xMin_dlib(xIndex) = settings.variablesRanges[parameterIndex].min;
      xMax_dlib(xIndex) = settings.variablesRanges[parameterIndex].max;
    }
    const double portionOfTotalFunctionCallsBudget = [&]() {
      if (settings.optimizationVariablesGroupsWeights.has_value()) {
        assert(settings.optimizationVariablesGroupsWeights->size() ==
               settings.optimizationStrategy.size());
        return settings.optimizationVariablesGroupsWeights
            .value()[parameterGroupIndex];

      } else {
        return static_cast<double>(parameterGroup.size()) /
               totalNumberOfOptimizationParameters;
      }
    }();
    const int optimizationNumberOfFunctionCalls =
        portionOfTotalFunctionCallsBudget * settings.dlib.numberOfFunctionCalls;
    const dlib::function_evaluation optimalResult_dlib = [&]() {
      if (parameterGroup.size() == 1) {
        dlib::function_evaluation result;
        double optimalX =
            optimizationState.optimizationInitialValue[parameterGroup[0]];
        double (*singleVariableObjective)(const double&) = &objective;
        try {
          result.y = dlib::find_min_single_variable(
              singleVariableObjective, optimalX, xMin_dlib(0), xMax_dlib(0),
              1e-5, optimizationNumberOfFunctionCalls);
        } catch (const std::exception&
                     e) {  // reference to the base of a polymorphic object
#ifdef POLYSCOPE_DEFINED
          std::cout << e.what() << std::endl;
#endif
        }

        result.x.set_size(1);
        result.x(0) = optimalX;
        return result;
      }
      double (*objF)(const dlib::matrix<double, 0, 1>&) = &objective;
      return dlib::find_min_global(
          objF, xMin_dlib, xMax_dlib,
          dlib::max_function_calls(optimizationNumberOfFunctionCalls),
          std::chrono::hours(24 * 365 * 290), settings.solverAccuracy);
    }();
    //        constexpr bool useBOBYQA = false;
    //        if (useBOBYQA) {
    //        const size_t npt = 2 *
    //        optimizationState.numberOfOptimizationParameters;
    //        //      ((n + 2) + ((n + 1) * (n + 2) / 2)) / 2;
    //        const double rhobeg = 0.1;
    //        //  const double rhobeg = 10;
    //        const double rhoend = rhobeg * 1e-6;
    //        //  const size_t maxFun = 10 * (x.size() ^ 2);
    //        const size_t bobyqa_maxFunctionCalls = 10000;
    //        dlib::matrix<double, 1, 0> x;
    //        dlib::function_evaluation optimalResult_dlib;
    //        optimalResult_dlib.x.set_size(parameterGroup.size());
    //        for (int xIndex = 0; xIndex < parameterGroup.size(); xIndex++) {
    //            const OptimizationParameterIndex parameterIndex =
    //            parameterGroup[xIndex]; optimalResult_dlib.x(xIndex) =
    //            optimizationState.optimizationInitialValue[parameterIndex];

    //            std::cout << "xIndex:" << xIndex << std::endl;
    //            std::cout << "xInit:" << optimalResult_dlib.x(xIndex) <<
    //            std::endl;
    //        }

    //        optimalResult_dlib.y = dlib::find_min_bobyqa(objF,
    //                                                     optimalResult_dlib.x,
    //                                                     npt,
    //                                                     xMin,
    //                                                     xMax,
    //                                                     rhobeg,
    //                                                     rhoend,
    //                                                     bobyqa_maxFunctionCalls);
    //        }

    parameterGroup_optimalResult.x.clear();
    parameterGroup_optimalResult.x.resize(parameterGroup.size());
    std::copy(optimalResult_dlib.x.begin(), optimalResult_dlib.x.end(),
              parameterGroup_optimalResult.x.begin());
    parameterGroup_optimalResult.y = optimalResult_dlib.y;

    optimizationState = g_optimizationState;
#endif

    const auto firstNonNullReducedSimulationJobIt =
        std::find_if(optimizationState.simulationJobs_reducedModel.begin(),
                     optimizationState.simulationJobs_reducedModel.end(),
                     [](const std::shared_ptr<SimulationJob>& pJob) {
                       return pJob != nullptr;
                     });
    function_updateReducedPattern(
        std::vector<double>(parameterGroup_optimalResult.x.begin(),
                            parameterGroup_optimalResult.x.end()),
        (*firstNonNullReducedSimulationJobIt)
            ->pMesh);  // TODO: Check if its ok to update only the changed
                       // parameters
#ifdef POLYSCOPE_DEFINED
    std::cout << "Optimal x:"
              << "\n";
    for (const auto& x : parameterGroup_optimalResult.x) {
      std::cout << x << std::endl;
    }
    std::cout << "optimal objective:"
              << objective(parameterGroup_optimalResult.x, optimizationState)
              << std::endl;
    //        std::cout << "Minima:" << minima << std::endl;
    std::cout << "optimizationState MIN:" << optimizationState.minY
              << std::endl;
    std::cout << "opt res y:" << parameterGroup_optimalResult.y << std::endl;
#endif

    optimization_optimalResult.y = parameterGroup_optimalResult.y;
    for (int xIndex = 0; xIndex < parameterGroup.size(); xIndex++) {
      const OptimizationParameterIndex parameterIndex = parameterGroup[xIndex];
      optimization_optimalResult.x[parameterIndex] =
          parameterGroup_optimalResult.x[xIndex];
    }
  }
  getResults(optimization_optimalResult, settings, results);
}

void ReducedModelOptimizer::constructAxialSimulationScenario(
    const double& forceMagnitude,
    const std::vector<std::pair<PatternVertexIndex, PatternVertexIndex>>&
        oppositeInterfaceViPairs,
    SimulationJob& job) {
  for (auto viPairIt = oppositeInterfaceViPairs.begin();
       viPairIt != oppositeInterfaceViPairs.end(); viPairIt++) {
    if (viPairIt != oppositeInterfaceViPairs.begin()) {
      CoordType forceDirection(1, 0, 0);
      job.nodalExternalForces[viPairIt->first] =
          Vector6d({forceDirection[0], forceDirection[1], forceDirection[2], 0,
                    0, 0}) *
          forceMagnitude;
      job.constrainedVertices[viPairIt->second] =
          std::unordered_set<DRMSimulationModel::DoFType>{0, 1, 2, 3, 4, 5};
    }
  }
}

void ReducedModelOptimizer::constructSSimulationScenario(
    const double& forceMagnitude,
    const std::vector<std::pair<PatternVertexIndex, PatternVertexIndex>>&
        oppositeInterfaceViPairs,
    SimulationJob& job) {
  for (auto viPairIt = oppositeInterfaceViPairs.begin();
       viPairIt != oppositeInterfaceViPairs.end(); viPairIt++) {
    if (viPairIt == oppositeInterfaceViPairs.begin()) {
      job.constrainedVertices[viPairIt->first] =
          std::unordered_set<DRMSimulationModel::DRMSimulationModel::DoFType>{
              0, 1, 2, 3, 4, 5};
      job.constrainedVertices[viPairIt->second] =
          std::unordered_set<DRMSimulationModel::DRMSimulationModel::DoFType>{
              0, 1, 2, 3, 4, 5};
    } else {
      CoordType forceDirection(0, 0, 1);
      job.nodalExternalForces[viPairIt->first] =
          Vector6d({0, 0, 1, 0, 0, 0}) * forceMagnitude;
      job.nodalExternalForces[viPairIt->second] =
          Vector6d({0, 0, -1, 0, 0, 0}) * forceMagnitude;
    }
  }
  job.label = "Axial";
}

void ReducedModelOptimizer::constructShearSimulationScenario(
    const double& forceMagnitude,
    const std::vector<std::pair<PatternVertexIndex, PatternVertexIndex>>&
        oppositeInterfaceViPairs,
    SimulationJob& job) {
  for (auto viPairIt = oppositeInterfaceViPairs.begin();
       viPairIt != oppositeInterfaceViPairs.end(); viPairIt++) {
    if (viPairIt != oppositeInterfaceViPairs.begin()) {
      CoordType forceDirection(0, 1, 0);
      const auto viPair = *viPairIt;
      job.nodalExternalForces[viPair.first] =
          Vector6d({forceDirection[0], forceDirection[1], forceDirection[2], 0,
                    0, 0}) *
          forceMagnitude;
      job.constrainedVertices[viPair.second] =
          std::unordered_set<DRMSimulationModel::DoFType>{0, 1, 2, 3, 4, 5};
    }
  }
  job.label = "Shear";
}

void ReducedModelOptimizer::constructBendingSimulationScenario(
    const double& forceMagnitude,
    const std::vector<std::pair<PatternVertexIndex, PatternVertexIndex>>&
        oppositeInterfaceViPairs,
    SimulationJob& job) {
  for (const auto& viPair : oppositeInterfaceViPairs) {
    job.nodalExternalForces[viPair.first] =
        Vector6d({0, 0, 1, 0, 0, 0}) * forceMagnitude;
    job.constrainedVertices[viPair.second] =
        std::unordered_set<DRMSimulationModel::DoFType>{0, 1, 2, 3, 4, 5};
  }
  job.label = "Bending";
}

/*NOTE: From the results it seems as if the forced displacements are different
 * in the linear and in the drm
 * */
void ReducedModelOptimizer::constructDomeSimulationScenario(
    const double& forceMagnitude,
    const std::vector<std::pair<PatternVertexIndex, PatternVertexIndex>>&
        oppositeInterfaceViPairs,
    SimulationJob& job) {
  for (auto viPairIt = oppositeInterfaceViPairs.begin();
       viPairIt != oppositeInterfaceViPairs.end(); viPairIt++) {
    const std::pair<PatternVertexIndex, PatternVertexIndex> viPair = *viPairIt;
    const CoordType interfaceVector = (job.pMesh->vert[viPair.first].cP() -
                                       job.pMesh->vert[viPair.second].cP());
    const VectorType momentAxis =
        vcg::RotationMatrix(VectorType(0, 0, 1), vcg::math::ToRad(90.0)) *
        interfaceVector.normalized();
    //        if (viPairIt == oppositeInterfaceViPairs.begin()) {
    //            //            job.nodalForcedDisplacements[viPair.first] =
    //            Eigen::Vector3d(-interfaceVector[0],
    //            // -interfaceVector[1],
    //            // 0)
    //            //                                                         *
    //            0.01
    //            /** std::abs(
    //                                  forceMagnitude)*/
    //            //                ; //NOTE:Should the forced displacement
    //            change relatively to the magnitude?
    //            //                              * std::abs(forceMagnitude /
    //            maxForceMagnitude_dome);
    //            //            job.nodalForcedDisplacements[viPair.second] =
    //            Eigen::Vector3d(interfaceVector[0],
    //            // interfaceVector[1],
    //            // 0)
    //            //                                                          *
    //            0.01 /** std::abs(forceMagnitude)*/;
    //            //                  * std::abs(forceMagnitude /
    //            maxForceMagnitude_dome);
    //            //            CoordType v = (pMesh->vert[viPair.first].cP() -
    //            pMesh->vert[viPair.second].cP())
    //            //                          ^ CoordType(0, 0, -1).Normalize();
    //            //            nodalForces[viPair.first] = Vector6d({0, 0, 0,
    //            v[0], v[1], 0}) * forceMagnitude
    //            //                                        * 0.0001;
    //            //            nodalForces[viPair.second] = Vector6d({0, 0, 0,
    //            -v[0], -v[1], 0}) * forceMagnitude
    //            //                                         * 0.0001;
    //                        job.constrainedVertices[viPair.first] = {0, 1, 2};
    //                        job.constrainedVertices[viPair.second] = {2};
    //            job.constrainedVertices[viPair.first] =
    //            std::unordered_set<DRMSimulationModel::DoFType>{0, 1, 2};
    //            job.constrainedVertices[viPair.second] =
    //            std::unordered_set<DRMSimulationModel::DoFType>{0, 2};
    //        } else {
    //            job.nodalExternalForces[viPair.first]
    //                = Vector6d({0, 0, 0, momentAxis[0], momentAxis[1],
    //                momentAxis[2]}) * forceMagnitude
    //                  / 3;
    //            job.nodalExternalForces[viPair.second]
    //                = Vector6d({0, 0, 0, -momentAxis[0], -momentAxis[1],
    //                -momentAxis[2]})
    //                          * forceMagnitude / 3;
    //            job.constrainedVertices[viPair.first] =
    //            std::unordered_set<DRMSimulationModel::DoFType>{2};
    //            job.constrainedVertices[viPair.second] =
    //            std::unordered_set<DRMSimulationModel::DoFType>{2};
    //        }
    job.nodalExternalForces[viPair.first] =
        Vector6d({0, 0, 0, momentAxis[0], momentAxis[1], momentAxis[2]}) *
        forceMagnitude / 3;
    job.nodalExternalForces[viPair.second] =
        Vector6d({0, 0, 0, -momentAxis[0], -momentAxis[1], -momentAxis[2]}) *
        forceMagnitude / 3;
    job.constrainedVertices[viPair.first] =
        std::unordered_set<DRMSimulationModel::DoFType>{0, 1, 2};
    job.constrainedVertices[viPair.second] =
        std::unordered_set<DRMSimulationModel::DoFType>{2};
  }
  job.label = "Dome";
}

void ReducedModelOptimizer::constructSaddleSimulationScenario(
    const double& forceMagnitude,
    const std::vector<std::pair<PatternVertexIndex, PatternVertexIndex>>&
        oppositeInterfaceViPairs,
    SimulationJob& job) {
  for (auto viPairIt = oppositeInterfaceViPairs.begin();
       viPairIt != oppositeInterfaceViPairs.end(); viPairIt++) {
    const auto& viPair = *viPairIt;
    CoordType v = (job.pMesh->vert[viPair.first].cP() -
                   job.pMesh->vert[viPair.second].cP()) ^
                  CoordType(0, 0, -1).Normalize();
    if (viPairIt == oppositeInterfaceViPairs.begin()) {
      job.nodalExternalForces[viPair.first] =
          Vector6d({0, 0, 0, v[0], v[1], 0}) * forceMagnitude;
      job.nodalExternalForces[viPair.second] =
          Vector6d({0, 0, 0, -v[0], -v[1], 0}) * forceMagnitude;
    } else {
      job.constrainedVertices[viPair.first] =
          std::unordered_set<DRMSimulationModel::DoFType>{2};
      job.constrainedVertices[viPair.second] =
          std::unordered_set<DRMSimulationModel::DoFType>{0, 1, 2};

      job.nodalExternalForces[viPair.first] =
          Vector6d({0, 0, 0, -v[0], -v[1], 0}) * forceMagnitude / 2;
      job.nodalExternalForces[viPair.second] =
          Vector6d({0, 0, 0, v[0], v[1], 0}) * forceMagnitude / 2;
    }
  }
  job.label = "Saddle";
}

// double fullPatternMaxSimulationForceRotationalObjective(const double
// &forceMagnitude)
//{
//    SimulationJob job;
//    job.pMesh =
//    g_baseScenarioMaxDisplacementHelperData.pFullPatternSimulationEdgeMesh;
//    g_baseScenarioMaxDisplacementHelperData.constructScenarioFunction(
//        forceMagnitude,
//        g_baseScenarioMaxDisplacementHelperData.fullPatternOppositeInterfaceViPairs,
//        job);
//    //    ReducedModelOptimizer::axialSimulationScenario(forceMagnitude,
//    // optimizationState.fullPatternInterfaceViPairs,
//    //                                                   job);

//    ChronosEulerSimulationModel simulator;
//    SimulationResults results =
//    simulator.executeSimulation(std::make_shared<SimulationJob>(job));
//    //    DRMSimulationModel simulator;
//    //    DRMSimulationModel::Settings settings;
//    //    //    settings.threshold_averageResidualToExternalForcesNorm = 1e-2;
//    //    settings.totalTranslationalKineticEnergyThreshold = 1e-10;
//    //    //    settings.viscousDampingFactor = 1e-2;
//    //    //    settings.useKineticDamping = true;
//    //    settings.shouldDraw = false;
//    //    settings.debugModeStep = 200;
//    //    //    settings.threshold_averageResidualToExternalForcesNorm = 1e-5;
//    //    settings.maxDRMIterations = 200000;
//    //    SimulationResults results =
//    simulator.executeSimulation(std::make_shared<SimulationJob>(job),
//    //                                                            settings);
//    const double &desiredRotationAngle
//        = g_baseScenarioMaxDisplacementHelperData
//              .desiredMaxRotationAngle; //TODO: move from OptimizationState to
//              a new struct
//    double error;
//    //    job.pMesh->setLabel("initial");
//    //    job.pMesh->registerForDrawing();
//    //    results.registerForDrawing();
//    //    polyscope::show();
//    std::string saveJobToPath;
//    if (!results.converged) {
//        //        std::cout << "Force used:" << forceMagnitude << std::endl;
//        error = std::numeric_limits<double>::max();
//        //        DRMSimulationModel::Settings debugSimulationSettings;
//        //        debugSimulationSettings.isDebugMode = true;
//        //        debugSimulationSettings.debugModeStep = 1000;
//        //        //            debugSimulationSettings.maxDRMIterations =
//        100000;
//        //        debugSimulationSettings.shouldDraw = true;
//        //        debugSimulationSettings.drawingStep =
//        debugSimulationSettings.debugModeStep;
//        //        debugSimulationSettings.shouldCreatePlots = true;
//        //        //            debugSimulationSettings.Dtini = 0.06;
//        //        debugSimulationSettings.beVerbose = true;
//        //        debugSimulationSettings.useAverage = true;
//        //        //
//        debugSimulationSettings.threshold_averageResidualToExternalForcesNorm
//        = 1e-3;
//        //
//        debugSimulationSettings.shouldUseTranslationalKineticEnergyThreshold =
//        true;
//        //        auto debugResults =
//        simulator.executeSimulation(std::make_shared<SimulationJob>(job),
//        // debugSimulationSettings);
//        //        std::terminate();
//        saveJobToPath = "../nonConvergingJobs";
//    } else {
//        error = std::abs(
//            results
//                .rotationalDisplacementQuaternion[g_baseScenarioMaxDisplacementHelperData
//                                                      .interfaceViForComputingScenarioError]
//                .angularDistance(Eigen::Quaterniond::Identity())
//            - desiredRotationAngle);
//        saveJobToPath = "../convergingJobs";
//    }
//    //    std::filesystem::path
//    outputPath(std::filesystem::path(saveJobToPath)
//    //                                         .append(job.pMesh->getLabel())
//    //                                         .append("mag_" +
//    optimizationState.currentScenarioName));
//    //    std::filesystem::create_directories(outputPath);
//    //    job.save(outputPath);
//    //    settings.save(outputPath);

//#ifdef POLYSCOPE_DEFINED
//    std::cout << "Force:" << forceMagnitude << " Error is:" <<
//    vcg::math::ToDeg(error) << std::endl;
//#endif
//    return error;
//}

void search(const std::function<double(const double&)> objectiveFunction,
            const double& targetY,
            double& optimalX,
            double& error,
            const double& epsilon,
            const double& maxIterations) {
  error = std::numeric_limits<double>::max();
  int iterationIndex = 0;
  double low = optimalX / 1e-3, high = optimalX * 1e3;
  while (error > epsilon && iterationIndex < maxIterations) {
    const double y = objectiveFunction(optimalX);
    error = std::abs(targetY - y);
    if (y > targetY) {
      high = optimalX;
    } else {
      low = optimalX;
    }
    optimalX = low + (high - low) / 2;

    iterationIndex++;
  }

  if (iterationIndex == maxIterations) {
    std::cerr << "Max iterations reached." << std::endl;
  }
}

// double fullPatternMaxSimulationForceTranslationalObjective(const double
// &forceMagnitude)
//{
//    SimulationJob job;
//    job.pMesh =
//    g_baseScenarioMaxDisplacementHelperData.pFullPatternSimulationEdgeMesh;
//    g_baseScenarioMaxDisplacementHelperData.constructScenarioFunction(
//        forceMagnitude,
//        g_baseScenarioMaxDisplacementHelperData.fullPatternOppositeInterfaceViPairs,
//        job);
//    //    ReducedModelOptimizer::axialSimulationScenario(forceMagnitude,
//    // optimizationState.fullPatternInterfaceViPairs,
//    //                                                   job);

//    ChronosEulerSimulationModel simulator;
//    SimulationResults results =
//    simulator.executeSimulation(std::make_shared<SimulationJob>(job));

//    //    DRMSimulationModel simulator;
//    //    DRMSimulationModel::Settings settings;
//    //    //    settings.totalResidualForcesNormThreshold = 1e-3;
//    //    settings.threshold_averageResidualToExternalForcesNorm = 1e-2;
//    //    settings.totalTranslationalKineticEnergyThreshold = 1e-8;
//    //    settings.viscousDampingFactor = 5e-3;
//    //    settings.useTranslationalKineticEnergyForKineticDamping = true;
//    //    //    settings.threshold_averageResidualToExternalForcesNorm = 1e-5;
//    //    //        settings.useAverage = true;
//    //    //        settings.totalTranslationalKineticEnergyThreshold = 1e-10;
//    //    //    settings.shouldUseTranslationalKineticEnergyThreshold = true;
//    //    //    settings.shouldDraw = true;
//    //    //    settings.isDebugMode = true;
//    //    //    settings.drawingStep = 200000;
//    //    //    settings.beVerbose = true;
//    //    //    settings.debugModeStep = 200;
//    //    settings.maxDRMIterations = 200000;
//    //    SimulationResults results =
//    simulator.executeSimulation(std::make_shared<SimulationJob>(job),
//    //                                                            settings);
//    const double &desiredDisplacementValue
//        = g_baseScenarioMaxDisplacementHelperData
//              .desiredMaxDisplacementValue; //TODO: move from
//              OptimizationState to a new struct
//    double error;
//    if (!results.converged) {
//        error = std::numeric_limits<double>::max();
//        std::filesystem::path outputPath(
//            std::filesystem::path("../nonConvergingJobs")
//                .append(job.pMesh->getLabel())
//                .append("mag_" +
//                g_baseScenarioMaxDisplacementHelperData.currentScenarioName));
//        std::filesystem::create_directories(outputPath);
//        job.save(outputPath);
//        //        std::terminate();
//    } else {
//        error = std::abs(results
//                             .displacements[g_baseScenarioMaxDisplacementHelperData
//                                                .interfaceViForComputingScenarioError]
//                             .getTranslation()
//                             .norm()
//                         - desiredDisplacementValue);
//    }

//#ifdef POLYSCOPE_DEFINED
//    std::cout << "Force:" << forceMagnitude << " Error is:" << error <<
//    std::endl;
//#endif

//    return error;
//}

#ifdef USE_ENSMALLEN
struct ForceMagnitudeOptimization {
  std::function<double(const double&)> objectiveFunction;
  ForceMagnitudeOptimization(std::function<double(const double&)>& f)
      : objectiveFunction(f) {}
  double Evaluate(const arma::mat& x) { return objectiveFunction(x(0, 0)); }
};
#endif

#ifdef DLIB_DEFINED
double ReducedModelOptimizer::computeFullPatternMaxSimulationForce(
    const BaseSimulationScenario& scenario) const {
  double forceMagnitude = 1;
  double minimumError;
  bool wasSuccessful = false;
  g_baseScenarioMaxDisplacementHelperData.constructScenarioFunction =
      constructBaseScenarioFunctions[scenario];
  const double baseTriangleHeight = vcg::Distance(
      baseTriangle.cP(0), (baseTriangle.cP(1) + baseTriangle.cP(2)) / 2);
  std::function<double(const double&)> objectiveFunction;
  double translationalOptimizationEpsilon{baseTriangleHeight * 0.001};
  double objectiveEpsilon = translationalOptimizationEpsilon;
  objectiveFunction = &fullPatternMaxSimulationForceTranslationalObjective;
  g_baseScenarioMaxDisplacementHelperData.interfaceViForComputingScenarioError =
      m_fullPatternOppositeInterfaceViPairs[1].first;
  g_baseScenarioMaxDisplacementHelperData.desiredMaxDisplacementValue =
      0.1 * baseTriangleHeight;
  g_baseScenarioMaxDisplacementHelperData.currentScenarioName =
      baseSimulationScenarioNames[scenario];
  g_baseScenarioMaxDisplacementHelperData.pFullPatternSimulationEdgeMesh =
      m_pFullPatternSimulationEdgeMesh;
  double forceMagnitudeEpsilon = 1e-4;

  switch (scenario) {
    case Axial:
      g_baseScenarioMaxDisplacementHelperData.desiredMaxDisplacementValue =
          0.03 * baseTriangleHeight;
      break;
    case Shear:
      g_baseScenarioMaxDisplacementHelperData.desiredMaxDisplacementValue =
          0.04 * baseTriangleHeight;
      break;
    case Bending:
      g_baseScenarioMaxDisplacementHelperData
          .interfaceViForComputingScenarioError =
          m_fullPatternOppositeInterfaceViPairs[0].first;
      g_baseScenarioMaxDisplacementHelperData.desiredMaxDisplacementValue =
          0.05 * baseTriangleHeight;
      break;
    case Dome:
      g_baseScenarioMaxDisplacementHelperData.desiredMaxRotationAngle =
          vcg::math::ToRad(20.0);
      objectiveFunction = &fullPatternMaxSimulationForceRotationalObjective;
      forceMagnitudeEpsilon *= 1e-2;
      objectiveEpsilon = vcg::math::ToRad(1.0);
      forceMagnitude = 0.6;
      break;
    case Saddle:
      g_baseScenarioMaxDisplacementHelperData
          .interfaceViForComputingScenarioError =
          m_fullPatternOppositeInterfaceViPairs[0].first;
      g_baseScenarioMaxDisplacementHelperData.desiredMaxDisplacementValue =
          0.05 * baseTriangleHeight;
      break;
    case S:
      g_baseScenarioMaxDisplacementHelperData
          .interfaceViForComputingScenarioError =
          m_fullPatternOppositeInterfaceViPairs[1].first;
      g_baseScenarioMaxDisplacementHelperData.desiredMaxDisplacementValue =
          0.05 * baseTriangleHeight;
      break;
    default:
      std::cerr << "Unrecognized base scenario" << std::endl;
      break;
  }

  constexpr int maxIterations = 1000;
  minimumError =
      dlib::find_min_single_variable(objectiveFunction, forceMagnitude, 1e-4,
                                     1e4, forceMagnitudeEpsilon, maxIterations);
#ifdef DLIB_DEFINED
#else
  //    ens::SA<> optimizer;
  //    arma::vec lowerBound("0.00001");
  //    arma::vec upperBound("10000");
  //    ens::LBestPSO optimizer(64, lowerBound, upperBound, maxIterations, 350,
  //    forceMagnitudeEpsilon); ForceMagnitudeOptimization f(objectiveFunction);
  //    // Create function to be optimized. arma::mat
  //    forceMagnitude_mat({forceMagnitude}); minimumError =
  //    optimizer.Optimize(f, forceMagnitude_mat); std::cout <<
  //    ReducedModelOptimization::baseSimulationScenarioNames[scenario] << ": "
  //              << optimalObjective << std::endl;
  //        forceMagnitude = forceMagnitude_mat(0, 0);

//    search(objectiveFunction,
//           optimizationState.desiredMaxDisplacementValue,
//           forceMagnitude,
//           minimumError,
//           objectiveEpsilon,
//           maxIterations);
#endif

  wasSuccessful = minimumError < objectiveEpsilon;

#ifdef POLYSCOPE_DEFINED
  std::cout << "Max "
            << ReducedModelOptimization::baseSimulationScenarioNames[scenario]
            << " magnitude:" << forceMagnitude << std::endl;
  if (!wasSuccessful) {
    SimulationJob job;
    job.pMesh = optimizationState.pFullPatternSimulationEdgeMesh;
    g_baseScenarioMaxDisplacementHelperData.constructScenarioFunction(
        forceMagnitude, optimizationState.fullPatternOppositeInterfaceViPairs,
        job);
    std::cerr
        << ReducedModelOptimization::baseSimulationScenarioNames[scenario] +
               " max scenario magnitude was not succefully determined."
        << std::endl;
    std::filesystem::path outputPath(
        std::filesystem::path("../nonConvergingJobs")
            .append(m_pFullPatternSimulationEdgeMesh->getLabel())
            .append("magFinal_" + ReducedModelOptimization::
                                      baseSimulationScenarioNames[scenario]));
    std::filesystem::create_directories(outputPath);
    job.save(outputPath);
    std::terminate();
  }
#endif

  return forceMagnitude;
}
#endif

void ReducedModelOptimizer::computeScenarioWeights(
    const std::vector<BaseSimulationScenario>& baseSimulationScenarios,
    const ReducedModelOptimization::Settings& optimizationSettings) {
  std::array<double, NumberOfBaseSimulationScenarios>
      baseScenario_equalizationWeights;
  baseScenario_equalizationWeights.fill(0);
  std::array<double, NumberOfBaseSimulationScenarios> baseScenario_userWeights;
  baseScenario_userWeights.fill(0);
  // Fill only used base scenario weights
  for (const BaseSimulationScenario& baseScenario : baseSimulationScenarios) {
    baseScenario_equalizationWeights[baseScenario] =
        static_cast<double>(simulationScenariosResolution[baseScenario]);
    baseScenario_userWeights[baseScenario] = baseScenarioWeights[baseScenario];
  }

  Utilities::normalize(baseScenario_userWeights.begin(),
                       baseScenario_userWeights.end());

  Utilities::normalize(baseScenario_equalizationWeights.begin(),
                       baseScenario_equalizationWeights.end());
  std::transform(baseScenario_equalizationWeights.begin(),
                 baseScenario_equalizationWeights.end(),
                 baseScenario_equalizationWeights.begin(), [](const double& d) {
                   if (d == 0) {
                     return d;
                   }
                   return 1 / d;
                 });
  Utilities::normalize(baseScenario_equalizationWeights.begin(),
                       baseScenario_equalizationWeights.end());

  std::array<double, NumberOfBaseSimulationScenarios> baseScenarios_weights;
  baseScenarios_weights.fill(0);
  for (const BaseSimulationScenario& baseScenario : baseSimulationScenarios) {
    baseScenarios_weights[baseScenario] =
        baseScenario_equalizationWeights[baseScenario] +
        2 * baseScenario_userWeights[baseScenario];
  }
  Utilities::normalize(baseScenarios_weights.begin(),
                       baseScenarios_weights.end());

  optimizationState.objectiveWeights.resize(totalNumberOfSimulationScenarios);
  optimizationState.scenarioWeights.resize(
      totalNumberOfSimulationScenarios,
      0);  // This essentially holds the base scenario weights but I use
           // totalNumberOfSimulationScenarios elements instead in order to have
           // O(1) access time during the optimization
  for (const BaseSimulationScenario& baseScenario : baseSimulationScenarios) {
    const int baseSimulationScenarioIndexOffset = std::accumulate(
        simulationScenariosResolution.begin(),
        simulationScenariosResolution.begin() + baseScenario, 0);
    for (int simulationScenarioIndex = 0;
         simulationScenarioIndex < simulationScenariosResolution[baseScenario];
         simulationScenarioIndex++) {
      const int globalScenarioIndex =
          baseSimulationScenarioIndexOffset + simulationScenarioIndex;
      optimizationState.scenarioWeights[globalScenarioIndex] =
          baseScenarios_weights[baseScenario];

      optimizationState.objectiveWeights[globalScenarioIndex] =
          optimizationSettings.perBaseScenarioObjectiveWeights[baseScenario];
    }
  }

  // Dont normalize since we want the base scenarios to be normalized not the
  // "optimizationState scenarios"
  //    Utilities::normalize(optimizationState.scenarioWeights.begin(),
  //    optimizationState.scenarioWeights.end());
}

std::vector<std::shared_ptr<SimulationJob>>
ReducedModelOptimizer::createFullPatternSimulationJobs(
    const std::shared_ptr<SimulationEdgeMesh>& pMesh,
    const std::array<double, NumberOfBaseSimulationScenarios>&
        baseScenarioMaxForceMagnitudes) const {
  std::vector<std::shared_ptr<SimulationJob>> scenarios;
  scenarios.resize(totalNumberOfSimulationScenarios);

  SimulationJob job;
  job.pMesh = pMesh;

  for (int baseScenario = Axial;
       baseScenario != NumberOfBaseSimulationScenarios; baseScenario++) {
    const double maxForceMagnitude =
        baseScenarioMaxForceMagnitudes[baseScenario];
    const int numberOfSimulationScenarios =
        simulationScenariosResolution[baseScenario];
    const double minForceMagnitude =
        scenarioIsSymmetrical[baseScenario]
            ? maxForceMagnitude / numberOfSimulationScenarios
            : -maxForceMagnitude;
    const double forceMagnitudeStep =
        numberOfSimulationScenarios == 1
            ? maxForceMagnitude
            : (maxForceMagnitude - minForceMagnitude) /
                  (numberOfSimulationScenarios - 1);
    const int baseSimulationScenarioIndexOffset = std::accumulate(
        simulationScenariosResolution.begin(),
        simulationScenariosResolution.begin() + baseScenario, 0);
    for (int simulationScenarioIndex = 0;
         simulationScenarioIndex < numberOfSimulationScenarios;
         simulationScenarioIndex++) {
      const int scenarioIndex =
          baseSimulationScenarioIndexOffset + simulationScenarioIndex;
      if (baseScenarioMaxForceMagnitudes[baseScenario] == -1) {
        scenarios[scenarioIndex] = nullptr;
        continue;
      }
      job.nodalExternalForces.clear();
      job.constrainedVertices.clear();
      job.nodalForcedDisplacements.clear();

      const double forceMagnitude =
          (forceMagnitudeStep * simulationScenarioIndex + minForceMagnitude);
      constructBaseScenarioFunctions[baseScenario](
          forceMagnitude, m_fullPatternOppositeInterfaceViPairs, job);

      job.label = baseSimulationScenarioNames[baseScenario] + "_" +
                  std::to_string(simulationScenarioIndex);

      scenarios[scenarioIndex] = std::make_shared<SimulationJob>(job);
    }
  }

#ifdef POLYSCOPE_DEFINED
  std::cout << "Created pattern simulation jobs" << std::endl;
#endif
  return scenarios;
}

void ReducedModelOptimizer::computeObjectiveValueNormalizationFactors(
    const ReducedModelOptimization::Settings& optimizationSettings) {
  //    m_pFullPatternSimulationEdgeMesh->registerForDrawing();
  //    m_pFullPatternSimulationEdgeMesh->save(std::filesystem::current_path().append("initial.ply"));
  // Compute the sum of the displacement norms
  std::vector<double> fullPatternTranslationalDisplacementNormSum(
      totalNumberOfSimulationScenarios);
  std::vector<double> fullPatternAngularDistance(
      totalNumberOfSimulationScenarios);
  for (int simulationScenarioIndex :
       optimizationState.simulationScenarioIndices) {
    double translationalDisplacementNormSum = 0;
    for (auto interfaceViPair : optimizationState.reducedToFullInterfaceViMap) {
      const int fullPatternVi = interfaceViPair.second;
      // If the full pattern vertex is translationally constrained dont take it
      // into account
      if (optimizationState.pSimulationJobs_pattern[simulationScenarioIndex]
              ->constrainedVertices.contains(fullPatternVi)) {
        const std::unordered_set<int> constrainedDof =
            optimizationState.pSimulationJobs_pattern[simulationScenarioIndex]
                ->constrainedVertices.at(fullPatternVi);
        if (constrainedDof.contains(0) && constrainedDof.contains(1) &&
            constrainedDof.contains(2)) {
          continue;
        }
      }

      const Vector6d& vertexDisplacement =
          optimizationState.fullPatternResults[simulationScenarioIndex]
              .displacements[fullPatternVi];
      //            std::cout << "vi:" << fullPatternVi << std::endl;
      //            std::cout << "displacement:" <<
      //            vertexDisplacement.getTranslation().norm() << std::endl;

      translationalDisplacementNormSum +=
          vertexDisplacement.getTranslation().norm();
    }
    //        optimizationState.fullPatternResults[simulationScenarioIndex].saveDeformedModel(
    //            std::filesystem::current_path());
    //        optimizationState.fullPatternResults[simulationScenarioIndex].registerForDrawing();
    //        polyscope::show();
    //        optimizationState.fullPatternResults[simulationScenarioIndex].unregister();
    double angularDistanceSum = 0;
    for (auto interfaceViPair : optimizationState.reducedToFullInterfaceViMap) {
      const int fullPatternVi = interfaceViPair.second;
      // If the full pattern vertex is rotationally constrained dont take it
      // into account
      if (optimizationState.pSimulationJobs_pattern[simulationScenarioIndex]
              ->constrainedVertices.contains(fullPatternVi)) {
        const std::unordered_set<int> constrainedDof =
            optimizationState.pSimulationJobs_pattern[simulationScenarioIndex]
                ->constrainedVertices.at(fullPatternVi);
        if (constrainedDof.contains(3) && constrainedDof.contains(5) &&
            constrainedDof.contains(4)) {
          continue;
        }
      }
      angularDistanceSum +=
          std::abs(optimizationState.fullPatternResults[simulationScenarioIndex]
                       .rotationalDisplacementQuaternion[fullPatternVi]
                       .angularDistance(Eigen::Quaterniond::Identity()));
    }

    fullPatternTranslationalDisplacementNormSum[simulationScenarioIndex] =
        translationalDisplacementNormSum;
    fullPatternAngularDistance[simulationScenarioIndex] = angularDistanceSum;
  }

  optimizationState.translationalDisplacementNormalizationValues.resize(
      totalNumberOfSimulationScenarios);
  optimizationState.rotationalDisplacementNormalizationValues.resize(
      totalNumberOfSimulationScenarios);
  for (int simulationScenarioIndex :
       optimizationState.simulationScenarioIndices) {
    if (optimizationSettings.normalizationStrategy ==
        Settings::NormalizationStrategy::Epsilon) {
      const double epsilon_translationalDisplacement =
          optimizationSettings.translationEpsilon;
      optimizationState.translationalDisplacementNormalizationValues
          [simulationScenarioIndex] = std::max(
          fullPatternTranslationalDisplacementNormSum[simulationScenarioIndex],
          epsilon_translationalDisplacement);
      const double epsilon_rotationalDisplacement =
          optimizationSettings.angularDistanceEpsilon;
      optimizationState
          .rotationalDisplacementNormalizationValues[simulationScenarioIndex] =
          std::max(fullPatternAngularDistance[simulationScenarioIndex],
                   epsilon_rotationalDisplacement);
    } else {
      optimizationState.translationalDisplacementNormalizationValues
          [simulationScenarioIndex] = 1;
      optimizationState
          .rotationalDisplacementNormalizationValues[simulationScenarioIndex] =
          1;
    }
  }
}

void ReducedModelOptimizer::optimize(
    Settings& optimizationSettings,
    ReducedModelOptimization::Results& results,
    const std::vector<BaseSimulationScenario>& desiredBaseSimulationScenarios) {
  std::unordered_set<BaseSimulationScenario> zeroMagnitudeScenarios;
  for (int baseScenario = Axial;
       baseScenario != NumberOfBaseSimulationScenarios; baseScenario++) {
    if (optimizationSettings.baseScenarioMaxMagnitudes[baseScenario] == 0) {
      zeroMagnitudeScenarios.insert(
          static_cast<BaseSimulationScenario>(baseScenario));
      continue;
    }
  }
  std::set<BaseSimulationScenario> set_desiredScenarios(
      desiredBaseSimulationScenarios.begin(),
      desiredBaseSimulationScenarios.end());
  std::set<BaseSimulationScenario> set_zeroScenarios(
      zeroMagnitudeScenarios.begin(), zeroMagnitudeScenarios.end());
  std::vector<BaseSimulationScenario> usedBaseSimulationScenarios;
  std::set_difference(set_desiredScenarios.begin(), set_desiredScenarios.end(),
                      set_zeroScenarios.begin(), set_zeroScenarios.end(),
                      std::back_inserter(usedBaseSimulationScenarios));

  assert(!optimizationSettings.optimizationStrategy.empty());
  assert(
      !optimizationSettings.optimizationVariablesGroupsWeights.has_value() ||
      (optimizationSettings.optimizationStrategy.size() ==
           optimizationSettings.optimizationVariablesGroupsWeights->size() &&
       std::accumulate(
           optimizationSettings.optimizationVariablesGroupsWeights->begin(),
           optimizationSettings.optimizationVariablesGroupsWeights->end(), 0)));

  for (int baseSimulationScenarioIndex : usedBaseSimulationScenarios) {
    // Increase the size of the vector holding the simulation scenario indices
    optimizationState.simulationScenarioIndices.resize(
        optimizationState.simulationScenarioIndices.size() +
        simulationScenariosResolution[baseSimulationScenarioIndex]);
    // Add the simulation scenarios indices that correspond to this base
    // simulation scenario
    std::iota(optimizationState.simulationScenarioIndices.end() -
                  simulationScenariosResolution[baseSimulationScenarioIndex],
              optimizationState.simulationScenarioIndices.end(),
              std::accumulate(simulationScenariosResolution.begin(),
                              simulationScenariosResolution.begin() +
                                  baseSimulationScenarioIndex,
                              0));
  }

  //    constexpr bool recomputeForceMagnitudes = false;
  //    std::array<double, NumberOfBaseSimulationScenarios>
  //    fullPatternSimulationScenarioMaxMagnitudes
  //        = getFullPatternMaxSimulationForces(usedBaseSimulationScenarios,
  //                                            optimizationSettings.intermediateResultsDirectoryPath,
  //                                            recomputeForceMagnitudes);
  //    for (int baseScenario = Axial; baseScenario !=
  //    NumberOfBaseSimulationScenarios; baseScenario++) {
  //        fullPatternSimulationScenarioMaxMagnitudes[baseScenario]
  //            =
  //            std::min(fullPatternSimulationScenarioMaxMagnitudes[baseScenario],
  //                       optimizationSettings.baseScenarioMaxMagnitudes[baseScenario]);
  //    }
  //    optimizationSettings.baseScenarioMaxMagnitudes =
  //    fullPatternSimulationScenarioMaxMagnitudes;
  std::array<double, NumberOfBaseSimulationScenarios>
      fullPatternSimulationScenarioMaxMagnitudes =
          optimizationSettings.baseScenarioMaxMagnitudes;
  optimizationState.pSimulationJobs_pattern = createFullPatternSimulationJobs(
      m_pSimulationEdgeMesh_pattern,
      fullPatternSimulationScenarioMaxMagnitudes);
  //  polyscope::removeAllStructures();

  //    ComputeScenarioWeights({Axial, Shear, Bending, Dome, Saddle},
  computeScenarioWeights(usedBaseSimulationScenarios, optimizationSettings);

  results.baseTriangle = baseTriangle;

  //    DRMSimulationModel::Settings drmSettings;
  //    drmSettings.threshold_averageResidualToExternalForcesNorm = 1e-5;
  //    drmSettings.maxDRMIterations = 200000;
  //    drmSettings.totalTranslationalKineticEnergyThreshold = 1e-8;
  //    drmSettings.totalTranslationalKineticEnergyThreshold = 1e-9;

  //    drmSettings.totalTranslationalKineticEnergyThreshold = 1e-10;
  //    drmSettings.gradualForcedDisplacementSteps = 20;
  //    drmSettings.linearGuessForceScaleFactor = 1;
  //    drmSettings.viscousDampingFactor = 5e-3;
  //    drmSettings.useTotalRotationalKineticEnergyForKineticDamping = true;
  //    drmSettings.shouldDraw = true;

  //    drmSettings.threshold_averageResidualToExternalForcesNorm = 1e-2;
  //    drmSettings.viscousDampingFactor = 5e-3;
  //    simulationSettings.viscousDampingFactor = 5e-3;
  //    simulationSettings.linearGuessForceScaleFactor = 1;
  //    simulationSettings.gamma = 0.3;
  //    simulationSettings.xi = 0.9999;
  //    drmSettings.debugModeStep = 100;
  //    drmSettings.shouldDraw = true;
  //    drmSettings.shouldCreatePlots = true;
  //    drmSettings.beVerbose = true;
  //    simulationSettings.useKineticDamping = true;
  //    simulationSettings.save(std::filesystem::path(std::string(__FILE__))
  //                                .parent_path()
  //                                .parent_path()
  //                                .append("DefaultSettings")
  //                                .append("DRMSettings")
  //                                .append("defaultDRMSimulationSettings.json"));

  //    simulationSettings.threshold_averageResidualToExternalForcesNorm = 1e-5;
  //    simulationSettings.viscousDampingFactor = 1e-3;
  //    simulationSettings.beVerbose = true;
  //    simulationSettings.shouldDraw = true;
  //    simulationSettings.isDebugMode = true;
  //    simulationSettings.debugModeStep = 100000;
  //#ifdef POLYSCOPE_DEFINED
  constexpr bool drawPatternSimulationResults = true;
  auto t1 = std::chrono::high_resolution_clock::now();
  //#endif
  results.wasSuccessful = true;
  optimizationState.fullPatternResults.resize(totalNumberOfSimulationScenarios);
  optimizationState.simulationJobs_reducedModel.resize(
      totalNumberOfSimulationScenarios);

  std::atomic<int> numberOfComputedJobs = 0;
  std::for_each(
//                                         std::execution::unseq,
#ifndef POLYSCOPE_DEFINED
      std::execution::par_unseq,
#endif
      optimizationState.simulationScenarioIndices.begin(),
      optimizationState.simulationScenarioIndices.end(),
      [&](const int& simulationScenarioIndex) {
        const std::shared_ptr<SimulationJob>& pSimulationJob_pattern =
            optimizationState.pSimulationJobs_pattern[simulationScenarioIndex];
        //            std::cout << "Executing " <<
        //            pFullPatternSimulationJob->getLabel() << std::endl;

        //            std::filesystem::path jobResultsDirectoryPath(
        //                std::filesystem::path(optimizationSettings.intermediateResultsDirectoryPath)
        //                    .append("FullPatternResults")
        //                    .append(m_pFullPatternSimulationEdgeMesh->getLabel())
        //                    .append(pPatternSimulationJob->getLabel()));
        std::unique_ptr<SimulationModel> pPatternSimulator =
            SimulationModelFactory::create(
                optimizationSettings.simulationModelLabel_groundTruth);
        pPatternSimulator->setStructure(m_pSimulationEdgeMesh_pattern);
        SimulationResults simulationResults_pattern;
        SimulationResults debug_chronosResults;
        if (optimizationSettings.simulationModelLabel_groundTruth ==
            DRMSimulationModel::label) {
#ifdef POLYSCOPE_DEFINED
          if (drawPatternSimulationResults) {
            ChronosEulerNonLinearSimulationModel debug_chronosModel;
            debug_chronosResults =
                debug_chronosModel.executeSimulation(pSimulationJob_pattern);
            debug_chronosResults.registerForDrawing(RGBColor({0, 0, 255}), true,
                                                    0.0001, true);
          }
#endif

          DRMSimulationModel::Settings drmSettings;
          drmSettings.threshold_averageResidualToExternalForcesNorm = 1e-5;
          drmSettings.threshold_totalTranslationalKineticEnergy = 1e-14;
          //          drmSettings.threshold_residualForcesMovingAverageDerivativeNorm
          //          =
          //              1e-11;
          drmSettings.maxDRMIterations = 10000;
          drmSettings.Dtini = 1;
          //          drmSettings.beVerbose = true;
          //          drmSettings.debugModeStep = 1e4;
          //          drmSettings.shouldCreatePlots = true;
          //          drmSettings.shouldDraw = true;
          //          drmSettings.threshold_totalTranslationalKineticEnergy =
          //          1e-8;
          pSimulationJob_pattern->pMesh->getLabel();
          simulationResults_pattern =
              dynamic_cast<DRMSimulationModel*>(pPatternSimulator.get())
                  ->executeSimulation(pSimulationJob_pattern, drmSettings);
          if (!simulationResults_pattern.converged) {
#ifdef POLYSCOPE_DEFINED
            std::filesystem::path outputPath(
                std::filesystem::path("../nonConvergingJobs")
                    .append(m_pSimulationEdgeMesh_pattern->getLabel())
                    .append("final_" + pSimulationJob_pattern->getLabel()));
            std::filesystem::create_directories(outputPath);
            pSimulationJob_pattern->save(outputPath);
            std::cerr << m_pSimulationEdgeMesh_pattern->getLabel()
                      << " sim scenario " << pSimulationJob_pattern->getLabel()
                      << " did not converge on first try." << std::endl;
#endif

            //            return;
            DRMSimulationModel::Settings drmSettings_secondTry = drmSettings;
            drmSettings_secondTry.linearGuessForceScaleFactor = 1;
            //            drmSettings_secondTry.viscousDampingFactor = 4e-8;
            //            *drmSettings_secondTry.maxDRMIterations *= 5;
            drmSettings_secondTry.maxDRMIterations = 2e6;
            drmSettings_secondTry.threshold_totalTranslationalKineticEnergy =
                1e-13;

            simulationResults_pattern =
                dynamic_cast<DRMSimulationModel*>(pPatternSimulator.get())
                    ->executeSimulation(pSimulationJob_pattern,
                                        drmSettings_secondTry);
          }

        } else {
          simulationResults_pattern =
              pPatternSimulator->executeSimulation(pSimulationJob_pattern);
        }

        //            ChronosEulerSimulationModel simulationModel_pattern;
        //            simulationModel_pattern.settings.analysisType
        //                =
        //                ChronosEulerSimulationModel::Settings::AnalysisType::NonLinear;
        //            const SimulationResults simulationResults_pattern
        //                =
        //                simulationModel_pattern.executeSimulation(pPatternSimulationJob);

        //            double error;
        //            if
        //            (!patternSimulationResults.isEqual(debug_eulerPatternSimulationResults,
        //            error)) {
        //                std::cerr << "The computed ground truth does not match
        //                the IGA model results. "
        //                             "Error is:"
        //                          << error << std::endl;
        //            }

        if (!simulationResults_pattern.converged) {
#ifdef POLYSCOPE_DEFINED
          std::filesystem::path outputPath(
              std::filesystem::path("../nonConvergingJobs")
                  .append(m_pSimulationEdgeMesh_pattern->getLabel())
                  .append("final_" + pSimulationJob_pattern->getLabel()));
          std::filesystem::create_directories(outputPath);
          pSimulationJob_pattern->save(outputPath);
          std::cerr << m_pSimulationEdgeMesh_pattern->getLabel()
                    << " did not converge." << std::endl;
          DRMSimulationModel::Settings drmSettings;
          drmSettings.threshold_averageResidualToExternalForcesNorm = 1e-5;
          drmSettings.maxDRMIterations = 200000;
          //          drmSettings.threshold_totalTranslationalKineticEnergy =
          //          1e-8;
          drmSettings.debugModeStep = 1e2;
          //          drmSettings.shouldDraw = true;
          simulationResults_pattern =
              dynamic_cast<DRMSimulationModel*>(pPatternSimulator.get())
                  ->executeSimulation(pSimulationJob_pattern, drmSettings);
          std::terminate();
          assert(false);
#endif
        }

        optimizationState.fullPatternResults[simulationScenarioIndex] =
            simulationResults_pattern;
        SimulationJob simulationJob_reducedModel;
        simulationJob_reducedModel.pMesh = m_pReducedModelSimulationEdgeMesh;
        computeReducedModelSimulationJob(*pSimulationJob_pattern,
                                         m_fullToReducedInterfaceViMap,
                                         simulationJob_reducedModel);
        optimizationState.simulationJobs_reducedModel[simulationScenarioIndex] =
            std::make_shared<SimulationJob>(simulationJob_reducedModel);
#ifdef POLYSCOPE_DEFINED
        std::cout << "Ran sim scenario:" << pSimulationJob_pattern->getLabel()
                  << ". ";
        std::cout << ++numberOfComputedJobs << "/"
                  << optimizationState.simulationScenarioIndices.size()
                  << std::endl;

        if (drawPatternSimulationResults) {
          optimizationState.pSimulationJobs_pattern[0]
              ->pMesh->registerForDrawing(
                  ReducedModelOptimization::Colors::patternInitial, false,
                  0.0001);
          pSimulationJob_pattern->registerForDrawing(
              optimizationState.pSimulationJobs_pattern[0]->pMesh->getLabel(),
              true);
          simulationResults_pattern.registerForDrawing(
              ReducedModelOptimization::Colors::patternDeformed, true, 0.0001,
              true);
          //                        SimulationResults fullPatternResults_linear
          //                            =
          //                            linearSimulator.executeSimulation(pFullPatternSimulationJob);
          //                        patternDrmResults.registerForDrawing(std::array<double,
          //                        3>{0, 255, 0}, true);
          //                        fullPatternResults_linear.labelPrefix +=
          //                        "_linear"; fullPatternResults_linear
          //                            .registerForDrawing(std::array<double,
          //                            3>{0, 0, 255}, true);

          //          ChronosEulerNonLinearSimulationModel debug_chronosModel;
          //          auto debug_chronosResults =
          //              debug_chronosModel.executeSimulation(pSimulationJob_pattern);
          //          debug_chronosResults.registerForDrawing(RGBColor({0, 0,
          //                    255}), true);
          polyscope::show();
          debug_chronosResults.unregister();
          simulationResults_pattern.unregister();
          //                fullPatternResults_linear.unregister();
          optimizationState.pSimulationJobs_pattern[0]->pMesh->unregister();
          //                debug_eulerPatternSimulationResults.unregister();
        }
#endif
      });

  auto t2 = std::chrono::high_resolution_clock::now();
  auto s_int = std::chrono::duration_cast<std::chrono::seconds>(t2 - t1);
#ifdef POLYSCOPE_DEFINED
  std::cout << "Computed ground of truth pattern results in:" << s_int.count()
            << " seconds." << std::endl;
  if (drawPatternSimulationResults) {
    optimizationState.pSimulationJobs_pattern[0]->pMesh->unregister();
  }
  optimizationState.simulationJobs_reducedModel[0]
      ->pMesh->updateEigenEdgeAndVertices();
#endif
  //    if (optimizationState.optimizationSettings.normalizationStrategy
  //        == Settings::NormalizationStrategy::Epsilon) {
  computeObjectiveValueNormalizationFactors(optimizationSettings);
//    }
#ifdef POLYSCOPE_DEFINED
  std::cout << "Running reduced model optimization" << std::endl;
#endif
  runOptimization(optimizationSettings, results);
}