from catopt_swarm import (
    ADMMSolver,
    ContractRegistry,
    ContractSpec,
    DroneControllerAdapter,
    GroundRoverControllerAdapter,
    LocalProblem,
    SafetyPolicy,
    verify_swarm_solution,
)
from catopt_swarm.registry import check_adapter_conformance


def test_solver_converges_and_respects_safety_policy():
    robots = [
        LocalProblem(robot_id="drone-1", target_pos=0.0, current_pos=0.0),
        LocalProblem(robot_id="drone-2", target_pos=10.0, current_pos=10.0),
    ]
    policy = SafetyPolicy(max_travel_distance=15.0, min_separation=1.0, energy_budget=20.0)
    solution = ADMMSolver(rho=1.5, max_iter=40, epsilon=1e-4).solve(robots, policy)

    assert solution.certificate.converged
    assert abs(solution.consensus - 5.0) < 0.25
    assert verify_swarm_solution(solution, policy).passed


def test_contract_registry_and_adapter_conformance():
    registry = ContractRegistry()
    spec = ContractSpec(
        contract_id="drone-patrol-v1",
        adapter_name="drone-controller",
        domain="aerial",
        version="1.0.0",
        invariants=("collision_free", "energy_budgeted"),
    )
    registry.register(spec)

    adapter = DroneControllerAdapter()
    report = check_adapter_conformance(adapter, spec)

    assert registry.has_contract("drone-patrol-v1")
    assert report.passed


def test_rover_adapter_maps_mission_to_problem():
    adapter = GroundRoverControllerAdapter()
    problem = adapter.to_local_problem("rover-1", {"target_pos": 3.5, "initial_pos": 1.0})

    assert problem.robot_id == "rover-1"
    assert problem.target_pos == 3.5
    assert problem.initial_pos == 1.0