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

const bool gShouldDraw = true;

FormFinder simulator;
std::vector<Eigen::MatrixX3d> g_optimalReducedModelDisplacements;
std::vector<SimulationJob> g_reducedPatternSimulationJob;
std::unordered_map<ReducedPatternVertexIndex, FullPatternVertexIndex>
    g_reducedToFullInterfaceViMap;
matplot::line_handle gPlotHandle;
std::vector<double> gObjectiveValueHistory;
Eigen::Vector4d g_initialX;
std::unordered_set<size_t> g_reducedPatternExludedEdges;
// double g_initialParameters;
Eigen::VectorXd g_initialParameters;
ReducedModelOptimizer::SimulationScenario g_chosenSimulationScenarioName;
std::vector<VectorType> g_innerHexagonVectors{6, VectorType(0, 0, 0)};
double g_innerHexagonInitialPos = 0;

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

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

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

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

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

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

void updateMesh(long n, const double *x) {
  std::shared_ptr<SimulationMesh> pReducedPatternSimulationMesh =
      g_reducedPatternSimulationJob[0].mesh;
  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(ei, 0) * x[ei];
    //        e.properties.G = g_initialParameters(1) * x[1];
    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;
  }

  if (n == 5) {
    assert(pReducedPatternSimulationMesh->EN() == 12);
    for (VertexIndex vi = 0; vi < pReducedPatternSimulationMesh->VN();
         vi += 2) {
      pReducedPatternSimulationMesh->vert[vi].P() =
          g_innerHexagonVectors[vi / 2] * x[4];
    }

    pReducedPatternSimulationMesh->updateEigenEdgeAndVertices();
    pReducedPatternSimulationMesh->updateInitialPositions();
    //    pReducedPatternSimulationMesh->registerForDrawing("Optimized
    //    hexagon"); polyscope::show();
  }
}

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

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

  // run simulations
  double error = 0;
  for (int simulationScenarioIndex = SimulationScenario::Axial;
       simulationScenarioIndex !=
       SimulationScenario::NumberOfSimulationScenarios;
       simulationScenarioIndex++) {
    SimulationResults reducedModelResults = simulator.executeSimulation(
        g_reducedPatternSimulationJob[simulationScenarioIndex], false, false);
    error += computeError(
        reducedModelResults,
        g_optimalReducedModelDisplacements[simulationScenarioIndex]);
  }

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

  return error;
}

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

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

  updateMesh(n, x);

  SimulationResults reducedModelResults = simulator.executeSimulation(
      g_reducedPatternSimulationJob[g_chosenSimulationScenarioName], false,
      false);
  const double error = computeError(
      reducedModelResults,
      g_optimalReducedModelDisplacements[g_chosenSimulationScenarioName]);

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

  return error;
}

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

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

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

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

  //  fullPattern.setLabel("FullPattern");
  //  reducedPattern.setLabel("ReducedPattern");
  // Create opposite vertex map
  m_fullPatternOppositeInterfaceViMap.clear();
  for (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());
    m_fullPatternOppositeInterfaceViMap[vi0] = vi1;
  }

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

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

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

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

void ReducedModelOptimizer::createSimulationMeshes(FlatPattern &fullModel,
                                                   FlatPattern &reducedModel) {
  if (typeid(CrossSectionType) != typeid(RectangularBeamDimensions)) {
    std::cerr << "Error: A rectangular cross section is expected." << std::endl;
    terminate();
  }
  m_pReducedPatternSimulationMesh =
      std::make_shared<SimulationMesh>(reducedModel);
  m_pReducedPatternSimulationMesh->setBeamCrossSection(
      CrossSectionType{0.002, 0.002});
  m_pReducedPatternSimulationMesh->setBeamMaterial(0.3, 1);
  m_pFullModelSimulationMesh = std::make_shared<SimulationMesh>(fullModel);
  m_pFullModelSimulationMesh->setBeamCrossSection(
      CrossSectionType{0.002, 0.002});
  m_pFullModelSimulationMesh->setBeamMaterial(0.3, 1);
}

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

