import os
import pickle
import time
import random

import matplotlib.pyplot as plt
import numpy as np

from robowaiter.scene.scene import Scene,init_world  # TODO: 文件名改成Scene.py
from navigate import Navigator




if __name__ == '__main__':

    file_name = 'map_5.pkl'
    if os.path.exists(file_name):
        with open(file_name, 'rb') as file:
            map = pickle.load(file)

    init_world(1, 11)
    scene = Scene(sceneID=0)

    navigator = Navigator(scene=scene,  area_range=[-350, 600, -400, 1450], map=map)

    '''场景1: 无行人环境 robot到达指定目标'''
    goal = np.array((-100, 700))


    '''场景2: 静止行人环境 robot到达指定目标'''
    # scene.clean_walker()
    # scene.add_walker(50, 300, 0)
    # scene.add_walker(-50, 500, 0)
    # goal = np.array((-100, 700))

    '''场景3: 移动行人环境 robot到达指定目标'''
    # scene.clean_walker()
    # scene.add_walker(50, 300, 0)
    # scene.add_walker(-50, 500, 0)
    # scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=False, speed=20, X=-50, Y=600, Yaw=0)])
    # scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=False, speed=20, X=100, Y=150, Yaw=0)])
    # goal = np.array((-100, 700))

    '''场景4: 行人自由移动 robot到达指定目标'''
    # # TODO: autowalk=True仿真器会闪退 ???
    # scene.clean_walker()
    # scene.add_walker(50, 300, 0)
    # scene.add_walker(-50, 500, 0)
    # scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=True, speed=20, X=0, Y=0, Yaw=0)])
    # scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=True, speed=20, X=0, Y=0, Yaw=0)])
    # time.sleep(5)
    # goal = np.array((-100, 700))

    navigator.navigate(goal, animation=False)

    scene.clean_walker()
    print(scene.status.collision)  # TODO: 不显示碰撞信息 ???