#include "reducedmodeloptimizer.hpp"
#include "linearsimulationmodel.hpp"
#include "simulationhistoryplotter.hpp"
#include "trianglepatterngeometry.hpp"
#include "trianglepattterntopology.hpp"
#include <chrono>
#include <dlib/global_optimization.h>
#include <dlib/optimization.h>

struct GlobalOptimizationVariables {
    std::vector<Eigen::MatrixX3d> g_optimalReducedModelDisplacements;
    std::vector<std::vector<Vector6d>> fullPatternDisplacements;
    std::vector<double> objectiveNormalizationValues;
    std::vector<std::shared_ptr<SimulationJob>> fullPatternSimulationJobs;
    std::vector<std::shared_ptr<SimulationJob>> reducedPatternSimulationJobs;
    std::unordered_map<ReducedPatternVertexIndex, FullPatternVertexIndex>
        reducedToFullInterfaceViMap;
    matplot::line_handle gPlotHandle;
    std::vector<double> gObjectiveValueHistory;
    Eigen::VectorXd g_initialParameters;
    std::vector<ReducedModelOptimizer::SimulationScenario>
        simulationScenarioIndices;
    std::vector<VectorType> g_innerHexagonVectors{6, VectorType(0, 0, 0)};
    double innerHexagonInitialRotationAngle{30};
    double g_innerHexagonInitialPos{0};
    double minY{DBL_MAX};
    std::vector<double> minX;
    int numOfSimulationCrashes{false};
    int numberOfFunctionCalls{0};
    int numberOfOptimizationParameters{5};
    ReducedModelOptimizer::Settings optimizationSettings;
} global;

std::vector<SimulationJob> reducedPatternMaximumDisplacementSimulationJobs;

double ReducedModelOptimizer::computeError(
    const std::vector<Vector6d> &reducedPatternDisplacements,
    const std::vector<Vector6d> &fullPatternDisplacements,
    const std::unordered_map<ReducedPatternVertexIndex, FullPatternVertexIndex>
        &reducedToFullInterfaceViMap,
    const double &normalizationFactor) {
    const double rawError =
        computeRawError(reducedPatternDisplacements, fullPatternDisplacements,
                        reducedToFullInterfaceViMap);
    if (global.optimizationSettings.normalizationStrategy !=
        Settings::NormalizationStrategy::NonNormalized) {
        return rawError / normalizationFactor;
    }
    return rawError;
}

double ReducedModelOptimizer::computeRawError(
    const std::vector<Vector6d> &reducedPatternDisplacements,
    const std::vector<Vector6d> &fullPatternDisplacements,
    const std::unordered_map<ReducedPatternVertexIndex, FullPatternVertexIndex>
        &reducedToFullInterfaceViMap) {
    double error = 0;
    for (const auto reducedFullViPair : reducedToFullInterfaceViMap) {
        const VertexIndex reducedModelVi = reducedFullViPair.first;
        Eigen::Vector3d reducedVertexDisplacement(
            reducedPatternDisplacements[reducedModelVi][0],
            reducedPatternDisplacements[reducedModelVi][1],
            reducedPatternDisplacements[reducedModelVi][2]);
        if (!std::isfinite(reducedVertexDisplacement[0]) ||
            !std::isfinite(reducedVertexDisplacement[1]) ||
            !std::isfinite(reducedVertexDisplacement[2])) {
            std::cout << "Displacements are not finite" << std::endl;
            std::terminate();
        }
        const VertexIndex fullModelVi = reducedFullViPair.second;
        Eigen::Vector3d fullVertexDisplacement(
            fullPatternDisplacements[fullModelVi][0],
            fullPatternDisplacements[fullModelVi][1],
            fullPatternDisplacements[fullModelVi][2]);
        Eigen::Vector3d errorVector =
            fullVertexDisplacement - reducedVertexDisplacement;
        //    error += errorVector.squaredNorm();
        error += errorVector.norm();
    }

    return error;
}

