import time
from pyaubo_agvc_sdk import RpcClient, Header, StationMark, PathStation, PathShape, Point2d, OccupancyGridMap, \
    Base64PngMap, RunningMode, Speed, NavGoalType, NavType, AdjustParam, Pose2d, AutoChargingCommand, AutoChargingType, \
    FirmwareUpdateParam, FirmwareUpdateMode, FirmwareUpdateProcessInfo, CalibrationType, CalibrationProcessInfo, \
    MapVirtualArea, MapVirtualAreaType, MapVirtualAreaShape, MapAllInfo, MapFormat, AgvDetails, ScriptRuntimeState, \
    IpAddressInfo, FeedbackStatus, NavStatus, SaveMapStatus, SwitchMapStatus


def login():
    """登录AGV并返回客户端实例"""
    ip = "192.168.192.100"
    port = 30104
    rpc_name = "admin"
    rpc_password = "123"
    agv = RpcClient()
    agv.setRequestTimeout(10000)  # 设置超时时间

    ret = agv.connect(ip, port)
    print(f"连接结果: {ret}")
    ret = agv.login(rpc_name, rpc_password)
    print(f"登录结果: {ret}")
    return agv


# ====================== 基础信息获取 ======================
def getAgvDetails(agv):
    """获取AGV硬件详细信息"""
    info = agv.getAgvDetails()
    print(f"AGV名称: {info.name}")
    print(f"固件版本: {info.version}")
    print(f"序列号SN: {info.SN}")
    print(f"剩余电量: {info.surplus_capacity}%")
    return info

def getRunningInfo(agv):
    """获取AGV运行状态信息"""
    info = agv.getRunningInfo()
    print(f"运行模式: {info.running_mode}")
    print(f"定位质量: {info.location_score}")
    print(f"总里程: {info.total_dist}")
    print(f"当前里程: {info.current_dist}")
    print(f"总运行时间: {info.total_running_time}")
    print(f"本次运行时间: {info.current_running_time}")
    print(f"电池电压: {info.battery_voltage}")
    print(f"电池电流: {info.battery_current}")
    print(f"剩余电压: {info.remaining_voltage}")
    print(f"电池状态: {info.battery_status}")
    print(f"安全触边状态: {info.safety_edge_status}")
    print(f"低电量: {info.low_battery}")
    print(f"暂停状态: {info.pause}")
    print(f"急停状态: {info.emergency_stop}")
    print(f"障碍物遮挡: {info.blocked}")
    print(f"定位丢失: {info.localization_loss}")
    print(f"故障报警: {info.fault_alarm}")
    return info

def getAgvCurrentPose(agv):
    """获取AGV当前位姿"""
    info = agv.getAgvCurrentPose()
    print(f"x: {info.x} m | y: {info.y} m | yaw: {info.yaw} rad")
    return info

def getLaserPointCloud(agv):
    """获取激光点云数据"""
    points = agv.getLaserPointCloud()
    print(f"激光点数量: {len(points)}")
    return points

def getAsyncInterfaceResultStatus(agv):
    """获取所有异步接口状态"""
    info = agv.getAsyncInterfaceResultStatus()
    print(f"保存地图状态: {info.save_map_status.status}")
    print(f"切换地图状态: {info.switch_map_status.status}")
    print(f"切换运行模式状态: {info.change_running_mode_status.status}")
    print(f"重定位状态: {info.relocation_status.status}")
    print(f"获取栅格地图状态: {info.get_grid_map_status.status}")
    print(f"发送栅格地图状态: {info.send_grid_map_status.status}")
    print(f"获取PNG地图状态: {info.get_png_map_status.status}")
    print(f"发送PNG地图状态: {info.send_png_map_status.status}")
    print(f"设置PNG地图信息状态: {info.set_png_all_status.status}")
    print(f"获取PNG地图信息状态: {info.get_png_all_status.status}")
    print(f"设置栅格地图信息状态: {info.set_grid_all_status.status}")
    print(f"获取栅格地图信息状态: {info.get_grid_all_status.status}")
    return info

