#include <fesa/elements/euler_beam_3d.hpp>

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

namespace {

constexpr double tolerance = 1.0e-10;

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::Vector12 multiply(const fesa::elements::Matrix12& matrix,
                                  const fesa::elements::Vector12& vector)
{
    fesa::elements::Vector12 result{};
    for (int row = 0; row < 12; ++row) {
        for (int column = 0; column < 12; ++column) {
            result[static_cast<std::size_t>(row)] +=
                entry(matrix, row, column) * vector[static_cast<std::size_t>(column)];
        }
    }
    return result;
}

bool throws_invalid_length(const fesa::elements::EulerBeam3DSection& section)
{
    try {
        (void)fesa::elements::euler_beam_3d_local_stiffness(0.0, section);
    } catch (const std::invalid_argument&) {
        return true;
    }
    return false;
}

bool throws_invalid_section(fesa::elements::EulerBeam3DSection section)
{
    section.second_moment_z = 0.0;
    try {
        (void)fesa::elements::euler_beam_3d_local_stiffness(2.0, section);
    } catch (const std::invalid_argument&) {
        return true;
    }
    return false;
}

} // namespace

int main()
{
    const double length = 2.0;
    const fesa::elements::EulerBeam3DSection section{
        210.0,
        80.0,
        3.0,
        4.0,
        5.0,
        6.0
    };

    const auto stiffness = fesa::elements::euler_beam_3d_local_stiffness(length, section);

    if (!close(entry(stiffness, 0, 0), 315.0) ||
        !close(entry(stiffness, 0, 6), -315.0) ||
        !close(entry(stiffness, 6, 6), 315.0)) {
        return 1;
    }

    if (!close(entry(stiffness, 3, 3), 160.0) ||
        !close(entry(stiffness, 3, 9), -160.0) ||
        !close(entry(stiffness, 9, 9), 160.0)) {
        return 2;
    }

    if (!close(entry(stiffness, 1, 1), 1890.0) ||
        !close(entry(stiffness, 1, 5), 1890.0) ||
        !close(entry(stiffness, 5, 5), 2520.0)) {
        return 3;
    }

    if (!close(entry(stiffness, 2, 2), 1575.0) ||
        !close(entry(stiffness, 2, 4), -1575.0) ||
        !close(entry(stiffness, 4, 4), 2100.0)) {
        return 4;
    }

    for (int row = 0; row < 12; ++row) {
        for (int column = 0; column < 12; ++column) {
            if (!close(entry(stiffness, row, column), entry(stiffness, column, row))) {
                return 5;
            }
        }
    }

    fesa::elements::Vector12 displacement{};
    displacement[0] = 0.1;
    displacement[5] = 0.2;
    displacement[8] = -0.05;

    const auto expected_forces = multiply(stiffness, displacement);
    const auto recovered_forces =
        fesa::elements::euler_beam_3d_local_end_forces(length, section, displacement);
    for (std::size_t index = 0; index < expected_forces.size(); ++index) {
        if (!close(recovered_forces[index], expected_forces[index])) {
            return 6;
        }
    }

    if (!throws_invalid_length(section)) {
        return 7;
    }
    if (!throws_invalid_section(section)) {
        return 8;
    }

    return 0;
}