import time
import matplotlib.pyplot as plt
from initial import initial_availabel,initial_statistics,initial_tree
from rd import readkwargs
from treecut import cut_tree,sort_tree
from treenode import Node
from change import switch_to_xy
from draw import DRAW_PATH_BY_TREE,plot_AGV
from choosecheck import choosecheck
import numpy as np
def model(MAP,AGV_PLAN,**kwargs):
    #必须的输入：
    #1、MAP_MATRIX : 以矩阵形式存储的地图信息
    #2、AGV_PLAN : AGV的调度任务
    #可选输入：
    #1、SAFEDISTANCE : 安全距离，默认为0（即不进行信息共享）
    #2、MAX_BACK_TIMES : 最大回返次数（在蒙特卡洛树搜索的模拟阶段），用来限制无限读取，建议设置为10
    #3、im_step : 某一步出现差距，带来的误差重要程度
    #4、im_self2other : 自身权重与他人权重比例，建议设置为1
    #5、MAX_ITER : 最大循迹步数\最大选择次数\最大模拟次数
    #6、DISPLAY : 显示统计结果\图像

    #序：解析kwargs
    MAX_ITER_STEP=kwargs['MAX_ITER'][0]
    MAX_ITER_SELECT=kwargs['MAX_ITER'][1]
    if 'SAFEDISTANCE' in kwargs.keys():
        SAFEDISTANCE=kwargs['SAFEDISTANCE']
    else:
        SAFEDISTANCE=0
    if 'DISPLAY' in kwargs.keys():
        display_flag=kwargs['DISPLAY']
        display_text=display_flag[0]
        display_image=display_flag[1]
    else:
        display_text = 0
        display_image = 0
    #1、初始化各参数
    AVAILABEL_NODE,STATIC_OBSTACLE=initial_availabel()
    cut_num,avoid_num,total_step,total_cut,total_avoid,finish_agv,STEP,ROTATE,PLAN,next_treenode = initial_statistics(AGV_PLAN)
    total_invalid=0
    # 2、以所有起终点重合为截至条件的循环
    # 创建蒙特卡洛树
    # NEXT_STEP=[[[],[],[],[]],] next_step=[]
    # 限制每次迭代最多次数
    # n棵树的循环
    # 选择   -》如果选择到最后是叶子节点，去执行修枝函数，并跳过本次循环
    # 扩展
    # 模拟
    # 反向传播
    # 如果所有树都有了下一步，执行起终点判断和更新函数，更新SCHE_PLAN，跳出当前循环
    finish_flag=1
    timeover_flag = 0
    for i in range(len(PLAN)):  # 将初始节点写入
        STEP[i].append(PLAN[i][0])
        ROTATE[i].append(0)
    time_start=time.time()
    #为kwargs新增键值对
    kwargs['AVAILABEL_NODE']=AVAILABEL_NODE
    kwargs['STATIC_OBSTACLE']=STATIC_OBSTACLE

    while finish_flag == 1:  # 通过while循环方便的再重启for循环
        for i in range(MAX_ITER_STEP):
            tree, dynamic_obstacle = initial_tree(PLAN, SAFEDISTANCE,AVAILABEL_NODE,STATIC_OBSTACLE,next_treenode)
            # 查看第一次跳点结果的画图函数
            # DRAW_PATH_BY_TREE(tree)
            # plt.show()

            next_treenode = [Node() for x in range(len(AGV_PLAN))]  #记录树走下一步时的节点
            for j in range(MAX_ITER_SELECT):  # 一轮迭代MAX_ITERATION遍
                for AGV in tree:  # 这里只要用需要计算的AGV数量
                    node = AGV.select(PLAN, dynamic_obstacle[AGV.AGV],**kwargs)
                    # 查看选择结果的画图函数
                    # DRAW_PATH_BY_TREE(tree)
                    # plot_AGV(node.location,"blue")
                    # plt.show()
                    dynamic_obstacle[AGV.AGV] = node.simulation(PLAN,dynamic_obstacle[AGV.AGV],
                                                                dynamic_obstacle[AGV.AGV + len(AGV_PLAN)], **kwargs)
                # plt.show()
            sort_index = sort_tree(tree)  # 根据下一步的最优值排序决定剪枝顺序
            for sort in sort_index:
                next = tree[sort].real_best_child()  # 确定下一步后再剪枝
                # choosecheck(next)
                if next.check==0:
                    total_invalid+=1
                if next != None:
                    next_treenode[tree[sort].AGV]=next
                else:
                    next_treenode[tree[sort].AGV]=tree[sort]
                cut_tree(tree, tree[sort], cut_num, avoid_num)
            for AGV in tree:  # 这里需要用真实的AGV数量,更新起终点信息
                if PLAN[AGV.AGV][1] == next_treenode[AGV.AGV].location:
                    STEP[AGV.AGV].append(next_treenode[AGV.AGV].location)  # 在对应树的STEP的末尾列表的末尾插入本次所走的值
                    ROTATE[AGV.AGV].append(next_treenode[AGV.AGV].rotation)
                    PLAN[AGV.AGV][0] =PLAN[AGV.AGV][1]
                    del PLAN[AGV.AGV][1]  # 删除本次的终点
                    if len(PLAN[AGV.AGV]) == 1:  # 如果本次终点就是最终点
                        PLAN[AGV.AGV][0] = -1  # 用-1来标识
                        finish_agv += 1
                else:
                    PLAN[AGV.AGV][0] = next_treenode[AGV.AGV].location
                    STEP[AGV.AGV].append(next_treenode[AGV.AGV].location)  # 在对应树的STEP的末尾列表的末尾插入本次所走的值
                    ROTATE[AGV.AGV].append(next_treenode[AGV.AGV].rotation)
            # 这里是每隔15或29次画一次运行图
            # if (i==14)|(i==29):
            #     draw_path = [Node() for x in range(len(STEP))]
            #     for i in range(len(STEP)):
            #         for j in range(len(STEP[i])-16,len(STEP[i])):
            #             if draw_path[i].location == -1:  # 说明是第一个
            #                 draw_path[i].location = STEP[i][j]
            #                 draw_path[i].rotation = ROTATE[i][j]
            #                 draw_path[i].AGV = i
            #                 node = draw_path[i]
            #             else:
            #                 sub_node = Node()
            #                 sub_node.location = STEP[i][j]
            #                 sub_node.rotation = ROTATE[i][j]
            #                 node.add_child(sub_node)
            #                 node = sub_node
            #     DRAW_PATH_BY_TREE(draw_path)
            #     plt.show()
            time_now = time.time()
            if (time_now - time_start > 60000):
                timeover_flag = 1
                break
            if finish_agv == len(AGV_PLAN):  # 如果所有AGV都完成了迭代
                finish_flag = 0  # 设置退出while循环标识
                break
            else:
                if i == MAX_ITER_STEP - 1:  # 如果迭代达到最大次数还没有找到解
                    finish_flag = 1  # 重置flag1=1，通过while再度开启for循环（其实不设置也不会出问题）
                    # i=0
                else:  # 如果迭代还没有达到最大次数
                    continue
        if timeover_flag == 1:
            break
    if timeover_flag == 1:
        return total_step, total_cut, total_avoid,total_invalid, (1,)
    time_end = time.time()
    draw_path = [Node() for x in range(len(STEP))]
    for i in range(len(STEP)):
        for j in range(len(STEP[i])):
            if draw_path[i].location == -1:  # 说明是第一个
                draw_path[i].location = STEP[i][j]
                draw_path[i].rotation = ROTATE[i][j]
                draw_path[i].AGV = i
                node = draw_path[i]
            else:
                sub_node = Node()
                sub_node.location = STEP[i][j]
                sub_node.rotation = ROTATE[i][j]
                node.add_child(sub_node)
                node = sub_node

        total_step += node.deep
        theory_total = 0
        for j in range(len(AGV_PLAN[i]) - 1):
            [des_x, des_y] = switch_to_xy(AGV_PLAN[i][j + 1])
            [x, y] = switch_to_xy(AGV_PLAN[i][j])
            theory_total += abs(des_x - x) + abs(des_y - y)+1
        total_cut += cut_num[i]
        total_avoid += avoid_num[i]
        if display_text==1:
            print("AGV", i + 1, ":(SD=",SAFEDISTANCE,')')
            print("总行驶里程为", node.deep)
            print("城市距离为", theory_total)
            print("总干扰次数为", cut_num[i])
            print("总避让次数为", avoid_num[i])
    if display_text == 1:
        print("有安全距离下的运行总耗时", (time_end - time_start)/len(AGV_PLAN))
    if display_image==1:
        DRAW_PATH_BY_TREE(draw_path)
        plt.show()
    return total_step, total_cut, total_avoid, total_invalid,(0, (time_end - time_start)/len(AGV_PLAN),theory_total)

def Astar_model(MAP,AGV_PLAN,**kwargs):
    # 1、初始化各参数
    AVAILABEL_NODE, STATIC_OBSTACLE = initial_availabel()
    total_step=0
    total_avoid=0


    return total_step,total_avoid,(0,1)