#include <fesa/elements/euler_beam_3d.hpp>

#include <cmath>
#include <cstddef>
#include <stdexcept>

namespace {

constexpr double tolerance = 1.0e-9;

bool close(double actual, double expected, double tol = tolerance)
{
    return std::abs(actual - expected) <= tol;
}

double entry(const fesa::elements::Matrix12& matrix, int row, int column)
{
    return matrix[static_cast<std::size_t>(row * 12 + column)];
}

fesa::elements::EulerBeam3DSection section()
{
    return fesa::elements::EulerBeam3DSection{
        210.0,
        80.0,
        3.0,
        4.0,
        5.0,
        6.0
    };
}

bool matrices_close(const fesa::elements::Matrix12& lhs, const fesa::elements::Matrix12& rhs)
{
    for (std::size_t index = 0; index < lhs.size(); ++index) {
        if (!close(lhs[index], rhs[index])) {
            return false;
        }
    }
    return true;
}

bool symmetric(const fesa::elements::Matrix12& matrix)
{
    for (int row = 0; row < 12; ++row) {
        for (int column = 0; column < 12; ++column) {
            if (!close(entry(matrix, row, column), entry(matrix, column, row))) {
                return false;
            }
        }
    }
    return true;
}

bool throws_parallel_orientation()
{
    const fesa::elements::EulerBeam3DGeometry geometry{
        {0.0, 0.0, 0.0},
        {2.0, 0.0, 0.0},
        {1.0, 0.0, 0.0}
    };

    try {
        (void)fesa::elements::euler_beam_3d_global_stiffness(geometry, section());
    } catch (const std::invalid_argument&) {
        return true;
    }
    return false;
}

bool throws_zero_orientation()
{
    const fesa::elements::EulerBeam3DGeometry geometry{
        {0.0, 0.0, 0.0},
        {2.0, 0.0, 0.0},
        {0.0, 0.0, 0.0}
    };

    try {
        (void)fesa::elements::euler_beam_3d_global_stiffness(geometry, section());
    } catch (const std::invalid_argument&) {
        return true;
    }
    return false;
}

} // namespace

int main()
{
    const auto beam_section = section();
    const fesa::elements::EulerBeam3DGeometry axis_aligned{
        {0.0, 0.0, 0.0},
        {2.0, 0.0, 0.0},
        {0.0, 1.0, 0.0}
    };

    const auto local = fesa::elements::euler_beam_3d_local_stiffness(2.0, beam_section);
    const auto global_identity =
        fesa::elements::euler_beam_3d_global_stiffness(axis_aligned, beam_section);
    if (!matrices_close(global_identity, local)) {
        return 1;
    }

    const fesa::elements::EulerBeam3DGeometry rotated{
        {0.0, 0.0, 0.0},
        {0.0, 2.0, 0.0},
        {1.0, 0.0, 0.0}
    };
    const auto rotated_stiffness =
        fesa::elements::euler_beam_3d_global_stiffness(rotated, beam_section);
    if (!symmetric(rotated_stiffness)) {
        return 2;
    }

    fesa::elements::Vector12 rigid_translation{};
    rigid_translation[0] = 0.2;
    rigid_translation[1] = -0.1;
    rigid_translation[2] = 0.3;
    rigid_translation[6] = 0.2;
    rigid_translation[7] = -0.1;
    rigid_translation[8] = 0.3;
    const auto rigid_forces =
        fesa::elements::euler_beam_3d_global_end_forces(rotated, beam_section, rigid_translation);
    for (double force : rigid_forces) {
        if (!close(force, 0.0)) {
            return 3;
        }
    }

    fesa::elements::Vector12 axial_extension{};
    axial_extension[6] = 0.1;
    const auto axial_forces = fesa::elements::euler_beam_3d_global_end_forces(
        axis_aligned,
        beam_section,
        axial_extension);
    if (!close(axial_forces[0], -31.5) ||
        !close(axial_forces[6], 31.5) ||
        !close(axial_forces[0] + axial_forces[6], 0.0)) {
        return 4;
    }

    if (!throws_parallel_orientation()) {
        return 5;
    }
    if (!throws_zero_orientation()) {
        return 6;
    }

    return 0;
}