import time
from pyaubo_agvc_sdk import RpcClient,Speed

def agv_client():
    agv = RpcClient()
    agv.setRequestTimeout(1000)  # 1秒超时
    
    # 连接到设备
    ip = "192.168.192.100"
    port = 30104
    ret = agv.connect(ip, port)
    print(f"Connect return code: {ret}")
    
    # 登录
    login_result = agv.login("admin", "admin")
    print(f"Login result: {login_result}")

    return agv

def example_agv_details(agv_client):
    """获取AGV详细信息"""
    # 获取设备信息
    details = agv_client.get_agv_details()
    
    # 打印设备信息
    print("\n" + "=" * 80)
    for field in [
        'name', 'version', 'MFD', 'SN', 'communication_version',
        'battery_version', 'loop_times', 'surplus_capacity',
        'left_motor_firmware_version', 'right_motor_firmware_version',
        'master_board_firmware_version', 'hardware_abstraction_version', 'max_speed'
    ]:
        value = getattr(details, field, "N/A")
        print(f"AGV {field.replace('_', ' ').title()}: {value}")
    print("-" * 80)

def example_running_info(agv_client):
    """获取AGV运行状态信息"""
    status = agv_client.get_running_info()
    
    # 打印运行状态
    header = status.header
    print("AGV Header:")
    for attr in ['id', 'name', 'stamp', 'map_id', 'error_code']:
        print(f"  {attr}: {getattr(header, attr, 'N/A')}")
    
    speed = status.current_speed
    print("AGV Current Speed:")
    for attr in ['v', 'w']:
        print(f"  {attr}: {getattr(speed, attr, 'N/A')}")
    
    print("AGV Low Battery:", status.low_battery)
    print("-" * 80)

def example_async_interface_status(agv_client):
    """获取异步接口状态"""
    async_status = agv_client.get_async_interface_result_status()
    
    # 打印状态
    print("Async Interface Result Status:", async_status.switch_map_status.status)

def example_laser_point_cloud(agv_client):
    """获取激光点云数据"""
    laser_scan = agv_client.get_laser_point_cloud()
    
    print("\nLaser Point Cloud (first 5 points):")
    for i, point in enumerate(laser_scan[:5]):
        print(f"  Point {i}: x={point.x}, y={point.y}")
    
    # 打印统计信息
    print(f"Total points: {len(laser_scan)}")
    print("=" * 80)

def example_speed_control(agv_client):
    """速度控制"""
    # 释放控制权
    agv_client.release_priority()
    # 获取控制权
    ret = agv_client.set_priority("admin", "192.168.192.100")
    print(f"Set Priority return code: {ret}")
    # 方法一：传入Speed对象
    speed = Speed()
    speed.v = 0.5
    speed.w = 0.0
    ret = agv_client.set_control_speed(speed)
    ret1 = agv_client.pause_agv_speed()
    print(ret1)
    time.sleep(5)
    ret2 = agv_client.resume_agv_speed()
    print(ret2)
    time.sleep(5)
    speed1 = Speed()
    speed1.v = 0.1
    speed1.w = 0.0
    ret = agv_client.set_control_speed(speed1)
    # 方法二：直接传入速度值
    while True:
        ret = agv_client.set_control_speed(0.5, 0.0)
        print(ret)

if __name__ == "__main__":
    agv = agv_client()
    print("\nGetting AGV details...")
    example_agv_details(agv)
    example_async_interface_status(agv)
    example_running_info(agv)
    example_laser_point_cloud(agv)
    example_speed_control(agv)
    agv.disconnect()