void ReducedModelOptimizer::initializePatterns(
    FlatPattern &fullPattern, FlatPattern &reducedPattern,
    const std::unordered_set<size_t> &reducedModelExcludedEdges) {
  assert(fullPattern.VN() == reducedPattern.VN() &&
         fullPattern.EN() >= reducedPattern.EN());
  polyscope::removeAllStructures();
  // Create copies of the input models
  FlatPattern copyFullPattern;
  FlatPattern copyReducedPattern;
  copyFullPattern.copy(fullPattern);
  copyReducedPattern.copy(reducedPattern);
  if (copyReducedPattern.EN() == 2) {
    const double h = copyReducedPattern.getBaseTriangleHeight();
    double baseTriangle_bottomEdgeSize = 2 * h / tan(vcg::math::ToRad(60.0));
    VectorType baseTriangle_leftBottomNode(-baseTriangle_bottomEdgeSize / 2, -h,
                                           0);

    const int fanSize = 6;
    const CoordType rotationAxis(0, 0, 1);
    for (int rotationCounter = 0; rotationCounter < fanSize;
         rotationCounter++) {
      VectorType rotatedVector =
          vcg::RotationMatrix(rotationAxis,
                              vcg::math::ToRad(rotationCounter * 60.0)) *
          baseTriangle_leftBottomNode;
      g_innerHexagonVectors[rotationCounter] = rotatedVector;
    }
  }
  computeMaps(copyFullPattern, copyReducedPattern, reducedModelExcludedEdges);
  const double innerHexagonInitialPos_x =
      copyReducedPattern.vert[0].cP()[0] / g_innerHexagonVectors[0][0];
  const double innerHexagonInitialPos_y =
      copyReducedPattern.vert[0].cP()[1] / g_innerHexagonVectors[0][1];
  g_innerHexagonInitialPos = innerHexagonInitialPos_x;
  createSimulationMeshes(copyFullPattern, copyReducedPattern);
  initializeStiffnesses();
}

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

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

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

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

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

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

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

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

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

