#include "reducedmodeloptimizer.hpp"
#include "hexagonremesher.hpp"
#include "linearsimulationmodel.hpp"
#include "simulationhistoryplotter.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<FullPatternVertexIndex, FullPatternVertexIndex>> &,
                       SimulationJob &)>
        constructScenarioFunction;
    double desiredMaxDisplacementValue;
    double desiredMaxRotationAngle;
    FullPatternVertexIndex interfaceViForComputingScenarioError;
    std::string currentScenarioName;
    std::shared_ptr<SimulationMesh> pFullPatternSimulationMesh;

    std::vector<std::pair<FullPatternVertexIndex, FullPatternVertexIndex>>
        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<ReducedPatternVertexIndex, FullPatternVertexIndex>
        &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<ReducedPatternVertexIndex, FullPatternVertexIndex>
        &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<ReducedPatternVertexIndex, FullPatternVertexIndex>
        &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<ReducedPatternVertexIndex, FullPatternVertexIndex>
        &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<ReducedPatternVertexIndex, FullPatternVertexIndex>
        &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);
    }
};
#endif

#ifdef 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<SimulationMesh> &pReducedPatternSimulationMesh
        = optimizationState
              .reducedPatternSimulationJobs[optimizationState.simulationScenarioIndices[0]]
              ->pMesh;
    function_updateReducedPattern(x, pReducedPatternSimulationMesh);
    //    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(pReducedPatternSimulationMesh);
    //    simulator.initialize();
    //    FormFinder simulator;

    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> &reducedJob
                = optimizationState.reducedPatternSimulationJobs[simulationScenarioIndex];

            //#ifdef POLYSCOPE_DEFINED
            //        std::cout << reducedJob->getLabel() << ":" << std::endl;
            //#endif
            SimulationResults reducedModelResults = simulator.executeSimulation(reducedJob);
            //    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;
            //                pReducedPatternSimulationMesh->registerForDrawing();
            //                polyscope::show();
            //                pReducedPatternSimulationMesh->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.pFullPatternSimulationMesh->registerForDrawing(Colors::fullDeformed);
                //                            optimizationState.fullPatternResults[simulationScenarioIndex].registerForDrawing(
                //                                Colors::fullDeformed);
                //                            polyscope::show();
                //                            reducedModelResults.unregister();
                //                            optimizationState.pFullPatternSimulationMesh->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);
#ifdef POLYSCOPE_DEFINED
        std::cout << "New best:" << totalError << std::endl;
        //        //        optimizationState.minX.assign(x.begin(), x.begin() + n);
        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::createSimulationMeshes(
    PatternGeometry &fullModel,
    PatternGeometry &reducedModel,
    std::shared_ptr<SimulationMesh> &pFullPatternSimulationMesh,
    std::shared_ptr<SimulationMesh> &pReducedPatternSimulationMesh)
{
    if (typeid(CrossSectionType) != typeid(RectangularBeamDimensions)) {
        std::cerr << "Error: A rectangular cross section is expected." << std::endl;
        std::terminate();
    }
    // Full pattern
    pFullPatternSimulationMesh = std::make_shared<SimulationMesh>(fullModel);
    pFullPatternSimulationMesh->setBeamCrossSection(CrossSectionType{0.002, 0.002});
    pFullPatternSimulationMesh->setBeamMaterial(0.3, youngsModulus);
    pFullPatternSimulationMesh->reset();

    // Reduced pattern
    pReducedPatternSimulationMesh = std::make_shared<SimulationMesh>(reducedModel);
    pReducedPatternSimulationMesh->setBeamCrossSection(CrossSectionType{0.002, 0.002});
    pReducedPatternSimulationMesh->setBeamMaterial(0.3, youngsModulus);
    pReducedPatternSimulationMesh->reset();
}

void ReducedModelOptimizer::createSimulationMeshes(PatternGeometry &fullModel,
                                                   PatternGeometry &reducedModel)
{
    ReducedModelOptimizer::createSimulationMeshes(fullModel,
                                                  reducedModel,
                                                  m_pFullPatternSimulationMesh,
                                                  m_pReducedPatternSimulationMesh);
}