# ====================== 站点管理 ======================
def getAllStations(agv):
    """获取所有站点"""
    info = agv.getAllStations()
    print(f"共 {len(info)} 个站点")
    for item in info:
        print(f"类型: {item.type}")
        print(f"坐标x: {item.pose.x} m")
        print(f"坐标y: {item.pose.y} m")
    return info

def addStation(agv):
    """添加单个站点"""
    header = Header(id="test_st_01", name="测试站点", map_id="default_map")
    mark = StationMark()
    mark.header = header
    mark.type = "ST"
    mark.pose.x = 0
    mark.pose.y = 0
    mark.pose.yaw = 0
    ret = agv.addStation(mark)
    print(f"添加站点结果: {ret}")

def addStations(agv):
    """批量添加站点"""
    header = Header(id="test_st_02", name="批量测试站点", map_id="default_map")
    mark = StationMark()
    mark.header = header
    mark.type = "ST"
    mark.pose = Pose2d(x=1.0, y=1.0, yaw=0.0)
    ret = agv.addStations([mark])
    print(f"批量添加站点结果: {ret}")

def addCPStationUseChargingBoard(agv):
    """添加充电板站点"""
    header = Header(id="cp_st_01", name="充电站点", map_id="default_map")
    ret = agv.addCPStationUseChargingBoard(header, 1.5)
    print(f"添加充电站点结果: {ret}")

def deleteStation(agv):
    """删除单个站点"""
    header = Header(id="test_st_01", map_id="default_map")
    ret = agv.deleteStation(header)
    print(f"删除站点结果: {ret}")

def deleteStations(agv):
    """批量删除站点"""
    header = Header(id="test_st_02", map_id="default_map")
    ret = agv.deleteStations([header])
    print(f"批量删除站点结果: {ret}")

# ====================== 路径管理 ======================
def getAllPaths(agv):
    """获取所有路径"""
    info = agv.getAllPaths()
    for path in info:
        print(f"路径形状: {path.shape}")
        print(f"使用方向: {path.use_direction}")
        print(f"最大速度: {path.max_speed}")
        print(f"起点站点ID: {path.start_station_id}")
        print(f"终点站点ID: {path.end_station_id}")
        for point in path.control_points:
            print(f"控制点x: {point.x} m")
            print(f"控制点y: {point.y} m")
    return info

def generatePath(agv):
    """生成单条路径"""
    header = Header(id="path_01", name="测试路径", map_id="default_map")
    path = PathStation()
    path.header = header
    path.shape = PathShape.LINE
    path.use_direction = True
    path.max_speed = 0.5
    path.start_station_id = "test_st_01"
    path.end_station_id = "test_st_02"
    path.control_points = [Point2d(0.5, 0.5)]
    ret = agv.generatePath(path)
    print(f"生成路径结果: {ret}")

def generatePaths(agv):
    """批量生成路径"""
    header = Header(id="path_02", name="批量测试路径", map_id="default_map")
    path = PathStation()
    path.header = header
    path.shape = PathShape.LINE
    path.use_direction = True
    path.max_speed = 0.5
    path.start_station_id = "test_st_01"
    path.end_station_id = "test_st_02"
    path.control_points = [Point2d(0.5, 0.5)]
    ret = agv.generatePaths([path])
    print(f"批量生成路径结果: {ret}")

def deletePath(agv):
    """删除单条路径"""
    header = Header(id="path_01", map_id="default_map")
    ret = agv.deletePath(header)
    print(f"删除路径结果: {ret}")

def deletePaths(agv):
    """批量删除路径"""
    header = Header(id="path_02", map_id="default_map")
    ret = agv.deletePaths([header])
    print(f"批量删除路径结果: {ret}")

# ====================== 地图管理 ======================
def getMapList(agv):
    """获取地图列表"""
    maps = agv.getMapList()
    print(f"地图数量: {len(maps)}")
    return maps

def getCurrentMapHeader(agv):
    """获取当前地图头信息"""
    info = agv.getCurrentMapHeader()
    print(f"地图ID: {info.id}")
    print(f"地图名称: {info.name}")
    print(f"时间戳: {info.stamp}")
    print(f"地图标识: {info.map_id}")
    print(f"错误码: {info.error_code}")
    return info

