import copy
import random
from gl import MAPSIZE
from treenode import Node,add_node
from change import switch_to_xy,switch_to_location,judge_xy,rotate

def initial_agvplan(agv_number,MAP,seed):
    AGV_PLAN = {}  # 使用字典存储，标号标识调用的AGV，标号后的数组表示运行的节点信息
    # 随机初始化任务序列
    shelf = []
    exit = []
    born = []
    random.seed(seed)
    for i in range(MAP.shape[0]):
        for j in range(MAP.shape[1]):
            if MAP[i, j] == 4:
                shelf.append(switch_to_location([i, j]))
            if MAP[i, j] == 1:
                exit.append(switch_to_location([i, j]))
            if MAP[i, j] == 3:
                born.append(switch_to_location([i, j]))
    agv_born = random.sample(born, agv_number)
    for i in range(agv_number):
        temp = [agv_born[i]]
        for j in range(2):  # 每辆两个任务
            temp.append(random.choice(shelf))
            temp.append(random.choice(exit))
        temp.append(agv_born[i])
        AGV_PLAN[i] = temp
    return AGV_PLAN
def initial_map(Special_structure):
    for i in range(len(Special_structure)):  # 多少个禁运区
        if Special_structure[i] != []:
            for j in range(len(Special_structure[i])):
                if len(Special_structure[i][j]) == 4:  # 即利用单个面积决定的禁运区
                    tmp1 = Special_structure[i][j][0][0] - 1
                    tmp2 = Special_structure[i][j][0][1] - 1
                    temp = tmp2
                    for k in range(Special_structure[i][j][1]):
                        for l in range(Special_structure[i][j][2]):
                            MAPSIZE[tmp1][tmp2] = Special_structure[i][j][3]   #将代表当前色块的权值赋给地图矩阵
                            tmp2 += 1
                        tmp1 += 1
                        tmp2 = temp
                if len(Special_structure[i][j]) == 7:  # 如果是利用多个面积决定的禁运区
                    for m in range(Special_structure[i][j][5]):
                        tmp1 = m * Special_structure[i][j][3] + Special_structure[i][j][0][0] - 1
                        tmp2 = m * Special_structure[i][j][4] + Special_structure[i][j][0][1] - 1
                        temp = tmp2
                        for k in range(Special_structure[i][j][1]):
                            for l in range(Special_structure[i][j][2]):
                                MAPSIZE[tmp1][tmp2] = Special_structure[i][j][6]
                                tmp2 += 1
                            tmp1 += 1
                            tmp2 = temp
    return MAPSIZE
def initial_availabel(**kwargs): #可达集合初始化
    tmp=[0,1]  #0时为shape[1]增减，1时为shape[0]增减
    max_index=MAPSIZE.shape[0]*MAPSIZE.shape[1]-1
    min_index=0
    AVAILABEL_NODE = [[] for x in range(
        MAPSIZE.shape[0] * MAPSIZE.shape[1])]  # 地图中每个节点的下一步可行,使用字典存储，比如说初始的0节点，由于在地图边缘，所以下一步只能走右或者上，即1，64（假设地图为64*64）
    # 为了区别不同的阻碍，同时又减少计算量，所以在存储空间上求得更大的占用，每类不同的障碍分别存储，最后再存储可行步（因此每个节点同时拥有3个list）
    STATIC_OBSTACLE = [[] for j in range(MAPSIZE.shape[0] * MAPSIZE.shape[1])]  # 静态障碍存储区
    #在地图中，i横轴，j纵轴
    for i in range(MAPSIZE.shape[0]):
        for j in range(MAPSIZE.shape[1]):
            step = []
            forbidden=[]

            number = i * MAPSIZE.shape[1] + j  # 计算当前位置的索引

            # 这里要说明，我们认为黑色实心障碍物为静态障碍物，而拣选台、AGV停放处和货架若没有AGV在，是可以穿过的
            for k in range(len(tmp)):
                if (0<=(i+tmp[k])<MAPSIZE.shape[0])&(0<=(j+(not tmp[k]))<MAPSIZE.shape[1]):
                    number1 = tmp[k] * MAPSIZE.shape[1] + (not tmp[k]) + number  # 计算下一步的索引
                    if MAPSIZE[i+tmp[k]][j+(not tmp[k])] != 5:
                        step.append(number1)
                    else:
                        forbidden.append(number1)
                if (0<=(i-tmp[k])<MAPSIZE.shape[0])&(0<=(j-(not tmp[k]))<MAPSIZE.shape[1]):
                    number2 = (-tmp[k]) * MAPSIZE.shape[1] - (not tmp[k]) + number
                    if MAPSIZE[i-tmp[k]][j-(not tmp[k])] != 5:
                        step.append(number2)
                    else:
                        forbidden.append(number2)
            AVAILABEL_NODE[number] = step  # 为当前位置创建可行节点的字典
            STATIC_OBSTACLE[number]=forbidden
    return AVAILABEL_NODE,STATIC_OBSTACLE
