"""Tests for placement tool."""

from __future__ import annotations

import pytest

from godotiq.tools.spatial.placement import (
    PlacementSuggestion,
    compute_placement,
    _apply_default_constraints,
    _calculate_clearance,
    _calculate_scale,
    _check_constraints,
    _direction_to_rotation,
    _distance_3d,
    _find_empty_markers,
    _find_excluded_zones,
    _grid_search,
    _infer_marker_groups,
    _resolve_reference,
)


# ---------------------------------------------------------------------------
# Fixtures
# ---------------------------------------------------------------------------


def _marker(name: str, pos: list[float], groups: list[str] | None = None, children: int = 0) -> dict:
    return {
        "name": name,
        "type": "Marker3D",
        "position": pos,
        "scale": [1, 1, 1],
        "depth": 1,
        "groups": groups or [],
        "children_count": children,
    }


def _node(name: str, ntype: str, pos: list[float], scale: list[float] | None = None, depth: int = 1) -> dict:
    return {
        "name": name,
        "type": ntype,
        "position": pos,
        "scale": scale or [1, 1, 1],
        "depth": depth,
        "groups": [],
        "children_count": 0,
    }


def _wall(name: str, pos: list[float]) -> dict:
    return _node(name, "StaticBody3D", pos)


# ---------------------------------------------------------------------------
# TestPlacement
# ---------------------------------------------------------------------------