def getGridMapFromAgv(agv):
    """从AGV获取栅格地图"""
    header = getCurrentMapHeader(agv)
    map_data = agv.getGridMapFromAgv(header)
    print(f"栅格地图分辨率: {map_data.resolution}")
    return map_data

def getBase64PngMapFromAgv(agv):
    """从AGV获取PNG地图"""
    header = getCurrentMapHeader(agv)
    map_data = agv.getBase64PngMapFromAgv(header)
    print(f"PNG地图宽度: {map_data.width}")
    return map_data

def previewPngMapFromAgv(agv):
    """预览PNG地图"""
    header = getCurrentMapHeader(agv)
    map_data = agv.previewPngMapFromAgv(header, 800, 600)
    print(f"预览地图完成")
    return map_data

def sendGridMapToAgv(agv):
    """发送栅格地图到AGV"""
    header = Header(id="grid_map_01", name="测试栅格地图", map_id="test_map")
    map_info = OccupancyGridMap()
    map_info.header = header
    map_info.resolution = 0.05
    map_info.width = 100
    map_info.height = 100
    map_info.origin = Pose2d(0,0,0)
    map_info.format = MapFormat.OCCUPANCY_GRID
    ret = agv.sendGridMapToAgv(map_info)
    print(f"发送栅格地图结果: {ret}")

def sendBase64PngMapToAgv(agv):
    """发送PNG地图到AGV"""
    header = Header(id="png_map_01", name="测试PNG地图", map_id="test_map")
    map_info = Base64PngMap()
    map_info.header = header
    map_info.resolution = 0.05
    map_info.width = 100
    map_info.height = 100
    map_info.origin = Pose2d(0,0,0)
    map_info.format = MapFormat.BASE64_PNG
    ret = agv.sendBase64PngMapToAgv(map_info)
    print(f"发送PNG地图结果: {ret}")

def saveMap(agv):
    """保存地图"""
    header = getCurrentMapHeader(agv)
    ret = agv.saveMap(header)
    print(f"保存地图结果: {ret}")

def switchMap(agv):
    """切换地图"""
    maps = getMapList(agv)
    if maps:
        ret = agv.switchMap(maps[0])
        print(f"切换地图结果: {ret}")

def deleteMap(agv):
    """删除地图"""
    header = Header(id="test_map", map_id="test_map")
    ret = agv.deleteMap(header)
    print(f"删除地图结果: {ret}")

# ====================== 虚拟区域管理 ======================
def getAllMapVirtualArea(agv):
    """获取所有虚拟区域"""
    areas = agv.getAllMapVirtualArea()
    print(f"虚拟区域数量: {len(areas)}")
    return areas

def addMapVirtualArea(agv):
    """添加虚拟区域"""
    area = MapVirtualArea()
    area.header = Header(id="virtual_01", name="障碍区域", map_id="default_map")
    area.type = MapVirtualAreaType.OBSTACLE
    area.shape = MapVirtualAreaShape.POLYGON
    area.width_speed = 0.5
    area.vertex = [Point2d(0,0), Point2d(1,0), Point2d(1,1), Point2d(0,1)]
    ret = agv.addMapVirtualArea(area)
    print(f"添加虚拟区域结果: {ret}")

def addMapVirtualAreas(agv):
    """批量添加虚拟区域"""
    area = MapVirtualArea()
    area.header = Header(id="virtual_02", name="减速区域", map_id="default_map")
    area.type = MapVirtualAreaType.SLOW_DOWN
    area.shape = MapVirtualAreaShape.CIRCLE
    area.width_speed = 0.3
    area.vertex = [Point2d(2,2)]
    ret = agv.addMapVirtualAreas([area])
    print(f"批量添加虚拟区域结果: {ret}")

def deleteMapVirtualArea(agv):
    """删除虚拟区域"""
    header = Header(id="virtual_01", map_id="default_map")
    ret = agv.deleteMapVirtualArea(header)
    print(f"删除虚拟区域结果: {ret}")