def initial_statistics(AGV_PLAN): #统计变量初始化
    PLAN=[]
    cut_num = [0 for x in range(len(AGV_PLAN))]
    avoid_num = [0 for x in range(len(AGV_PLAN))]  # 计避让次数
    STEP = [[] for x in range(len(AGV_PLAN))]
    ROTATE=[[] for x in range(len(AGV_PLAN))]
    next_treenode=[Node() for x in range(len(AGV_PLAN))]
    total_step=0
    total_cut=0
    total_avoid=0
    finish_agv=0
    agv_plan=[]
    for key in AGV_PLAN.keys():
        agv_plan.append(AGV_PLAN[key])
    PLAN=copy.deepcopy(agv_plan)
    return cut_num,avoid_num,total_step,total_cut,total_avoid,finish_agv,STEP,ROTATE,PLAN,next_treenode
def initial_tree(PLAN,SAFEDISTANCE,AVAILABEL_NODE,STATIC_OBSTACLE,next_treenode):
    tree = []
    next_step = []  # 找到最优子节点后决定走一步，形式为[[263，262]]
    NEXT_STEP = []  # 这是所有AGV的下一步的合集
    dynamic_obstacle = [[] for x in range(2 * len(PLAN))]
    for i in range(len(PLAN)):  # PLAN一直是总AGV个数
        if PLAN[i][0] != -1:
            tree.append(Node())
            tree[len(tree) - 1].location = PLAN[i][0]
            tree[len(tree) - 1].AGV = i
            tree[len(tree)-1].rotation = next_treenode[i].rotation
    for i in range(len(tree)):
        dynamic_obstacle[tree[i].AGV].append(tree[i].location)  # 将自己当前位置也加入（为了和Nosafedistance代码逻辑保持一致）
        for j in range(i + 1, len(tree), 1):
            [des_x, des_y] = switch_to_xy(tree[j].location)  # 判断树之间的相互距离
            [x, y] = switch_to_xy(tree[i].location)
            # 如果小于安全距离，在i与j之间建立通道连接
            if abs(des_y - y) + abs(des_x - x) <= SAFEDISTANCE:
                dynamic_obstacle[tree[j].AGV].append(tree[i].location)
                dynamic_obstacle[tree[i].AGV].append(tree[j].location)
                dynamic_obstacle[tree[i].AGV + len(PLAN)].append(tree[j])
                dynamic_obstacle[tree[j].AGV + len(PLAN)].append(tree[i])

            # for j in range(len(AVAILABEL_NODE[PLAN[i][0]])):
            #     temp_node = Node()  # 定义子节点
            #     temp_node.location = AVAILABEL_NODE[PLAN[i][0]][j]
            #     tree[i].add_child(temp_node)
    jump(tree, AVAILABEL_NODE, STATIC_OBSTACLE, dynamic_obstacle, PLAN)
    # monte_carlo_tree_cut(tree, AVAILABEL_NODE, next_step, dynamic_obstacle, PLAN)  # 修枝后正式开始选择
    return tree, dynamic_obstacle
