#include "fesa/elements/euler_beam_3d.hpp"

#include <gtest/gtest.h>

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

namespace fesa {
namespace {

constexpr std::size_t kElementDofCount = 12U;
constexpr double kMatrixTolerance = 1.0e-12;
constexpr double kRigidTolerance = 1.0e-10;
constexpr double kAnalyticalTolerance = 1.0e-9;

Node makeNode(std::array<double, 3> coordinates, std::size_t line) {
    return {{"Beam-1", static_cast<std::int64_t>(line), std::to_string(line)},
            coordinates,
            {"beam-test.inp", line}};
}

LinearElasticMaterial makeMaterial(double youngsModulus = 210.0e9,
                                   double poissonRatio = 0.3) {
    return {"Steel", youngsModulus, poissonRatio, {"beam-test.inp", 20U}};
}

GeneralBeamSection makeSection(
    std::array<double, 3> firstAxis = {0.0, 1.0, 0.0},
    std::vector<std::array<double, 2>> sectionPoints = {}) {
    return {"Section-1",
            0.012,
            2.5e-5,
            0.0,
            4.0e-5,
            1.5e-5,
            firstAxis,
            std::move(sectionPoints),
            {"beam-test.inp", 30U}};
}

EulerBeam3D requireBeam(const Node& firstNode,
                        const Node& secondNode,
                        const GeneralBeamSection& section,
                        const LinearElasticMaterial& material) {
    auto result = EulerBeam3D::create(firstNode, secondNode, section, material);
    if (!result.HasValue()) {
        throw std::runtime_error{"Expected a valid EulerBeam3D fixture."};
    }
    return std::move(result.Value());
}

EulerBeam3D alignedBeam(double length,
                        const GeneralBeamSection& section = makeSection(),
                        const LinearElasticMaterial& material = makeMaterial()) {
    return requireBeam(
        makeNode({0.0, 0.0, 0.0}, 1U),
        makeNode({length, 0.0, 0.0}, 2U),
        section,
        material);
}

double maximumAbsoluteEntry(const Matrix& matrix) {
    double maximum = 0.0;
    for (std::size_t row = 0; row < matrix.Rows(); ++row) {
        for (std::size_t column = 0; column < matrix.Columns(); ++column) {
            maximum = (std::max)(maximum, std::abs(matrix(row, column)));
        }
    }
    return maximum;
}

bool matrixIsFinite(const Matrix& matrix) {
    for (std::size_t row = 0; row < matrix.Rows(); ++row) {
        for (std::size_t column = 0; column < matrix.Columns(); ++column) {
            if (!std::isfinite(matrix(row, column))) {
                return false;
            }
        }
    }
    return true;
}

double normalizedMatrixError(const Matrix& actual, const Matrix& expected) {
    if (actual.Rows() != expected.Rows() || actual.Columns() != expected.Columns()) {
        throw std::invalid_argument{"Matrix comparison requires equal shapes."};
    }

    double maximumDifference = 0.0;
    for (std::size_t row = 0; row < actual.Rows(); ++row) {
        for (std::size_t column = 0; column < actual.Columns(); ++column) {
            maximumDifference = (std::max)(
                maximumDifference,
                std::abs(actual(row, column) - expected(row, column)));
        }
    }

    const double scale = (std::max)(
        1.0,
        (std::max)(maximumAbsoluteEntry(actual), maximumAbsoluteEntry(expected)));
    return maximumDifference / scale;
}

double vectorNorm(const Vector& vector) {
    double sum = 0.0;
    for (std::size_t index = 0; index < vector.Size(); ++index) {
        sum += vector[index] * vector[index];
    }
    return std::sqrt(sum);
}

double quadraticEnergy(const Matrix& matrix, const Vector& vector) {
    const Vector product = matrix.Multiply(vector);
    double value = 0.0;
    for (std::size_t index = 0; index < vector.Size(); ++index) {
        value += vector[index] * product[index];
    }
    return value;
}

void expectScaledNear(double actual, double expected, double relativeTolerance) {
    const double scale = (std::max)(1.0, std::abs(expected));
    EXPECT_LE(std::abs(actual - expected), relativeTolerance * scale);
}

void expectRelativeNear(double actual, double expected, double relativeTolerance) {
    ASSERT_NE(expected, 0.0);
    EXPECT_LE(std::abs(actual - expected) / std::abs(expected), relativeTolerance);
}

Matrix expectedClosedStiffness(double length,
                               const GeneralBeamSection& section,
                               const LinearElasticMaterial& material) {
    Matrix expected{kElementDofCount, kElementDofCount};
    const double shearModulus =
        material.youngsModulus / (2.0 * (1.0 + material.poissonRatio));

    const auto addBlock = [&expected](const std::vector<std::size_t>& indices,
                                      const std::vector<double>& values) {
        const std::size_t width = indices.size();
        for (std::size_t row = 0; row < width; ++row) {
            for (std::size_t column = 0; column < width; ++column) {
                expected(indices[row], indices[column]) = values[row * width + column];
            }
        }
    };

    const double axial = material.youngsModulus * section.area / length;
    addBlock({0U, 6U}, {axial, -axial, -axial, axial});

    const double torsion = shearModulus * section.torsionalConstant / length;
    addBlock({3U, 9U}, {torsion, -torsion, -torsion, torsion});

    const auto bendingBlock = [length](double flexuralRigidity, double rotationSign) {
        const double v = 12.0 * flexuralRigidity / (length * length * length);
        const double c = rotationSign * 6.0 * flexuralRigidity / (length * length);
        const double d = 4.0 * flexuralRigidity / length;
        const double e = 2.0 * flexuralRigidity / length;
        return std::vector<double>{
            v, c, -v, c,
            c, d, -c, e,
            -v, -c, v, -c,
            c, e, -c, d};
    };

    addBlock(
        {1U, 5U, 7U, 11U},
        bendingBlock(material.youngsModulus * section.i22, 1.0));
    addBlock(
        {2U, 4U, 8U, 10U},
        bendingBlock(material.youngsModulus * section.i11, -1.0));
    return expected;
}

std::array<double, kElementDofCount> symmetricEigenvalues(Matrix matrix) {
    for (std::size_t iteration = 0; iteration < 100U * kElementDofCount; ++iteration) {
        std::size_t p = 0U;
        std::size_t q = 1U;
        double maximumOffDiagonal = 0.0;
        for (std::size_t row = 0; row < kElementDofCount; ++row) {
            for (std::size_t column = row + 1U; column < kElementDofCount; ++column) {
                const double candidate = std::abs(matrix(row, column));
                if (candidate > maximumOffDiagonal) {
                    maximumOffDiagonal = candidate;
                    p = row;
                    q = column;
                }
            }
        }
        if (maximumOffDiagonal <=
            1.0e-14 * (std::max)(1.0, maximumAbsoluteEntry(matrix))) {
            break;
        }

        const double app = matrix(p, p);
        const double aqq = matrix(q, q);
        const double apq = matrix(p, q);
        const double angle = 0.5 * std::atan2(2.0 * apq, aqq - app);
        const double cosine = std::cos(angle);
        const double sine = std::sin(angle);

        for (std::size_t index = 0; index < kElementDofCount; ++index) {
            if (index == p || index == q) {
                continue;
            }
            const double aip = matrix(index, p);
            const double aiq = matrix(index, q);
            matrix(index, p) = cosine * aip - sine * aiq;
            matrix(p, index) = matrix(index, p);
            matrix(index, q) = sine * aip + cosine * aiq;
            matrix(q, index) = matrix(index, q);
        }

        matrix(p, p) = cosine * cosine * app - 2.0 * sine * cosine * apq +
                       sine * sine * aqq;
        matrix(q, q) = sine * sine * app + 2.0 * sine * cosine * apq +
                       cosine * cosine * aqq;
        matrix(p, q) = 0.0;
        matrix(q, p) = 0.0;
    }

    std::array<double, kElementDofCount> eigenvalues{};
    for (std::size_t index = 0; index < kElementDofCount; ++index) {
        eigenvalues[index] = matrix(index, index);
    }
    return eigenvalues;
}

std::size_t symmetricRank(const Matrix& matrix, double relativeTolerance) {
    const auto eigenvalues = symmetricEigenvalues(matrix);
    double maximum = 0.0;
    for (const double value : eigenvalues) {
        maximum = (std::max)(maximum, std::abs(value));
    }
    return static_cast<std::size_t>(std::count_if(
        eigenvalues.begin(),
        eigenvalues.end(),
        [maximum, relativeTolerance](double value) {
            return std::abs(value) > relativeTolerance * maximum;
        }));
}

Matrix testOnlyOnePointStiffness(double length,
                                 const GeneralBeamSection& section,
                                 const LinearElasticMaterial& material) {
    // At xi=0 the two bending curvature rows retain only the nodal rotations.
    // This deliberately under-integrated negative control is independent of production.
    Matrix b{4U, kElementDofCount};
    b(0U, 0U) = -1.0 / length;
    b(0U, 6U) = 1.0 / length;
    b(1U, 3U) = -1.0 / length;
    b(1U, 9U) = 1.0 / length;
    b(2U, 4U) = -1.0 / length;
    b(2U, 10U) = 1.0 / length;
    b(3U, 5U) = -1.0 / length;
    b(3U, 11U) = 1.0 / length;

    const double shearModulus =
        material.youngsModulus / (2.0 * (1.0 + material.poissonRatio));
    const std::array<double, 4> diagonal = {
        material.youngsModulus * section.area,
        shearModulus * section.torsionalConstant,
        material.youngsModulus * section.i11,
        material.youngsModulus * section.i22};

    Matrix stiffness{kElementDofCount, kElementDofCount};
    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) * length;
            }
        }
    }
    return stiffness;
}

