#ifndef BEAMFORMFINDER_HPP
#define BEAMFORMFINDER_HPP

#include "elementalmesh.hpp"
#include "polyscope/curve_network.h"
#include "polyscope/polyscope.h"
#include "simulationresult.hpp"
#include <Eigen/Dense>
#include <filesystem>
#include <unordered_set>

struct SimulationJob;

enum DoF { Ux = 0, Uy, Uz, Nx, Ny, Nr, NumDoF };
using DoFType = int;
using EdgeType = VCGEdgeMesh::EdgeType;
using VertexType = VCGEdgeMesh::VertexType;

struct DifferentiateWithRespectTo {
  const VertexType &v;
  const DoFType &dofi;
};
class FormFinder {

private:
  const double Dtini{0.1};
  double Dt{Dtini};
  double t{0};
  const double xi{0.9969};
  const double totalResidualForcesNormThreshold{300};
  size_t currentSimulationStep{0};
  int mDrawingStep{1};
  std::unique_ptr<SimulationMesh> mesh;
  std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
      constrainedVertices;
  SimulationHistory history;
  //  Eigen::Tensor<double, 4> theta3Derivatives;
  //  std::unordered_map<MyKeyType, double, key_hash> theta3Derivatives;
  bool shouldApplyInitialDistortion = false;
  std::unordered_set<VertexIndex> rigidSupports;

  void reset();
  void updateNodalInternalForces(
      const std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
          &fixedVertices);
  void updateNodalExternalForces(
      const std::unordered_map<VertexIndex, Vector6d> &nodalForces,
      const std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
          &fixedVertices);
  void updateResidualForces();
  void updateRotationalDisplacements();
  void updateElementalLengths();

  void updateNodalMasses();

  void updateNodalAccelerations();

  void updateNodalVelocities();

  void updateNodalDisplacements();

  void applyForcedDisplacements(
      const std::unordered_map<VertexIndex, Eigen::Vector3d>
          nodalForcedDisplacements);

  ::Vector6d computeElementTorsionalForce(
      const Element &element, const Vector6d &displacementDifference,
      const std::unordered_set<DoFType> &constrainedDof);

  //  BeamFormFinder::Vector6d computeElementFirstBendingForce(
  //      const Element &element, const Vector6d &displacementDifference,
  //      const std::unordered_set<gsl::index> &constrainedDof);

  //  BeamFormFinder::Vector6d computeElementSecondBendingForce(
  //      const Element &element, const Vector6d &displacementDifference,
  //      const std::unordered_set<gsl::index> &constrainedDof);

  void updateKineticEnergy();

  void resetVelocities();

  SimulationResults computeResults(const SimulationMesh &initialiMesh);

  void updateNodePosition(
      VertexType &v,
      const std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
          &fixedVertices);

  void applyDisplacements(
      const std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
          &fixedVertices);

  void draw(const string &screenshotsFolder);

  void
  updateNodalInternalForce(Vector6d &nodalInternalForce,
                           const Vector6d &elementInternalForce,
                           const std::unordered_set<DoFType> &nodalFixedDof);

  ::Vector6d computeElementInternalForce(
      const Element &elem, const Node &n0, const Node &n1,
      const std::unordered_set<DoFType> &n0ConstrainedDof,
      const std::unordered_set<DoFType> &n1ConstrainedDof);

  ::Vector6d computeElementAxialForce(const ::EdgeType &e) const;
  VectorType computeDisplacementDifferenceDerivative(
      const EdgeType &e, const DifferentiateWithRespectTo &dui) const;
  double
  computeDerivativeElementLength(const EdgeType &e,
                                 const DifferentiateWithRespectTo &dui) const;

  VectorType computeDerivativeT1(const EdgeType &e,
                                 const DifferentiateWithRespectTo &dui) const;

  VectorType
  computeDerivativeOfNormal(const VertexType &v,
                            const DifferentiateWithRespectTo &dui) const;

  VectorType computeDerivativeT3(const EdgeType &e,
                                 const DifferentiateWithRespectTo &dui) const;

  VectorType computeDerivativeT2(const EdgeType &e,
                                 const DifferentiateWithRespectTo &dui) const;