class TestPlacement:
    def test_find_empty_marker(self):
        """Empty Marker3D near reference should be top suggestion."""
        nodes = [
            _marker("PrinterSlot_P3", [0, 0, 0], ["printer_slots"]),
            _marker("PrinterSlot_P5", [2, 0, 0], ["printer_slots"], children=0),
        ]
        result = compute_placement(nodes, near="PrinterSlot_P3", object_type="printer")
        assert len(result["suggestions"]) > 0
        top = result["suggestions"][0]
        assert top["source"] == "marker"
        assert top["confidence"] >= 0.85

    def test_occupied_marker_skipped(self):
        """Marker with children_count > 0 should be skipped."""
        nodes = [
            _marker("PrinterSlot_P3", [0, 0, 0], ["printer_slots"]),
            _marker("PrinterSlot_P5", [2, 0, 0], ["printer_slots"], children=3),
        ]
        result = compute_placement(nodes, near="PrinterSlot_P3", object_type="printer")
        marker_suggestions = [s for s in result["suggestions"] if s["source"] == "marker"]
        # P3 is the reference, P5 is occupied — no marker suggestions except P3 itself
        for s in marker_suggestions:
            assert "PrinterSlot_P5" not in s["reason"]

    def test_grid_search_avoids_walls(self):
        """Grid candidates near walls should be filtered out."""
        nodes = [
            _node("Ref", "Node3D", [0, 0, 0]),
            _wall("Wall_N", [0, 0, -0.2]),  # wall very close to north
        ]
        result = compute_placement(nodes, near="Ref")
        # No suggestion should be at the wall position
        for s in result["suggestions"]:
            pos = s["position"]
            # Should not be within 0.3m of wall at z=-0.2
            dist = ((pos[0] - 0) ** 2 + (pos[2] - (-0.2)) ** 2) ** 0.5
            assert dist >= 0.3 or s["source"] == "marker"

    def test_grid_search_avoids_overlap(self):
        """Grid candidates overlapping existing nodes should be filtered out."""
        nodes = [
            _node("Ref", "Node3D", [0, 0, 0]),
            _node("Blocker", "MeshInstance3D", [0.5, 0, 0]),
        ]
        result = compute_placement(nodes, near="Ref")
        for s in result["suggestions"]:
            pos = s["position"]
            dist = ((pos[0] - 0.5) ** 2 + (pos[2] - 0) ** 2) ** 0.5
            assert dist >= 0.4 or s["source"] == "marker"

    def test_marker_higher_confidence_than_grid(self):
        """Marker suggestions should always outrank grid suggestions."""
        nodes = [
            _node("Ref", "Node3D", [0, 0, 0]),
            _marker("Slot_1", [1, 0, 0], children=0),
        ]
        result = compute_placement(nodes, near="Ref")
        markers = [s for s in result["suggestions"] if s["source"] == "marker"]
        grids = [s for s in result["suggestions"] if s["source"] == "grid_search"]
        if markers and grids:
            assert markers[0]["confidence"] > grids[0]["confidence"]

    def test_scale_from_similar_objects(self):
        """Scale should be averaged from similar objects in scene."""
        nodes = [
            _node("Ref", "Node3D", [0, 0, 0]),
            _node("Printer_1", "Node3D", [5, 0, 0], scale=[2, 2, 2]),
            _node("Printer_2", "Node3D", [10, 0, 0], scale=[4, 4, 4]),
        ]
        result = compute_placement(nodes, near="Ref", object_type="printer")
        if result["suggestions"]:
            assert result["suggestions"][0]["scale"] == [3.0, 3.0, 3.0]
        assert "2" in result["scale_reasoning"]

    def test_no_similar_objects_default_scale(self):
        """No similar objects → scale [1,1,1]."""
        nodes = [_node("Ref", "Node3D", [0, 0, 0])]
        result = compute_placement(nodes, near="Ref", object_type="alien_spaceship")
        assert result["scale_reasoning"]
        if result["suggestions"]:
            assert result["suggestions"][0]["scale"] == [1, 1, 1]

    def test_face_direction_south(self):
        """face_direction='south' → rotation [0, 180, 0]."""
        nodes = [
            _node("Ref", "Node3D", [0, 0, 0]),
            _marker("Slot", [2, 0, 0], children=0),
        ]
        result = compute_placement(
            nodes, near="Ref", constraints={"face_direction": "south"}
        )
        if result["suggestions"]:
            assert result["suggestions"][0]["rotation_deg"] == [0, 180, 0]

    def test_reference_not_found(self):
        """Unknown reference node → error."""
        nodes = [_node("Ref", "Node3D", [0, 0, 0])]
        result = compute_placement(nodes, near="NonExistent")
        assert "error" in result

    def test_excluded_zones_detected(self):
        """LockedZone nodes should appear in excluded_zones."""
        nodes = [
            _node("Ref", "Node3D", [0, 0, 0]),
            _node("LockedZone_RnD", "Node3D", [3, 0, -4]),
        ]
        result = compute_placement(nodes, near="Ref")
        assert len(result["excluded_zones"]) >= 1
        assert "LockedZone" in result["excluded_zones"][0]["reason"]

    def test_clearance_calculation(self):
        """Clearance should report distance in 4 directions."""
        nodes = [
            _wall("Wall_E", [5, 0, 0]),
        ]
        clearance = _calculate_clearance([0, 0, 0], nodes)
        assert clearance["E"] < 10.0
        assert clearance["E"] == 5.0

    def test_on_floor_constraint(self):
        """on_floor=true → Y coordinate forced to 0."""
        nodes = [
            _node("Ref", "Node3D", [0, 5, 0]),  # ref at Y=5
        ]
        result = compute_placement(nodes, near="Ref", constraints={"on_floor": True})
        for s in result["suggestions"]:
            assert s["position"][1] == 0.0

    def test_max_suggestions_limit(self):
        """Never return more than max_suggestions."""
        nodes = [_node("Ref", "Node3D", [0, 0, 0])]
        result = compute_placement(nodes, near="Ref", max_suggestions=1)
        assert len(result["suggestions"]) <= 1

    def test_fuzzy_reference_match(self):
        """Partial name match should find reference node."""
        nodes = [_node("PrinterSlot_P3", "Marker3D", [5, 0, 3])]
        result = compute_placement(nodes, near="PrinterSlot")
        assert "error" not in result or "not found" not in result.get("error", "")
        assert result["reference_position"] == [5, 0, 3]

    def test_empty_nodes_error(self):
        """Empty world_nodes list → error."""
        result = compute_placement([], near="Ref")
        assert "error" in result

    def test_near_position_direct(self):
        """near_position takes precedence over near."""
        nodes = [_node("Ref", "Node3D", [0, 0, 0])]
        result = compute_placement(nodes, near_position=[10, 0, 10])
        assert result["reference_position"] == [10, 0, 10]

    def test_no_floor_constraint(self):
        """on_floor=false → Y coordinate preserved from grid search."""
        nodes = [
            _node("Ref", "Node3D", [0, 3, 0]),
        ]
        result = compute_placement(nodes, near="Ref", constraints={"on_floor": False})
        # At least some suggestions should preserve the Y=3 reference height
        if result["suggestions"]:
            for s in result["suggestions"]:
                assert s["position"][1] == 3