void updateMesh(long n, const double *x) {
    std::shared_ptr<SimulationMesh> &pReducedPatternSimulationMesh =
        global.reducedPatternSimulationJobs[global.simulationScenarioIndices[0]]
            ->pMesh;
    //  const Element &elem = g_reducedPatternSimulationJob[0]->mesh->elements[0];
    //  std::cout << elem.axialConstFactor << " " << elem.torsionConstFactor << "
    //  "
    //            << elem.firstBendingConstFactor << " "
    //            << elem.secondBendingConstFactor << std::endl;
    for (EdgeIndex ei = 0; ei < pReducedPatternSimulationMesh->EN(); ei++) {
        Element &e = pReducedPatternSimulationMesh->elements[ei];
        //    if (g_reducedPatternExludedEdges.contains(ei)) {
        //      continue;
        //    }
        //    e.properties.E = g_initialParameters * x[ei];
        //    e.properties.E = g_initialParameters(0) * x[0];
        //    e.properties.G = g_initialParameters(1) * x[1];
        e.setDimensions(
            RectangularBeamDimensions(global.g_initialParameters(0) * x[0],
                                      global.g_initialParameters(0) * x[0] /
                                          (global.g_initialParameters(1) * x[1])));
        e.setMaterial(ElementMaterial(e.material.poissonsRatio,
                                      global.g_initialParameters(2) * x[2]));
        //    e.properties.A = g_initialParameters(0) * x[0];
        //    e.properties.J = g_initialParameters(1) * x[1];
        //    e.properties.I2 = g_initialParameters(2) * x[2];
        //    e.properties.I3 = g_initialParameters(3) * x[3];
        //    e.properties.G = e.properties.E / (2 * (1 + 0.3));
        //    e.axialConstFactor = e.properties.E * e.properties.A /
        //    e.initialLength; e.torsionConstFactor = e.properties.G *
        //    e.properties.J / e.initialLength; e.firstBendingConstFactor =
        //        2 * e.properties.E * e.properties.I2 / e.initialLength;
        //    e.secondBendingConstFactor =
        //        2 * e.properties.E * e.properties.I3 / e.initialLength;
    }

    //  std::cout << elem.axialConstFactor << " " << elem.torsionConstFactor << "
    //  "
    //            << elem.firstBendingConstFactor << " "
    //            << elem.secondBendingConstFactor << std::endl;
    //  const Element &e = pReducedPatternSimulationMesh->elements[0];
    //  std::cout << e.axialConstFactor << " " << e.torsionConstFactor << " "
    //            << e.firstBendingConstFactor << " " <<
    //            e.secondBendingConstFactor
    //            << std::endl;

    assert(pReducedPatternSimulationMesh->EN() == 12);
    auto R = vcg::RotationMatrix(
        ReducedModelOptimizer::patternPlaneNormal,
        vcg::math::ToRad(x[4] - global.innerHexagonInitialRotationAngle));
    //    for (VertexIndex vi = 0; vi < pReducedPatternSimulationMesh->VN();
    //         vi += 2) {
    for (int rotationCounter = 0;
         rotationCounter < ReducedModelOptimizer::fanSize; rotationCounter++) {
        pReducedPatternSimulationMesh->vert[2 * rotationCounter].P() =
            R * global.g_innerHexagonVectors[rotationCounter] * x[3];
    }

    pReducedPatternSimulationMesh->reset();
#ifdef POLYSCOPE_DEFINED
    pReducedPatternSimulationMesh->updateEigenEdgeAndVertices();
#endif
}

double ReducedModelOptimizer::objective(double b, double r, double E) {
    std::vector<double> x{b, r, E};
    return ReducedModelOptimizer::objective(x.size(), x.data());
}

double ReducedModelOptimizer::objective(double b, double h, double E,
                                        double innerHexagonSize,
                                        double innerHexagonRotationAngle) {
    std::vector<double> x{b, h, E, innerHexagonSize, innerHexagonRotationAngle};
    return ReducedModelOptimizer::objective(x.size(), x.data());
}

