import time
import sys
from pyaubo_agvc_sdk import RpcClient, RunningMode, Header, ChangeRunningModeStatus, Speed, SaveMapStatus, \
    SwitchMapStatus

ip = "192.168.192.100"
port = 30104
rpc_name = "aa"
rpc_password = "bb"


def login():
    agv = RpcClient()
    agv.setRequestTimeout(10000)

    ret = agv.connect(ip, port)
    print(f"connect result: {ret}")
    ret = agv.login(rpc_name, rpc_password)
    print(f"login result: {ret}")
    return agv


def intoMappingMode(agv):
    # 释放权限 + 抢占控制权
    agv.releasePriority()
    running_mode = RunningMode.MAPPING
    command_header = Header()
    command_header.id = "create_map"
    
    # 第一次切换模式
    ret = agv.changeRunningMode(running_mode, command_header)
    print(f"changeRunningMode first call result: {ret}")

    agv.setPriority("aa", "bb")
    command_header.id = "func_test"

    # 第二次切换模式
    ret = agv.changeRunningMode(running_mode, command_header)

    while True:
        # 获取异步状态
        mode_status = agv.getAsyncInterfaceResultStatus()
        print(f"mode status id: {mode_status.change_running_mode_status.header.id}")
        
        if mode_status.change_running_mode_status.header.id == command_header.id:
            if mode_status.change_running_mode_status.status == ChangeRunningModeStatus.FINISHED:
                # 再次调用并获取返回值
                ret = agv.changeRunningMode(running_mode, command_header)
                print(f"changeRunningMode 建图模式返回值(int): {ret}")
                
                # 判断返回值是否为 1010000
                if ret != 1010000:
                    print(f"[ERROR] 切换建图模式失败，返回值: {ret}，程序退出")
                    agv.disconnect()
                    sys.exit(1)
                
                break
        time.sleep(1)
        
    print("change running mode finished")
                
    # 开环前进
    start = time.perf_counter()
    print("start open looping control")
    while True:
        end = time.perf_counter()
        duration = end - start
        if duration < 2:
            speed = Speed()
            speed.v = 0.5
            speed.w = 0.0
            agv.setControlSpeed(speed)
        else:
            start = time.perf_counter()
            break
        time.sleep(0.01)

    # 开环旋转
    while True:
        end = time.perf_counter()
        duration = end - start
        if duration < 2:
            speed = Speed()
            speed.v = 0.0
            speed.w = 0.5
            agv.setControlSpeed(speed)
        else:
            break
        time.sleep(0.01)

    print("end open looping control")

    # 保存地图
    map_header = Header()
    map_header.id = "save_map_test"
    map_header.map_id = "map"
    map_header.name = "test_map"
    map_header.stamp = 11111111

    agv.saveMap(map_header)
    while True:
        mode_status = agv.getAsyncInterfaceResultStatus()
        print(f"mode status id: {mode_status.save_map_status.header.id}")
        if mode_status.save_map_status.header.id == map_header.id:
            if mode_status.save_map_status.status == SaveMapStatus.FINISHED:
                agv.saveMap(map_header)
                break
        time.sleep(1)

    print("save map finished")

    # 切换地图
    map_header.id = "switch_map_test"
    map_header.map_id = "map"
    agv.switchMap(map_header)

    while True:
        mode_status = agv.getAsyncInterfaceResultStatus()
        print(f"mode status id: {mode_status.switch_map_status.header.id}")
        if mode_status.switch_map_status.header.id == map_header.id:
            if mode_status.switch_map_status.status == SwitchMapStatus.FINISHED:
                agv.switchMap(map_header)
                break
        time.sleep(1)
    print("change mode finished")

    # 切换回导航模式
    running_mode = RunningMode.NAVIGATE
    agv.changeRunningMode(running_mode, command_header)
    while True:
        mode_status = agv.getAsyncInterfaceResultStatus()
        print(f"mode status id: {mode_status.change_running_mode_status.header.id}")
        if mode_status.change_running_mode_status.header.id == command_header.id:
            if mode_status.change_running_mode_status.status == ChangeRunningModeStatus.FINISHED:
                agv.changeRunningMode(running_mode, command_header)
                break
        time.sleep(1)
    print("change running mode finished")


if __name__ == '__main__':
    agv = login()
    intoMappingMode(agv)
    agv.disconnect()