def deleteMapVirtualAreas(agv):
    """批量删除虚拟区域"""
    header = Header(id="virtual_02", map_id="default_map")
    ret = agv.deleteMapVirtualAreas([header])
    print(f"批量删除虚拟区域结果: {ret}")

# ====================== 完整地图信息管理 ======================
def getGridMapAllInfo(agv):
    """获取栅格地图完整信息"""
    header = getCurrentMapHeader(agv)
    info = agv.getGridMapAllInfo(header)
    print(f"完整地图-站点数: {len(info.stations)}")
    return info

def getPngMapAllInfo(agv):
    """获取PNG地图完整信息"""
    header = getCurrentMapHeader(agv)
    info = agv.getPngMapAllInfo(header)
    print(f"完整地图-路径数: {len(info.paths)}")
    return info

def setGridMapAllInfo(agv):
    """设置栅格地图完整信息"""
    map_all_info = MapAllInfo()
    map_all_info.map.header = getCurrentMapHeader(agv)
    ret = agv.setGridMapAllInfo(map_all_info)
    print(f"设置完整栅格地图结果: {ret}")

def setPngMapAllInfo(agv):
    """设置PNG地图完整信息"""
    map_all_info = MapAllInfo()
    map_all_info.map.header = getCurrentMapHeader(agv)
    ret = agv.setPngMapAllInfo(map_all_info)
    print(f"设置完整PNG地图结果: {ret}")

def deleteMapAllInfo(agv):
    """删除完整地图信息"""
    header = getCurrentMapHeader(agv)
    ret = agv.deleteMapAllInfo(header)
    print(f"删除完整地图结果: {ret}")

# ====================== 运行模式与控制 ======================
def changeRunningMode(agv):
    """切换运行模式"""
    command_header = Header()
    command_header.id = "change_running_mode"
    ret = agv.changeRunningMode(RunningMode.NAVIGATE, 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.change_running_mode(running_mode, command_header)
                print(f"change_running_mode result: {ret}")
                break
        # 当异步接口状态指示完成时，获取接口运行接口，并跳出循环
        time.sleep(1)
    print("change running mode finished")

def setControlSpeed(agv):
    """设置控制速度"""
    start = time.perf_counter()
    print("start open looping control")
    while True:
        end = time.perf_counter()
        duration = end - start
        # 使用时间作为判断标准，agv连续前进2s
        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)

def pauseAgvSpeed(agv):
    """暂停AGV运动"""
    ret = agv.pauseAgvSpeed()
    print(f"暂停结果: {ret}")

def resumeAgvSpeed(agv):
    """恢复AGV运动"""
    ret = agv.resumeAgvSpeed()
    print(f"恢复结果: {ret}")

# ====================== 导航功能 ======================
def setNavGoal(agv):
    """设置导航目标点"""
    nav_goal = NavGoalType()
    nav_goal.header = Header()
    nav_goal.header.id = "test_nav_goal_1"
    nav_goal.header.map_id = "current_map"
    nav_goal.type = NavType.PATH_TO_STATION
    nav_goal.forward_flag = False
    nav_goal.goal_end_rotate = False
    nav_goal.max_vel = Speed()
    nav_goal.max_vel.v = 0.5
    nav_goal.stations_id = ["add_station_2", "add_station_1"]
    ret = agv.setNavGoal(nav_goal)
    print(f"设置导航目标结果: {ret}")

def getNavInfo(agv):
    """获取导航信息"""
    info = agv.getNavInfo()
    print(f"导航状态: {info.status} | 导航类型: {info.nav_type}")
    return info

def cancelNavigation(agv):
    """取消导航"""
    ret = agv.cancelNavigation()
    print(f"取消导航结果: {ret}")