# ---------------------------------------------------------------------------
# TestHelpers
# ---------------------------------------------------------------------------


class TestHelpers:
    def test_infer_marker_groups_printer(self):
        groups = _infer_marker_groups("printer")
        assert "printer_slots" in groups

    def test_infer_marker_groups_unknown(self):
        groups = _infer_marker_groups("alien_spaceship")
        assert groups == []

    def test_infer_marker_groups_shelf(self):
        groups = _infer_marker_groups("shelf")
        assert "furniture_shelf_slots" in groups

    def test_direction_to_rotation(self):
        assert _direction_to_rotation("south") == [0, 180, 0]
        assert _direction_to_rotation("east") == [0, 90, 0]
        assert _direction_to_rotation("north") == [0, 0, 0]
        assert _direction_to_rotation("west") == [0, -90, 0]

    def test_direction_to_rotation_unknown(self):
        assert _direction_to_rotation("up") == [0, 0, 0]

    def test_clearance_with_walls(self):
        """Wall to the east should reduce east clearance."""
        nodes = [_wall("EastWall", [3, 0, 0])]
        c = _calculate_clearance([0, 0, 0], nodes)
        assert c["E"] == 3.0
        assert c["W"] == 10.0

    def test_excluded_zones_locked(self):
        nodes = [{"name": "LockedZone_RnD", "position": [3, 0, -4]}]
        zones = _find_excluded_zones(nodes)
        assert len(zones) == 1
        assert "LockedZone" in zones[0]["reason"]

    def test_excluded_zones_door(self):
        nodes = [{"name": "Door_Main", "position": [1, 0, 0]}]
        zones = _find_excluded_zones(nodes)
        assert len(zones) == 1
        assert "Door" in zones[0]["reason"]
        assert zones[0]["radius"] == 0.8

    def test_distance_3d(self):
        assert _distance_3d([0, 0, 0], [3, 4, 0]) == 5.0
        assert _distance_3d([1, 1, 1], [1, 1, 1]) == 0.0

    def test_distance_3d_short_list(self):
        assert _distance_3d([0], [1, 2, 3]) == float("inf")

    def test_resolve_reference_exact(self):
        nodes = [{"name": "Target", "position": [1, 2, 3]}]
        assert _resolve_reference("Target", None, nodes) == [1, 2, 3]

    def test_resolve_reference_position(self):
        assert _resolve_reference("", [5, 6, 7], []) == [5, 6, 7]

    def test_resolve_reference_not_found(self):
        assert _resolve_reference("Missing", None, []) is None

    def test_apply_default_constraints(self):
        c = _apply_default_constraints({})
        assert c["on_floor"] is True
        assert c["min_wall_distance"] == 0.3
        assert c["min_object_distance"] == 0.4

    def test_apply_custom_constraints(self):
        c = _apply_default_constraints({"on_floor": False, "min_wall_distance": 1.0})
        assert c["on_floor"] is False
        assert c["min_wall_distance"] == 1.0

    def test_check_constraints_blocked_by_wall(self):
        nodes = [_wall("Wall", [0.1, 0, 0])]
        result = _check_constraints([0, 0, 0], nodes, {"min_wall_distance": 0.3, "min_object_distance": 0.4})
        assert result["blocked"] is True

    def test_check_constraints_clear(self):
        nodes = [_wall("Wall", [5, 0, 0])]
        result = _check_constraints([0, 0, 0], nodes, {"min_wall_distance": 0.3, "min_object_distance": 0.4})
        assert result["blocked"] is False

    def test_calculate_scale_average(self):
        nodes = [
            _node("Printer_1", "Node3D", [0, 0, 0], scale=[2, 4, 6]),
            _node("Printer_2", "Node3D", [5, 0, 0], scale=[4, 6, 8]),
        ]
        info = _calculate_scale("printer", nodes)
        assert info["recommended_scale"] == [3.0, 5.0, 7.0]

    def test_calculate_scale_no_type(self):
        info = _calculate_scale("", [])
        assert info["recommended_scale"] == [1, 1, 1]


class TestServerRegistration:
    def test_tool_registered(self):
        from godotiq.server import mcp
        tool_names = [t.name for t in mcp._tool_manager.list_tools()]
        assert "godotiq_placement" in tool_names