Vector solveFixedFirstNode(const Matrix& stiffness,
                           const std::array<double, 6>& freeEndLoad) {
    std::array<std::array<double, 7>, 6> augmented{};
    for (std::size_t row = 0; row < 6U; ++row) {
        for (std::size_t column = 0; column < 6U; ++column) {
            augmented[row][column] = stiffness(row + 6U, column + 6U);
        }
        augmented[row][6U] = freeEndLoad[row];
    }

    for (std::size_t pivot = 0; pivot < 6U; ++pivot) {
        std::size_t pivotRow = pivot;
        for (std::size_t row = pivot + 1U; row < 6U; ++row) {
            if (std::abs(augmented[row][pivot]) >
                std::abs(augmented[pivotRow][pivot])) {
                pivotRow = row;
            }
        }
        if (std::abs(augmented[pivotRow][pivot]) <=
            std::numeric_limits<double>::min()) {
            throw std::runtime_error{"Cantilever fixture is singular."};
        }
        std::swap(augmented[pivot], augmented[pivotRow]);

        const double pivotValue = augmented[pivot][pivot];
        for (std::size_t column = pivot; column < 7U; ++column) {
            augmented[pivot][column] /= pivotValue;
        }
        for (std::size_t row = 0; row < 6U; ++row) {
            if (row == pivot) {
                continue;
            }
            const double factor = augmented[row][pivot];
            for (std::size_t column = pivot; column < 7U; ++column) {
                augmented[row][column] -= factor * augmented[pivot][column];
            }
        }
    }

    Vector displacement{kElementDofCount};
    for (std::size_t component = 0; component < 6U; ++component) {
        displacement[component + 6U] = augmented[component][6U];
    }
    return displacement;
}

