#ifndef LINEARSIMULATIONMODEL_HPP
#define LINEARSIMULATIONMODEL_HPP

//#include "beam.hpp"
#include "simulation_structs.hpp"
#include "threed_beam_fea.h"
#include <filesystem>
#include <vector>

// struct BeamSimulationProperties {
//  float crossArea;
//  float I2;
//  float I3;
//  float polarInertia;
//  float G;
//  // Properties used by fea
//  float EA;
//  float EIz;
//  float EIy;
//  float GJ;

//  BeamSimulationProperties(const BeamDimensions &dimensions,
//                           const BeamMaterial &material);
//};

// struct NodalForce {
//  int index;
//  int dof;
//  double magnitude;
//};

// struct SimulationJob {
//  Eigen::MatrixX3d nodes;
//  Eigen::MatrixX2i elements;
//  Eigen::MatrixX3d elementalNormals;
//  Eigen::VectorXi fixedNodes;
//  std::vector<NodalForce> nodalForces;
//  std::vector<BeamDimensions> beamDimensions;
//  std::vector<BeamMaterial> beamMaterial;
//};

// struct SimulationResults {
//  std::vector<Eigen::VectorXd> edgeForces; ///< Force values per force
//  component
//                                           ///< #force components x #edges
//  Eigen::MatrixXd
//      nodalDisplacements; ///< The displacement of each node #nodes x 3
//  SimulationResults(const fea::Summary &feaSummary);
//  SimulationResults() {}
//};

class LinearSimulationModel {
public:
    LinearSimulationModel(){

    }
    static std::vector<fea::Elem>
    getFeaElements(const std::shared_ptr<SimulationJob> &job) {
        const int numberOfEdges = job->pMesh->EN();
        std::vector<fea::Elem> elements(numberOfEdges);
        for (int edgeIndex = 0; edgeIndex < numberOfEdges; edgeIndex++) {
                const SimulationMesh::CoordType &evn0 =
                        job->pMesh->edge[edgeIndex].cV(0)->cN();
                const SimulationMesh::CoordType &evn1 = job->pMesh->edge[edgeIndex].cV(1)->cN();
                const std::vector<double> nAverageVector{(evn0[0] + evn1[0]) / 2,
                                                         (evn0[1] + evn1[1]) / 2,
                                                         (evn0[2] + evn1[2]) / 2};
                const Element &element = job->pMesh->elements[edgeIndex];
                const double E = element.material.youngsModulus;
                fea::Props feaProperties(E * element.A, E * element.I3, E * element.I2,
                                         element.G * element.J, nAverageVector);
                const int vi0 = job->pMesh->getIndex(job->pMesh->edge[edgeIndex].cV(0));
                const int vi1 = job->pMesh->getIndex(job->pMesh->edge[edgeIndex].cV(1));
                elements[edgeIndex] = fea::Elem(vi0, vi1, feaProperties);
            }

        return elements;
    }

    static std::vector<fea::Node>
    getFeaNodes(const std::shared_ptr<SimulationJob> &job) {
        const int numberOfNodes = job->pMesh->VN();
        std::vector<fea::Node> feaNodes(numberOfNodes);
        for (int vi = 0; vi < numberOfNodes; vi++) {
                const CoordType &p = job->pMesh->vert[vi].cP();
                feaNodes[vi] = fea::Node(p[0], p[1], p[2]);
            }

        return feaNodes;
    }
    static std::vector<fea::BC>
    getFeaFixedNodes(const std::shared_ptr<SimulationJob> &job) {
        std::vector<fea::BC> boundaryConditions;
        boundaryConditions.reserve(job->constrainedVertices.size() * 6
                                   + job->nodalForcedDisplacements.size() * 3);
        for (auto fixedVertex : job->constrainedVertices) {
                const int vertexIndex = fixedVertex.first;
                for (int dofIndex : fixedVertex.second) {
                    boundaryConditions.emplace_back(
                        fea::BC(vertexIndex, static_cast<fea::DOF>(dofIndex), 0));
                }
        }

        for (auto forcedDisplacement : job->nodalForcedDisplacements) {
            const int vi = forcedDisplacement.first;
            for (int dofIndex = 0; dofIndex < 3; dofIndex++) {
                boundaryConditions.emplace_back(fea::BC(vi,
                                                        static_cast<fea::DOF>(dofIndex),
                                                        forcedDisplacement.second[dofIndex]));
            }
        }

        return boundaryConditions;
    }

