#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(B);
    handle.col(1).topRows<3>()                          = std::move(N);
    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>());
		if (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);
		}

    // 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