Vector solveDenseSystem(Matrix matrix, Vector rightHandSide) {
    if (matrix.Rows() != matrix.Columns() ||
        matrix.Rows() != rightHandSide.Size()) {
        throw std::invalid_argument{"Dense test solve requires a square system."};
    }

    for (std::size_t pivot = 0; pivot < matrix.Rows(); ++pivot) {
        std::size_t pivotRow = pivot;
        for (std::size_t row = pivot + 1U; row < matrix.Rows(); ++row) {
            if (std::abs(matrix(row, pivot)) >
                std::abs(matrix(pivotRow, pivot))) {
                pivotRow = row;
            }
        }
        if (!(std::abs(matrix(pivotRow, pivot)) > 0.0) ||
            !std::isfinite(matrix(pivotRow, pivot))) {
            throw std::runtime_error{"Uniform-load test fixture is singular."};
        }
        for (std::size_t column = pivot; column < matrix.Columns(); ++column) {
            std::swap(matrix(pivot, column), matrix(pivotRow, column));
        }
        std::swap(rightHandSide[pivot], rightHandSide[pivotRow]);

        const double pivotValue = matrix(pivot, pivot);
        for (std::size_t column = pivot; column < matrix.Columns(); ++column) {
            matrix(pivot, column) /= pivotValue;
        }
        rightHandSide[pivot] /= pivotValue;
        for (std::size_t row = 0; row < matrix.Rows(); ++row) {
            if (row == pivot) {
                continue;
            }
            const double factor = matrix(row, pivot);
            for (std::size_t column = pivot; column < matrix.Columns(); ++column) {
                matrix(row, column) -= factor * matrix(pivot, column);
            }
            rightHandSide[row] -= factor * rightHandSide[pivot];
        }
    }
    return rightHandSide;
}

Vector solveUniformTransverseCantilever(std::size_t elementCount,
                                        double length,
                                        double lineLoad,
                                        const GeneralBeamSection& section,
                                        const LinearElasticMaterial& material) {
    const double elementLength = length / static_cast<double>(elementCount);
    const std::size_t systemSize = 2U * (elementCount + 1U);
    Matrix assembledStiffness{systemSize, systemSize};
    Vector assembledLoad{systemSize};
    const std::array<std::size_t, 4> bendingDofs = {1U, 5U, 7U, 11U};

    // Test-only direct assembly keeps this evidence at the formulation boundary:
    // two [v,rz] DOFs per node, with no Domain, parser, or DLOAD path.
    for (std::size_t element = 0; element < elementCount; ++element) {
        const EulerBeam3D beam = alignedBeam(elementLength, section, material);
        const Matrix elementStiffness = beam.localStiffness();
        const Vector elementLoad = beam.localEquivalentLoad(
            {0.0, lineLoad, 0.0, 0.0});
        const std::array<std::size_t, 4> assembledDofs = {
            2U * element,
            2U * element + 1U,
            2U * (element + 1U),
            2U * (element + 1U) + 1U};
        for (std::size_t row = 0; row < bendingDofs.size(); ++row) {
            assembledLoad[assembledDofs[row]] += elementLoad[bendingDofs[row]];
            for (std::size_t column = 0; column < bendingDofs.size(); ++column) {
                assembledStiffness(assembledDofs[row], assembledDofs[column]) +=
                    elementStiffness(bendingDofs[row], bendingDofs[column]);
            }
        }
    }

    const std::size_t freeSize = systemSize - 2U;
    Matrix freeStiffness{freeSize, freeSize};
    Vector freeLoad{freeSize};
    for (std::size_t row = 0; row < freeSize; ++row) {
        freeLoad[row] = assembledLoad[row + 2U];
        for (std::size_t column = 0; column < freeSize; ++column) {
            freeStiffness(row, column) =
                assembledStiffness(row + 2U, column + 2U);
        }
    }

    const Vector freeDisplacement =
        solveDenseSystem(std::move(freeStiffness), std::move(freeLoad));
    Vector nodalDisplacement{systemSize};
    for (std::size_t dof = 0; dof < freeSize; ++dof) {
        nodalDisplacement[dof + 2U] = freeDisplacement[dof];
    }
    return nodalDisplacement;
}

double uniformLoadInteriorDisplacementError(
    const Vector& nodalDisplacement,
    std::size_t elementCount,
    double length,
    double lineLoad,
    double flexuralRigidity) {
    const double elementLength = length / static_cast<double>(elementCount);
    const std::array<double, 5> gaussPoints = {
        -0.9061798459386640,
        -0.5384693101056831,
        0.0,
        0.5384693101056831,
        0.9061798459386640};
    const std::array<double, 5> gaussWeights = {
        0.2369268850561891,
        0.4786286704993665,
        0.5688888888888889,
        0.4786286704993665,
        0.2369268850561891};
    double squaredError = 0.0;
    double squaredReference = 0.0;

    // Five-point integration is independent of production and exactly integrates
    // the squared error between cubic Hermite interpolation and the quartic beam solution.
    for (std::size_t element = 0; element < elementCount; ++element) {
        for (std::size_t point = 0; point < gaussPoints.size(); ++point) {
            const double r = 0.5 * (1.0 + gaussPoints[point]);
            const double rSquared = r * r;
            const double rCubed = rSquared * r;
            const double h1 = 1.0 - 3.0 * rSquared + 2.0 * rCubed;
            const double h2 = elementLength * (r - 2.0 * rSquared + rCubed);
            const double h3 = 3.0 * rSquared - 2.0 * rCubed;
            const double h4 = elementLength * (-rSquared + rCubed);
            const double interpolated =
                h1 * nodalDisplacement[2U * element] +
                h2 * nodalDisplacement[2U * element + 1U] +
                h3 * nodalDisplacement[2U * (element + 1U)] +
                h4 * nodalDisplacement[2U * (element + 1U) + 1U];
            const double x = elementLength *
                             (static_cast<double>(element) + r);
            const double analytical =
                lineLoad * x * x *
                (6.0 * length * length - 4.0 * length * x + x * x) /
                (24.0 * flexuralRigidity);
            const double weight = 0.5 * elementLength * gaussWeights[point];
            const double difference = interpolated - analytical;
            squaredError += weight * difference * difference;
            squaredReference += weight * analytical * analytical;
        }
    }
    return std::sqrt(squaredError / squaredReference);
}

Matrix transformationFromKnownRows(
    const std::array<std::array<double, 3>, 3>& rotation) {
    Matrix transformation{kElementDofCount, kElementDofCount};
    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) {
                transformation(block * 3U + row, block * 3U + column) =
                    rotation[row][column];
            }
        }
    }
    return transformation;
}

