from catopt_swarm import LocalProblem, SafetyPolicy
from catopt_swarm.solver import ADMMSolver


def main() -> None:
    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)
    solver = ADMMSolver(rho=1.5, max_iter=40, epsilon=1e-4)
    solution = solver.solve(robots, policy)
    print(f"Final agreed rendezvous point: {solution.consensus:.3f}")


if __name__ == "__main__":
    main()