#include "beamformfinder.hpp"
#include "edgemesh.hpp"
#include "flatpattern.hpp"
#include "polyscope/curve_network.h"
#include "polyscope/point_cloud.h"
#include "polyscope/polyscope.h"
#include "reducedmodeloptimizer.hpp"
#include "simulationhistoryplotter.hpp"
#include <chrono>
#include <filesystem>
#include <iostream>
#include <stdexcept>
#include <string>

int main(int argc, char *argv[]) {
  //  FlatPattern pattern("/home/iason/Models/valid_6777.ply");
  //  FlatPattern pattern("/home/iason/Models/simple_beam_paper_example.ply");
  //  pattern.savePly("fannedValid.ply");

  registerWorldAxes();
  // Create reduced models
  const std::vector<size_t> numberOfNodesPerSlot{1, 0, 0, 2, 1, 2, 1};
  std::vector<vcg::Point2i> singleBarReducedModelEdges{vcg::Point2i(0, 3)};
  FlatPattern singleBarReducedModel(numberOfNodesPerSlot,
                                    singleBarReducedModelEdges);
  singleBarReducedModel.setLabel("Single bar reduced model");

  std::vector<vcg::Point2i> CCWreducedModelEdges{vcg::Point2i(1, 5),
                                                 vcg::Point2i(3, 5)};
  FlatPattern CWReducedModel(numberOfNodesPerSlot, CCWreducedModelEdges);
  CWReducedModel.setLabel("CW reduced model");

  std::vector<vcg::Point2i> CWreducedModelEdges{vcg::Point2i(1, 5),
                                                vcg::Point2i(3, 1)};
  FlatPattern CCWReducedModel(numberOfNodesPerSlot, CWreducedModelEdges);
  CCWReducedModel.setLabel("CCW reduced model");

  std::vector<FlatPattern *> reducedModels{&singleBarReducedModel,
                                           &CWReducedModel, &CCWReducedModel};

  ReducedModelOptimizer optimizer(numberOfNodesPerSlot);
  std::string fullPatternsTestSetDirectory =
      "/home/iason/Models/TestSet_validPatterns";
  for (const auto &entry :
       filesystem::directory_iterator(fullPatternsTestSetDirectory)) {
    const auto filepath =
        //          entry.path();
        std::filesystem::path("/home/iason/Models/valid_6777.ply");
    const auto filepathString = filepath.string();
    // Use only the base triangle version
    const std::string tiledSuffix = "_tiled.ply";
    if (filepathString.compare(filepathString.size() - tiledSuffix.size(),
                               tiledSuffix.size(), tiledSuffix) == 0) {
      continue;
    }
    FlatPattern pattern(filepathString);
    pattern.setLabel(filepath.stem().string());
    std::cout << "Testing Pattern:" << filepathString << std::endl;
    for (FlatPattern *pReducedModel : reducedModels) {
      pReducedModel = reducedModels[0];
      std::unordered_set<size_t> optimizationExcludedEi;
      if (pReducedModel !=
          reducedModels[0]) { // assumes that the singleBar reduced model is the
                              // first in the reducedModels vector
        optimizationExcludedEi.insert(0);
      }
      optimizer.initialize(pattern, *pReducedModel, optimizationExcludedEi);
      optimizer.initialize(pattern, *pReducedModel, optimizationExcludedEi);
      Eigen::VectorXd optimalParameters = optimizer.optimize();
    }
  }

  //  // Full model simulation
  //  std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
  //  fixedVertices;
  //  //  fixedVertices[0] = std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
  //  fixedVertices[3] = std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
  //  //  fixedVertices[7] = std::unordered_set<DoFType>{0, 1, 2};
  //  std::unordered_map<VertexIndex, Vector6d> nodalForces{
  //      {15, {0, 0, 2000, 0, 0, 0}}};
  //  SimulationJob fullModelSimulationJob{
  //      std::make_shared<ElementalMesh>(pattern), fixedVertices, nodalForces,
  //      {}};
  //  Simulator formFinder;
  //  SimulationResults fullModelResults =
  //      formFinder.executeSimulation(fullModelSimulationJob);
  //  fullModelResults.simulationLabel = "Full Model";
  //  fullModelResults.draw(fullModelSimulationJob);
  //  fullModelSimulationJob.draw();
  //  double stiffnessFactor = 1;

  //  while (true) {
  // Reduced model simulation
  //  SimulationJob reducedModelSimulationJob =
  //      optimizer.getReducedSimulationJob(fullModelSimulationJob);
  //  const std::vector<double> stiffnessVector{
  //      fullModelSimulationJob.mesh->elements[0].properties.A,
  //      stiffnessFactor *
  //      fullModelSimulationJob.mesh->elements[0].properties.J, stiffnessFactor
  //      * fullModelSimulationJob.mesh->elements[0].properties.I2,
  //      stiffnessFactor *
  //      fullModelSimulationJob.mesh->elements[0].properties.I3};
  //  for (EdgeIndex ei = 0; ei < reducedModelSimulationJob.mesh->EN(); ei++) {
  //    BeamFormFinder::Element &e =
  //    reducedModelSimulationJob.mesh->elements[ei]; e.properties.A =
  //    0.00035185827018667374;
  //    // stifnessVector[0];
  //    e.properties.J = // stiffnessVector[1];
  //        7.709325104874406e-08;
  //    e.properties.I2 = -0.0000015661453308127776;
  //    //        stiffnessVector[2];
  //    e.properties.I3 = 3.7099813776947167e-07; // stiffnessVector[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;
  //  }

  //  SimulationResults reducedModelResults =
  //      formFinder.executeSimulation(reducedModelSimulationJob, true);
  //  reducedModelResults.simulationLabel =
  //      "Reduced Model_" + std::to_string(stiffnessFactor);
  //  reducedModelResults.draw(reducedModelSimulationJob);
  //    SimulationResultsReporter resultsReporter;
  //    resultsReporter.reportResults(
  //        {reducedModelResults},
  //        std::filesystem::current_path().append("Results"),
  //        "_" + std::to_string(stiffnessFactor));
  //    if (reducedModelResults.history.numberOfSteps ==
  //        BeamFormFinder::Simulator::maxDRMIterations) {
  //      break;
  //    }
  //    stiffnessFactor *= 1.5;
  //  }

  // Beam
  //  VCGEdgeMesh beamMesh;
  //  beamMesh.loadFromPly("/home/iason/Models/simple_beam_model_2elem_1m.ply");
  //  const VertexIndex reducedModelOpposite_vi =
  //      std::ceil(reducedModelSimulationJob.mesh->VN() / 2.0);
  //  auto v0 =
  //  reducedModelSimulationJob.mesh->vert[reducedModelOpposite_vi].cP() -
  //            reducedModelSimulationJob.mesh->vert[1].cP();
  //  auto v1 = beamMesh.vert[2].cP() - beamMesh.vert[0].cP();
  //  vcg::Matrix44d R;
  //  const double rotationTheta =
  //      std::asin((v0 ^ v1).Norm() / (v0.Norm() * v1.Norm()));
  //  R.SetRotateRad(-rotationTheta, v0 ^ v1);
  //  vcg::tri::UpdatePosition<VCGEdgeMesh>::Matrix(beamMesh, R);
  //  vcg::tri::UpdatePosition<VCGEdgeMesh>::Scale(beamMesh, v0.Norm() /
  //  v1.Norm()); vcg::tri::UpdatePosition<VCGEdgeMesh>::Translate(
  //      beamMesh, reducedModelSimulationJob.mesh->vert[1].cP());
  //  std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
  //      beamFixedVertices;
  //  beamFixedVertices[0] = std::unordered_set<DoFType>{0, 1, 2, 3, 4, 5};
  //  std::unordered_map<VertexIndex, Eigen::Vector3d> beamNodalForces{
  //      {2, Eigen::Vector3d(0, 0, 1000)}};
  //  SimulationJob beamSimulationJob{std::make_shared<ElementalMesh>(beamMesh),
  //                                  beamFixedVertices,
  //                                  beamNodalForces,
  //                                  {}};
  //  //  for (EdgeIndex ei = 0; ei < beamSimulationJob.mesh->EN(); ei++) {
  //  //    BeamFormFinder::Element &e = beamSimulationJob.mesh->elements[ei];
  //  //    e.properties.A = stifnessVector[0];
  //  //    e.properties.J = stifnessVector[1];
  //  //    e.properties.I2 = stifnessVector[2];
  //  //    e.properties.I3 = stifnessVector[3];
  //  //  }
  //  //  beamSimulationJob.draw();
  //  SimulationResults beamSimulationResults =
  //      formFinder.executeSimulation(beamSimulationJob);
  //  beamSimulationResults.simulationLabel = "Beam";
  //  beamSimulationResults.draw(beamSimulationJob);
  return 0;
}