Vector transposeMultiply(const Matrix& matrix, const Vector& vector) {
    if (matrix.Rows() != vector.Size()) {
        throw std::invalid_argument{"Transpose multiply dimension mismatch."};
    }
    Vector result{matrix.Columns()};
    for (std::size_t column = 0; column < matrix.Columns(); ++column) {
        for (std::size_t row = 0; row < matrix.Rows(); ++row) {
            result[column] += matrix(row, column) * vector[row];
        }
    }
    return result;
}

double determinant(const std::array<std::array<double, 3>, 3>& matrix) {
    return matrix[0][0] *
               (matrix[1][1] * matrix[2][2] - matrix[1][2] * matrix[2][1]) -
           matrix[0][1] *
               (matrix[1][0] * matrix[2][2] - matrix[1][2] * matrix[2][0]) +
           matrix[0][2] *
               (matrix[1][0] * matrix[2][1] - matrix[1][1] * matrix[2][0]);
}

TEST(EulerBeam3D, HermiteAndBMatrixMatchReviewedSigns) {
    const double length = 2.5;
    const auto section = makeSection();
    const auto material = makeMaterial();
    const auto beam = alignedBeam(length, section, material);

    const double axialStrain = 0.012;
    const double twist = -0.021;
    const std::array<double, 4> v = {1.2, -0.4, 0.3, -0.07};
    const std::array<double, 4> w = {-0.8, 0.6, -0.2, 0.05};
    const auto value = [](const std::array<double, 4>& coefficients, double x) {
        return coefficients[0] + coefficients[1] * x + coefficients[2] * x * x +
               coefficients[3] * x * x * x;
    };
    const auto slope = [](const std::array<double, 4>& coefficients, double x) {
        return coefficients[1] + 2.0 * coefficients[2] * x +
               3.0 * coefficients[3] * x * x;
    };
    const auto curvature = [](const std::array<double, 4>& coefficients, double x) {
        return 2.0 * coefficients[2] + 6.0 * coefficients[3] * x;
    };

    Vector displacement{kElementDofCount};
    displacement[0U] = 0.2;
    displacement[1U] = value(v, 0.0);
    displacement[2U] = value(w, 0.0);
    displacement[3U] = -0.1;
    displacement[4U] = -slope(w, 0.0);
    displacement[5U] = slope(v, 0.0);
    displacement[6U] = displacement[0U] + axialStrain * length;
    displacement[7U] = value(v, length);
    displacement[8U] = value(w, length);
    displacement[9U] = displacement[3U] + twist * length;
    displacement[10U] = -slope(w, length);
    displacement[11U] = slope(v, length);

    const BeamRecovery recovery = beam.recover(displacement);
    const double inverseSqrtThree = 1.0 / std::sqrt(3.0);
    const std::array<double, 2> gaussXi = {-inverseSqrtThree, inverseSqrtThree};
    for (std::size_t point = 0; point < gaussXi.size(); ++point) {
        const double x = 0.5 * length * (1.0 + gaussXi[point]);
        EXPECT_NEAR(recovery.gaussGeneralizedStrains[point][0U], axialStrain, 1.0e-14);
        EXPECT_NEAR(recovery.gaussGeneralizedStrains[point][1U], twist, 1.0e-14);
        EXPECT_NEAR(
            recovery.gaussGeneralizedStrains[point][2U],
            -curvature(w, x),
            1.0e-13);
        EXPECT_NEAR(
            recovery.gaussGeneralizedStrains[point][3U],
            curvature(v, x),
            1.0e-13);
    }

    EXPECT_NEAR(
        recovery.endpointSectionResultants[0U][2U],
        material.youngsModulus * section.i11 * -curvature(w, 0.0),
        1.0e-5);
    EXPECT_NEAR(
        recovery.endpointSectionResultants[1U][2U],
        material.youngsModulus * section.i11 * -curvature(w, length),
        1.0e-5);
    EXPECT_NEAR(
        recovery.endpointSectionResultants[0U][3U],
        material.youngsModulus * section.i22 * curvature(v, 0.0),
        1.0e-5);
    EXPECT_NEAR(
        recovery.endpointSectionResultants[1U][3U],
        material.youngsModulus * section.i22 * curvature(v, length),
        1.0e-5);
}

TEST(EulerBeam3D, TwoPointGaussMatchesClosedStiffness) {
    const double length = 3.7;
    auto section = makeSection();
    section.area = 0.019;
    section.i11 = 3.1e-5;
    section.i22 = 7.4e-5;
    section.torsionalConstant = 2.2e-5;
    const auto material = makeMaterial(73.0e9, 0.27);
    const auto beam = alignedBeam(length, section, material);

    const Matrix actual = beam.localStiffness();
    const Matrix closed = expectedClosedStiffness(length, section, material);
    EXPECT_LE(normalizedMatrixError(actual, closed), kMatrixTolerance);

    Matrix transpose{actual.Rows(), actual.Columns()};
    for (std::size_t row = 0; row < actual.Rows(); ++row) {
        for (std::size_t column = 0; column < actual.Columns(); ++column) {
            transpose(row, column) = actual(column, row);
        }
    }
    EXPECT_LE(normalizedMatrixError(actual, transpose), kMatrixTolerance);
}