  double computeDerivativeTheta2(const EdgeType &e, const VertexIndex &evi,
                                 const VertexIndex &dwrt_evi,
                                 const DoFType &dwrt_dofi) const;

  void updateElementalFrames();

  VectorType computeDerivativeOfR(const EdgeType &e,
                                  const DifferentiateWithRespectTo &dui) const;

  bool isRigidSupport(const VertexType &v) const;

  static double computeDerivativeOfNorm(const VectorType &x,
                                        const VectorType &derivativeOfX);
  static VectorType computeDerivativeOfCrossProduct(
      const VectorType &a, const VectorType &derivativeOfA, const VectorType &b,
      const VectorType &derivativeOfB);

  double computeTheta3(const EdgeType &e, const VertexType &v);
  double computeDerivativeTheta3(const EdgeType &e, const VertexType &v,
                                 const DifferentiateWithRespectTo &dui) const;
  double computeTotalPotentialEnergy() const;
  void computeRigidSupports();
  void updateNormalDerivatives();
  void updateT1Derivatives();
  void updateT2Derivatives();
  void updateT3Derivatives();
  void updateRDerivatives();

  double computeDerivativeTheta1(const EdgeType &e, const VertexIndex &evi,
                                 const VertexIndex &dwrt_evi,
                                 const DoFType &dwrt_dofi) const;

  //  void updatePositionsOnTheFly(
  //      const std::unordered_map<VertexIndex,
  //      std::unordered_set<gsl::index>>
  //          &fixedVertices);

  void updateResidualForcesOnTheFly(
      const std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
          &fixedVertices);

  void updatePositionsOnTheFly(
      const std::unordered_map<VertexIndex, std::unordered_set<DoFType>>
          &fixedVertices);

public:
  FormFinder();
  SimulationResults executeSimulation(
      const SimulationJob &job, const bool &beVerbose = false,
      const bool &shouldDraw = false,
      const size_t &maxDRMIterations = FormFinder::maxDRMIterations);
  inline static const size_t maxDRMIterations{50000};
};

template <typename PointType> PointType Cross(PointType p1, PointType p2) {
  return p1 ^ p2;
}

inline size_t currentStep{0};
inline bool TriggerBreakpoint(const VertexIndex &vi, const EdgeIndex &ei,
                              const DoFType &dofi) {
  const size_t numberOfVertices = 10;
  const VertexIndex middleNodeIndex = numberOfVertices / 2;
  //  return vi == middleNodeIndex && dofi == 1;
  return dofi == 1 && ((vi == 1 && ei == 0) || (vi == 9 && ei == 9));
}

inline bool TriggerBreakpoint(const VertexIndex &vi, const EdgeIndex &ei) {
  const size_t numberOfVertices = 10;
  const VertexIndex middleNodeIndex = numberOfVertices / 2;
  return (vi == middleNodeIndex);
  //  return (vi == 0 || vi == numberOfVertices - 1) && currentStep == 1;
  return (vi == 1 && ei == 0) || (vi == 9 && ei == 9);
}

namespace Eigen {
template <class Matrix>
void write_binary(const std::string &filename, const Matrix &matrix) {
  std::ofstream out(filename,
                    std::ios::out | std::ios::binary | std::ios::trunc);
  typename Matrix::Index rows = matrix.rows(), cols = matrix.cols();
  out.write((char *)(&rows), sizeof(typename Matrix::Index));
  out.write((char *)(&cols), sizeof(typename Matrix::Index));
  out.write((char *)matrix.data(),
            rows * cols * sizeof(typename Matrix::Scalar));
  out.close();
}
template <class Matrix>
void read_binary(const std::string &filename, Matrix &matrix) {
  std::ifstream in(filename, std::ios::in | std::ios::binary);
  typename Matrix::Index rows = 0, cols = 0;
  in.read((char *)(&rows), sizeof(typename Matrix::Index));
  in.read((char *)(&cols), sizeof(typename Matrix::Index));
  matrix.resize(rows, cols);
  in.read((char *)matrix.data(), rows * cols * sizeof(typename Matrix::Scalar));
  in.close();
}
} // namespace Eigen

#endif // BEAMFORMFINDER_HPP