#include <fesa/fem/beam_frame.hpp>

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

namespace fesa {
namespace {

constexpr double kParallelTolerance =
    64.0 * std::numeric_limits<double>::epsilon();

double length(const Vec3 value) {
    return std::hypot(value.x, value.y, value.z);
}

double max_abs_component(const Vec3 value) {
    return std::max(
        std::abs(value.x),
        std::max(std::abs(value.y), std::abs(value.z)));
}

double dot(const Vec3 first, const Vec3 second) {
    return first.x * second.x + first.y * second.y + first.z * second.z;
}

Vec3 cross(const Vec3 first, const Vec3 second) {
    return {
        first.y * second.z - first.z * second.y,
        first.z * second.x - first.x * second.z,
        first.x * second.y - first.y * second.x,
    };
}

Vec3 normalized(const Vec3 value) {
    const double scale = max_abs_component(value);
    const Vec3 scaled{
        value.x / scale,
        value.y / scale,
        value.z / scale,
    };
    const double scaled_length = length(scaled);
    return {
        scaled.x / scaled_length,
        scaled.y / scaled_length,
        scaled.z / scaled_length,
    };
}

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

}  // namespace

BeamFrameResult make_beam_frame(
    const Vec3& first,
    const Vec3& second,
    const Vec3& orientation) {
    if (!is_finite(first) || !is_finite(second)) {
        return error_result(
            "model.nonfinite_value",
            "Beam frame requires finite node coordinates.");
    }

    const Vec3 axis{
        second.x - first.x,
        second.y - first.y,
        second.z - first.z,
    };
    if (!is_finite(axis)) {
        return error_result(
            "model.nonfinite_value",
            "Beam frame axis is not representable as a finite vector.");
    }

    if (max_abs_component(axis) == 0.0) {
        return error_result(
            "model.zero_length_element",
            "Beam frame requires distinct node coordinates.");
    }

    if (!is_finite(orientation)) {
        return error_result(
            "model.nonfinite_value",
            "Beam frame requires a finite orientation vector.");
    }

    if (max_abs_component(orientation) == 0.0) {
        return error_result(
            "model.invalid_orientation",
            "Beam frame requires a nonzero orientation vector.");
    }

    const Vec3 ex = normalized(axis);
    const Vec3 orientation_unit = normalized(orientation);
    const double axial_projection = dot(orientation_unit, ex);
    const Vec3 transverse{
        orientation_unit.x - axial_projection * ex.x,
        orientation_unit.y - axial_projection * ex.y,
        orientation_unit.z - axial_projection * ex.z,
    };
    const double transverse_length = length(transverse);
    if (transverse_length <= kParallelTolerance) {
        return error_result(
            "model.invalid_orientation",
            "Beam orientation is parallel to the element axis.");
    }

    const Vec3 ey = normalized(transverse);
    const Vec3 ez = cross(ex, ey);
    return {
        BeamFrame{ex, ey, ez},
        {},
    };
}

Matrix12 beam_transformation(const BeamFrame& frame) {
    Matrix12 transformation{};
    const std::array<Vec3, 3> basis{frame.ex, frame.ey, frame.ez};
    constexpr std::array<std::size_t, 4> block_offsets{0, 3, 6, 9};

    for (const std::size_t offset : block_offsets) {
        for (std::size_t row = 0; row < basis.size(); ++row) {
            transformation[offset + row][offset] = basis[row].x;
            transformation[offset + row][offset + 1] = basis[row].y;
            transformation[offset + row][offset + 2] = basis[row].z;
        }
    }

    return transformation;
}

}  // namespace fesa
