from agent.config.settings import settings
from core.uas_control.controllers.robot_dog_controller import RobotDogController
from core.uas_control.controllers.uav_controller import UAVController
from core.uas_control.protocols.mavlink_adapter import MAVLinkAdapter
from core.uas_control.protocols.ros_adapter import ROSAdapter
from core.uas_control.protocols.simulation_adapter import SimulationAdapter

uas_type = settings.device.type
protocol = settings.device.protocol
device_id = settings.device.device_id

if protocol == "mavlink":
    adapter = MAVLinkAdapter(device_id=device_id, **settings.device.params)
elif protocol == "ros":
    adapter = ROSAdapter(device_id=device_id, **settings.device.params)
else:
    adapter = SimulationAdapter(device_id=device_id, **settings.device.params)

if uas_type == "uav":
    controller = UAVController(device_id=device_id, adapter=adapter)
elif uas_type == "robot_dog":
    controller = RobotDogController(device_id=device_id, adapter=adapter)
else:
    controller = UAVController(device_id=device_id, adapter=adapter)

# results, errors = [], []
# controller.command_result.connect(lambda r: results.append(r))
# controller.error_occurred.connect(lambda m: errors.append(m))
# assert controller.connect(), "连接应成功"