void ReducedModelOptimizer::computeMaps(
    const std::unordered_map<size_t, std::unordered_set<size_t>> &slotToNode,
    PatternGeometry &fullPattern,
    ReducedModel &reducedPattern,
    std::unordered_map<ReducedPatternVertexIndex, FullPatternVertexIndex>
        &reducedToFullInterfaceViMap,
    std::unordered_map<FullPatternVertexIndex, ReducedPatternVertexIndex>
        &fullToReducedInterfaceViMap,
    std::vector<std::pair<FullPatternVertexIndex, ReducedPatternVertexIndex>>
        &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;
    reducedPattern.deleteDanglingVertices(pu_reducedModel);
    const size_t reducedModelBaseTriangleInterfaceVi = pu_reducedModel.remap[baseTriangleInterfaceVi];
    const size_t reducedModelInterfaceVertexOffset
        = reducedPattern.VN() /*- 1*/ /*- reducedModelBaseTriangleInterfaceVi*/;
    Results::applyOptimizationResults_reducedModel_nonFanned(initialHexagonSize,
                                                             0,
                                                             baseTriangle,
                                                             reducedPattern);
    reducedPattern.createFan(); //TODO: should be an input parameter from main
    for (size_t fanIndex = 0; fanIndex < fanSize; fanIndex++) {
        reducedToFullInterfaceViMap[reducedModelInterfaceVertexOffset * fanIndex
                                    + reducedModelBaseTriangleInterfaceVi]
            = fullModelBaseTriangleInterfaceVi + fanIndex * fullPatternInterfaceVertexOffset;
    }
    fullToReducedInterfaceViMap.clear();
    constructInverseMap(reducedToFullInterfaceViMap, fullToReducedInterfaceViMap);

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

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

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

#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;
    createSimulationMeshes(copyFullPattern, copyReducedModel);
    initializeUpdateReducedPatternFunctions();
    initializeOptimizationParameters(m_pFullPatternSimulationMesh, 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.baseTriangleFullPattern.copy(fullPattern);
    initializePatterns(fullPattern, reducedModel, optimizationSettings.variablesRanges);
    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<SimulationMesh> &pReducedModelSimulationMesh) {
            //        std::shared_ptr<VCGEdgeMesh> pReducedModel_edgeMesh = std::dynamic_pointer_cast<VCGEdgeMesh>(
            //            pReducedModelSimulationMesh);
            //        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::fanSize;
                 rotationCounter++) {
                pReducedModelSimulationMesh->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<SimulationMesh> &pReducedModelSimulationMesh) {
            //            std::shared_ptr<VCGEdgeMesh> pReducedModel_edgeMesh
            //                = std::dynamic_pointer_cast<VCGEdgeMesh>(pReducedModelSimulationMesh);
            //            ReducedModel::updateFannedGeometry_theta(newTheta_degrees, pReducedModel_edgeMesh);

            const CoordType baseTriangleHexagonVertexPosition = pReducedModelSimulationMesh->vert[0]
                                                                    .cP();
            const CoordType thetaRotatedHexagonBaseTriangleVertexPosition
                = vcg::RotationMatrix(PatternGeometry::DefaultNormal,
                                      vcg::math::ToRad(newTheta_degrees))
                  * baseTriangleHexagonVertexPosition;
            for (int rotationCounter = 0; rotationCounter < ReducedModelOptimizer::fanSize;
                 rotationCounter++) {
                pReducedModelSimulationMesh->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<SimulationMesh> &pReducedPatternSimulationMesh) {
            for (EdgeIndex ei = 0; ei < pReducedPatternSimulationMesh->EN(); ei++) {
                Element &e = pReducedPatternSimulationMesh->elements[ei];
                e.setMaterial(ElementMaterial(e.material.poissonsRatio, newE));
            }
        };
    optimizationState.functions_updateReducedPatternParameter[A] =
        [](const double &newA, std::shared_ptr<SimulationMesh> &pReducedPatternSimulationMesh) {
            assert(pReducedPatternSimulationMesh != nullptr);
            const double beamWidth = std::sqrt(newA);
            const double beamHeight = beamWidth;
            for (EdgeIndex ei = 0; ei < pReducedPatternSimulationMesh->EN(); ei++) {
                Element &e = pReducedPatternSimulationMesh->elements[ei];
                e.setDimensions(RectangularBeamDimensions(beamWidth, beamHeight));
            }
        };

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

void ReducedModelOptimizer::initializeOptimizationParameters(
    const std::shared_ptr<SimulationMesh> &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<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<SimulationMesh> &pReducedPatternSimulationMesh
    //        = 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++) {
        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_pReducedPatternSimulationMesh); //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_pReducedPatternSimulationMesh->updateEigenEdgeAndVertices();
#endif
    m_pReducedPatternSimulationMesh->reset();
    results.fullPatternYoungsModulus = youngsModulus;
    // 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
    LinearSimulationModel simulator;
    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.pFullPatternSimulationMesh->registerForDrawing(Colors::fullDeformed);
    //#endif

    results.perScenario_fullPatternPotentialEnergy.resize(
        optimizationState.simulationScenarioIndices.size());
    for (int i = 0; i < optimizationState.simulationScenarioIndices.size(); i++) {
        const int simulationScenarioIndex = optimizationState.simulationScenarioIndices[i];
        std::shared_ptr<SimulationJob> &pReducedJob
            = optimizationState.reducedPatternSimulationJobs[simulationScenarioIndex];
        SimulationResults reducedModelResults = simulator.executeSimulation(pReducedJob);

#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.reducedPatternSimulationJobs[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);
        optimizationState.fullPatternResults[simulationScenarioIndex].registerForDrawing(
            Colors::fullDeformed);
        polyscope::show();
        reducedModelResults.unregister();
        optimizationState.fullPatternResults[simulationScenarioIndex].unregister();

#endif
    }

    for (int simulationScenarioIndex : optimizationState.simulationScenarioIndices) {
        results.fullPatternSimulationJobs.push_back(
            optimizationState.fullPatternSimulationJobs[simulationScenarioIndex]);
        results.reducedPatternSimulationJobs.push_back(
            optimizationState.reducedPatternSimulationJobs[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_pFullPatternSimulationMesh->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.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.pFullPatternSimulationMesh = m_pFullPatternSimulationMesh;

#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<SimulationMesh> &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;
#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;
#endif
        parameterGroup_optimalResult.x.clear();
        parameterGroup_optimalResult.x.resize(parameterGroup.size());
        std::copy(x.begin(), x.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.reducedPatternSimulationJobs.begin(),
                           optimizationState.reducedPatternSimulationJobs.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<FullPatternVertexIndex, FullPatternVertexIndex>>
        &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<DoFType>{0, 1, 2, 3, 4, 5};
        }
    }
}

void ReducedModelOptimizer::constructSSimulationScenario(
    const double &forceMagnitude,
    const std::vector<std::pair<FullPatternVertexIndex, FullPatternVertexIndex>>
        &oppositeInterfaceViPairs,
    SimulationJob &job)
{
    for (auto viPairIt = oppositeInterfaceViPairs.begin();
         viPairIt != oppositeInterfaceViPairs.end();
         viPairIt++) {
        if (viPairIt == oppositeInterfaceViPairs.begin()) {
            job.constrainedVertices[viPairIt->first] = std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
            job.constrainedVertices[viPairIt->second] = std::unordered_set<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;
        }
    }
}

void ReducedModelOptimizer::constructShearSimulationScenario(
    const double &forceMagnitude,
    const std::vector<std::pair<FullPatternVertexIndex, FullPatternVertexIndex>>
        &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<DoFType>{0, 1, 2, 3, 4, 5};
        }
    }
}

void ReducedModelOptimizer::constructBendingSimulationScenario(
    const double &forceMagnitude,
    const std::vector<std::pair<FullPatternVertexIndex, FullPatternVertexIndex>>
        &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<DoFType>{0, 1, 2, 3, 4, 5};
    }
}

