import numpy as np
import pytest
from scipy.spatial.transform import Rotation
import yaml

from iiwa_genesis.config import ROOT, load_config


def tool_config(tmp_path, tools):
    data = yaml.safe_load((ROOT / "configs/scene.yaml").read_text())
    data["tools"] = tools
    path = tmp_path / "scene.yaml"
    path.write_text(yaml.safe_dump(data))
    return load_config(path)


@pytest.mark.parametrize("allowed", [[], "hammer1", [1], ["hammer1", "hammer1"],
                                     ["missing"], ["../hammer1"], ["hammer1.obj"]])
def test_invalid_tool_allowlist_is_rejected(tmp_path, allowed):
    with pytest.raises(ValueError, match="tools.allowed"):
        tool_config(tmp_path, {"allowed": allowed})


@pytest.mark.parametrize("seed", [True, 1.5, "42", -1])
def test_invalid_tool_seed_is_rejected(tmp_path, seed):
    with pytest.raises(ValueError, match="tools.seed"):
        tool_config(tmp_path, {"allowed": ["hammer1"], "seed": seed})


@pytest.mark.parametrize("tools", [None, ["hammer1"]])
def test_invalid_tools_section_is_rejected(tmp_path, tools):
    with pytest.raises(ValueError, match="tools"):
        tool_config(tmp_path, tools)


def test_empty_config_reports_validation_error(tmp_path):
    path = tmp_path / "empty.yaml"
    path.write_text("")
    with pytest.raises(ValueError, match="configuration"):
        load_config(path)


def test_tool_defaults_and_singleton_selection(tmp_path):
    from iiwa_genesis.tools import choose_tool_spawn

    cfg = tool_config(tmp_path, {"allowed": ["hammer3"]})
    assert cfg["tools"]["seed"] is None
    assert choose_tool_spawn(cfg).name == "hammer3"


def test_seed_repeats_spawn_and_only_allowed_tools_can_be_selected(tmp_path):
    from iiwa_genesis.tools import choose_tool_spawn

    cfg = tool_config(tmp_path, {"allowed": ["hammer2", "hammer5"], "seed": 123})
    first = choose_tool_spawn(cfg)
    assert choose_tool_spawn(cfg) == first
    cfg["scene"]["seed"] = 999
    assert choose_tool_spawn(cfg) == first
    choices = set()
    positions = set()
    for seed in range(12):
        cfg["tools"]["seed"] = seed
        spawn = choose_tool_spawn(cfg)
        choices.add(spawn.name)
        positions.add(spawn.position)
    assert choices == {"hammer2", "hammer5"}
    assert len(positions) > 1


@pytest.mark.parametrize("name", [f"hammer{i}" for i in range(1, 6)])
@pytest.mark.parametrize("table_pose", [((0, 0, 0), (0, 0, 0)),
                                        ((1.2, -.7, .2), (5, -3, 60))])
def test_exported_tool_fits_table_without_blender_offset(tmp_path, name, table_pose):
    from iiwa_genesis.tools import choose_tool_spawn

    cfg = tool_config(tmp_path, {"allowed": [name], "seed": 9})
    cfg["table"]["position"], cfg["table"]["euler"] = table_pose
    # Read the authored Y-up vertices independently of placement code.
    raw = np.array([[float(x) for x in line.split()[1:4]]
                    for line in (ROOT / f"iiwa_genesis_description/tools/hammer/{name}.obj").read_text().splitlines()
                    if line.startswith("v ")])
    zup = raw[:, [0, 2, 1]] * [1, -1, 1]
    table_rotation = Rotation.from_euler("xyz", table_pose[1], degrees=True)
    for seed in (0, 1, 9, 37):
        cfg["tools"]["seed"] = seed
        spawn = choose_tool_spawn(cfg)
        world = Rotation.from_quat(spawn.quaternion, scalar_first=True).apply(zup) + spawn.position
        local = table_rotation.inv().apply(world - table_pose[0])
        assert local[:, 0].min() >= .06 - 1e-9
        assert local[:, 0].max() <= .58 + 1e-9
        assert local[:, 1].min() >= -.48 - 1e-9
        assert local[:, 1].max() <= .48 + 1e-9
        assert local[:, 2].min() == pytest.approx(.805)
        assert local[:, 2].max() < .87  # lying flat, not standing on the handle


def test_missing_material_is_rejected(tmp_path, monkeypatch):
    from iiwa_genesis import config

    description = tmp_path / "description"
    asset = description / "tools/hammer/hammer1.obj"
    asset.parent.mkdir(parents=True)
    asset.write_text("mtllib hammer1.mtl\nv 0 0 0\n")
    (description / "urdf").symlink_to(config.DESCRIPTION / "urdf", target_is_directory=True)
    monkeypatch.setattr(config, "DESCRIPTION", description)
    with pytest.raises(ValueError, match="hammer1.mtl"):
        tool_config(tmp_path, {"allowed": ["hammer1"]})