#include "internal_primitive_desc.hpp"

namespace internal
{
cylinder::cylinder(const cylinder_descriptor_t &desc, aabb_t<> &aabb)
{
    vec3d_conversion(desc.bottom_origion, this->bottom_center);
    vec3d_conversion(desc.offset, this->offset);
    this->radius = desc.radius;

    // NOTE: A rough AABB bounding box
    const auto radius_vec = Eigen::Vector3d{radius, radius, radius};
    aabb                  = {bottom_center - radius_vec, bottom_center + radius_vec};
    aabb.extend(bottom_center + offset + radius_vec);
    aabb.extend(bottom_center + offset - radius_vec);
}

cylinder::cylinder(cylinder_descriptor_t &&desc, aabb_t<> &aabb)
{
    vec3d_conversion(std::move(desc.bottom_origion), this->bottom_center);
    vec3d_conversion(std::move(desc.offset), this->offset);
    this->radius = std::move(desc.radius);

    // NOTE: A rough AABB bounding box
    const auto radius_vec = Eigen::Vector3d{radius, radius, radius};
    aabb                  = {bottom_center - radius_vec, bottom_center + radius_vec};
    aabb.extend(bottom_center + offset + radius_vec);
    aabb.extend(bottom_center + offset - radius_vec);
}

// TODO: CPM needs to be reviewed and optimized.
template <typename Routine>
static inline process_return_type_t<Routine> evaluate(const cylinder                                           &desc,
                                                      [[maybe_unused]] const Eigen::Ref<const Eigen::Vector3d> &point)
{
    Eigen::Vector3d ba   = -desc.offset;
    Eigen::Vector3d pa   = point - (desc.bottom_center + desc.offset);
    auto            baba = ba.squaredNorm();
    auto            paba = pa.dot(ba);
    auto            x    = (pa * baba - ba * paba).norm() - desc.radius * baba;
    auto            y    = abs(paba - baba * 0.5) - baba * 0.5;
    auto            x2   = x * x;
    auto            y2   = y * y * baba;
    auto            d    = (std::max(x, y) < 0.0) ? -std::min(x2, y2) : (((x > 0.0) ? x2 : 0.0) + ((y > 0.0) ? y2 : 0.0));

    if constexpr (is_evaluation_routine_v<Routine>)
        return sign(d) * std::sqrt(abs(d)) / baba;
    else
        return desc.bottom_center + desc.offset + ba * x + pa * y;
}

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

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