TEST(EulerBeam3D, HasSixRigidModesRankSixAndPositiveDeformationEnergy) {
    const double length = 2.0;
    auto section = makeSection();
    section.area = 1.4;
    section.i11 = 0.8;
    section.i22 = 1.1;
    section.torsionalConstant = 0.6;
    const auto material = makeMaterial(5.0, 0.25);
    const Matrix stiffness = alignedBeam(length, section, material).localStiffness();

    std::array<Vector, 6> rigidModes = {
        Vector{kElementDofCount}, Vector{kElementDofCount}, Vector{kElementDofCount},
        Vector{kElementDofCount}, Vector{kElementDofCount}, Vector{kElementDofCount}};
    rigidModes[0U][0U] = rigidModes[0U][6U] = 1.0;
    rigidModes[1U][1U] = rigidModes[1U][7U] = 1.0;
    rigidModes[2U][2U] = rigidModes[2U][8U] = 1.0;
    rigidModes[3U][3U] = rigidModes[3U][9U] = 1.0;
    rigidModes[4U][4U] = rigidModes[4U][10U] = 1.0;
    rigidModes[4U][8U] = -length;
    rigidModes[5U][5U] = rigidModes[5U][11U] = 1.0;
    rigidModes[5U][7U] = length;

    const double stiffnessScale = (std::max)(1.0, maximumAbsoluteEntry(stiffness));
    for (const Vector& mode : rigidModes) {
        const double normalizedResidual =
            vectorNorm(stiffness.Multiply(mode)) /
            (stiffnessScale * (std::max)(1.0, vectorNorm(mode)));
        EXPECT_LE(normalizedResidual, kRigidTolerance);
    }

    const std::array<double, kElementDofCount> q = {
        1.0, 1.0, 1.0, length, length, length,
        1.0, 1.0, 1.0, length, length, length};
    Matrix scaled{kElementDofCount, kElementDofCount};
    for (std::size_t row = 0; row < kElementDofCount; ++row) {
        for (std::size_t column = 0; column < kElementDofCount; ++column) {
            scaled(row, column) = stiffness(row, column) / (q[row] * q[column]);
        }
    }
    const auto eigenvalues = symmetricEigenvalues(scaled);
    double maximumSingularValue = 0.0;
    for (const double value : eigenvalues) {
        maximumSingularValue = (std::max)(maximumSingularValue, std::abs(value));
    }
    const auto positiveCount = std::count_if(
        eigenvalues.begin(), eigenvalues.end(), [maximumSingularValue](double value) {
            return std::abs(value) > kRigidTolerance * maximumSingularValue;
        });
    EXPECT_EQ(positiveCount, 6);
    for (const double value : eigenvalues) {
        EXPECT_GE(value, -kRigidTolerance * maximumSingularValue);
    }

    for (std::size_t component = 0; component < 6U; ++component) {
        Vector deformation{kElementDofCount};
        deformation[6U + component] = 1.0;
        EXPECT_GT(quadraticEnergy(stiffness, deformation), 0.0);
    }
}

TEST(EulerBeam3D, RotatedTransformPreservesWorkAndEnergy) {
    const double inverseSqrtTwo = 1.0 / std::sqrt(2.0);
    const std::array<std::array<double, 3>, 3> rotation = {{
        {{2.0 / 3.0, 2.0 / 3.0, 1.0 / 3.0}},
        {{-inverseSqrtTwo, inverseSqrtTwo, 0.0}},
        {{-inverseSqrtTwo / 3.0, -inverseSqrtTwo / 3.0,
          4.0 * inverseSqrtTwo / 3.0}}}};
    const Matrix transformation = transformationFromKnownRows(rotation);

    for (std::size_t row = 0; row < 3U; ++row) {
        for (std::size_t column = 0; column < 3U; ++column) {
            double dot = 0.0;
            for (std::size_t component = 0; component < 3U; ++component) {
                dot += rotation[row][component] * rotation[column][component];
            }
            EXPECT_NEAR(dot, row == column ? 1.0 : 0.0, 1.0e-14);
        }
    }
    EXPECT_NEAR(determinant(rotation), 1.0, 1.0e-14);

    const auto section = makeSection({-2.0, 2.0, 0.0});
    const auto material = makeMaterial();
    const auto beam = requireBeam(
        makeNode({1.0, -2.0, 0.5}, 1U),
        makeNode({3.0, 0.0, 1.5}, 2U),
        section,
        material);
    const Matrix local = beam.localStiffness();
    const Matrix global = beam.globalStiffness();

    Matrix expectedGlobal{kElementDofCount, kElementDofCount};
    const Matrix localTimesTransform = local.Multiply(transformation);
    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) {
                expectedGlobal(row, column) +=
                    transformation(inner, row) * localTimesTransform(inner, column);
            }
        }
    }
    EXPECT_LE(normalizedMatrixError(global, expectedGlobal), kMatrixTolerance);

    Vector localDisplacement{kElementDofCount};
    for (std::size_t index = 0; index < localDisplacement.Size(); ++index) {
        localDisplacement[index] = 0.01 * static_cast<double>(index + 1U) - 0.04;
    }
    const Vector globalDisplacement = transposeMultiply(transformation, localDisplacement);
    const Vector localForce = local.Multiply(localDisplacement);
    const Vector globalForce = global.Multiply(globalDisplacement);
    const Vector expectedGlobalForce = transposeMultiply(transformation, localForce);
    for (std::size_t index = 0; index < kElementDofCount; ++index) {
        expectScaledNear(globalForce[index], expectedGlobalForce[index], kMatrixTolerance);
    }
    expectScaledNear(
        quadraticEnergy(global, globalDisplacement),
        quadraticEnergy(local, localDisplacement),
        kMatrixTolerance);

    Vector globalVariation{kElementDofCount};
    for (std::size_t index = 0; index < globalVariation.Size(); ++index) {
        globalVariation[index] = 0.03 - 0.002 * static_cast<double>(index);
    }
    const Vector localVariation = transformation.Multiply(globalVariation);
    expectScaledNear(
        globalVariation.Dot(globalForce),
        localVariation.Dot(localForce),
        kMatrixTolerance);

    const BeamRecovery recovery = beam.recover(globalDisplacement);
    EXPECT_NEAR(
        recovery.gaussGeneralizedStrains[0U][0U],
        (localDisplacement[6U] - localDisplacement[0U]) / 3.0,
        1.0e-14);
}

