"""Tests for buffer allocation (strategy-layer temporary parking).

Uses a headless sim so footprint/reachability queries hit real PyBullet, but
objects are teleported into controlled layouts.
"""

import math
import os
import sys

import pybullet as pb
import pytest

sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))

from llm_panda import config


@pytest.fixture
def sim():
    from main import PandaSimulation
    simulation = PandaSimulation(gui=False, seed=3)
    yield simulation
    simulation.disconnect()


def _isolate(sim, keep):
    """Park every object except those named in `keep` far off the table."""
    for oid in sim.scene.object_ids:
        if sim.scene.get_name(oid) not in keep:
            pb.resetBasePositionAndOrientation(oid, [0.5, 0.62, 0.34], [0, 0, 0, 1])


# --------------------------------------------------------------------------- #
# Strategy: buffer allocation
# --------------------------------------------------------------------------- #
def test_buffer_spot_is_clear_of_avoided_object(sim):
    _isolate(sim, {"banana"})
    ban = sim.scene.name_to_id("banana")
    ban_pos = [0.50, 0.0, 0.33]
    pb.resetBasePositionAndOrientation(ban, ban_pos, [0, 0, 0, 1])
    for _ in range(30):
        pb.stepSimulation()
    spot = sim.scene.find_buffer_spot(
        (0.08, 0.08), exclude_ids=[], avoid_positions=[ban_pos],
    )
    assert spot is not None
    d = math.dist(spot[:2], ban_pos[:2])
    assert d >= config.BUFFER_MIN_CLEARANCE - 1e-6
    # And it must be a real spot on the table top.
    assert abs(spot[2] - config.TABLE_TOP_Z) < 1e-6