#include "paddle.hpp"

#include "general.hpp"

#include <cmath>


using namespace frog::geo;


std::optional<vec3> frog::geo::collision_2D(geo::paddle paddle,
                                            cylindrical3 coords,
                                            float radius)
{
    if (coords.y < paddle.pos.y || coords.y > paddle.pos.y + paddle.height)
        return std::nullopt;

    if (coords.r + radius < paddle.pos.r)
        return std::nullopt;

    if (coords.r - radius > paddle.pos.r + paddle.thickness)
        return std::nullopt;

    float diff = angle_diff(coords.theta,
                           paddle.pos.theta + paddle.angular_size / 2);

    float angular_radius = std::atan2(radius, coords.r);

    if (std::abs(diff) > paddle.angular_size / 2 + angular_radius)
        return std::nullopt;

    if (std::abs(diff) > paddle.angular_size / 2)
    {
        vec3 up = { 0, 1, 0 };

        vec3 vec = geo::to_cartesian(coords);

        vec3 normal = vec.cross(up) * (diff > 0 ? -1 : 1);

        return { normal };
    }

    bool inside = coords.r < paddle.pos.r + paddle.thickness / 2;

    vec3 normal = geo::to_cartesian(coords) * (inside ? -1 : 1);

    normal.y() = 0;

    return { normal.normalized() };
}
