#include "internal_primitive_desc.hpp"

namespace internal
{
extrude_helixline::extrude_helixline(const extrude_helixline_descriptor_t &desc, aabb_t<> &aabb)
    : axis(desc.axis, this->axis_to_world, aabb)
{
    const auto           &matrix_handle = this->axis_to_world.matrix();
    const Eigen::Vector3d N             = -matrix_handle.col(0);
    const Eigen::Vector3d T             = (matrix_handle.col(2) * this->axis.height //
                               + this->axis.radius * this->axis.total_theta * matrix_handle.col(1))
                                  .normalized();
    const Eigen::Vector3d B = N.cross(T);

    aabb_t<2> profile_aabb{};
    this->profile.build_as_profile(desc.profiles[0], B, N, matrix_handle.col(3), profile_aabb);
    const auto profile_max_extent  = profile_aabb.max().cwiseMax(profile_aabb.min().cwiseAbs());
    aabb.min()                    -= Eigen::Vector3d{profile_max_extent.x(), profile_max_extent.x(), profile_max_extent.y()};
    aabb.max()                    += Eigen::Vector3d{profile_max_extent.x(), profile_max_extent.x(), profile_max_extent.y()};
    aabb.transform(this->axis_to_world);
}

extrude_helixline::extrude_helixline(extrude_helixline_descriptor_t &&desc, aabb_t<> &aabb)
    : axis(std::move(desc.axis), this->axis_to_world, aabb)
{
    const auto           &matrix_handle = this->axis_to_world.matrix();
    const Eigen::Vector3d N             = -matrix_handle.col(0);
    const Eigen::Vector3d T             = (matrix_handle.col(2) * this->axis.height //
                               + this->axis.radius * this->axis.total_theta * matrix_handle.col(1))
                                  .normalized();
    const Eigen::Vector3d B = N.cross(T);

    aabb_t<2> profile_aabb{};
    this->profile.build_as_profile(std::move(desc.profiles[0]), B, N, matrix_handle.col(3), profile_aabb);
    const auto profile_max_extent  = profile_aabb.max().cwiseMax(profile_aabb.min().cwiseAbs());
    aabb.min()                    -= Eigen::Vector3d{profile_max_extent.x(), profile_max_extent.x(), profile_max_extent.y()};
    aabb.max()                    += Eigen::Vector3d{profile_max_extent.x(), profile_max_extent.x(), profile_max_extent.y()};
    aabb.transform(this->axis_to_world);
}

static inline auto get_local_TBN(const helixline &line, const Eigen::Ref<const Eigen::Vector3d> &q, double t)
{
    Eigen::Transform<double, 3, Eigen::Isometry> TBN{};
    auto                                        &handle = TBN.matrix();
    const auto                                   T      = line.calculate_tangent(t);
    const auto                                   N      = line.calculate_normal(t);
    const auto                                   B      = N.cross(T);
    handle.col(0).topRows<3>()                          = std::move(N);
    handle.col(1).topRows<3>()                          = std::move(B);
    handle.col(2).topRows<3>()                          = std::move(T);
    handle.col(3).topRows<3>()                          = std::move(q);

    return TBN;
}

template <typename Routine>
static inline process_return_type_t<Routine> evaluate(const extrude_helixline                                  &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, 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
    auto [profile_closest_param, dist] = desc.profile.calculate_closest_param((inv_TBN * local_p).topRows<3>());

    bool isOut                  = desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param);
    profile_closest_param.point = TBN * profile_closest_param.point;
    if (!isOut) {
        // inside or projected point inside
        const auto local_p_3d = local_p.topRows<3>() / local_p[3];
        if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) {
            // inside
            // max() with two planes
            dist                            = -dist;
            Eigen::Vector3d start_tangent   = desc.axis.calculate_tangent(0);
            auto            sdf_start_plane = -start_tangent.dot(local_p_3d);
            if (sdf_start_plane > dist) {
                dist                        = sdf_start_plane;
                profile_closest_param.point = local_p_3d + start_tangent * dist;
            } else {
                Eigen::Vector3d final_v       = desc.axis.evaluate(1);
                Eigen::Vector3d final_tangent = desc.axis.calculate_tangent(1);
                auto            sdf_end_plane = final_tangent.dot(local_p_3d - final_v);
                if (sdf_end_plane > dist) {
                    dist                        = sdf_end_plane;
                    profile_closest_param.point = local_p_3d - final_tangent * dist;
                }
            }
        } else {
            // outside, must closer to the plane
            if (axis_closest_param.t < EPSILON) {
                Eigen::Vector3d start_tangent   = desc.axis.calculate_tangent(0);
                auto            sdf_start_plane = -start_tangent.dot(local_p_3d);
                dist                            = sdf_start_plane;
                profile_closest_param.point     = local_p_3d + start_tangent * dist;
            } else {
                Eigen::Vector3d final_v       = desc.axis.evaluate(1);
                Eigen::Vector3d final_tangent = desc.axis.calculate_tangent(1);
                auto            sdf_end_plane = final_tangent.dot(local_p_3d - final_v);
                dist                          = sdf_end_plane;
                profile_closest_param.point   = local_p_3d - final_tangent * dist;
            }
        }
    } else {
        // outside, nothing to do
    }


    // if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) {
    //     // if !is_peak_value, the point is two sides away, and pmc is always true
    //     dist *= desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param) * 2 - 1;
    // }

    // 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_helixline::evaluate_sdf([[maybe_unused]] const Eigen::Ref<const Eigen::Vector3d> &point) const
{
    return evaluate<evaluation_routine_tag>(*this, point);
}

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