#include "internal_primitive_desc.hpp"

namespace internal
{
extrude_polyline::extrude_polyline(const extrude_polyline_descriptor_t &desc, aabb_t<> &aabb)
{
    this->axis.build_as_axis(desc.axis, desc.profiles[0].reference_normal, this->axis_to_world, aabb);
    const auto &matrix_handle = this->axis_to_world.matrix();
    aabb_t<2>   profile_aabb{};
    this->profile.build_as_profile(desc.profiles[0],
                                   matrix_handle.col(0),
                                   matrix_handle.col(1),
                                   matrix_handle.col(3),
                                   profile_aabb);
    const auto profile_max_extent  = profile_aabb.max().cwiseMax(profile_aabb.min().cwiseAbs()).maxCoeff();
    aabb.min().array()            -= profile_max_extent;
    aabb.max().array()            += profile_max_extent;
}

extrude_polyline::extrude_polyline(extrude_polyline_descriptor_t &&desc, aabb_t<> &aabb)
{
    this->axis.build_as_axis(std::move(desc.axis), desc.profiles[0].reference_normal, this->axis_to_world, aabb);
    const auto &matrix_handle = this->axis_to_world.matrix();
    aabb_t<2>   profile_aabb{};
    this->profile.build_as_profile(std::move(desc.profiles[0]),
                                   matrix_handle.col(0),
                                   matrix_handle.col(1),
                                   matrix_handle.col(3),
                                   profile_aabb);
    const auto profile_max_extent  = profile_aabb.max().cwiseMax(profile_aabb.min().cwiseAbs()).maxCoeff();
    aabb.min().array()            -= profile_max_extent;
    aabb.max().array()            += profile_max_extent;
}

// CAUTION: take care of relations between line's local TBN and plane's local TBN
// CAUTION: all internal calculations of polyline should assume that p is in axis/plane's local TBN space
static inline auto get_local_TBN(const polyline                          &line,
                                 const Eigen::Ref<const Eigen::Vector4d> &p,
                                 const Eigen::Ref<const Eigen::Vector3d> &q,
                                 double                                   t)
{
    Eigen::Vector2d local_tangent, local_normal;
    if (t >= 0 && t <= line.start_indices.size() && std::abs(t - std::round(t)) <= EPSILON) {
        // if between two segments, use local vec{pq} on the plane as normal
        local_normal = (q.topRows<2>() - p.topRows<2>()).normalized();
        if (local_normal.dot(line.calculate_normal(t)) < 0) local_normal *= -1;
        local_tangent = {local_normal[1], -local_normal[0]};
    } else {
        local_tangent = line.calculate_tangent(t);
        local_normal  = line.calculate_normal(t);
    }

    Eigen::Transform<double, 3, Eigen::Isometry> TBN{};
    auto                                        &handle = TBN.matrix();
    handle.col(0)[2]                                    = 1;
    handle.col(1).topRows<2>()                          = std::move(local_normal);
    handle.col(2).topRows<2>()                          = std::move(local_tangent);
    handle.col(3).topRows<3>()                          = std::move(q);

    return TBN;
}

template <typename Routine>
static inline process_return_type_t<Routine> evaluate(const extrude_polyline                                   &desc,
                                                      [[maybe_unused]] const Eigen::Ref<const Eigen::Vector3d> &point)
{
    const auto world_to_axis           = inversed_affine_transform(desc.axis_to_world);
    const auto local_p                 = world_to_axis * Eigen::Vector4d{point[0], point[1], point[2], 1};
    const auto [axis_closest_param, _] = desc.axis.calculate_closest_param(local_p.topRows<3>());
    const auto TBN                     = get_local_TBN(desc.axis, local_p, axis_closest_param.point, axis_closest_param.t);
    const auto inv_TBN                 = inversed_affine_transform(TBN);

    // TODO: add support for non-parallel (ray vs. profile plane) case
    // for now just assume that profile plane is parallel to axis
    const auto [profile_closest_param, dist] = desc.profile.calculate_closest_param((inv_TBN * local_p).topRows<3>());

    // transform closest point to world space
    // the distance should be unchanged during transformation, since no scaling is involved
    if constexpr (std::is_same_v<Routine, evaluation_routine_tag>)
        return dist;
    else
        return desc.axis_to_world * TBN * profile_closest_param.point;
}

double extrude_polyline::evaluate_sdf([[maybe_unused]] const Eigen::Ref<const Eigen::Vector3d> &point) const
{
    return evaluate<evaluation_routine_tag>(*this, point);
}

Eigen::Vector3d extrude_polyline::evaluate_cpm([[maybe_unused]] const Eigen::Ref<const Eigen::Vector3d> &point) const
{
    return evaluate<closest_point_routine_tag>(*this, point);
}
} // namespace internal