double ReducedModelOptimizer::objective(long n, const double *x) {
    //  std::cout.precision(17);

    //  const Element &e =
    //  global.reducedPatternSimulationJobs[0]->pMesh->elements[0]; std::cout <<
    //  e.axialConstFactor << " " << e.torsionConstFactor << " "
    //            << e.firstBendingConstFactor << " " <<
    //            e.secondBendingConstFactor
    //            << std::endl;
    updateMesh(n, x);
    //  std::cout << e.axialConstFactor << " " << e.torsionConstFactor << " "
    //            << e.firstBendingConstFactor << " " <<
    //            e.secondBendingConstFactor
    //            << std::endl;

    // run simulations
    double totalError = 0;
//    LinearSimulationModel simulator;
    FormFinder simulator;
    for (const int simulationScenarioIndex : global.simulationScenarioIndices) {
        SimulationResults reducedModelResults = simulator.executeSimulation(
            global.reducedPatternSimulationJobs[simulationScenarioIndex]);
        //#ifdef POLYSCOPE_DEFINED
        //    global.reducedPatternSimulationJobs[simulationScenarioIndex]
        //        ->pMesh->registerForDrawing(colors.reducedInitial);
        //    reducedModelResults.registerForDrawing(colors.reducedDeformed);
        //    polyscope::show();
        //    reducedModelResults.unregister();
        //#endif
        //    std::string filename;
        if (!reducedModelResults.converged) {
            totalError += std::numeric_limits<double>::max();
            global.numOfSimulationCrashes++;
#ifdef POLYSCOPE_DEFINED
            std::cout << "Failed simulation" << std::endl;
#endif
        } else {
            double simulationScenarioError = computeError(
                reducedModelResults.displacements,
                global.fullPatternDisplacements[simulationScenarioIndex],
                global.reducedToFullInterfaceViMap,
                global.objectiveNormalizationValues[simulationScenarioIndex]);

            //        if (global.optimizationSettings.normalizationStrategy !=
            //        NormalizationStrategy::Epsilon &&
            //            simulationScenarioError > 1) {
            //          std::cout << "Simulation scenario "
            //                    <<
            //                    simulationScenarioStrings[simulationScenarioIndex]
            //                    << " results in an error bigger than one." <<
            //                    std::endl;
            //          for (size_t parameterIndex = 0; parameterIndex < n;
            //               parameterIndex++) {
            //            std::cout << "x[" + std::to_string(parameterIndex) + "]="
            //                      << x[parameterIndex] << std::endl;
            //          }
            //        }

            //#ifdef POLYSCOPE_DEFINED
            //        ReducedModelOptimizer::visualizeResults(
            //            global.fullPatternSimulationJobs[simulationScenarioIndex],
            //            global.reducedPatternSimulationJobs[simulationScenarioIndex],
            //            global.reducedToFullInterfaceViMap, false);

            //        ReducedModelOptimizer::visualizeResults(
            //            global.fullPatternSimulationJobs[simulationScenarioIndex],
            //            std::make_shared<SimulationJob>(
            //                reducedPatternMaximumDisplacementSimulationJobs
            //                    [simulationScenarioIndex]),
            //            global.reducedToFullInterfaceViMap, true);
            //        polyscope::removeAllStructures();
            //#endif // POLYSCOPE_DEFINED
            totalError += simulationScenarioError;
        }
    }
    //  std::cout << error << std::endl;
    if (totalError < global.minY) {
        global.minY = totalError;
        global.minX.assign(x, x + n);
    }
#ifdef POLYSCOPE_DEFINED
    if (++global.numberOfFunctionCalls % 100 == 0) {
        std::cout << "Number of function calls:" << global.numberOfFunctionCalls
                  << std::endl;
    }
#endif
    // compute error and return it
    //  global.gObjectiveValueHistory.push_back(error);
    //  auto xPlot = matplot::linspace(0, gObjectiveValueHistory.size(),
    //                                 gObjectiveValueHistory.size());
    //  std::vector<double> colors(gObjectiveValueHistory.size(), 2);
    //  if (g_firstRoundIterationIndex != 0) {
    //    for_each(colors.begin() + g_firstRoundIterationIndex, colors.end(),
    //             [](double &c) { c = 0.7; });
    //  }
    //  gPlotHandle = matplot::scatter(xPlot, gObjectiveValueHistory, 6, colors);
    //  SimulationResultsReporter::createPlot("Number of Steps", "Objective
    //  value",
    //                                        gObjectiveValueHistory);

    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;
        terminate();
    }
    // Full pattern
    pFullPatternSimulationMesh = std::make_shared<SimulationMesh>(fullModel);
    pFullPatternSimulationMesh->setBeamCrossSection(
        CrossSectionType{0.002, 0.002});
    pFullPatternSimulationMesh->setBeamMaterial(0.3, 1 * 1e9);

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

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

void ReducedModelOptimizer::computeMaps(
    const std::unordered_set<size_t> &reducedModelExcludedEdges,
    const std::unordered_map<size_t, std::unordered_set<size_t>> &slotToNode,
    PatternGeometry &fullPattern, PatternGeometry &reducedPattern,
    std::unordered_map<ReducedPatternVertexIndex, FullPatternVertexIndex>
        &reducedToFullInterfaceViMap,
    std::unordered_map<FullPatternVertexIndex, ReducedPatternVertexIndex>
        &fullToReducedInterfaceViMap,
    std::unordered_map<FullPatternVertexIndex, ReducedPatternVertexIndex>
        &fullPatternOppositeInterfaceViMap) {

    // 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<PatternGeometry>::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 global object. Should this be a
    // function parameter?
    //  global.reducedPatternExludedEdges.clear();
    //  const size_t reducedBaseTriangleNumberOfEdges = reducedPattern.EN();
    //  for (size_t fanIndex = 0; fanIndex < fanSize; fanIndex++) {
    //    for (const size_t ei : reducedModelExcludedEdges) {
    //      global.reducedPatternExludedEdges.insert(
    //          fanIndex * reducedBaseTriangleNumberOfEdges + ei);
    //    }
    //  }

    // Construct reduced->full and full->reduced interface vi map
    reducedToFullInterfaceViMap.clear();
    vcg::tri::Allocator<PatternGeometry>::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*/;
    reducedPattern.createFan();
    for (size_t fanIndex = 0; fanIndex < fanSize; fanIndex++) {
        reducedToFullInterfaceViMap[reducedModelInterfaceVertexOffset * fanIndex +
                                    reducedModelBaseTriangleInterfaceVi] =
                fullModelBaseTriangleInterfaceVi +
                fanIndex * fullPatternInterfaceVertexOffset;
    }
    fullToReducedInterfaceViMap.clear();
    constructInverseMap(reducedToFullInterfaceViMap, fullToReducedInterfaceViMap);

    // Create opposite vertex map
    fullPatternOppositeInterfaceViMap.clear();
    for (int fanIndex = fanSize / 2 - 1; fanIndex >= 0; fanIndex--) {
        const size_t vi0 = fullModelBaseTriangleInterfaceVi +
                           fanIndex * fullPatternInterfaceVertexOffset;
        const size_t vi1 = vi0 + (fanSize / 2) * fullPatternInterfaceVertexOffset;
        assert(vi0 < fullPattern.VN() && vi1 < fullPattern.VN());
        fullPatternOppositeInterfaceViMap[vi0] = vi1;
    }

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

        //    std::vector<glm::vec3> colors_reducedPatternExcludedEdges(
        //        reducedPattern.EN(), glm::vec3(0, 0, 0));
        //    for (const size_t ei : global.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 :
             fullPatternOppositeInterfaceViMap) {
            auto color = polyscope::getNextUniqueColor();
            nodeColorsOpposite[oppositeVerts.first] = color;
            nodeColorsOpposite[oppositeVerts.second] = color;
        }
        fullPattern.registerForDrawing();
        polyscope::getCurveNetwork(fullPattern.getLabel())
            ->addNodeColorQuantity("oppositeMap", nodeColorsOpposite)
            ->setEnabled(true);
        polyscope::show();

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

                auto color = polyscope::getNextUniqueColor();
                nodeColorsReducedToFull_reduced[vi] = color;
                nodeColorsReducedToFull_full[global.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, PatternGeometry &reducedPattern,
    const std::unordered_set<size_t> &reducedModelExcludedEdges) {
    ReducedModelOptimizer::computeMaps(
        reducedModelExcludedEdges, slotToNode, fullPattern, reducedPattern,
        global.reducedToFullInterfaceViMap, m_fullToReducedInterfaceViMap,
        m_fullPatternOppositeInterfaceViMap);
}

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