TEST(EulerBeam3D, ConstantLineLoadMatchesAllSignedComponents) {
    const double length = 4.0;
    const ConstantLocalLineLoad load{2.5, -3.0, 5.5, -7.0};
    const Vector equivalent = alignedBeam(length).localEquivalentLoad(load);
    const std::array<double, kElementDofCount> expected = {
        5.0, -6.0, 11.0, -14.0, -22.0 / 3.0, -4.0,
        5.0, -6.0, 11.0, -14.0, 22.0 / 3.0, 4.0};
    ASSERT_EQ(equivalent.Size(), expected.size());
    for (std::size_t index = 0; index < expected.size(); ++index) {
        expectScaledNear(equivalent[index], expected[index], kMatrixTolerance);
    }

    auto convergenceSection = makeSection();
    convergenceSection.area = 1.0;
    convergenceSection.i11 = 1.0;
    convergenceSection.i22 = 1.0;
    convergenceSection.torsionalConstant = 1.0;
    const auto convergenceMaterial = makeMaterial(5.0, 0.25);
    const double transverseLoad = -3.0;
    const std::array<std::size_t, 3> elementCounts = {1U, 2U, 4U};
    std::array<double, 3> relativeErrors{};
    for (std::size_t mesh = 0; mesh < elementCounts.size(); ++mesh) {
        const double elementLength =
            length / static_cast<double>(elementCounts[mesh]);
        const Vector elementLoad = alignedBeam(
            elementLength,
            convergenceSection,
            convergenceMaterial)
                                       .localEquivalentLoad(
                                           {0.0, transverseLoad, 0.0, 0.0});
        expectRelativeNear(
            elementLoad[1U], transverseLoad * elementLength / 2.0, kMatrixTolerance);
        expectRelativeNear(
            elementLoad[5U],
            transverseLoad * elementLength * elementLength / 12.0,
            kMatrixTolerance);
        expectRelativeNear(
            elementLoad[7U], transverseLoad * elementLength / 2.0, kMatrixTolerance);
        expectRelativeNear(
            elementLoad[11U],
            -transverseLoad * elementLength * elementLength / 12.0,
            kMatrixTolerance);

        const Vector nodalDisplacement = solveUniformTransverseCantilever(
            elementCounts[mesh],
            length,
            transverseLoad,
            convergenceSection,
            convergenceMaterial);
        relativeErrors[mesh] = uniformLoadInteriorDisplacementError(
            nodalDisplacement,
            elementCounts[mesh],
            length,
            transverseLoad,
            convergenceMaterial.youngsModulus * convergenceSection.i22);
    }
    EXPECT_GT(relativeErrors[0U], relativeErrors[1U]);
    EXPECT_GT(relativeErrors[1U], relativeErrors[2U]);
    EXPECT_NEAR(
        std::log(relativeErrors[0U] / relativeErrors[1U]) / std::log(2.0),
        4.0,
        1.0e-8);
    EXPECT_NEAR(
        std::log(relativeErrors[1U] / relativeErrors[2U]) / std::log(2.0),
        4.0,
        1.0e-8);
}

TEST(EulerBeam3D, AnalyticalAxialTorsionAndTwoPlaneBendingRecover) {
    const double length = 3.0;
    const auto section = makeSection();
    const auto material = makeMaterial();
    const auto beam = alignedBeam(length, section, material);
    const Matrix stiffness = beam.localStiffness();
    const double shearModulus =
        material.youngsModulus / (2.0 * (1.0 + material.poissonRatio));

    const double axialForce = 1250.0;
    const Vector axial = solveFixedFirstNode(stiffness, {axialForce, 0.0, 0.0, 0.0, 0.0, 0.0});
    expectRelativeNear(
        axial[6U],
        axialForce * length / (material.youngsModulus * section.area),
        kAnalyticalTolerance);
    const BeamRecovery axialRecovery = beam.recover(axial);
    expectRelativeNear(
        axialRecovery.equilibriumEndActions[0U][0U],
        -axialForce,
        kAnalyticalTolerance);
    expectRelativeNear(
        axialRecovery.equilibriumEndActions[1U][0U],
        axialForce,
        kAnalyticalTolerance);
    expectRelativeNear(
        axialRecovery.endpointSectionResultants[0U][0U],
        axialForce,
        kAnalyticalTolerance);
    expectRelativeNear(
        axialRecovery.endpointSectionResultants[1U][0U],
        axialForce,
        kAnalyticalTolerance);

    const double torque = -870.0;
    const Vector torsion = solveFixedFirstNode(stiffness, {0.0, 0.0, 0.0, torque, 0.0, 0.0});
    expectRelativeNear(
        torsion[9U],
        torque * length / (shearModulus * section.torsionalConstant),
        kAnalyticalTolerance);
    const BeamRecovery torsionRecovery = beam.recover(torsion);
    expectRelativeNear(
        torsionRecovery.equilibriumEndActions[0U][3U],
        -torque,
        kAnalyticalTolerance);
    expectRelativeNear(
        torsionRecovery.equilibriumEndActions[1U][3U],
        torque,
        kAnalyticalTolerance);
    expectRelativeNear(
        torsionRecovery.endpointSectionResultants[0U][1U],
        torque,
        kAnalyticalTolerance);

    const double localYForce = 640.0;
    const Vector localY = solveFixedFirstNode(stiffness, {0.0, localYForce, 0.0, 0.0, 0.0, 0.0});
    expectRelativeNear(
        localY[7U],
        localYForce * length * length * length /
            (3.0 * material.youngsModulus * section.i22),
        kAnalyticalTolerance);
    expectRelativeNear(
        localY[11U],
        localYForce * length * length /
            (2.0 * material.youngsModulus * section.i22),
        kAnalyticalTolerance);
    const BeamRecovery localYRecovery = beam.recover(localY);
    expectRelativeNear(
        localYRecovery.equilibriumEndActions[0U][1U],
        -localYForce,
        kAnalyticalTolerance);
    expectRelativeNear(
        localYRecovery.equilibriumEndActions[1U][1U],
        localYForce,
        kAnalyticalTolerance);
    expectRelativeNear(
        localYRecovery.equilibriumEndActions[0U][5U],
        -localYForce * length,
        kAnalyticalTolerance);
    expectRelativeNear(
        localYRecovery.endpointSectionResultants[0U][3U],
        localYForce * length,
        kAnalyticalTolerance);
    EXPECT_NEAR(localYRecovery.endpointSectionResultants[1U][3U], 0.0, 1.0e-8);

    const double localZForce = -510.0;
    const Vector localZ = solveFixedFirstNode(stiffness, {0.0, 0.0, localZForce, 0.0, 0.0, 0.0});
    expectRelativeNear(
        localZ[8U],
        localZForce * length * length * length /
            (3.0 * material.youngsModulus * section.i11),
        kAnalyticalTolerance);
    expectRelativeNear(
        localZ[10U],
        -localZForce * length * length /
            (2.0 * material.youngsModulus * section.i11),
        kAnalyticalTolerance);
    const BeamRecovery localZRecovery = beam.recover(localZ);
    expectRelativeNear(
        localZRecovery.equilibriumEndActions[0U][2U],
        -localZForce,
        kAnalyticalTolerance);
    expectRelativeNear(
        localZRecovery.equilibriumEndActions[1U][2U],
        localZForce,
        kAnalyticalTolerance);
    expectRelativeNear(
        localZRecovery.equilibriumEndActions[0U][4U],
        localZForce * length,
        kAnalyticalTolerance);
    expectRelativeNear(
        localZRecovery.endpointSectionResultants[0U][2U],
        -localZForce * length,
        kAnalyticalTolerance);
    EXPECT_NEAR(localZRecovery.endpointSectionResultants[1U][2U], 0.0, 1.0e-8);
}

