#include <fesa/elements/beam/beam3d2.hpp>

#include <array>
#include <cmath>
#include <cstddef>
#include <optional>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

#include <fesa/fem/gauss_rule.hpp>
#include <fesa/fem/line2_shape.hpp>

namespace fesa {
namespace {

using StrainMatrix =
    std::array<std::array<double, 12>, 6>;

BeamKernelResult error_result(std::string code, std::string message) {
    BeamKernelResult result;
    result.diagnostics.push_back({
        DiagnosticStage::model,
        Severity::error,
        std::move(code),
        std::move(message),
        std::nullopt,
    });
    return result;
}

bool is_positive_finite(const double value) {
    return std::isfinite(value) && value > 0.0;
}

std::array<double, 6> constitutive_values(
    const Beam3D2Input& input) {
    const double shear_modulus =
        input.material.young / (2.0 * (1.0 + input.material.poisson));
    return {
        input.material.young * input.section.area,
        shear_modulus * input.section.shear_area_y,
        shear_modulus * input.section.shear_area_z,
        shear_modulus * input.section.torsion_j,
        input.material.young * input.section.iy,
        input.material.young * input.section.iz,
    };
}

std::optional<BeamKernelResult> validate_properties(
    const Beam3D2Input& input) {
    if (!std::isfinite(input.material.young) ||
        !std::isfinite(input.material.poisson)) {
        return error_result(
            "model.nonfinite_value",
            "Beam kernel requires finite elastic constants.");
    }
    if (input.material.young <= 0.0 ||
        input.material.poisson <= -1.0 ||
        input.material.poisson >= 0.5) {
        return error_result(
            "model.invalid_material",
            "Beam kernel requires E > 0 and -1 < nu < 0.5.");
    }

    if (!is_positive_finite(input.section.area) ||
        !is_positive_finite(input.section.iy) ||
        !is_positive_finite(input.section.iz) ||
        !is_positive_finite(input.section.torsion_j) ||
        !is_positive_finite(input.section.shear_area_y) ||
        !is_positive_finite(input.section.shear_area_z)) {
        return error_result(
            "model.invalid_section",
            "Beam kernel requires positive finite section properties.");
    }
    return std::nullopt;
}

StrainMatrix strain_matrix(
    const double xi,
    const double jacobian) {
    StrainMatrix strain{};
    const auto shape = line2_shape(xi);
    const auto natural_derivative = line2_shape_derivative(xi);

    for (std::size_t node = 0; node < shape.size(); ++node) {
        const std::size_t offset = node * 6;
        const double derivative = natural_derivative[node] / jacobian;
        strain[0][offset] = derivative;
        strain[1][offset + 1] = derivative;
        strain[1][offset + 5] = -shape[node];
        strain[2][offset + 2] = derivative;
        strain[2][offset + 4] = shape[node];
        strain[3][offset + 3] = derivative;
        strain[4][offset + 4] = derivative;
        strain[5][offset + 5] = derivative;
    }
    return strain;
}

template <std::size_t ComponentCount>
void integrate_components(
    Matrix12& stiffness,
    const std::span<const GaussPoint1D> rule,
    const std::array<std::size_t, ComponentCount>& components,
    const std::array<double, 6>& constitutive,
    const double jacobian) {
    for (const GaussPoint1D& point : rule) {
        const StrainMatrix strain = strain_matrix(point.xi, jacobian);
        const double integration_weight = jacobian * point.weight;
        for (std::size_t row = 0; row < stiffness.size(); ++row) {
            for (std::size_t column = row;
                 column < stiffness[row].size();
                 ++column) {
                double entry = 0.0;
                for (const std::size_t component : components) {
                    entry +=
                        strain[component][row] *
                        constitutive[component] *
                        strain[component][column];
                }
                stiffness[row][column] +=
                    entry * integration_weight;
            }
        }
    }
}

void mirror_upper_triangle(Matrix12& matrix) {
    for (std::size_t row = 0; row < matrix.size(); ++row) {
        for (std::size_t column = row + 1;
             column < matrix[row].size();
             ++column) {
            matrix[column][row] = matrix[row][column];
        }
    }
}

Matrix12 transform_stiffness(
    const Matrix12& local,
    const Matrix12& transformation) {
    Matrix12 global{};
    for (std::size_t row = 0; row < global.size(); ++row) {
        for (std::size_t column = row;
             column < global[row].size();
             ++column) {
            double entry = 0.0;
            for (std::size_t local_row = 0;
                 local_row < local.size();
                 ++local_row) {
                for (std::size_t local_column = 0;
                     local_column < local[local_row].size();
                     ++local_column) {
                    entry +=
                        transformation[local_row][row] *
                        local[local_row][local_column] *
                        transformation[local_column][column];
                }
            }
            global[row][column] = entry;
        }
    }
    mirror_upper_triangle(global);
    return global;
}

std::array<double, 12> transform_displacement(
    const Matrix12& transformation,
    const std::span<const double, 12> global_displacement) {
    std::array<double, 12> local_displacement{};
    for (std::size_t row = 0; row < transformation.size(); ++row) {
        for (std::size_t column = 0;
             column < transformation[row].size();
             ++column) {
            local_displacement[row] +=
                transformation[row][column] * global_displacement[column];
        }
    }
    return local_displacement;
}

std::array<double, 6> evaluate_strain(
    const StrainMatrix& strain_matrix,
    const std::array<double, 12>& local_displacement) {
    std::array<double, 6> strain{};
    for (std::size_t component = 0; component < strain.size(); ++component) {
        for (std::size_t dof = 0; dof < local_displacement.size(); ++dof) {
            strain[component] +=
                strain_matrix[component][dof] * local_displacement[dof];
        }
    }
    return strain;
}

bool is_finite(const Matrix12& matrix) {
    for (const auto& row : matrix) {
        for (const double value : row) {
            if (!std::isfinite(value)) {
                return false;
            }
        }
    }
    return true;
}

}  // namespace

BeamKernelResult compute_beam3d2(const Beam3D2Input& input) {
    if (const auto validation = validate_properties(input);
        validation.has_value()) {
        return *validation;
    }

    BeamFrameResult frame_result = make_beam_frame(
        input.coordinates[0],
        input.coordinates[1],
        input.section.orientation);
    if (!frame_result.frame.has_value()) {
        return {std::nullopt, std::move(frame_result.diagnostics)};
    }

    const Vec3 axis{
        input.coordinates[1].x - input.coordinates[0].x,
        input.coordinates[1].y - input.coordinates[0].y,
        input.coordinates[1].z - input.coordinates[0].z,
    };
    const double length = std::hypot(axis.x, axis.y, axis.z);
    const double jacobian = length / 2.0;
    if (!std::isfinite(jacobian) || jacobian <= 0.0) {
        return error_result(
            "model.zero_length_element",
            "Beam kernel requires a representable positive Jacobian.");
    }

    const std::array<double, 6> constitutive =
        constitutive_values(input);
    for (const double value : constitutive) {
        if (!is_positive_finite(value)) {
            return error_result(
                "model.nonfinite_value",
                "Beam constitutive stiffness is not finite and positive.");
        }
    }

    Matrix12 local{};
    integrate_components(
        local,
        gauss_rule_1d(2),
        std::array<std::size_t, 4>{0, 3, 4, 5},
        constitutive,
        jacobian);
    integrate_components(
        local,
        gauss_rule_1d(1),
        std::array<std::size_t, 2>{1, 2},
        constitutive,
        jacobian);
    mirror_upper_triangle(local);

    const Matrix12 transformation =
        beam_transformation(*frame_result.frame);
    Matrix12 global = transform_stiffness(local, transformation);
    if (!is_finite(local) || !is_finite(global)) {
        return error_result(
            "model.nonfinite_value",
            "Beam stiffness contains a nonfinite value.");
    }

    return {
        Beam3D2Contribution{
            std::move(local),
            std::move(global),
            *frame_result.frame,
        },
        {},
    };
}

std::vector<BeamSectionResult> recover_beam3d2(
    const Beam3D2Input& input,
    const std::span<const double, 12> element_displacement,
    const std::span<const std::array<double, 2>> recovery_points) {
    const BeamKernelResult kernel = compute_beam3d2(input);
    if (!kernel.contribution.has_value()) {
        const std::string message = kernel.diagnostics.empty()
            ? "Beam recovery requires a valid Beam3D2 input."
            : kernel.diagnostics.front().message;
        throw std::invalid_argument{message};
    }

    const Vec3 axis{
        input.coordinates[1].x - input.coordinates[0].x,
        input.coordinates[1].y - input.coordinates[0].y,
        input.coordinates[1].z - input.coordinates[0].z,
    };
    const double jacobian = std::hypot(axis.x, axis.y, axis.z) / 2.0;
    const Matrix12 transformation =
        beam_transformation(kernel.contribution->frame);
    const std::array<double, 12> local_displacement =
        transform_displacement(transformation, element_displacement);
    const std::array<double, 6> center_strain = evaluate_strain(
        strain_matrix(0.0, jacobian),
        local_displacement);
    const std::array<double, 6> constitutive =
        constitutive_values(input);

    std::vector<BeamSectionResult> results;
    results.reserve(input.node_ids.size());
    for (std::size_t end = 0; end < input.node_ids.size(); ++end) {
        const double xi = end == 0 ? -1.0 : 1.0;
        std::array<double, 6> section_strain = evaluate_strain(
            strain_matrix(xi, jacobian),
            local_displacement);
        section_strain[1] = center_strain[1];
        section_strain[2] = center_strain[2];

        std::array<double, 6> section_force{};
        for (std::size_t component = 0;
             component < section_force.size();
             ++component) {
            section_force[component] =
                constitutive[component] * section_strain[component];
        }

        std::vector<double> sigma_xx;
        sigma_xx.reserve(recovery_points.size());
        for (const auto& point : recovery_points) {
            const double y = point[0];
            const double z = point[1];
            sigma_xx.push_back(
                input.material.young *
                (section_strain[0] + z * section_strain[4] -
                 y * section_strain[5]));
        }

        results.push_back({
            xi,
            input.node_ids[end],
            section_strain,
            section_force,
            section_force[0] / input.section.area,
            std::move(sigma_xx),
        });
    }
    return results;
}

}  // namespace fesa