void ReducedModelOptimizer::initializePatterns(
    PatternGeometry &fullPattern, PatternGeometry &reducedPattern,
    const std::unordered_set<size_t> &reducedModelExcludedEdges) {
    // fullPattern.setLabel("full_pattern_" + fullPattern.getLabel());
    // reducedPattern.setLabel("reduced_pattern_" + reducedPattern.getLabel());
    assert(fullPattern.VN() == reducedPattern.VN() &&
           fullPattern.EN() >= reducedPattern.EN());
#if POLYSCOPE_DEFINED
    polyscope::removeAllStructures();
#endif
    // Create copies of the input models
    PatternGeometry copyFullPattern;
    PatternGeometry copyReducedPattern;
    copyFullPattern.copy(fullPattern);
    copyReducedPattern.copy(reducedPattern);
    /*
   * Here we create the vector that connects the central node with the bottom
   * left node of the base triangle. During the optimization the vi%2==0 nodes
   * move on these vectors.
   * */
    const double h = copyReducedPattern.getBaseTriangleHeight();
    double baseTriangle_bottomEdgeSize = 2 * h / tan(vcg::math::ToRad(60.0));
    VectorType baseTriangle_leftBottomNode(-baseTriangle_bottomEdgeSize / 2, -h,
                                           0);
    for (int rotationCounter = 0; rotationCounter < fanSize; rotationCounter++) {
        VectorType rotatedVector =
            vcg::RotationMatrix(patternPlaneNormal,
                                vcg::math::ToRad(rotationCounter * 60.0)) *
            baseTriangle_leftBottomNode;
        global.g_innerHexagonVectors[rotationCounter] = rotatedVector;
    }
    const double innerHexagonInitialPos_x =
        copyReducedPattern.vert[0].cP()[0] / global.g_innerHexagonVectors[0][0];
    const double innerHexagonInitialPos_y =
        copyReducedPattern.vert[0].cP()[1] / global.g_innerHexagonVectors[0][1];
    global.g_innerHexagonInitialPos = innerHexagonInitialPos_x;
    global.innerHexagonInitialRotationAngle =
        30; /* NOTE: Here I assume that the CW reduced pattern is given as input.
             This is not very generic */
    computeMaps(copyFullPattern, copyReducedPattern, reducedModelExcludedEdges);
    createSimulationMeshes(copyFullPattern, copyReducedPattern);
    initializeOptimizationParameters(m_pReducedPatternSimulationMesh);
}