TEST(EulerBeam3D, RejectsInvalidGeometryAndProperties) {
    const Node origin = makeNode({0.0, 0.0, 0.0}, 1U);
    const Node unitX = makeNode({1.0, 0.0, 0.0}, 2U);
    const auto validSection = makeSection();
    const auto validMaterial = makeMaterial();

    const auto expectFailure = [](const Result<EulerBeam3D>& result,
                                  const std::string& code) {
        if (result.HasValue()) {
            const Matrix stiffness = result.Value().localStiffness();
            ADD_FAILURE()
                << "Invalid fixture was accepted; local stiffness finite="
                << matrixIsFinite(stiffness)
                << ", maximum absolute entry=" << maximumAbsoluteEntry(stiffness);
            return;
        }
        EXPECT_EQ(result.GetStatus().Category(), FailureCategory::kModel);
        ASSERT_EQ(result.GetStatus().Diagnostics().size(), 1U);
        EXPECT_EQ(result.GetStatus().Diagnostics()[0U].code, code);
    };

    expectFailure(
        EulerBeam3D::create(origin, origin, validSection, validMaterial),
        "invalid-beam-length");
    expectFailure(
        EulerBeam3D::create(
            origin,
            makeNode({1.0e-12, 0.0, 0.0}, 2U),
            validSection,
            validMaterial),
        "invalid-beam-length");

    const double coordinate = 1048576.0;
    const Node scaledFirst = makeNode({coordinate, 0.0, 0.0}, 1U);
    const Node belowThreshold = makeNode({coordinate + coordinate * 0.5e-12, 0.0, 0.0}, 2U);
    const Node aboveThreshold = makeNode({coordinate + coordinate * 2.0e-12, 0.0, 0.0}, 2U);
    expectFailure(
        EulerBeam3D::create(scaledFirst, belowThreshold, validSection, validMaterial),
        "invalid-beam-length");
    EXPECT_TRUE(EulerBeam3D::create(
                    scaledFirst, aboveThreshold, validSection, validMaterial)
                    .HasValue());

    auto parallelGuide = validSection;
    parallelGuide.firstAxis = {1.0, 0.0, 0.0};
    expectFailure(
        EulerBeam3D::create(origin, unitX, parallelGuide, validMaterial),
        "invalid-beam-guide-vector");
    auto guideAtThreshold = validSection;
    guideAtThreshold.firstAxis = {1.0, 1.0e-12, 0.0};
    expectFailure(
        EulerBeam3D::create(origin, unitX, guideAtThreshold, validMaterial),
        "invalid-beam-guide-vector");
    auto guideAboveThreshold = validSection;
    guideAboveThreshold.firstAxis = {1.0, 2.0e-12, 0.0};
    EXPECT_TRUE(EulerBeam3D::create(origin, unitX, guideAboveThreshold, validMaterial).HasValue());

    auto invalidMaterial = validMaterial;
    invalidMaterial.youngsModulus = 0.0;
    expectFailure(
        EulerBeam3D::create(origin, unitX, validSection, invalidMaterial),
        "invalid-beam-property");
    invalidMaterial = validMaterial;
    invalidMaterial.poissonRatio = -2.0;
    expectFailure(
        EulerBeam3D::create(origin, unitX, validSection, invalidMaterial),
        "invalid-beam-property");

    for (std::size_t property = 0; property < 4U; ++property) {
        auto invalidSection = validSection;
        double* properties[] = {
            &invalidSection.area,
            &invalidSection.i11,
            &invalidSection.i22,
            &invalidSection.torsionalConstant};
        *properties[property] = 0.0;
        expectFailure(
            EulerBeam3D::create(origin, unitX, invalidSection, validMaterial),
            "invalid-beam-property");
    }

    auto overflowMaterial = makeMaterial(
        std::numeric_limits<double>::max() / 4.0,
        0.25);
    auto overflowSection = validSection;
    overflowSection.area = 8.0;
    expectFailure(
        EulerBeam3D::create(origin, unitX, overflowSection, overflowMaterial),
        "invalid-beam-property");

    auto underflowSection = validSection;
    underflowSection.area = std::numeric_limits<double>::denorm_min();
    underflowSection.i11 = std::numeric_limits<double>::denorm_min();
    underflowSection.i22 = std::numeric_limits<double>::denorm_min();
    underflowSection.torsionalConstant =
        std::numeric_limits<double>::denorm_min();
    expectFailure(
        EulerBeam3D::create(
            origin,
            unitX,
            underflowSection,
            makeMaterial(0.5, 0.25)),
        "invalid-beam-property");

    auto lengthScaledSection = validSection;
    lengthScaledSection.area = 1.0;
    lengthScaledSection.i11 = 1.0;
    lengthScaledSection.i22 = 1.0;
    lengthScaledSection.torsionalConstant = 1.0;
    expectFailure(
        EulerBeam3D::create(
            origin,
            makeNode({1.0e103, 0.0, 0.0}, 2U),
            lengthScaledSection,
            makeMaterial(1.0, 0.25)),
        "invalid-beam-property");

    auto coupledSection = validSection;
    coupledSection.i12 = 1.0e-9;
    expectFailure(
        EulerBeam3D::create(origin, unitX, coupledSection, validMaterial),
        "unsupported-coupled-section");
}

