#include "linearsimulationmodel.hpp"
//#include "gsl/gsl"

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

    return elements;
}

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

    return feaNodes;
}

std::vector<fea::BC> LinearSimulationModel::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;
}

std::vector<fea::Force> LinearSimulationModel::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;
            }
            fea::Force f(nodalForce.first, dofIndex, nodalForce.second[dofIndex]);
            nodalForces.emplace_back(f);
        }
    }

    return nodalForces;
}

SimulationResults LinearSimulationModel::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.setLabelPrefix("Linear");

    //  // 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.dimensions.A);
        const double elementPotentialEnergy_axial_v1 = std::pow(elementForces[6], 2)
                                                       * element.initialLength
                                                       / (4 * element.material.youngsModulus
                                                          * element.dimensions.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.dimensions.A
                                                           * element.material.G);
        const double elementPotentialEnergy_shearZ_v0 = std::pow(elementForces[2], 2)
                                                        * element.initialLength
                                                        / (4 * element.dimensions.A
                                                           * element.material.G);
        const double elementPotentialEnergy_shearY_v1 = std::pow(elementForces[7], 2)
                                                        * element.initialLength
                                                        / (4 * element.dimensions.A
                                                           * element.material.G);
        const double elementPotentialEnergy_shearZ_v1 = std::pow(elementForces[8], 2)
                                                        * element.initialLength
                                                        / (4 * element.dimensions.A
                                                           * element.material.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.dimensions.inertia.I2);
        const double elementPotentialEnergy_bendingZ_v0 = std::pow(elementForces[5], 2)
                                                          * element.initialLength
                                                          / (4 * element.material.youngsModulus
                                                             * element.dimensions.inertia.I3);
        const double elementPotentialEnergy_bendingY_v1 = std::pow(elementForces[10], 2)
                                                          * element.initialLength
                                                          / (4 * element.material.youngsModulus
                                                             * element.dimensions.inertia.I2);
        const double elementPotentialEnergy_bendingZ_v1 = std::pow(elementForces[11], 2)
                                                          * element.initialLength
                                                          / (4 * element.material.youngsModulus
                                                             * element.dimensions.inertia.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.dimensions.inertia.J
                                                            * element.material.G);
        const double elementPotentialEnergy_torsion_v1 = std::pow(elementForces[9], 2)
                                                         * element.initialLength
                                                         / (4 * element.dimensions.inertia.J
                                                            * element.material.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;
    }
    results.pJob = simulationJob;

    return results;
}

SimulationResults LinearSimulationModel::executeSimulation(
    const std::shared_ptr<SimulationJob> &pSimulationJob)
{
    assert(pSimulationJob->pMesh->VN() != 0);
    const bool wasInitializedWithDifferentStructure = pSimulationJob->pMesh->EN()
                                                          != simulator.structure.elems.size()
                                                      || pSimulationJob->pMesh->VN()
                                                             != simulator.structure.nodes.size();
    if (!simulator.wasInitialized() || wasInitializedWithDifferentStructure) {
        setStructure(pSimulationJob->pMesh);
    }
    //  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(pSimulationJob);
    auto nodalForces = getFeaNodalForces(pSimulationJob);
    fea::Summary feaResults = simulator.solve(fixedVertices, nodalForces, ties, equations, opts);

    SimulationResults results = getResults(feaResults, pSimulationJob);
    results.pJob = pSimulationJob;
    return results;
}

void LinearSimulationModel::setStructure(const std::shared_ptr<SimulationMesh> &pMesh)
{
    fea::BeamStructure structure(getFeaNodes(pMesh), getFeaElements(pMesh));
    simulator.initialize(structure);
}

void LinearSimulationModel::printInfo(const fea::BeamStructure &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;
    }
}