void ReducedModelOptimizer::initializeOptimizationParameters(
    const std::shared_ptr<SimulationMesh> &mesh) {
    global.numberOfOptimizationParameters = 5;
    global.g_initialParameters.resize(global.numberOfOptimizationParameters);
    // Save save the beam stiffnesses
    //  for (size_t ei = 0; ei < pReducedModelElementalMesh->EN(); ei++) {
    //    Element &e = pReducedModelElementalMesh->elements[ei];
    //    if (g_reducedPatternExludedEdges.contains(ei)) {
    //      const double stiffnessFactor = 5;
    //      e.axialConstFactor *= stiffnessFactor;
    //      e.torsionConstFactor *= stiffnessFactor;
    //      e.firstBendingConstFactor *= stiffnessFactor;
    //      e.secondBendingConstFactor *= stiffnessFactor;
    //    }
    const double initialB = std::sqrt(mesh->elements[0].A);
    const double initialRatio = 1;
    global.g_initialParameters(0) = initialB;
    global.g_initialParameters(1) = initialRatio;
    global.g_initialParameters(2) = mesh->elements[0].material.youngsModulus;
    global.g_initialParameters(3) = global.g_innerHexagonInitialPos;
    global.innerHexagonInitialRotationAngle = 30;
    global.g_initialParameters(4) = global.innerHexagonInitialRotationAngle;
    //  g_initialParameters =
    //      m_pReducedPatternSimulationMesh->elements[0].properties.E;
    //  for (size_t ei = 0; ei < m_pReducedPatternSimulationMesh->EN(); ei++) {
    //  }
    //  g_initialParameters(0) = mesh->elements[0].properties.E;
    //  g_initialParameters(1) = mesh->elements[0].properties.G;
    //  g_initialParameters(0) = mesh->elements[0].properties.A;
    //  g_initialParameters(1) = mesh->elements[0].properties.J;
    //  g_initialParameters(2) = mesh->elements[0].properties.I2;
    //  g_initialParameters(3) = mesh->elements[0].properties.I3;
    //  }
}

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

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

    //  std::unordered_map<VertexIndex, VectorType>
    //  reducedModelNodalForcedNormals; for (auto fullModelNodalForcedRotation :
    //       simulationJobOfFullModel.nodalForcedNormals) {
    //    reducedModelNodalForcedNormals[simulationJobFullToReducedMap.at(
    //        fullModelNodalForcedRotation.first)] =
    //        fullModelNodalForcedRotation.second;
    //  }
    simulationJobOfReducedModel.constrainedVertices = reducedModelFixedVertices;
    simulationJobOfReducedModel.nodalExternalForces = reducedModelNodalForces;
    simulationJobOfReducedModel.label = simulationJobOfFullModel.getLabel();
    //  simulationJobOfReducedModel.nodalForcedNormals =
    //      reducedModelNodalForcedNormals;
}

#if POLYSCOPE_DEFINED
void ReducedModelOptimizer::visualizeResults(
    const std::vector<std::shared_ptr<SimulationJob>>
        &fullPatternSimulationJobs,
    const std::vector<std::shared_ptr<SimulationJob>>
        &reducedPatternSimulationJobs,
    const std::vector<SimulationScenario> &simulationScenarios,
    const std::unordered_map<ReducedPatternVertexIndex, FullPatternVertexIndex>
        &reducedToFullInterfaceViMap) {
    FormFinder simulator;
    std::shared_ptr<SimulationMesh> pFullPatternSimulationMesh =
        fullPatternSimulationJobs[0]->pMesh;
    pFullPatternSimulationMesh->registerForDrawing();
    pFullPatternSimulationMesh->savePly(pFullPatternSimulationMesh->getLabel() +
                                        "_undeformed.ply");
    reducedPatternSimulationJobs[0]->pMesh->savePly(
        reducedPatternSimulationJobs[0]->pMesh->getLabel() + "_undeformed.ply");
    double totalError = 0;
    for (const int simulationScenarioIndex : simulationScenarios) {
        const std::shared_ptr<SimulationJob> &pFullPatternSimulationJob =
            fullPatternSimulationJobs[simulationScenarioIndex];
        pFullPatternSimulationJob->registerForDrawing(
            pFullPatternSimulationMesh->getLabel());
        SimulationResults fullModelResults =
            simulator.executeSimulation(pFullPatternSimulationJob);
        fullModelResults.registerForDrawing();
        // fullModelResults.saveDeformedModel();
        const std::shared_ptr<SimulationJob> &pReducedPatternSimulationJob =
            reducedPatternSimulationJobs[simulationScenarioIndex];
        SimulationResults reducedModelResults =
            simulator.executeSimulation(pReducedPatternSimulationJob);
        double normalizationFactor = 1;
        if (global.optimizationSettings.normalizationStrategy !=
            Settings::NormalizationStrategy::NonNormalized) {
            normalizationFactor =
                global.objectiveNormalizationValues[simulationScenarioIndex];
        }
        reducedModelResults.saveDeformedModel();
        fullModelResults.saveDeformedModel();
        double error = computeError(
            reducedModelResults.displacements, fullModelResults.displacements,
            reducedToFullInterfaceViMap, normalizationFactor);
        std::cout << "Error of simulation scenario "
                  << simulationScenarioStrings[simulationScenarioIndex] << " is "
                  << error << std::endl;
        totalError += error;
        reducedModelResults.registerForDrawing();
        //    firstOptimizationRoundResults[simulationScenarioIndex].registerForDrawing();
        //    registerWorldAxes();
        const std::string screenshotFilename =
            "/home/iason/Coding/Projects/Approximating shapes with flat "
            "patterns/RodModelOptimizationForPatterns/build/OptimizationResults/"
            "Images/" +
            pFullPatternSimulationMesh->getLabel() + "_" +
            simulationScenarioStrings[simulationScenarioIndex];
        polyscope::show();
        polyscope::screenshot(screenshotFilename, false);
        fullModelResults.unregister();
        reducedModelResults.unregister();
        //    firstOptimizationRoundResults[simulationScenarioIndex].unregister();
    }
    std::cout << "Total error:" << totalError << std::endl;
}

