#include "fesa/elements/euler_beam_3d.hpp"

#include <algorithm>
#include <array>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

namespace fesa {
namespace {

constexpr std::size_t kElementDofCount = 12U;
constexpr std::size_t kGeneralizedComponentCount = 4U;
constexpr double kGeometryTolerance = 1.0e-12;
constexpr double kStiffnessInvariantTolerance = 1.0e-12;

using Vector3 = std::array<double, 3>;

double norm(const Vector3& value) {
    return std::hypot(value[0], value[1], value[2]);
}

double dot(const Vector3& lhs, const Vector3& rhs) {
    return lhs[0] * rhs[0] + lhs[1] * rhs[1] + lhs[2] * rhs[2];
}

Vector3 cross(const Vector3& lhs, const Vector3& rhs) {
    return {
        lhs[1] * rhs[2] - lhs[2] * rhs[1],
        lhs[2] * rhs[0] - lhs[0] * rhs[2],
        lhs[0] * rhs[1] - lhs[1] * rhs[0]};
}

bool isFinite(const Vector3& value) {
    return std::isfinite(value[0]) && std::isfinite(value[1]) &&
           std::isfinite(value[2]);
}

std::string elementIdentity(const Node& firstNode, const Node& secondNode) {
    return firstNode.sourceId.instanceName + ":" +
           firstNode.sourceId.sourceLabelText + "-" +
           secondNode.sourceId.sourceLabelText;
}

Result<EulerBeam3D> modelFailure(const std::string& code,
                                 const SourceLocation& location,
                                 const std::string& identity,
                                 const std::string& message) {
    return Result<EulerBeam3D>::failure(Status::failure(
        FailureCategory::model,
        {{Severity::error, code, location, "*ELEMENT", identity, message}}));
}

Matrix transformation(const std::array<double, 9>& rotation) {
    Matrix result{kElementDofCount, kElementDofCount};
    // Blocks preserve [translation, rotation] at node 1 then node 2.
    for (std::size_t block = 0; block < 4U; ++block) {
        for (std::size_t row = 0; row < 3U; ++row) {
            for (std::size_t column = 0; column < 3U; ++column) {
                result(block * 3U + row, block * 3U + column) =
                    rotation[row * 3U + column];
            }
        }
    }
    return result;
}

Matrix strainDisplacement(double xi, double length) {
    Matrix b{kGeneralizedComponentCount, kElementDofCount};
    const double r = 0.5 * (1.0 + xi);
    const double inverseLength = 1.0 / length;
    const double inverseLengthSquared = inverseLength * inverseLength;

    b(0U, 0U) = -inverseLength;
    b(0U, 6U) = inverseLength;
    b(1U, 3U) = -inverseLength;
    b(1U, 9U) = inverseLength;

    // theta_y=-w' makes kappa_y=-w''; theta_z=v' makes kappa_z=v''.
    b(2U, 2U) = (6.0 - 12.0 * r) * inverseLengthSquared;
    b(2U, 4U) = (-4.0 + 6.0 * r) * inverseLength;
    b(2U, 8U) = (-6.0 + 12.0 * r) * inverseLengthSquared;
    b(2U, 10U) = (-2.0 + 6.0 * r) * inverseLength;

    b(3U, 1U) = (-6.0 + 12.0 * r) * inverseLengthSquared;
    b(3U, 5U) = (-4.0 + 6.0 * r) * inverseLength;
    b(3U, 7U) = (6.0 - 12.0 * r) * inverseLengthSquared;
    b(3U, 11U) = (-2.0 + 6.0 * r) * inverseLength;
    return b;
}

std::array<double, kGeneralizedComponentCount> constitutiveDiagonal(
    double youngsModulus,
    double shearModulus,
    double area,
    double iy,
    double iz,
    double torsionalConstant) {
    return {
        youngsModulus * area,
        shearModulus * torsionalConstant,
        youngsModulus * iy,
        youngsModulus * iz};
}

Matrix closedStiffness(double length,
                       const std::array<double, kGeneralizedComponentCount>& diagonal) {
    Matrix closed{kElementDofCount, kElementDofCount};
    const auto addBlock = [&closed](const std::array<std::size_t, 2>& indices,
                                    double coefficient) {
        closed(indices[0], indices[0]) = coefficient;
        closed(indices[0], indices[1]) = -coefficient;
        closed(indices[1], indices[0]) = -coefficient;
        closed(indices[1], indices[1]) = coefficient;
    };
    addBlock({0U, 6U}, diagonal[0U] / length);
    addBlock({3U, 9U}, diagonal[1U] / length);

    const auto addBendingBlock = [&closed, length](
                                     const std::array<std::size_t, 4>& indices,
                                     double flexuralRigidity,
                                     double rotationSign) {
        const double value = 12.0 * flexuralRigidity /
                             (length * length * length);
        const double coupling = rotationSign * 6.0 * flexuralRigidity /
                                (length * length);
        const double diagonalRotation = 4.0 * flexuralRigidity / length;
        const double offDiagonalRotation = 2.0 * flexuralRigidity / length;
        const std::array<double, 16> block = {
            value, coupling, -value, coupling,
            coupling, diagonalRotation, -coupling, offDiagonalRotation,
            -value, -coupling, value, -coupling,
            coupling, offDiagonalRotation, -coupling, diagonalRotation};
        for (std::size_t row = 0; row < indices.size(); ++row) {
            for (std::size_t column = 0; column < indices.size(); ++column) {
                closed(indices[row], indices[column]) = block[row * indices.size() + column];
            }
        }
    };
    addBendingBlock({1U, 5U, 7U, 11U}, diagonal[3U], 1.0);
    addBendingBlock({2U, 4U, 8U, 10U}, diagonal[2U], -1.0);
    return closed;
}

double normalizedMatrixError(const Matrix& lhs, const Matrix& rhs) {
    double maximumDifference = 0.0;
    double scale = 1.0;
    for (std::size_t row = 0; row < lhs.rows(); ++row) {
        for (std::size_t column = 0; column < lhs.columns(); ++column) {
            const double lhsValue = lhs(row, column);
            const double rhsValue = rhs(row, column);
            if (!std::isfinite(lhsValue) || !std::isfinite(rhsValue)) {
                return std::numeric_limits<double>::infinity();
            }
            const double difference = std::abs(lhsValue - rhsValue);
            if (!std::isfinite(difference)) {
                return std::numeric_limits<double>::infinity();
            }
            maximumDifference = (std::max)(
                maximumDifference,
                difference);
            scale = (std::max)(scale, std::abs(lhsValue));
            scale = (std::max)(scale, std::abs(rhsValue));
        }
    }
    if (!std::isfinite(maximumDifference) || !std::isfinite(scale) ||
        !(scale > 0.0)) {
        return std::numeric_limits<double>::infinity();
    }
    const double normalizedError = maximumDifference / scale;
    return std::isfinite(normalizedError)
               ? normalizedError
               : std::numeric_limits<double>::infinity();
}

std::array<double, kGeneralizedComponentCount> generalizedStrain(
    const Matrix& b,
    const Vector& localDisplacement) {
    std::array<double, kGeneralizedComponentCount> strain{};
    for (std::size_t component = 0; component < strain.size(); ++component) {
        for (std::size_t dof = 0; dof < localDisplacement.size(); ++dof) {
            strain[component] += b(component, dof) * localDisplacement[dof];
        }
    }
    return strain;
}

std::array<double, kGeneralizedComponentCount> generalizedResultant(
    const std::array<double, kGeneralizedComponentCount>& strain,
    const std::array<double, kGeneralizedComponentCount>& diagonal) {
    std::array<double, kGeneralizedComponentCount> resultant{};
    for (std::size_t component = 0; component < resultant.size(); ++component) {
        resultant[component] = diagonal[component] * strain[component];
    }
    return resultant;
}

Matrix kinematicInterpolation(double xi, double length) {
    Matrix interpolation{4U, kElementDofCount};
    const double r = 0.5 * (1.0 + xi);
    const double rSquared = r * r;
    const double rCubed = rSquared * r;
    const double n1 = 1.0 - r;
    const double n2 = r;
    const double h1 = 1.0 - 3.0 * rSquared + 2.0 * rCubed;
    const double h2 = length * (r - 2.0 * rSquared + rCubed);
    const double h3 = 3.0 * rSquared - 2.0 * rCubed;
    const double h4 = length * (-rSquared + rCubed);

    interpolation(0U, 0U) = n1;
    interpolation(0U, 6U) = n2;
    interpolation(1U, 1U) = h1;
    interpolation(1U, 5U) = h2;
    interpolation(1U, 7U) = h3;
    interpolation(1U, 11U) = h4;
    interpolation(2U, 2U) = h1;
    interpolation(2U, 4U) = -h2;
    interpolation(2U, 8U) = h3;
    interpolation(2U, 10U) = -h4;
    interpolation(3U, 3U) = n1;
    interpolation(3U, 9U) = n2;
    return interpolation;
}

} // namespace

Result<EulerBeam3D> EulerBeam3D::create(
    const Node& firstNode,
    const Node& secondNode,
    const GeneralBeamSection& section,
    const LinearElasticMaterial& material) {
    const std::string identity = elementIdentity(firstNode, secondNode);
    const Vector3& first = firstNode.coordinates;
    const Vector3& second = secondNode.coordinates;
    const Vector3 delta = {
        second[0] - first[0], second[1] - first[1], second[2] - first[2]};
    const double length = norm(delta);
    const double coordinateScale =
        (std::max)({1.0, norm(first), norm(second)});
    if (!isFinite(first) || !isFinite(second) || !isFinite(delta) ||
        !std::isfinite(length) || !std::isfinite(coordinateScale) ||
        !(length > kGeometryTolerance * coordinateScale)) {
        return modelFailure(
            "invalid-beam-length",
            firstNode.location,
            identity,
            "Beam length must exceed the scale-aware geometry threshold.");
    }

    const Vector3 ex = {delta[0] / length, delta[1] / length, delta[2] / length};
    const Vector3& guide = section.firstAxis;
    const double guideNorm = norm(guide);
    const double guideProjection = dot(guide, ex);
    const Vector3 eyTrial = {
        guide[0] - guideProjection * ex[0],
        guide[1] - guideProjection * ex[1],
        guide[2] - guideProjection * ex[2]};
    const double eyTrialNorm = norm(eyTrial);
    if (!isFinite(guide) || !std::isfinite(guideNorm) || !isFinite(eyTrial) ||
        !std::isfinite(eyTrialNorm) ||
        !(eyTrialNorm > kGeometryTolerance * (std::max)(1.0, guideNorm))) {
        return modelFailure(
            "invalid-beam-guide-vector",
            section.location,
            identity,
            "Beam guide vector must define a scale-aware transverse direction.");
    }

    if (!std::isfinite(section.i12)) {
        return modelFailure(
            "invalid-beam-property",
            section.location,
            identity,
            "Beam section properties must be finite and positive.");
    }
    if (section.i12 != 0.0) {
        return modelFailure(
            "unsupported-coupled-section",
            section.location,
            identity,
            "The Euler beam kernel requires exact I12=0.");
    }

    const double shearModulus =
        material.youngsModulus / (2.0 * (1.0 + material.poissonRatio));
    const std::array<double, 6> positiveProperties = {
        material.youngsModulus,
        shearModulus,
        section.area,
        section.i11,
        section.i22,
        section.torsionalConstant};
    if (!std::isfinite(material.poissonRatio) ||
        std::any_of(
            positiveProperties.begin(),
            positiveProperties.end(),
            [](double property) { return !std::isfinite(property) || !(property > 0.0); })) {
        return modelFailure(
            "invalid-beam-property",
            section.location,
            identity,
            "E, G, A, Iy, Iz, and J must be finite and positive.");
    }

    const auto derivedRigidity = constitutiveDiagonal(
        material.youngsModulus,
        shearModulus,
        section.area,
        section.i11,
        section.i22,
        section.torsionalConstant);
    const double lengthSquared = length * length;
    const double lengthCubed = lengthSquared * length;
    // These are every distinct positive magnitude used by the exact axial,
    // torsion, and two bending closed-form blocks. Reject arithmetic
    // overflow/underflow without introducing a conditioning threshold.
    const std::array<double, 14> requiredStiffnessMagnitudes = {
        derivedRigidity[0U],
        derivedRigidity[1U],
        derivedRigidity[2U],
        derivedRigidity[3U],
        derivedRigidity[0U] / length,
        derivedRigidity[1U] / length,
        12.0 * derivedRigidity[2U] / lengthCubed,
        6.0 * derivedRigidity[2U] / lengthSquared,
        4.0 * derivedRigidity[2U] / length,
        2.0 * derivedRigidity[2U] / length,
        12.0 * derivedRigidity[3U] / lengthCubed,
        6.0 * derivedRigidity[3U] / lengthSquared,
        4.0 * derivedRigidity[3U] / length,
        2.0 * derivedRigidity[3U] / length};
    if (std::any_of(
            requiredStiffnessMagnitudes.begin(),
            requiredStiffnessMagnitudes.end(),
            [](double magnitude) {
                return !std::isfinite(magnitude) || !(magnitude > 0.0);
            })) {
        return modelFailure(
            "invalid-beam-property",
            section.location,
            identity,
            "Derived beam stiffness coefficients must be finite and positive.");
    }

    const Vector3 ey = {
        eyTrial[0] / eyTrialNorm,
        eyTrial[1] / eyTrialNorm,
        eyTrial[2] / eyTrialNorm};
    const Vector3 ez = cross(ex, ey);
    // Rows map global vectors to the approved right-handed local (ex,ey,ez) basis.
    const std::array<double, 9> rotation = {
        ex[0], ex[1], ex[2],
        ey[0], ey[1], ey[2],
        ez[0], ez[1], ez[2]};

    return Result<EulerBeam3D>::success(EulerBeam3D{
        length,
        material.youngsModulus,
        shearModulus,
        section.area,
        section.i11,
        section.i22,
        section.torsionalConstant,
        rotation,
        section.sectionPoints});
}

Matrix EulerBeam3D::localStiffness() const {
    const auto diagonal = constitutiveDiagonal(
        youngsModulus_, shearModulus_, area_, iy_, iz_, torsionalConstant_);
    Matrix stiffness{kElementDofCount, kElementDofCount};
    const double inverseSqrtThree = 1.0 / std::sqrt(3.0);
    const std::array<double, 2> gaussPoints = {-inverseSqrtThree, inverseSqrtThree};
    const double jacobian = 0.5 * length_;

    // Both Gauss points are required: a one-point bending rule loses two ranks.
    for (const double xi : gaussPoints) {
        const Matrix b = strainDisplacement(xi, length_);
        for (std::size_t row = 0; row < kElementDofCount; ++row) {
            for (std::size_t column = 0; column < kElementDofCount; ++column) {
                for (std::size_t component = 0; component < diagonal.size(); ++component) {
                    stiffness(row, column) +=
                        b(component, row) * diagonal[component] *
                        b(component, column) * jacobian;
                }
            }
        }
    }

    const Matrix closed = closedStiffness(length_, diagonal);
    if (normalizedMatrixError(stiffness, closed) > kStiffnessInvariantTolerance) {
        throw std::logic_error{
            "Two-point Euler beam stiffness violated the closed-form invariant."};
    }
    return stiffness;
}

Matrix EulerBeam3D::globalStiffness() const {
    const Matrix local = localStiffness();
    const Matrix transform = transformation(rotation_);
    const Matrix localTimesTransform = local.multiply(transform);
    Matrix global{kElementDofCount, kElementDofCount};
    // Kg=T^T*Kl*T while dl=T*dg.
    for (std::size_t row = 0; row < kElementDofCount; ++row) {
        for (std::size_t column = 0; column < kElementDofCount; ++column) {
            for (std::size_t inner = 0; inner < kElementDofCount; ++inner) {
                global(row, column) +=
                    transform(inner, row) * localTimesTransform(inner, column);
            }
        }
    }
    return global;
}

Vector EulerBeam3D::localEquivalentLoad(const ConstantLocalLineLoad& load) const {
    const std::array<double, 4> components = {load.px, load.py, load.pz, load.mx};
    Vector equivalent{kElementDofCount};
    const double inverseSqrtThree = 1.0 / std::sqrt(3.0);
    const std::array<double, 2> gaussPoints = {-inverseSqrtThree, inverseSqrtThree};
    const double jacobian = 0.5 * length_;
    for (const double xi : gaussPoints) {
        const Matrix interpolation = kinematicInterpolation(xi, length_);
        for (std::size_t dof = 0; dof < equivalent.size(); ++dof) {
            for (std::size_t component = 0; component < components.size(); ++component) {
                equivalent[dof] +=
                    interpolation(component, dof) * components[component] * jacobian;
            }
        }
    }
    return equivalent;
}

BeamRecovery EulerBeam3D::recover(const Vector& globalElementDisplacement) const {
    const Matrix transform = transformation(rotation_);
    const Vector localDisplacement = transform.multiply(globalElementDisplacement);
    const auto diagonal = constitutiveDiagonal(
        youngsModulus_, shearModulus_, area_, iy_, iz_, torsionalConstant_);
    BeamRecovery recovery{};

    // With parser/CLI distributed loading excluded, Kl*dl is the local outward end action.
    const Vector endAction = localStiffness().multiply(localDisplacement);
    for (std::size_t endpoint = 0; endpoint < 2U; ++endpoint) {
        for (std::size_t component = 0; component < 6U; ++component) {
            recovery.equilibriumEndActions[endpoint][component] =
                endAction[endpoint * 6U + component];
        }

        const double xi = endpoint == 0U ? -1.0 : 1.0;
        recovery.endpointSectionResultants[endpoint] = generalizedResultant(
            generalizedStrain(strainDisplacement(xi, length_), localDisplacement),
            diagonal);
    }

    const double inverseSqrtThree = 1.0 / std::sqrt(3.0);
    const std::array<double, 2> gaussPoints = {-inverseSqrtThree, inverseSqrtThree};
    for (std::size_t point = 0; point < gaussPoints.size(); ++point) {
        recovery.gaussGeneralizedStrains[point] = generalizedStrain(
            strainDisplacement(gaussPoints[point], length_), localDisplacement);
        recovery.gaussGeneralizedResultants[point] = generalizedResultant(
            recovery.gaussGeneralizedStrains[point], diagonal);

        if (sectionPoints_.empty()) {
            recovery.stressPoints.push_back({
                static_cast<int>(point + 1U),
                0U,
                0.0,
                0.0,
                youngsModulus_ * recovery.gaussGeneralizedStrains[point][0U],
                "fesa-default"});
            continue;
        }

        for (std::size_t sectionPoint = 0; sectionPoint < sectionPoints_.size();
             ++sectionPoint) {
            const double x1 = sectionPoints_[sectionPoint][0U];
            const double x2 = sectionPoints_[sectionPoint][1U];
            const auto& strain = recovery.gaussGeneralizedStrains[point];
            // x1=y and x2=z: S11=E(epsilon0+x2*kappa_y-x1*kappa_z).
            recovery.stressPoints.push_back({
                static_cast<int>(point + 1U),
                sectionPoint + 1U,
                x1,
                x2,
                youngsModulus_ *
                    (strain[0U] + x2 * strain[2U] - x1 * strain[3U]),
                "input"});
        }
    }
    return recovery;
}

EulerBeam3D::EulerBeam3D(
    double length,
    double youngsModulus,
    double shearModulus,
    double area,
    double iy,
    double iz,
    double torsionalConstant,
    std::array<double, 9> rotation,
    std::vector<std::array<double, 2>> sectionPoints)
    : length_{length},
      youngsModulus_{youngsModulus},
      shearModulus_{shearModulus},
      area_{area},
      iy_{iy},
      iz_{iz},
      torsionalConstant_{torsionalConstant},
      rotation_{rotation},
      sectionPoints_{std::move(sectionPoints)} {}

} // namespace fesa