    static std::vector<fea::Force>
    getFeaNodalForces(const std::shared_ptr<SimulationJob> &job) {
        std::vector<fea::Force> nodalForces;
        nodalForces.reserve(job->nodalExternalForces.size() * 6);

        for (auto nodalForce : job->nodalExternalForces) {
                for (int dofIndex = 0; dofIndex < 6; dofIndex++) {
                        if (nodalForce.second[dofIndex] == 0) {
                                continue;
                            }
                        nodalForces.emplace_back(
                                    fea::Force(nodalForce.first, dofIndex, nodalForce.second[dofIndex]));
                    }
            }

        return nodalForces;
    }
    static SimulationResults getResults(const fea::Summary &feaSummary,
                                        const std::shared_ptr<SimulationJob> &simulationJob)
    {
        SimulationResults results;
        results.converged = feaSummary.converged;
        if (!results.converged) {
            return results;
        }

        results.executionTime = feaSummary.total_time_in_ms * 1000;
        // displacements
        results.displacements.resize(feaSummary.num_nodes);
        results.rotationalDisplacementQuaternion.resize(feaSummary.num_nodes);
        for (int vi = 0; vi < feaSummary.num_nodes; vi++) {
            results.displacements[vi] = Vector6d(feaSummary.nodal_displacements[vi]);

            const Vector6d &nodalDisplacement = results.displacements[vi];
            Eigen::Quaternion<double> q_nx;
            q_nx = Eigen::AngleAxis<double>(nodalDisplacement[3], Eigen::Vector3d(1, 0, 0));
            Eigen::Quaternion<double> q_ny;
            q_ny = Eigen::AngleAxis<double>(nodalDisplacement[4], Eigen::Vector3d(0, 1, 0));
            Eigen::Quaternion<double> q_nz;
            q_nz = Eigen::AngleAxis<double>(nodalDisplacement[5], Eigen::Vector3d(0, 0, 1));
            results.rotationalDisplacementQuaternion[vi] = q_nx * q_ny * q_nz;
            //            results.rotationalDisplacementQuaternion[vi] = q_nz * q_ny * q_nx;
            //            results.rotationalDisplacementQuaternion[vi] = q_nz * q_nx * q_ny;
        }

        //  // Convert forces
        //  // Convert to vector of eigen matrices of the form force component-> per
        //  // Edge
        //  const int numDof = 6;
        //  const size_t numberOfEdges = feaSummary.element_forces.size();
        //  edgeForces =
        //      std::vector<Eigen::VectorXd>(numDof, Eigen::VectorXd(2 *
        //      numberOfEdges));
        //  for (gsl::index edgeIndex = 0; edgeIndex < numberOfEdges; edgeIndex++) {
        //    for (gsl::index forceComponentIndex = 0; forceComponentIndex < numDof;
        //         forceComponentIndex++) {
        //      (edgeForces[forceComponentIndex])(2 * edgeIndex) =
        //          feaSummary.element_forces[edgeIndex][forceComponentIndex];
        //      (edgeForces[forceComponentIndex])(2 * edgeIndex + 1) =
        //          feaSummary.element_forces[edgeIndex][numDof +
        //          forceComponentIndex];
        //    }
        //  }

        for (int ei = 0; ei < feaSummary.num_elems; ei++) {
            const std::vector<double> &elementForces = feaSummary.element_forces[ei];
            const Element &element = simulationJob->pMesh->elements[ei];
            //Axial
            const double elementPotentialEnergy_axial_v0 = std::pow(elementForces[0], 2)
                                                           * element.initialLength
                                                           / (4 * element.material.youngsModulus
                                                              * element.A);
            const double elementPotentialEnergy_axial_v1 = std::pow(elementForces[6], 2)
                                                           * element.initialLength
                                                           / (4 * element.material.youngsModulus
                                                              * element.A);
            const double elementPotentialEnergy_axial = elementPotentialEnergy_axial_v0
                                                        + elementPotentialEnergy_axial_v1;
            //Shear
            const double elementPotentialEnergy_shearY_v0 = std::pow(elementForces[1], 2)
                                                            * element.initialLength
                                                            / (4 * element.A * element.G);
            const double elementPotentialEnergy_shearZ_v0 = std::pow(elementForces[2], 2)
                                                            * element.initialLength
                                                            / (4 * element.A * element.G);
            const double elementPotentialEnergy_shearY_v1 = std::pow(elementForces[7], 2)
                                                            * element.initialLength
                                                            / (4 * element.A * element.G);
            const double elementPotentialEnergy_shearZ_v1 = std::pow(elementForces[8], 2)
                                                            * element.initialLength
                                                            / (4 * element.A * element.G);
            const double elementPotentialEnergy_shear = elementPotentialEnergy_shearY_v0
                                                        + elementPotentialEnergy_shearZ_v0
                                                        + elementPotentialEnergy_shearY_v1
                                                        + elementPotentialEnergy_shearZ_v1;
            //Bending
            const double elementPotentialEnergy_bendingY_v0 = std::pow(elementForces[4], 2)
                                                              * element.initialLength
                                                              / (4 * element.material.youngsModulus
                                                                 * element.I2);
            const double elementPotentialEnergy_bendingZ_v0 = std::pow(elementForces[5], 2)
                                                              * element.initialLength
                                                              / (4 * element.material.youngsModulus
                                                                 * element.I3);
            const double elementPotentialEnergy_bendingY_v1 = std::pow(elementForces[10], 2)
                                                              * element.initialLength
                                                              / (4 * element.material.youngsModulus
                                                                 * element.I2);
            const double elementPotentialEnergy_bendingZ_v1 = std::pow(elementForces[11], 2)
                                                              * element.initialLength
                                                              / (4 * element.material.youngsModulus
                                                                 * element.I3);
            const double elementPotentialEnergy_bending = elementPotentialEnergy_bendingY_v0
                                                          + elementPotentialEnergy_bendingZ_v0
                                                          + elementPotentialEnergy_bendingY_v1
                                                          + elementPotentialEnergy_bendingZ_v1;
            //Torsion
            const double elementPotentialEnergy_torsion_v0 = std::pow(elementForces[3], 2)
                                                             * element.initialLength
                                                             / (4 * element.J * element.G);
            const double elementPotentialEnergy_torsion_v1 = std::pow(elementForces[9], 2)
                                                             * element.initialLength
                                                             / (4 * element.J * element.G);
            const double elementPotentialEnergy_torsion = elementPotentialEnergy_torsion_v0
                                                          + elementPotentialEnergy_torsion_v1;
            const double elementInternalPotentialEnergy = elementPotentialEnergy_axial
                                                          + elementPotentialEnergy_shear
                                                          + elementPotentialEnergy_bending
                                                          + elementPotentialEnergy_torsion;
            results.internalPotentialEnergy += elementInternalPotentialEnergy;
        }

        return results;
    }

