#include "ai/lane_registry.h"

void LaneRegistry::Add(int shooter, Vec2 from, Vec2 to, float reserve)
{
    if (count >= NUM_AGENTS) return;
    lanes[count++] = FireLane{from, to, shooter, reserve};
}

Vec2 LaneRegistry::AvoidanceForce(int agentIndex, Vec2 pos, float bodyRadius, float maxSpeed) const
{
    // Резерв шире, чем «тело уже в линии»: агент должен отходить ЗАРАНЕЕ,
    // иначе он успевает влезть под пулю, которая уже летит.
    const float wide = BlockRadius(bodyRadius) * g_tune.laneReserveScale;
    Vec2 force{};

    for (int i = 0; i < count; ++i)
    {
        const FireLane& lane = lanes[i];
        if (lane.shooter == agentIndex) continue;

        const float threshold = (lane.reserve > 0.0f) ? (bodyRadius + lane.reserve) : wide;
        const float d = DistancePointToSegment(pos, lane.from, lane.to);
        if (d >= threshold) continue;

        const Vec2 axis = Normalized(lane.to - lane.from);
        if (LengthSq(axis) < 0.5f) continue;

        // В сторону ближайшего края линии.
        const Vec2 n = Perp(axis);
        const float side = Dot(pos - lane.from, n);
        Vec2 away = (side >= 0.0f) ? n : Vec2{-n.x, -n.y};
        if (std::fabs(side) < 1e-4f)   // ровно на оси — разводим детерминированно
            away = (agentIndex & 1) ? n : Vec2{-n.x, -n.y};

        // Чем ближе к оси, тем сильнее; laneAvoidStrength делает отклик резче.
        const float strength = Clampf((1.0f - d / threshold) * g_tune.laneAvoidStrength, 0.0f, 1.0f);
        force += away * (maxSpeed * strength);
    }
    return force;
}

int LaneRegistry::CountBlocked(int exceptShooter, Vec2 pos, float bodyRadius,
                               bool shootingOnly) const
{
    const float narrow = BlockRadius(bodyRadius);
    int n = 0;
    for (int i = 0; i < count; ++i)
    {
        if (lanes[i].shooter == exceptShooter) continue;
        if (shootingOnly && lanes[i].reserve > 0.0f) continue;   // это сектор клинка
        const float threshold = (lanes[i].reserve > 0.0f) ? (bodyRadius + lanes[i].reserve) : narrow;
        if (DistancePointToSegment(pos, lanes[i].from, lanes[i].to) < threshold) ++n;
    }
    return n;
}