def relocation(agv):
    # 抢夺控制权
    agv.release_priority()
    agv.set_priority("aa", "bb")
    # 查看定位是否丢失
    running_info = agv.get_running_info()
    if running_info.localization_loss:
        # 设置重定位位置参数 及 指定参数
        init_pose = Pose2d()
        command_header = Header()
        init_pose.x = 0.0
        init_pose.y = 0.0
        init_pose.yaw = 0.0
        command_header.id = "test_relocation"
        command_header.map_id = "current_map"
        command_header.name = "test_map"

        # 调用重定位接口
        agv.relocation(init_pose, command_header)

        # 判断是否重定位成功
        while True:
            # 获取异步接口运行状态
            mode_status = agv.get_async_interface_result_status()
            print(f"mode status id: {mode_status.relocation_status.header.id}")
            if mode_status.relocation_status.header.id == command_header.id:
                if mode_status.relocation_status.status == RelocationStatus.FINISHED:
                    ret = agv.relocation(init_pose, command_header)
                    print(f"relocation result: {ret}")
                    break
            # 当异步接口状态指示完成时，获取接口运行接口，并跳出循环
            time.sleep(1)

def autoAlignRailway(agv):
    """自动对齐轨道"""
    ret = agv.autoAlignRailway()
    print(f"自动对齐轨道结果: {ret}")

# ====================== 自动充电 ======================
def sendAutoChargingCommand(agv):
    """发送自动充电指令（新版推荐）"""
    cmd = AutoChargingCommand()
    cmd.header = Header(id="charge_cmd")
    cmd.auto_charging_type = AutoChargingType.FORCE_TO_BOARD
    cmd.cp_station_id = "cp_st_01"
    cmd.max_speed = Speed(v=0.2, w=0.2)
    ret = agv.sendAutoChargingCommand(cmd)
    print(f"自动充电指令结果: {ret}")

# ====================== 系统 ======================
def restartAgv(agv):
    """重启AGV"""
    ret = agv.restartAgv()
    print(f"重启结果: {ret}")

def setAgvName(agv):
    """设置AGV名称"""
    ret = agv.setAgvName("MyAGV")
    print(f"设置AGV名称结果: {ret}")

# ====================== 故障与诊断 ======================
def errorCodeDecoder(agv):
    """错误码解析"""
    msg = agv.errorCodeDecoder(10100000)
    print(f"错误码0解析内容: {msg}")

def getCurrentErrorCodes(agv):
    """获取当前错误码"""
    codes = agv.getCurrentErrorCodes()
    print(f"当前错误数量: {len(codes)}")

def isObstacleAhead(agv):
    """前方是否有障碍"""
    ret = agv.isObstacleAhead(detect_distance=1.0)
    print(f"前方是否有障碍: {ret}")

def getNearestStation(agv):
    """获取最近站点"""
    station = agv.getNearestStation(detect_distance=5.0)
    if station:
        print(f"最近站点: {station.header.name}")

def soundLightPrompt(agv):
    """声光提示"""
    ret = agv.soundLightPrompt()
    print(f"声光提示结果: {ret}")

# ====================== 标定功能 ======================
def calibrationDemo(agv):
    """激光里程计标定演示"""
    agv.startCollectCalibrationData(CalibrationType.LASER_ODOM)
    time.sleep(3)
    agv.cancelCollectCalibrationData(CalibrationType.LASER_ODOM)
    print("标定数据采集完成")

# ====================== 权限管理 ======================
def priorityDemo(agv):
    """权限控制演示"""
    agv.setPriority("test_client", "192.168.192.100")
    time.sleep(2)
    agv.releasePriority()
    print("权限控制完成")

# ====================== 主函数 ======================
if __name__ == "__main__":

    agv = login()
    
    agv.releasePriority()
    agv.setPriority("test_client", "192.168.192.100")
    
    #getAgvDetails(agv)
    #getRunningInfo(agv)
    #getAgvCurrentPose(agv)
    #getLaserPointCloud(agv)

    #getMapList(agv)
    #getCurrentMapHeader(agv)
    #getAllStations(agv)

    #changeRunningMode(agv)
    #setControlSpeed(agv)
    #relocation(agv)
    #setNavGoal(agv)

    #errorCodeDecoder(agv)
    #getCurrentErrorCodes(agv)
    #soundLightPrompt(agv)

    agv.disconnect()