  SimulationResults
  executeSimulation(const std::shared_ptr<SimulationJob> &simulationJob) {
      assert(simulationJob->pMesh->VN() != 0);
      fea::Job job(getFeaNodes(simulationJob), getFeaElements(simulationJob));
      //  printInfo(job);

      // create the default options
      fea::Options opts;
      opts.save_elemental_forces = false;
      opts.save_nodal_displacements = false;
      opts.save_nodal_forces = false;
      opts.save_report = false;
      opts.save_tie_forces = false;
      //  if (!elementalForcesOutputFilepath.empty()) {
      //    opts.save_elemental_forces = true;
      //    opts.elemental_forces_filename = elementalForcesOutputFilepath;
      //  }
      //  if (!nodalDisplacementsOutputFilepath.empty()) {
      //    opts.save_nodal_displacements = true;
      //    opts.nodal_displacements_filename = nodalDisplacementsOutputFilepath;
      //  }

      // have the program output status updates
      opts.verbose = false;

      // form an empty vector of ties
      std::vector<fea::Tie> ties;

      // also create an empty list of equations
      std::vector<fea::Equation> equations;
      auto fixedVertices = getFeaFixedNodes(simulationJob);
      auto nodalForces = getFeaNodalForces(simulationJob);
      fea::Summary feaResults =
              fea::solve(job, fixedVertices, nodalForces, ties, equations, opts);

      SimulationResults results = getResults(feaResults, simulationJob);
      results.job = simulationJob;
      return results;
  }
  //  SimulationResults getResults() const;
  //  void setResultsNodalDisplacementCSVFilepath(const std::string
  //  &outputPath); void setResultsElementalForcesCSVFilepath(const std::string
  //  &outputPath);

private:
  //  std::string nodalDisplacementsOutputFilepath{"nodal_displacement.csv"};
  //  std::string elementalForcesOutputFilepath{"elemental_forces.csv"};
  //  SimulationResults results;

  static void printInfo(const fea::Job &job) {
      std::cout << "Details regarding the fea::Job:" << std::endl;
      std::cout << "Nodes:" << std::endl;
      for (fea::Node n : job.nodes) {
              std::cout << n << std::endl;
          }
      std::cout << "Elements:" << std::endl;
      for (Eigen::Vector2i e : job.elems) {
              std::cout << e << std::endl;
          }
  }
};

#endif // LINEARSIMULATIONMODEL_HPP