/*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<FullPatternVertexIndex, FullPatternVertexIndex>>
        &oppositeInterfaceViPairs,
    SimulationJob &job)
{
    for (auto viPairIt = oppositeInterfaceViPairs.begin();
         viPairIt != oppositeInterfaceViPairs.end();
         viPairIt++) {
        const auto viPair = *viPairIt;
        CoordType interfaceVector = (job.pMesh->vert[viPair.first].cP()
                                     - job.pMesh->vert[viPair.second].cP());
        VectorType momentAxis = vcg::RotationMatrix(VectorType(0, 0, 1), vcg::math::ToRad(90.0))
                                * interfaceVector.Normalize();
        //        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<DoFType>{0, 1, 2};
        //            job.constrainedVertices[viPair.second] = std::unordered_set<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<DoFType>{2};
        //            job.constrainedVertices[viPair.second] = std::unordered_set<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<DoFType>{0, 1, 2};
        job.constrainedVertices[viPair.second] = std::unordered_set<DoFType>{2};
    }
}

void ReducedModelOptimizer::constructSaddleSimulationScenario(
    const double &forceMagnitude,
    const std::vector<std::pair<FullPatternVertexIndex, FullPatternVertexIndex>>
        &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<DoFType>{2};
            job.constrainedVertices[viPair.second] = std::unordered_set<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;
        }
    }
}

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

    DRMSimulationModel simulator;
    DRMSimulationModel::Settings settings;
    //    settings.totalExternalForcesNormPercentageTermination = 1e-2;
    settings.totalTranslationalKineticEnergyThreshold = 1e-10;
    //    settings.viscousDampingFactor = 1e-2;
    //    settings.useKineticDamping = true;
    settings.shouldDraw = false;
    settings.debugModeStep = 200;
    //    settings.averageResidualForcesCriterionThreshold = 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.totalExternalForcesNormPercentageTermination = 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.pFullPatternSimulationMesh;
    g_baseScenarioMaxDisplacementHelperData.constructScenarioFunction(
        forceMagnitude,
        g_baseScenarioMaxDisplacementHelperData.fullPatternOppositeInterfaceViPairs,
        job);
    //    ReducedModelOptimizer::axialSimulationScenario(forceMagnitude,
    //                                                   optimizationState.fullPatternInterfaceViPairs,
    //                                                   job);

    DRMSimulationModel simulator;
    DRMSimulationModel::Settings settings;
    //    settings.totalResidualForcesNormThreshold = 1e-3;
    settings.totalExternalForcesNormPercentageTermination = 1e-2;
    settings.totalTranslationalKineticEnergyThreshold = 1e-8;
    settings.viscousDampingFactor = 5e-3;
    settings.useTranslationalKineticEnergyForKineticDamping = true;
    //    settings.averageResidualForcesCriterionThreshold = 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);
        settings.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.pFullPatternSimulationMesh = m_pFullPatternSimulationMesh;
    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.pFullPatternSimulationMesh;
        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_pFullPatternSimulationMesh->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<SimulationMesh> &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();
            job.label = baseSimulationScenarioNames[baseScenario] + "_"
                        + std::to_string(simulationScenarioIndex);

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

            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_pFullPatternSimulationMesh->registerForDrawing();
    //    m_pFullPatternSimulationMesh->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.fullPatternSimulationJobs[simulationScenarioIndex]
                    ->constrainedVertices.contains(fullPatternVi)) {
                const std::unordered_set<int> constrainedDof
                    = optimizationState.fullPatternSimulationJobs[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.fullPatternSimulationJobs[simulationScenarioIndex]
                    ->constrainedVertices.contains(fullPatternVi)) {
                const std::unordered_set<int> constrainedDof
                    = optimizationState.fullPatternSimulationJobs[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.fullPatternSimulationJobs
        = createFullPatternSimulationJobs(m_pFullPatternSimulationMesh,
                                          fullPatternSimulationScenarioMaxMagnitudes);
    //  polyscope::removeAllStructures();

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

    results.baseTriangle = baseTriangle;

    DRMSimulationModel::Settings drmSettings;
    drmSettings.totalExternalForcesNormPercentageTermination = 1e-5;
    drmSettings.maxDRMIterations = 200000;
    //    drmSettings.totalTranslationalKineticEnergyThreshold = 1e-10;
    drmSettings.totalTranslationalKineticEnergyThreshold = 1e-9;
    //    drmSettings.gradualForcedDisplacementSteps = 20;
    //    drmSettings.linearGuessForceScaleFactor = 1;
    //    drmSettings.viscousDampingFactor = 5e-3;
    //    drmSettings.useTotalRotationalKineticEnergyForKineticDamping = true;
    //    drmSettings.shouldDraw = true;

    //    drmSettings.totalExternalForcesNormPercentageTermination = 1e-2;
    drmSettings.totalTranslationalKineticEnergyThreshold = 1e-8;
    //    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.averageResidualForcesCriterionThreshold = 1e-5;
    //    simulationSettings.viscousDampingFactor = 1e-3;
    //    simulationSettings.beVerbose = true;
    //    simulationSettings.shouldDraw = true;
    //    simulationSettings.isDebugMode = true;
    //    simulationSettings.debugModeStep = 100000;
    //#ifdef POLYSCOPE_DEFINED
    constexpr bool drawFullPatternSimulationResults = false;
    auto t1 = std::chrono::high_resolution_clock::now();
    //#endif
    results.wasSuccessful = true;
    optimizationState.fullPatternResults.resize(totalNumberOfSimulationScenarios);
    optimizationState.reducedPatternSimulationJobs.resize(totalNumberOfSimulationScenarios);

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

            std::filesystem::path jobResultsDirectoryPath(
                std::filesystem::path(optimizationSettings.intermediateResultsDirectoryPath)
                    .append("FullPatternResults")
                    .append(m_pFullPatternSimulationMesh->getLabel())
                    .append(pFullPatternSimulationJob->getLabel()));
            //                .append(pFullPatternSimulationJob->getLabel() + ".json")
            //#ifdef POLYSCOPE_DEFINED
            //            constexpr bool recomputeFullPatternResults = true;
            //#else
            //            constexpr bool recomputeFullPatternResults = true;
            //#endif
            SimulationResults patternDrmResults;
            //            if (!recomputeFullPatternResults && std::filesystem::exists(jobResultsDirectoryPath)) {
            //                fullPatternResults
            //                    .load(std::filesystem::path(jobResultsDirectoryPath).append("Results"),
            //                          pFullPatternSimulationJob);
            //            } else {
            //        const bool fullPatternScenarioMagnitudesExist = std::filesystem::exists(
            //            patternMaxForceMagnitudesFilePath);
            //        if (fullPatternScenarioMagnitudesExist) {
            //            nlohmann::json json;
            //            std::ifstream ifs(patternMaxForceMagnitudesFilePath.string());
            //            ifs >> json;
            //            fullPatternSimulationScenarioMaxMagnitudes
            //                = static_cast<std::vector<std::pair<BaseSimulationScenario, double>>>(
            //                    json.at("maxMagn"));
            //            const bool shouldRecompute = fullPatternSimulationScenarioMaxMagnitudes.size()
            //                                         != desiredBaseSimulationScenarioIndices.size();
            //            if (!shouldRecompute) {
            //                return fullPatternSimulationScenarioMaxMagnitudes;
            //            }
            //        }
            //#ifdef POLYSCOPE_DEFINED
            //        LinearSimulationModel linearSimulator;
            //        SimulationResults fullPatternResults = linearSimulator.executeSimulation(
            //            pFullPatternSimulationJob);

            //#else
            DRMSimulationModel simulator;
            patternDrmResults = simulator.executeSimulation(pFullPatternSimulationJob, drmSettings);
            //                fullPatternResults.save(jobResultsDirectoryPath);
            //            }
            //#endif
            //        if (!fullPatternResults.converged) {
            //            DRMSimulationModel::Settings simulationSettings_secondRound;
            //            simulationSettings_secondRound.viscousDampingFactor = 2e-3;
            //            simulationSettings_secondRound.useKineticDamping = true;
            //            simulationSettings.maxDRMIterations = 200000;
            //            fullPatternResults = simulator.executeSimulation(pFullPatternSimulationJob,
            //                                                             simulationSettings_secondRound);

            if (!patternDrmResults.converged) {
#ifdef POLYSCOPE_DEFINED
                std::filesystem::path outputPath(
                    std::filesystem::path("../nonConvergingJobs")
                        .append(m_pFullPatternSimulationMesh->getLabel())
                        .append("final_" + pFullPatternSimulationJob->getLabel()));
                std::filesystem::create_directories(outputPath);
                pFullPatternSimulationJob->save(outputPath);
//                drmSettings_secondTry.save(outputPath);
#endif
                //#ifdef POLYSCOPE_DEFINED
                //                SimulationResults fullPatternResults_linear;
                //                if (drawFullPatternSimulationResults) {
                //                    LinearSimulationModel linearSimulator;
                //                    fullPatternResults_linear = linearSimulator.executeSimulation(
                //                        pFullPatternSimulationJob);
                //                    fullPatternResults_linear.labelPrefix += "_linear";
                //                    fullPatternResults_linear.registerForDrawing(std::array<double, 3>{0, 0, 255},
                //                                                                 true);
                //                    std::cout << pFullPatternSimulationJob->getLabel()
                //                              << " did not converge on the first try. Trying again.." << std::endl;
                //                }
                //#endif
                DRMSimulationModel::Settings drmSettings_secondTry = drmSettings;
                //                drmSettings_secondTry.shouldDraw = true;
                //                drmSettings_secondTry.debugModeStep = 20000;
                //                drmSettings_secondTry.shouldCreatePlots = true;
                //                drmSettings_secondTry.beVerbose = true;
                drmSettings_secondTry.linearGuessForceScaleFactor = 1;
                drmSettings_secondTry.viscousDampingFactor = 4e-8;
                *drmSettings_secondTry.maxDRMIterations *= 5;
                drmSettings_secondTry.useTotalRotationalKineticEnergyForKineticDamping = true;
                //                drmSettings.totalTranslationalKineticEnergyThreshold = std::nullopt;
                //                drmSettings.totalExternalForcesNormPercentageTermination = 1e-3;
                drmSettings.totalTranslationalKineticEnergyThreshold = 1e-10;
                //                drmSettings_secondTry.gamma = 2;
                //                drmSettings_secondTry.xi = 0.98;
                SimulationResults drmResults_secondTry
                    = simulator.executeSimulation(pFullPatternSimulationJob, drmSettings_secondTry);

                if (drmResults_secondTry.converged) {
#ifdef POLYSCOPE_DEFINED
                    std::cout << "Simulation job " << pFullPatternSimulationJob->getLabel()
                              << " converged after a second try." << std::endl;
#endif
                    patternDrmResults = drmResults_secondTry;
                } else {
                    results.wasSuccessful = false;
                    std::cerr << "Simulation job " << pFullPatternSimulationJob->getLabel()
                              << " of pattern " << pFullPatternSimulationJob->pMesh->getLabel()
                              << " did not converge." << std::endl;
#ifdef POLYSCOPE_DEFINED
                    std::filesystem::path outputPath(
                        std::filesystem::path("../nonConvergingJobs")
                            .append(m_pFullPatternSimulationMesh->getLabel())
                            .append("final_" + pFullPatternSimulationJob->getLabel()));
                    std::filesystem::create_directories(outputPath);
                    pFullPatternSimulationJob->save(outputPath);
                    drmSettings_secondTry.save(outputPath);

                    if (drawFullPatternSimulationResults) {
                        optimizationState.fullPatternSimulationJobs[0]->pMesh->registerForDrawing(
                            ReducedModelOptimization::Colors::fullInitial);
                        pFullPatternSimulationJob->registerForDrawing(
                            optimizationState.fullPatternSimulationJobs[0]->pMesh->getLabel(), true);
                        patternDrmResults.registerForDrawing(std::array<double, 3>{0, 255, 0}, 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);
                        polyscope::show();
                        patternDrmResults.unregister();
                        //                                        fullPatternResults_linear.unregister();
                        optimizationState.fullPatternSimulationJobs[0]->pMesh->unregister();
                    }
#endif
                    return;
                }
            }

            optimizationState.fullPatternResults[simulationScenarioIndex] = patternDrmResults;
            SimulationJob reducedPatternSimulationJob;
            reducedPatternSimulationJob.pMesh = m_pReducedPatternSimulationMesh;
            computeReducedModelSimulationJob(*pFullPatternSimulationJob,
                                             m_fullToReducedInterfaceViMap,
                                             reducedPatternSimulationJob);
            optimizationState.reducedPatternSimulationJobs[simulationScenarioIndex]
                = std::make_shared<SimulationJob>(reducedPatternSimulationJob);
        //                    std::cout << "Ran sim scenario:" << simulationScenarioIndex << std::endl;
#ifdef POLYSCOPE_DEFINED
            if (drawFullPatternSimulationResults) {
                optimizationState.fullPatternSimulationJobs[0]->pMesh->registerForDrawing(
                    ReducedModelOptimization::Colors::fullInitial);
                pFullPatternSimulationJob->registerForDrawing(optimizationState
                                                                  .fullPatternSimulationJobs[0]
                                                                  ->pMesh->getLabel(),
                                                              true);
                patternDrmResults.registerForDrawing(std::array<double, 3>{0, 255, 0}, 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);
                polyscope::show();
                patternDrmResults.unregister();
                //                fullPatternResults_linear.unregister();
                optimizationState.fullPatternSimulationJobs[0]->pMesh->unregister();
            }
#endif
        });

    auto t2 = std::chrono::high_resolution_clock::now();
    auto s_int = std::chrono::duration_cast<std::chrono::seconds>(t2 - t1);
    std::cout << s_int.count() << std::endl;
    results.wasSuccessful = false;
    return;
#ifdef POLYSCOPE_DEFINED
    std::cout << "Computed ground of truth pattern results in:" << s_int.count() << " seconds."
              << std::endl;
    if (drawFullPatternSimulationResults) {
        optimizationState.fullPatternSimulationJobs[0]->pMesh->unregister();
    }
    optimizationState.reducedPatternSimulationJobs[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);
}