def jump(tree, AVAILABEL_NODE, STATIC_OBSTACLE, dynamic_obstacle, PLAN):
    best_direction = []
    for i in range(len(tree)):  # 把小车位置作为动态障碍纳入影响
        best_direction_tmp = []
        # if (abs(tree[i].location-PLAN[i][1])==20)|(abs(tree[i].location-PLAN[i][1])==1):
        #     print()
        if (int(PLAN[tree[i].AGV][1] / MAPSIZE.shape[1]) - int(tree[i].location / MAPSIZE.shape[1])) > 0:
            best_direction_tmp.append(tree[i].location + MAPSIZE.shape[1])
        elif (int(PLAN[tree[i].AGV][1] / MAPSIZE.shape[1]) - int(tree[i].location / MAPSIZE.shape[1])) < 0:
            best_direction_tmp.append(tree[i].location - MAPSIZE.shape[1])
        if (PLAN[tree[i].AGV][1] % MAPSIZE.shape[1] - tree[i].location % MAPSIZE.shape[1]) > 0:  # 整除只能除一方
            best_direction_tmp.append(tree[i].location + 1)
        elif (PLAN[tree[i].AGV][1] % MAPSIZE.shape[1] - tree[i].location % MAPSIZE.shape[1]) < 0:  # 整除只能除一方
            best_direction_tmp.append(tree[i].location - 1)
        best_direction.append(best_direction_tmp)
        # 这里是辅助判断是否需要加入等待选项，即如果优秀的方向在动态障碍阻碍下，就加入等待
        if len(best_direction[i]) == 1:
            if (best_direction[i][0] in dynamic_obstacle):
                add_node(tree[i],tree[i].location)
        if len(best_direction[i]) > 1:
            if (best_direction[i][0] in dynamic_obstacle) & (best_direction[i][1] in dynamic_obstacle):
                add_node(tree[i],tree[i].location)
        [des_x_flag, des_y_flag] = judge_xy(tree[i].location, PLAN[tree[i].AGV][1])
        [des_x, des_y] = switch_to_xy(PLAN[tree[i].AGV][1])
        for j in range(len(AVAILABEL_NODE[tree[i].location])):
            if AVAILABEL_NODE[tree[i].location][j] in dynamic_obstacle:
                continue
            [x_flag, y_flag] = judge_xy(tree[i].location, AVAILABEL_NODE[tree[i].location][j])
            [x, y] = switch_to_xy(tree[i].location)
            tmp_tree = tree[i]
            while 1:
                [x, y] = [x + x_flag, y + y_flag]
                location = switch_to_location([x, y])
                sub_node=add_node(tmp_tree,location)
                if (des_x_flag==x_flag) | (des_y_flag==y_flag):#check为正
                    tmp_tree.children[len(tmp_tree.children)-1].check=1
                else:
                    tmp_tree.children[len(tmp_tree.children)-1].check=0
                if location == PLAN[tree[i].AGV][1]:  # 如果本次跳点已经跳到了终点，不再对终点做跳点或转折处理
                    break
                tmp_tree = sub_node  # 移动树节点
                [tmp_x, tmp_y] = [x_flag + des_x_flag, 0] if x_flag == 0 else [0, y_flag + des_y_flag]
                if (switch_to_location([x + tmp_x, y + tmp_y]) in AVAILABEL_NODE[location]) & (
                        switch_to_location([x + tmp_x, y + tmp_y]) not in dynamic_obstacle):  # 判断是否可以进行有利转折
                    location=switch_to_location([x + tmp_x, y + tmp_y])
                    add_node(tmp_tree,location)
                    if not ((x_flag == des_x_flag) | (y_flag == des_y_flag)):  # 如果没有向着目标点扩展，在第一次遇转折点就可以停止跳点工作了
                        break
                if (x_flag == des_x_flag) | (y_flag == des_y_flag):  # 如果是向着目标点扩展
                    if ((x == des_x) & (x_flag == des_x_flag)) | (
                            (y == des_y) & (y_flag == des_y_flag)):  # 如果沿着当前方向扩展到了最远（不是达到边界，而是再扩展就远离目标点了）
                        break
                if (switch_to_location([x + x_flag, y + y_flag]) in STATIC_OBSTACLE[switch_to_location([x, y])]) | (
                        [x + x_flag, y + y_flag] in [[x + x_flag, -1], [x + x_flag, 20], [-1, y + y_flag],
                                                     [20, y + y_flag]]):  # 遇静态障碍(包括边界)
                    if switch_to_location([x + tmp_x, y + tmp_y]) in dynamic_obstacle:  # 如果转折点不能走是因为动态障碍导致
                        # 不再继续扩展节点，将当前节点做动态障碍阻碍标识
                        break
                    else:
                        if switch_to_location([x + tmp_x, y + tmp_y]) in STATIC_OBSTACLE[location]:  # 转折点无法前进是由于静态障碍导致
                            # 只能选择与转折点反向做扩展，将当前节点做静态阻碍标识
                            location = switch_to_location([x - tmp_x, y - tmp_y]) if (0 <= (x - tmp_x) <
                                                                                               MAPSIZE.shape[0]) & (0 <= (
                                        y - tmp_y) < MAPSIZE.shape[1]) else -1
                            if location == -1:  # 当前一步完全不可行
                                for m in AVAILABEL_NODE[tmp_tree.parent.location]:  # 从最高层的可行解集中删除这一步，避免之后再走
                                    if m == tmp_tree.location:
                                        del m
                                del tmp_tree  # 删除当前节点
                                break
                            else:
                                add_node(tmp_tree,location)
                                break
                        else:
                            break
                if switch_to_location([x + x_flag, y + y_flag]) in dynamic_obstacle:  # 前进方向遇动态障碍
                    if (switch_to_location([x + tmp_x, y + tmp_y]) in AVAILABEL_NODE[location]):  # 如转折点已触发
                        break
                    else:
                        # 不再继续扩展节点，将当前节点做动态障碍阻碍标识
                        break
        for j in range(len(tree[i].children)):
            if tree[i].children[j].rotation==0 & tree[i].children[j].location!=tree[i].location: #前进
                tree[i].children[j].check=1