void ReducedModelOptimizer::registerResultsForDrawing(
    const std::shared_ptr<SimulationJob> &pFullPatternSimulationJob,
    const std::shared_ptr<SimulationJob> &pReducedPatternSimulationJob,
    const std::unordered_map<ReducedPatternVertexIndex, FullPatternVertexIndex>
        &reducedToFullInterfaceViMap) {
    FormFinder simulator;
    std::shared_ptr<SimulationMesh> pFullPatternSimulationMesh =
        pFullPatternSimulationJob->pMesh;
    pFullPatternSimulationMesh->registerForDrawing();
    //  pFullPatternSimulationMesh->savePly(pFullPatternSimulationMesh->getLabel()
    //  +
    //                                      "_undeformed.ply");
    //  reducedPatternSimulationJobs[0]->pMesh->savePly(
    //      reducedPatternSimulationJobs[0]->pMesh->getLabel() +
    //      "_undeformed.ply");
    pFullPatternSimulationJob->registerForDrawing(
        pFullPatternSimulationMesh->getLabel());
    SimulationResults fullModelResults =
        simulator.executeSimulation(pFullPatternSimulationJob);
    fullModelResults.registerForDrawing();
    // fullModelResults.saveDeformedModel();
    SimulationResults reducedModelResults =
        simulator.executeSimulation(pReducedPatternSimulationJob);
    //    reducedModelResults.saveDeformedModel();
    //    fullModelResults.saveDeformedModel();
    double error = computeRawError(
        reducedModelResults.displacements, fullModelResults.displacements,
        reducedToFullInterfaceViMap/*,
      global.reducedPatternMaximumDisplacementNormSum[simulationScenarioIndex]*/);
    std::cout << "Error is " << error << std::endl;
    reducedModelResults.registerForDrawing();
}
#endif // POLYSCOPE_DEFINED

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

ReducedModelOptimizer::Results
ReducedModelOptimizer::runOptimization(const Settings &settings) {

    global.gObjectiveValueHistory.clear();
    dlib::matrix<double, 0, 1> xMin(global.numberOfOptimizationParameters);
    dlib::matrix<double, 0, 1> xMax(global.numberOfOptimizationParameters);
    for (int i = 0; i < global.numberOfOptimizationParameters; i++) {
        xMin(i) = settings.xRanges[i].min;
        xMax(i) = settings.xRanges[i].max;
    }

    auto start = std::chrono::system_clock::now();
    dlib::function_evaluation result;
    double (*objF)(double, double, double, double, double) = &objective;
    result = dlib::find_min_global(
        objF, xMin, xMax,
        dlib::max_function_calls(settings.numberOfFunctionCalls),
        std::chrono::hours(24 * 365 * 290), settings.solutionAccuracy);
    auto end = std::chrono::system_clock::now();
    auto elapsed =
        std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
    Results results;
    results.numberOfSimulationCrashes = global.numOfSimulationCrashes;
    results.x = global.minX;
    results.objectiveValue = global.minY;
    if (global.minY != result.y) {
        std::cerr << "Global min objective is not equal to result objective"
                  << std::endl;
    }

    // Compute obj value per simulation scenario
    results.rawObjectiveValue=0;
    updateMesh(results.x.size(), results.x.data());
    results.objectiveValuePerSimulationScenario.resize(
        NumberOfSimulationScenarios);
    FormFinder::Settings simulationSettings;
    FormFinder simulator;
    for (int simulationScenarioIndex = 0;
         simulationScenarioIndex < NumberOfSimulationScenarios;
         simulationScenarioIndex++) {
        SimulationResults reducedModelResults = simulator.executeSimulation(
            global.reducedPatternSimulationJobs[simulationScenarioIndex],
            simulationSettings);
        const double error = computeError(
            reducedModelResults.displacements,
            global.fullPatternDisplacements[simulationScenarioIndex],
            global.reducedToFullInterfaceViMap,
            global.objectiveNormalizationValues[simulationScenarioIndex]);
        results.rawObjectiveValue+=computeRawError(reducedModelResults.displacements,
            global.fullPatternDisplacements[simulationScenarioIndex],
                                                     global.reducedToFullInterfaceViMap);
        results.objectiveValuePerSimulationScenario[simulationScenarioIndex] =
            error;
    }
    //  if (result.y !=
    //      std::accumulate(results.objectiveValuePerSimulationScenario.begin(),
    //                      results.objectiveValuePerSimulationScenario.end(), 0))
    //                      {
    //    std::cerr
    //        << "Sum of per scenario objectives is not equal to result objective"
    //        << std::endl;
    //  }
    results.time = elapsed.count() / 1000.0;
    const bool printDebugInfo = false;
    if (printDebugInfo) {
        std::cout << "Finished optimizing." << endl;
        //  std::cout << "Solution x:" << endl;
        //  std::cout << result.x << endl;
        std::cout << "Objective value:" << global.minY << endl;
    }

    return results;
}