TEST(EulerBeam3D, RecoversSectionPointAndDefaultCentroidS11) {
    const double length = 2.0;
    const double epsilon = 0.01;
    const double kappaY = 0.02;
    const double kappaZ = -0.03;
    const auto material = makeMaterial();
    auto section = makeSection({0.0, 1.0, 0.0}, {{0.25, -0.5}, {-0.4, 0.3}});
    const auto beam = alignedBeam(length, section, material);

    Vector displacement{kElementDofCount};
    displacement[6U] = epsilon * length;
    displacement[7U] = 0.5 * kappaZ * length * length;
    displacement[8U] = -0.5 * kappaY * length * length;
    displacement[10U] = kappaY * length;
    displacement[11U] = kappaZ * length;

    const BeamRecovery recovery = beam.recover(displacement);
    ASSERT_EQ(recovery.stressPoints.size(), 4U);
    for (std::size_t gaussPoint = 0; gaussPoint < 2U; ++gaussPoint) {
        for (std::size_t point = 0; point < section.sectionPoints.size(); ++point) {
            const BeamStressPoint& stress =
                recovery.stressPoints[gaussPoint * section.sectionPoints.size() + point];
            const double x1 = section.sectionPoints[point][0U];
            const double x2 = section.sectionPoints[point][1U];
            EXPECT_EQ(stress.gaussPoint, static_cast<int>(gaussPoint + 1U));
            EXPECT_EQ(stress.sectionPoint, point + 1U);
            EXPECT_DOUBLE_EQ(stress.x1, x1);
            EXPECT_DOUBLE_EQ(stress.x2, x2);
            expectScaledNear(
                stress.s11,
                material.youngsModulus * (epsilon + x2 * kappaY - x1 * kappaZ),
                kMatrixTolerance);
        }
    }

    const auto defaultBeam = alignedBeam(length, makeSection(), material);
    const BeamRecovery defaultRecovery = defaultBeam.recover(displacement);
    ASSERT_EQ(defaultRecovery.stressPoints.size(), 2U);
    for (std::size_t gaussPoint = 0; gaussPoint < 2U; ++gaussPoint) {
        const BeamStressPoint& stress = defaultRecovery.stressPoints[gaussPoint];
        EXPECT_EQ(stress.gaussPoint, static_cast<int>(gaussPoint + 1U));
        EXPECT_EQ(stress.sectionPoint, 0U);
        EXPECT_DOUBLE_EQ(stress.x1, 0.0);
        EXPECT_DOUBLE_EQ(stress.x2, 0.0);
        EXPECT_EQ(stress.source, "fesa-default");
        expectScaledNear(
            stress.s11,
            material.youngsModulus * epsilon,
            kMatrixTolerance);
    }
}

TEST(EulerBeam3D, ReproducesConstantStrainTwistAndCurvaturePatches) {
    const double length = 2.8;
    const double epsilon = -0.014;
    const double twist = 0.023;
    const double kappaY = -0.031;
    const double kappaZ = 0.047;
    const auto section = makeSection();
    const auto material = makeMaterial();
    const double shearModulus =
        material.youngsModulus / (2.0 * (1.0 + material.poissonRatio));
    const auto beam = alignedBeam(length, section, material);

    Vector displacement{kElementDofCount};
    displacement[6U] = epsilon * length;
    displacement[7U] = 0.5 * kappaZ * length * length;
    displacement[8U] = -0.5 * kappaY * length * length;
    displacement[9U] = twist * length;
    displacement[10U] = kappaY * length;
    displacement[11U] = kappaZ * length;

    const std::array<double, 4> expectedStrain = {epsilon, twist, kappaY, kappaZ};
    const std::array<double, 4> expectedResultant = {
        material.youngsModulus * section.area * epsilon,
        shearModulus * section.torsionalConstant * twist,
        material.youngsModulus * section.i11 * kappaY,
        material.youngsModulus * section.i22 * kappaZ};
    const BeamRecovery recovery = beam.recover(displacement);
    for (std::size_t point = 0; point < 2U; ++point) {
        for (std::size_t component = 0; component < 4U; ++component) {
            expectScaledNear(
                recovery.gaussGeneralizedStrains[point][component],
                expectedStrain[component],
                kMatrixTolerance);
            expectScaledNear(
                recovery.gaussGeneralizedResultants[point][component],
                expectedResultant[component],
                kMatrixTolerance);
            expectScaledNear(
                recovery.endpointSectionResultants[point][component],
                expectedResultant[component],
                kMatrixTolerance);
        }
    }

    const std::array<std::size_t, 4> endActionComponents = {0U, 3U, 4U, 5U};
    for (std::size_t component = 0; component < expectedResultant.size(); ++component) {
        expectScaledNear(
            recovery.equilibriumEndActions[0U][endActionComponents[component]],
            -expectedResultant[component],
            kMatrixTolerance);
        expectScaledNear(
            recovery.equilibriumEndActions[1U][endActionComponents[component]],
            expectedResultant[component],
            kMatrixTolerance);
    }
    EXPECT_NEAR(recovery.equilibriumEndActions[0U][1U], 0.0, 1.0e-8);
    EXPECT_NEAR(recovery.equilibriumEndActions[0U][2U], 0.0, 1.0e-8);
    EXPECT_NEAR(recovery.equilibriumEndActions[1U][1U], 0.0, 1.0e-8);
    EXPECT_NEAR(recovery.equilibriumEndActions[1U][2U], 0.0, 1.0e-8);
}

TEST(EulerBeam3D, OnePointNegativeControlHasRankFour) {
    const double length = 3.7;
    auto section = makeSection();
    section.area = 1.0;
    section.i11 = 0.7;
    section.i22 = 1.2;
    section.torsionalConstant = 0.9;
    const auto material = makeMaterial(4.0, 0.25);

    const Matrix onePoint = testOnlyOnePointStiffness(length, section, material);
    const Matrix production = alignedBeam(length, section, material).localStiffness();
    EXPECT_EQ(symmetricRank(onePoint, kRigidTolerance), 4U);
    EXPECT_EQ(symmetricRank(production, kRigidTolerance), 6U);
}

} // namespace
} // namespace fesa