Eigen::VectorXd ReducedModelOptimizer::runOptimization(
    double (*pObjectiveFunction)(long, const double *)) {

  gObjectiveValueHistory.clear();

  const size_t n = m_pReducedPatternSimulationMesh->EN() == 12 ? 5 : 4;
  const size_t npt = 2 * n;
  //      ((n + 2) + ((n + 1) * (n + 2) / 2)) / 2;
  assert(npt <= (n + 1) * (n + 2) / 2 && npt >= n + 2);
  assert(npt <= 2 * n + 1 && "The choice of the number of interpolation "
                             "conditions is not recommended.");
  // Set initial guess of solution

  std::vector<double> x(n, 2);
  x[4] = g_innerHexagonInitialPos;
  if (!initialGuess.empty()) {
    x = initialGuess;
  } //      {0.10000000000000	001, 2, 1.9999999971613847, 6.9560343643347764};
  //      {1, 5.9277};
  //      {0.0001, 2, 2.000000005047502, 1.3055270196964464};
  //      {initialGuess(0), initialGuess(1), initialGuess(2),
  //      initialGuess(3)};
  const double xMin = 1e-2;
  const double xMax = 5000;
  //  assert(x.end() == find_if(x.begin(), x.end(), [&](const double &d) {
  //           return d >= xMax || d <= xMin;
  //         }));
  std::vector<double> xLow(x.size(), xMin);
  std::vector<double> xUpper(x.size(), xMax);
  if (n == 5) {
    xLow[4] = 0.1;
    xUpper[4] = 1;
  }
  const double maxX = *std::max_element(
      x.begin(), x.end(),
      [](const double &a, const double &b) { return abs(a) < abs(b); });
  //  const double rhobeg = std::min(0.95, 0.2 * maxX);
  const double rhobeg = 0.01;
  //  const double rhobeg = 10;
  const double rhoend = rhobeg * 1e-6;
  const size_t wSize = (npt + 5) * (npt + n) + 3 * n * (n + 5) / 2;
  std::vector<double> w(wSize);
  //  const size_t maxFun = 10 * (x.size() ^ 2);
  const size_t maxFun = 120;
  bobyqa(pObjectiveFunction, n, npt, x.data(), xLow.data(), xUpper.data(),
         rhobeg, rhoend, maxFun, w.data());

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

void ReducedModelOptimizer::setInitialGuess(std::vector<double> v) {
  initialGuess = v;
}

std::vector<SimulationJob> ReducedModelOptimizer::createScenarios(
    const std::shared_ptr<SimulationMesh> &pMesh) {
  std::vector<SimulationJob> scenarios;
  std::unordered_map<VertexIndex, std::unordered_set<DoFType>> fixedVertices;
  std::unordered_map<VertexIndex, Vector6d> nodalForces;
  // NOTE: Assuming that the first interface node lays on the y axis
  const double forceMagnitude = 1;
  // Assuming the patterns lays on the x-y plane
  const CoordType patternPlaneNormal(0, 0, 1);
  // Make the first interface node lay on the x axis
  //  const size_t fullPatternFirstInterfaceNodeIndex =
  //      m_fullPatternOppositeInterfaceViMap.begin()->second;
  //  CoordType fullPatternFirstInterfaceNodePosition =
  //      m_pFullModelSimulationMesh->vert[fullPatternFirstInterfaceNodeIndex].cP();
  //  CoordType centerOfMass(0, 0, 0);
  //  for (size_t vi = 0; vi < pMesh->VN(); vi++) {
  //    centerOfMass = centerOfMass + pMesh->vert[vi].P();
  //  }
  //  centerOfMass /= pMesh->VN();
  //  vcg::tri::UpdatePosition<SimulationMesh>::Translate(
  //      *m_pFullModelSimulationMesh, -centerOfMass);
  //  vcg::tri::UpdatePosition<SimulationMesh>::Translate(
  //      *m_pReducedPatternSimulationMesh, centerOfMass);

  //  const vcg::Matrix33d R = vcg::RotationMatrix(
  //      fullPatternFirstInterfaceNodePosition,
  //      CoordType(fullPatternFirstInterfaceNodePosition.Norm(), 0, 0), false);
  //  std::for_each(m_pFullModelSimulationMesh->vert.begin(),
  //                m_pFullModelSimulationMesh->vert.end(), [&](auto &v) {
  //                  v.P() = R * v.P();
  //                  v.N() = R * v.N();
  //                });
  //  std::for_each(m_pReducedPatternSimulationMesh->vert.begin(),
  //                m_pReducedPatternSimulationMesh->vert.end(), [&](auto &v) {
  //                  v.P() = R * v.P();
  //                  v.N() = R * v.N();
  //                });

  //  m_pFullModelSimulationMesh->updateEigenEdgeAndVertices();
  //  m_pReducedPatternSimulationMesh->updateEigenEdgeAndVertices();

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

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

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

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

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

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

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

  return scenarios;
}

void ReducedModelOptimizer::optimize(const int &simulationScenario) {

  std::vector<SimulationJob> simulationJobs =
      createScenarios(m_pFullModelSimulationMesh);
  g_optimalReducedModelDisplacements.resize(6);
  g_reducedPatternSimulationJob.resize(6);
  //  polyscope::removeAllStructures();

  for (int simulationScenarioIndex = SimulationScenario::Axial;
       simulationScenarioIndex !=
       SimulationScenario::NumberOfSimulationScenarios;
       simulationScenarioIndex++) {
    const SimulationJob &fullPatternSimulationJob =
        simulationJobs[simulationScenarioIndex];
    //  fullPatternSimulationJob.mesh->savePly(
    //      "Fanned_" + m_pFullModelSimulationMesh->getLabel() + ".ply");
    SimulationResults fullModelResults =
        simulator.executeSimulation(fullPatternSimulationJob, false);
    //    fullModelResults.label =
    //        "fullModel_" + SimulationScenarioStrings[simulationScenarioIndex];
    //    fullModelResults.registerForDrawing(fullPatternSimulationJob);
    computeDesiredReducedModelDisplacements(
        fullModelResults,
        g_optimalReducedModelDisplacements[simulationScenarioIndex]);
    computeReducedModelSimulationJob(
        fullPatternSimulationJob,
        g_reducedPatternSimulationJob[simulationScenarioIndex]);
  }

  if (simulationScenario == -1) {
    double (*pObjectiveFunction)(long, const double *) = &objectiveAllScenarios;
    runOptimization(pObjectiveFunction);
  } else { // run chosen
    g_chosenSimulationScenarioName = SimulationScenario(simulationScenario);
    double (*pObjectiveFunction)(long, const double *) =
        &objectiveSingleScenario;
    runOptimization(pObjectiveFunction);
  }

  m_pFullModelSimulationMesh->registerForDrawing();
  double error = 0;
  for (int simulationScenarioIndex = SimulationScenario::Axial;
       simulationScenarioIndex !=
       SimulationScenario::NumberOfSimulationScenarios;
       simulationScenarioIndex++) {
    const SimulationJob &fullPatternSimulationJob =
        simulationJobs[simulationScenarioIndex];
    SimulationResults fullModelResults =
        simulator.executeSimulation(fullPatternSimulationJob, false);
    fullModelResults.label =
        "fullModel_" + SimulationScenarioStrings[simulationScenarioIndex];
    fullModelResults.registerForDrawing(fullPatternSimulationJob);
    const SimulationJob &reducedPatternSimulationJob =
        g_reducedPatternSimulationJob[simulationScenarioIndex];
    SimulationResults reducedModelResults =
        simulator.executeSimulation(reducedPatternSimulationJob, false, false);
    error += computeError(
        reducedModelResults,
        g_optimalReducedModelDisplacements[simulationScenarioIndex]);
    reducedModelResults.label =
        "reducedModel_" + SimulationScenarioStrings[simulationScenarioIndex];
    reducedModelResults.registerForDrawing(reducedPatternSimulationJob);
    //    registerWorldAxes();
    std::cout << "A full:"
              << m_pFullModelSimulationMesh->elements[0].properties.A
              << std::endl;
    std::cout << "A reduced:"
              << m_pReducedPatternSimulationMesh->elements[0].properties.A
              << std::endl;
    polyscope::show();
    fullModelResults.unregister();
    reducedModelResults.unregister();
  }
}