std::vector<std::shared_ptr<SimulationJob>>
ReducedModelOptimizer::createScenarios(
    const std::shared_ptr<SimulationMesh> &pMesh) {
    std::vector<std::shared_ptr<SimulationJob>> scenarios;
    scenarios.resize(SimulationScenario::NumberOfSimulationScenarios);
    std::unordered_map<VertexIndex, std::unordered_set<DoFType>> fixedVertices;
    std::unordered_map<VertexIndex, Vector6d> nodalForces;
    const double forceMagnitude = 10;

    //// Axial
    SimulationScenario scenarioName = SimulationScenario::Axial;
    // NewMethod
    for (auto viPairIt = m_fullPatternOppositeInterfaceViMap.begin();
         viPairIt != m_fullPatternOppositeInterfaceViMap.end(); viPairIt++) {
        if (viPairIt != m_fullPatternOppositeInterfaceViMap.begin()) {
            CoordType forceDirection(1, 0, 0);
            const auto viPair = *viPairIt;
            nodalForces[viPair.first] =
                Vector6d({forceDirection[0], forceDirection[1], forceDirection[2], 0,
                          0, 0}) *
                forceMagnitude * 8;
            fixedVertices[viPair.second] =
                std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
        }
    }
    // OldMethod
    //  for (const auto &viPair : m_fullPatternOppositeInterfaceViMap) {
    //    CoordType forceDirection =
    //        (pMesh->vert[viPair.first].cP() - pMesh->vert[viPair.second].cP())
    //            .Normalize();
    //    nodalForces[viPair.first] = Vector6d({forceDirection[0],
    //    forceDirection[1],
    //                                          forceDirection[2], 0, 0, 0}) *
    //                                forceMagnitude * 10;
    //    fixedVertices[viPair.second] =
    //        std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
    //  }
    scenarios[scenarioName] = std::make_shared<SimulationJob>(
        SimulationJob(pMesh, simulationScenarioStrings[scenarioName],
                      fixedVertices, nodalForces, {}));

    //// Shear
    scenarioName = SimulationScenario::Shear;
    fixedVertices.clear();
    nodalForces.clear();
    // NewMethod
    for (auto viPairIt = m_fullPatternOppositeInterfaceViMap.begin();
         viPairIt != m_fullPatternOppositeInterfaceViMap.end(); viPairIt++) {
        if (viPairIt != m_fullPatternOppositeInterfaceViMap.begin()) {
            CoordType forceDirection(0, 1, 0);
            const auto viPair = *viPairIt;
            nodalForces[viPair.first] =
                Vector6d({forceDirection[0], forceDirection[1], forceDirection[2], 0,
                          0, 0}) *
                forceMagnitude * 8;
            fixedVertices[viPair.second] =
                std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
        }
    }
    // OldMethod
    //  for (const auto &viPair : m_fullPatternOppositeInterfaceViMap) {
    //    CoordType v =
    //        (pMesh->vert[viPair.first].cP() - pMesh->vert[viPair.second].cP())
    //            .Normalize();
    //    CoordType forceDirection = (v ^ patternPlaneNormal).Normalize();
    //    nodalForces[viPair.first] = Vector6d({forceDirection[0],
    //    forceDirection[1],
    //                                          forceDirection[2], 0, 0, 0}) *
    //                                0.40 * forceMagnitude;
    //    fixedVertices[viPair.second] =
    //        std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
    //  }
    scenarios[scenarioName] = std::make_shared<SimulationJob>(
        SimulationJob(pMesh, simulationScenarioStrings[scenarioName],
                      fixedVertices, nodalForces, {}));

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

    ////  Bending
    scenarioName = SimulationScenario::Bending;
    fixedVertices.clear();
    nodalForces.clear();
    for (const auto &viPair : m_fullPatternOppositeInterfaceViMap) {
        nodalForces[viPair.first] = Vector6d({0, 0, forceMagnitude, 0, 0, 0}) * 1;
        fixedVertices[viPair.second] =
            std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
    }
    scenarios[scenarioName] = std::make_shared<SimulationJob>(
        SimulationJob(pMesh, simulationScenarioStrings[scenarioName],
                      fixedVertices, nodalForces, {}));

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

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

            nodalForces[viPair.first] =
                Vector6d({0, 0, 0, -v[0], -v[1], 0}) * 0.1 * forceMagnitude;
            nodalForces[viPair.second] =
                Vector6d({0, 0, 0, v[0], v[1], 0}) * 0.1 * forceMagnitude;
        }
    }
    scenarios[scenarioName] = std::make_shared<SimulationJob>(
        SimulationJob(pMesh, simulationScenarioStrings[scenarioName],
                      fixedVertices, nodalForces, {}));

    return scenarios;
}

