import time
import creabot

# ---------------------- 原有初始化与核心功能函数（完全保留） ----------------------
# 初始化机器人和地图
def init(id, name):
    bot = creabot.Creabot(id)
    # 获取地图列表
    maplist = bot.list_map()
    # 找到默认地图
    targetmap = next((f for f in maplist if f["name"] == name), None)
    # 返回地图信息
    if targetmap:
        mapid = targetmap["id"]
        bot.set_map(mapid)
        pointlist = bot.list_map_point(mapid)
        print(pointlist)
        return bot, mapid, pointlist
    else:
        print("目标地图未找到")
        exit()

# 重定位函数
def anchor(bot, pointlist):
    # 提取类型为"anchor_point"的点位信息
    dst = next((f for f in pointlist if f["type"] == "anchor_point"), None)
    if dst:
        print("正在重定位")
        bot.tts_sync("开始重定位，请稍候")
        bot.relocate_sync(dst["x"], dst["y"], dst["theta"])
        bot.tts_sync("重定位完成")
    else:
        print("没有找到定位点")
        bot.tts_sync("没有找到定位点，将直接尝试导航")

# 导航到目的地函数
def nav_to(bot, pointlist, name):
    # 先执行重定位（导航前确保位置准确，保留原有重定位逻辑）
    anchor(bot, pointlist)
    # 取出需要导航到目的地的点位信息
    dst = next((f for f in pointlist if f["name"] == name), None)
    if dst:
        print(f"正在导航去{name}点")
        bot.tts_sync(f"正在导航去{name}点，请稍候")
        # 调用导航接口
        bot.start_navigation_sync(dst["x"], dst["y"], dst["theta"], 0.5)
        bot.tts_sync(f"已到达{name}点")
    else:
        print("点位名称不存在")
        bot.tts_sync("点位名称不存在，请确认指令")

# 物体检测与开关门函数（保留，可通过语音指令触发）
def check_object_and_door(bot):
    bot.tts_sync("开始检测物体")
    while True:
        obj = bot.exist_object()
        if obj:
            bot.tts_sync('发现物体，正在关门')
            bot.door_ctrl(0)
            break
        else:
            bot.tts_sync('当前没有检测到物品')
            
            #break# 调整为检测1次后退出，避免循环阻塞语音交互

# ---------------------- 调整流程：先语音交互，再执行导航 ----------------------
def main():
    # 1. 先初始化机器人（基础准备，不执行重定位和检测）
    bot, pointlist = init("172.20.10.7",'10.23.9') 
    # 2. 直接进入语音控制循环（优先语音交互）
    while True:
        bot.tts_sync('黑子说话')
        
        # 等待5秒接收语音输入
        res = bot.asr_sync(3)
        print("你说的内容：", res)
        
        if res:
            # 导航指令：收到指令后再执行导航（含重定位）
            if '四五六' in res or '456' in res:
                bot.tts_sync('我将去四五六，再去一二三')
                nav_to(bot, pointlist, "456")
                bot.tts_sync('放东西')
                bot.door_ctrl(1)
                bot.light_ctrl(1)
                bot.tts_sync('三秒关门')
                time.sleep(3)
                check_object_and_door(bot)
                bot.door_ctrl(0)
                bot.light_ctrl(0)

                bot.tts_sync('我要去一二三了')
                nav_to(bot,pointlist,'123')
                bot.door_ctrl(1)
                while True:
                    result = bot.exist_object()
                    if result:
                        bot.tts_sync('赶紧拿走')
                        bot.light_ctrl(1)
                    else:
                        bot.tts_sync('拿走不谢')
                        bot.light_ctrl(0)
                        break
                bot.tts_sync('两秒后关门')
                time.sleep(2)
                bot.door_ctrl(0)

            else:
                bot.tts_sync(f'我听到你说：{res}，我不知道啥意思')
        
        # 等待3秒后再次监听语音
        time.sleep(3)


def charge():
    bot,pointlist = init("172.20.10.7",'10.23.9')
    bot.tts_sync('请下达下一指令')
    res = bot.asr_sync(3)
    print('你说的内容',{res})
    if res:
        if "充电" in res:
            bot.tts_sync('出发充电')
            nav_to(bot,pointlist,'111')
            
        else:
            bot.tts_sync(f'我听到你说{res},我不知道是啥')


def charge_ctrl():
    bot,pointlist = init("172.20.10.7",'10.23.9')
    bot.tts_sync('寻找充电桩')
    for point in pointlist:
        if point["type"] == "charge":  # 接口文档4.1.3定义：充电桩点位类型为"charge"
            charger = {
                "x": point["x"],
                "y": point["y"],
                "theta": point["theta"]
            }
            print(f"自动识别充电桩：名称={point['name']}，坐标=({charger['x']},{charger['y']})，方向={charger['theta']}")
            break
    if not charger:
        raise Exception("未在地图点位列表中找到充电桩（类型为'charge'的点位），无法执行充电任务")
    bot.tts_sync('前往充电桩')
    bot.start_navigation_sync(charger["x"], charger["y"], charger["theta"], 0.5)
    bot.tts_sync('已到达充电桩')
    chargin = bot.dock_charge_on_sync(
        map_id=bot.get_map_id(),
        x=charger["x"],
        y=charger["y"],
        theta=charger["theta"])
    if not chargin:  # 0=上桩成功
        raise Exception(f"上桩失败，错误码：{chargin['code']}（0=成功，6016=未检测到充电桩）")
    bot.tts_sync("上桩成功,倒计时10秒")
    time.sleep(10)
    bot.tts_sync('开始下桩')
    bot.dock_charge_off_sync()
    bot.tts_sync("下桩成功")

    
# ---------------------- 程序入口 ----------------------
if __name__ == "__main__":
    main()
    nav_to("172.20.10.7",'10.23.9','111')
    charge()
    charge_ctrl()