void ReducedModelOptimizer::computeObjectiveValueNormalizationFactors() {
    if (global.optimizationSettings.normalizationStrategy ==
        Settings::NormalizationStrategy::Epsilon) {

        // Compute the sum of the displacement norms
        std::vector<double> fullPatternDisplacementNormSum(
            NumberOfSimulationScenarios);
        for (int simulationScenarioIndex : global.simulationScenarioIndices) {
            double displacementNormSum = 0;
            for (auto interfaceViPair : global.reducedToFullInterfaceViMap) {
                const Vector6d &vertexDisplacement =
                    global.fullPatternDisplacements[simulationScenarioIndex]
                                                   [interfaceViPair.second];
                displacementNormSum += vertexDisplacement.getTranslation().norm();
            }
            fullPatternDisplacementNormSum[simulationScenarioIndex] =
                displacementNormSum;
        }

        for (int simulationScenarioIndex : global.simulationScenarioIndices) {
            if (global.optimizationSettings.normalizationStrategy ==
                Settings::NormalizationStrategy::Epsilon) {
                const double epsilon =
                    global.optimizationSettings.normalizationParameter;
                if (epsilon > fullPatternDisplacementNormSum[simulationScenarioIndex]) {
                    //          std::cout << "Epsilon used in "
                    //                    <<
                    //                    simulationScenarioStrings[simulationScenarioIndex]
                    //                    << std::endl;
                }
                global.objectiveNormalizationValues[simulationScenarioIndex] = std::max(
                    fullPatternDisplacementNormSum[simulationScenarioIndex], epsilon);
                //            displacementNormSum;
            }
        }
    }
}

ReducedModelOptimizer::Results ReducedModelOptimizer::optimize(
    const Settings &optimizationSettings,
    const std::vector<SimulationScenario> &simulationScenarios) {

    global.simulationScenarioIndices = simulationScenarios;
    if (global.simulationScenarioIndices.empty()) {
        global.simulationScenarioIndices = {
            SimulationScenario::Axial, SimulationScenario::Shear,
            SimulationScenario::Bending, SimulationScenario::Dome,
            SimulationScenario::Saddle};
    }

    global.g_optimalReducedModelDisplacements.resize(NumberOfSimulationScenarios);
    global.reducedPatternSimulationJobs.resize(NumberOfSimulationScenarios);
    global.fullPatternDisplacements.resize(NumberOfSimulationScenarios);
    global.objectiveNormalizationValues.resize(NumberOfSimulationScenarios);
    global.minY = std::numeric_limits<double>::max();
    global.numOfSimulationCrashes = 0;
    global.numberOfFunctionCalls = 0;
    global.optimizationSettings = optimizationSettings;
    global.fullPatternSimulationJobs =
        createScenarios(m_pFullPatternSimulationMesh);
    reducedPatternMaximumDisplacementSimulationJobs.resize(
        NumberOfSimulationScenarios);
    //  polyscope::removeAllStructures();

    FormFinder::Settings simulationSettings;
    //  settings.shouldDraw = true;
    for (int simulationScenarioIndex : global.simulationScenarioIndices) {
        const std::shared_ptr<SimulationJob> &pFullPatternSimulationJob =
            global.fullPatternSimulationJobs[simulationScenarioIndex];
        SimulationResults fullModelResults = simulator.executeSimulation(
            pFullPatternSimulationJob, simulationSettings);
        global.fullPatternDisplacements[simulationScenarioIndex] =
            fullModelResults.displacements;
        SimulationJob reducedPatternSimulationJob;
        reducedPatternSimulationJob.pMesh = m_pReducedPatternSimulationMesh;
        computeReducedModelSimulationJob(*pFullPatternSimulationJob,
                                         m_fullToReducedInterfaceViMap,
                                         reducedPatternSimulationJob);
        global.reducedPatternSimulationJobs[simulationScenarioIndex] =
            std::make_shared<SimulationJob>(reducedPatternSimulationJob);
    }

    if (global.optimizationSettings.normalizationStrategy !=
        Settings::NormalizationStrategy::NonNormalized) {
        computeObjectiveValueNormalizationFactors();
    }
    Results optResults = runOptimization(optimizationSettings);
    for (int simulationScenarioIndex : global.simulationScenarioIndices) {
        optResults.fullPatternSimulationJobs.push_back(
            global.fullPatternSimulationJobs[simulationScenarioIndex]);
        optResults.reducedPatternSimulationJobs.push_back(
            global.reducedPatternSimulationJobs[simulationScenarioIndex]);
    }
#ifdef POLYSCOPE_DEFINED
    optResults.draw();
#endif // POLYSCOPE_DEFINED

    return optResults;
}