from data import whichmap, distance
from road_list import T2_road
import math
import sys

sell1=[]
sell2=[]
sell3=[]
sell4=['1', '2']
sell5=['1', '3']
sell6=['2', '3']
sell7=['4', '5', '6']
sell8=['7']
sell9=['1', '2', '3', '4', '5', '6', '7']


class Control():
    def __init__(self, frame_id, WB, WB_indexnumber, robot, stat, WBlist, needlist, distance_dic, distance_roboteachother,\
                  distance_9to456, distance_7to456, free_set, WB_lock, WB_distribution_stat, dic_countneed, exist, robot_step, road_list):
        self.frame_id = frame_id
        self.stat = stat
        self.WBlist = WBlist
        self.WB = WB
        self.WB_indexnumber = WB_indexnumber
        self.robot = robot
        self.needlist = needlist
        self.distance_dic = distance_dic
        self.distance_roboteachother = distance_roboteachother
        self.distance_9to456 = distance_9to456
        self.distance_7to456 = distance_7to456
        self.robot_stat = free_set
        self.WB_lock = WB_lock
        self.WB_distribution_stat = WB_distribution_stat
        self.dic_countneed = dic_countneed
        self.exist = exist
        self.exist456 = self.exist[3:6]
        self.exist123 = self.exist[0:3]
        self.road_list = road_list
        self.robot_step = robot_step
        
    def testgoodcansell(self, goods, wb):
        if goods == '1' and wb[0] == '4':
            if self.WB[wb][4] != '2' and self.WB[wb][4] != '6':
                return True
        elif goods == '2' and wb[0] == '4':
            if self.WB[wb][4] != '4' and self.WB[wb][4] != '6':
                return True
        elif goods == '1' and wb[0] == '5':
            if self.WB[wb][4] != '2' and self.WB[wb][4] != '10':
                return True
        elif goods == '3' and wb[0] == '5':
            if self.WB[wb][4] != '8' and self.WB[wb][4] != '10':
                return True
        elif goods == '2' and wb[0] == '6':
            if self.WB[wb][4] != '4' and self.WB[wb][4] != '12':
                return True
        elif goods == '3' and wb[0] == '6':
            if self.WB[wb][4] != '8' and self.WB[wb][4] != '12':
                return True
        elif goods == '4' or goods == '5' or goods == '6' or goods == '7':
            return True
        return False
    # 判断两个方向是不是面向四个角落中的一个
    def facetocorner(self, o, p):
        if o > 20/360*math.pi*2 and o < 70/360*math.pi*2 and p > 20/360*math.pi*2 and p < 70/360*math.pi*2:
            return True
        elif o > 110/360*math.pi*2 and o < 160/360*math.pi*2 and p > 110/360*math.pi*2 and p < 160/360*math.pi*2:
            return True
        elif o > -160/360*math.pi*2 and o < -110/360*math.pi*2 and p > -160/360*math.pi*2 and p < -110/360*math.pi*2:
            return True
        elif o > -80/360*math.pi*2 and o < -10/360*math.pi*2 and p > -80/360*math.pi*2 and p < -10/360*math.pi*2:
            return True
        else:
            return False
    def wbincorner(self, wb):
        wb_x = float(self.WB[wb][1])
        wb_y = float(self.WB[wb][1])
        if ((wb_x - 0)**2 + (wb_y - 0)**2)**0.5 < 1.8 or ((wb_x - 50)**2 + (wb_y - 0)**2)**0.5 < 1.8 or\
                ((wb_x - 0)**2 + (wb_y - 50)**2)**0.5 < 1.8 or ((wb_x - 50)**2 + (wb_y - 50)**2)**0.5 < 1.8:
            return True
        return False
    def wbinedge(self, wb):
        wb_x = float(self.WB[wb][1])
        wb_y = float(self.WB[wb][2])
        if wb_x < 3 or wb_y < 3 or wb_x > 47 or wb_y > 47:
            return True
        return False
    def robotneercorner(self, i):
        x_i = float(self.robot[str(i)][8])
        y_i = float(self.robot[str(i)][9])
        if ((x_i - 0)**2 + (y_i - 0)**2)**0.5 < 1.8 or ((x_i - 50)**2 + (y_i - 0)**2)**0.5 < 1.8 or\
                ((x_i - 0)**2 + (y_i - 50)**2)**0.5 < 1.8 or ((x_i - 50)**2 + (y_i - 50)**2)**0.5 < 1.8:
            return True
        return False
    def robotneeredge(self, i):
        if (float(self.robot[str(i)][9]) < 5 or float(self.robot[str(i)][9]) > 45 or float(self.robot[str(i)][8]) < 5 or float(self.robot[str(i)][8]) > 45) and Control.robotneercorner(self, i) == False:
            return True
        return False
    # 检测是不是有两个机器人以同一个角落为第一目的地行进
    def testcorner(self):
        for i in range(4):
            for j in range(i+1, 4):
                Oi = float(self.robot[str(i)][7])
                Oj = float(self.robot[str(j)][7])
                if self.robot_stat[i] != 'free' and self.robot_stat[i] == self.robot_stat[j] and Control.wbincorner(self, self.robot_stat[i]) and Control.wbincorner(self, self.robot_stat[j]) and Control.facetocorner(self, Oi, Oj)\
                    or (self.robot_stat[i] != 'free' and self.robot_stat[i] == self.robot_stat[j] and Control.wbinedge(self, self.robot_stat[i]) and Oi > 50/360*math.pi*2 and Oi < 110/360*math.pi*2 and Oj > 50/360*math.pi*2 and Oj < 110/360*math.pi*2\
                       ):
                    itowb = distance(self.WB, self.robot_stat[i], self.robot, str(i))
                    jtowb = distance(self.WB, self.robot_stat[j], self.robot, str(j))
                    itoj = ((float(self.robot[str(i)][8]) - float(self.robot[str(j)][8]))**2 + ((float(self.robot[str(i)][9]) - float(self.robot[str(j)][9]))**2))**0.5
                    # if itowb < jtowb and itoj < 8:
                    #     return j
                    # elif itowb > jtowb and itoj < 8:
                    #     return i 
                    # sys.stderr.write(str(self.robot_stat)+'\n')
                    if itowb > 4 and itowb < 7 :
                        return i
                    if  jtowb > 4 and jtowb < 7:
                        return j

        return -1
    
    # 检测是不是有两个机器人以同一个角落为第一目的地行进
    def testsafedis(self):
        for i in range(4):
            for j in range(i+1, 4):
                Oi = float(self.robot[str(i)][7])
                Oj = float(self.robot[str(j)][7])
                if self.robot_stat[i] != 'free' and self.robot_stat[i] == self.robot_stat[j] and Control.wbincorner(self, self.robot_stat[i]) and Control.wbincorner(self, self.robot_stat[j]) and Control.facetocorner(self, Oi, Oj)\
                    or (self.robot_stat[i] != 'free' and self.robot_stat[i] == self.robot_stat[j] and Control.wbinedge(self, self.robot_stat[i]) and Oi > 50/360*math.pi*2 and Oi < 110/360*math.pi*2 and Oj > 50/360*math.pi*2 and Oj < 110/360*math.pi*2\
                        and (Control.robotneeredge(self, i) or Control.robotneeredge(self, j))):
                    itowb = distance(self.WB, self.robot_stat[i], self.robot, str(i))
                    jtowb = distance(self.WB, self.robot_stat[j], self.robot, str(j))
                    itoj = ((float(self.robot[str(i)][8]) - float(self.robot[str(j)][8]))**2 + ((float(self.robot[str(i)][9]) - float(self.robot[str(j)][9]))**2))**0.5
                    if itowb < jtowb and itoj < self.safedistance:
                        return j
                    elif itowb > jtowb and itoj < self.safedistance:
                        return i 
        return -1
    # robot控制中心
    def robotControl(self):
        
        # # 计算四个机器人与目标的距离
        ls_d = [0] * 4
        for i in range(4):
            if self.robot_stat[i] != 'free':
                x_robot = float(self.robot[str(i)][8])
                y_robot = float(self.robot[str(i)][9])
                x_WB = float(self.WB[self.robot_stat[i]][1])
                y_WB = float(self.WB[self.robot_stat[i]][2])
                ls_d[i] = ((x_robot - x_WB)**2 + (y_robot - y_WB)**2)**0.5
        for i in range(4):
            if self.robot_stat[i] != 'free':
                x_robot = float(self.robot[str(i)][8])
                y_robot = float(self.robot[str(i)][9])
                # 优先检测边缘避障
                if (float(self.robot[str(i)][9]) < 1.5 and float(self.robot[str(i)][7]) > -math.pi*(2/3) and float(self.robot[str(i)][7]) < -math.pi/3)\
                or (float(self.robot[str(i)][9]) > 48.5 and float(self.robot[str(i)][7]) > math.pi/3 and float(self.robot[str(i)][7]) < math.pi*(2/3))\
                or (float(self.robot[str(i)][8]) < 1.5 and float(self.robot[str(i)][7]) > math.pi*(2/3) and float(self.robot[str(i)][7]) < -math.pi*(2/3))\
                or (float(self.robot[str(i)][8]) > 48.5 and float(self.robot[str(i)][7]) > -math.pi/3 and float(self.robot[str(i)][7]) < -math.pi/3):
                    Control.goToedge(self, i, self.robot_stat[i], self.edgespeed, self.edgerotatespeed)   
                elif Control.testsafedis(self) != -1 and i == Control.testsafedis(self):
                    rob = Control.testsafedis(self)
                    Control.goTo_slow(self, rob, self.robot_stat[rob], 0.1)
                    break
                elif Control.testcorner(self) != -1 and i == Control.testcorner(self):
                    rk= Control.testcorner(self)
                    if float(self.robot[str(rk)][8]) > 40 and self.map == 1:
                        Control.goTo_avoid(self, rk, 6, -1)
                    else:
                        Control.goTo_avoid(self, rk, 6, 1)
                    break
                elif ((x_robot - 0)**2 + (y_robot - 0)**2)**0.5 < self.distocorner or ((x_robot - 50)**2 + (y_robot - 0)**2)**0.5 < self.distocorner or\
                ((x_robot - 0)**2 + (y_robot - 50)**2)**0.5 < self.distocorner or ((x_robot - 50)**2 + (y_robot - 50)**2)**0.5 < self.distocorner:
                    Control.goTocorner(self, i, self.robot_stat[i], self.edgespeed, self.edgerotatespeed) 
                elif self.robot_stat[i][0] == '7' and self.robot[str(i)][1] == '0' and int(self.WB[self.robot_stat[i]][3]) > 0 and self.WB[self.robot_stat[i]][5] == '0':
                    x_robot = float(self.robot[str(i)][8])
                    y_robot = float(self.robot[str(i)][9])
                    x_WB7_n = float(self.WB[self.robot_stat[i]][1])
                    y_WB7_n = float(self.WB[self.robot_stat[i]][2])
                    distance_robot_wb7 = ((x_robot - x_WB7_n)**2 + (y_robot - y_WB7_n)**2)**0.5
                    if distance_robot_wb7 <= 0.4:
                        Control.goTo_slow(self, i, self.robot_stat[i], 0.1)
                    else:
                        v = (distance_robot_wb7) / (float(self.WB[self.robot_stat[i]][3])/50/2)
                        Control.goTo(self, i, self.robot_start[i], self.robot_stat[i], v)
                else:   
                        # 等待避障
                        flag = Control.testgoodcansell(self, self.robot[str(i)][1], self.robot_stat[i])
                        # 计算是否有偏移
                        w, v = Control.avoid(self)
                        # k = Control.avoid_long(self)
                        # 其次检测将来是否会碰撞
                        # if k[i] != 0:
                        #     Control.goTo_avoid(self, i, k[i], 0)
                        #     pass
                        if w[i] != 0:
                            Control.goTo_avoid(self, i, 6, w[i])
                        # elif v[i] != 0:
                        #     Control.goTo_avoid(self, i, v[i], 0)
                        else:
                            # 全速前进
                            if self.robot_start[i] == self.robot_stat[i]:
                                Control.goTo_avoid(self, i, 0, 0)
                            else:
                                Control.goTo(self, i, self.robot_start[i], self.robot_stat[i], 6)
            else:
                Control.goTo_avoid(self, i, 0, 0)
                            
                           
    def goTo(self, robot_id, wb_start, wb_destination, line_speed):
        sys.stderr.write(str(wb_start)+str(wb_destination)+'\n')
        # 判定手上有没有货来决定走哪条路径
        if self.robot[str(robot_id)][1] != '0': #有货
            road = 'big'
        else:                                   #没有货
            road = 'small'
        # 获取寻路列表
        # if robot_id == 3:
        #     sys.stderr.write(str(self.robot_stat)+'\n')
        #     sys.stderr.write(str(wb_start)+ '\t'+'\n')
        roadlist = self.road_list[wb_start+'to'+wb_destination]['points_list_'+road]
        if self.robot_step[robot_id] < len(roadlist)-1:#如果是3，那么0，1，2可以进入循环
            x = float(self.robot[str(robot_id)][8]) # 机器人坐标
            y = float(self.robot[str(robot_id)][9])
            a = float(roadlist[self.robot_step[robot_id]][0]) # step个节点的目标坐标
            b = float(roadlist[self.robot_step[robot_id]][1])
            # sys.stderr.write(str(roadlist[self.robot_step[robot_id]])+ '\t'+'\n')
            distance_robot_wb = ((x - a)**2 + (y - b)**2)**0.5
            
            if distance_robot_wb < 2:
                line_speed = 1
            if distance_robot_wb < 1:
                line_speed = 2
            
            o_0= float(self.robot[str(robot_id)][7])  # 当前朝向
            o = math.atan2(b-y, a-x)  # 目标方向的角度
            af = math.atan(0.01/((a-x)**2 + (b-y)**2 + 0.00000001)**0.5)  # 满足0.1距离的偏移角度
            if abs(o-o_0) <= af:
                sys.stdout.write('rotate %d %f\n' % (robot_id, 0))
                sys.stdout.write('forward %d %f\n' % (robot_id, line_speed))
            elif abs(o-o_0) > af and (abs(o-o_0) <= math.pi/10 or math.pi*2-abs(o-o_0) <= math.pi/10):
                # 小于一定阈值转向减速防止震荡
                if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                    theta = o_0-o if o_0 > o and o_0-o < math.pi else math.pi*2-(o-o_0)
                    v = 6 * math.cos(theta)
                    w = math.pi * math.sin(theta)
                    sys.stdout.write('forward %d %f\n' % (robot_id, v))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, -w))  # 负数表示顺时针旋转
                else:
                    theta = math.pi*2-(o_0-o) if o_0 > o and o_0-o > math.pi else o-o_0
                    v = 6 * math.cos(theta)
                    w = math.pi * math.sin(theta)
                    sys.stdout.write('forward %d %f\n' % (robot_id, v))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, w))
            else:
                # 全力转向
                if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                    sys.stdout.write('forward %d %f\n' % (robot_id, self.firstrotatespeed))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, -math.pi))  # 负数表示顺时针旋转
                else:
                    sys.stdout.write('forward %d %f\n' % (robot_id, self.firstrotatespeed))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, math.pi))
            
            if distance_robot_wb < 0.5:
                self.robot_step[robot_id] += 1
        else:# 最后一段节点，前往最终目的地
            x = float(self.robot[str(robot_id)][8]) # 机器人坐标
            y = float(self.robot[str(robot_id)][9])
            a = float(roadlist[self.robot_step[robot_id]][0]) # step个节点的目标坐标
            b = float(roadlist[self.robot_step[robot_id]][1])
            o_0= float(self.robot[str(robot_id)][7])  # 当前朝向
            o = math.atan2(b-y, a-x)  # 目标方向的角度
            af = math.atan(0.1/((a-x)**2 + (b-y)**2)**0.5)  # 满足0.4距离的偏移角度
            distance_robot_wb = ((x - a)**2 + (y - b)**2)**0.5
            if distance_robot_wb < 1:
                line_speed = 1
            if distance_robot_wb < 2 and abs(o-o_0) > af and self.robot_stat[robot_id] == wb_destination:
                # 全力小速度转向
                    if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                        sys.stdout.write('forward %d %f\n' % (robot_id, 0))
                        sys.stdout.write('rotate %d %f\n' % (robot_id, -math.pi))  # 负数表示顺时针旋转
                    else:
                        sys.stdout.write('forward %d %f\n' % (robot_id, 0))
                        sys.stdout.write('rotate %d %f\n' % (robot_id, math.pi))
            else:
                # if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                #         theta = o_0-o if o_0 > o and o_0-o < math.pi else o-o_0 - math.pi
                #         v = 3*math.cos(theta) + 3
                #         r = 0.5*math.pi*(math.cos(v/6*math.pi)+1)
                #         sys.stderr.write(str(v)+'\n')
                #         sys.stdout.write('forward %d %f\n' % (robot_id, v))
                #         sys.stdout.write('rotate %d %f\n' % (robot_id, -r))  # 负数表示顺时针旋转
                # else:
                #         theta = o_0-o-math.pi if o_0 > o and o_0-o > math.pi else o-o_0
                #         v = 3*math.cos(theta) + 3
                #         r = 0.5*math.pi*(math.cos(v/6*math.pi)+1)
                #         sys.stdout.write('forward %d %f\n' % (robot_id, v))
                #         sys.stdout.write('rotate %d %f\n' % (robot_id, r))

                if abs(o-o_0) <= af:
                    sys.stdout.write('rotate %d %f\n' % (robot_id, 0))
                    sys.stdout.write('forward %d %f\n' % (robot_id, line_speed))
                elif abs(o-o_0) > af and abs(o-o_0) <= math.pi/4:
                    # 小于一定阈值转向减速防止震荡
                    if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                        theta = o_0-o if o_0 > o and o_0-o < math.pi else o-o_0 - math.pi
                        v = 6 * math.cos(theta)
                        w = math.pi * math.sin(theta)
                        sys.stdout.write('forward %d %f\n' % (robot_id, v))
                        sys.stdout.write('rotate %d %f\n' % (robot_id, -w))  # 负数表示顺时针旋转
                    else:
                        theta = o_0-o-math.pi if o_0 > o and o_0-o > math.pi else o-o_0
                        v = 6 * math.cos(theta)
                        w = math.pi * math.sin(theta)
                        sys.stdout.write('forward %d %f\n' % (robot_id, v))
                        sys.stdout.write('rotate %d %f\n' % (robot_id, w))
                elif abs(o-o_0) > math.pi/4 and abs(o-o_0) < self.rotaterange:
                    if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                        sys.stdout.write('forward %d %f\n' % (robot_id, 6))
                        sys.stdout.write('rotate %d %f\n' % (robot_id, -math.pi))  # 负数表示顺时针旋转
                    else:
                        sys.stdout.write('forward %d %f\n' % (robot_id, 6))
                        sys.stdout.write('rotate %d %f\n' % (robot_id, math.pi))
                elif abs(o-o_0) > self.rotaterange and abs(o-o_0) < math.pi*3/4:
                    # 全力转向
                    if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                        sys.stdout.write('forward %d %f\n' % (robot_id, self.rotatespeed))
                        sys.stdout.write('rotate %d %f\n' % (robot_id, -math.pi))  # 负数表示顺时针旋转
                    else:
                        sys.stdout.write('forward %d %f\n' % (robot_id, self.rotatespeed))
                        sys.stdout.write('rotate %d %f\n' % (robot_id, math.pi))
                else:
                    # 全力转向
                    if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                        sys.stdout.write('forward %d %f\n' % (robot_id, self.firstrotatespeed))
                        sys.stdout.write('rotate %d %f\n' % (robot_id, -math.pi))  # 负数表示顺时针旋转
                    else:
                        sys.stdout.write('forward %d %f\n' % (robot_id, self.firstrotatespeed))
                        sys.stdout.write('rotate %d %f\n' % (robot_id, math.pi))
    
    def goToedge(self, robot_id, wb_destination, line_speed, edgerotatespeed):
        x = float(self.robot[str(robot_id)][8]) # 机器人坐标
        y = float(self.robot[str(robot_id)][9])
        a = float(self.WB[wb_destination][1]) # 目标坐标
        b = float(self.WB[wb_destination][2])
        o_0= float(self.robot[str(robot_id)][7])  # 当前朝向
        o = math.atan2(b-y, a-x)  # 目标方向的角度
        af = math.atan(0.3/((a-x)**2 + (b-y)**2)**0.5)  # 满足0.4距离的偏移角度
        distance_robot_wb = ((x - a)**2 + (y - b)**2)**0.5
        if distance_robot_wb < 2 and abs(o-o_0) > af and self.robot_stat[robot_id] == wb_destination:
            # 全力小速度转向
                if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                    sys.stdout.write('forward %d %f\n' % (robot_id, 0))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, -math.pi))  # 负数表示顺时针旋转
                else:
                    sys.stdout.write('forward %d %f\n' % (robot_id, 0))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, math.pi))
        else:
            if abs(o-o_0) <= af:
                sys.stdout.write('rotate %d %f\n' % (robot_id, 0))
                sys.stdout.write('forward %d %f\n' % (robot_id, line_speed))
            elif abs(o-o_0) > af and abs(o-o_0) <= af*4:
                # 小于一定阈值转向减速防止震荡
                if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                    sys.stdout.write('forward %d %f\n' % (robot_id, 6))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, -self.rotateshake))  # 负数表示顺时针旋转
                else:
                    sys.stdout.write('forward %d %f\n' % (robot_id, 6))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, self.rotateshake))
            elif abs(o-o_0) > af*4 and abs(o-o_0) < self.rotaterange:
                if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                    sys.stdout.write('forward %d %f\n' % (robot_id, 6))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, -math.pi))  # 负数表示顺时针旋转
                else:
                    sys.stdout.write('forward %d %f\n' % (robot_id, 6))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, math.pi))
            else:
                # 全力转向
                if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                    sys.stdout.write('forward %d %f\n' % (robot_id, edgerotatespeed))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, -math.pi))  # 负数表示顺时针旋转
                else:
                    sys.stdout.write('forward %d %f\n' % (robot_id, edgerotatespeed))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, math.pi))

    def goTocorner(self, robot_id, wb_destination, line_speed, cornerrotatespeed):
        x = float(self.robot[str(robot_id)][8]) # 机器人坐标
        y = float(self.robot[str(robot_id)][9])
        a = float(self.WB[wb_destination][1]) # 目标坐标
        b = float(self.WB[wb_destination][2])
        o_0= float(self.robot[str(robot_id)][7])  # 当前朝向
        o = math.atan2(b-y, a-x)  # 目标方向的角度
        af = math.atan(0.3/((a-x)**2 + (b-y)**2)**0.5)  # 满足0.4距离的偏移角度
        distance_robot_wb = ((x - a)**2 + (y - b)**2)**0.5
        if distance_robot_wb < 2 and abs(o-o_0) > af and self.robot_stat[robot_id] == wb_destination:
            # 全力小速度转向
                if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                    sys.stdout.write('forward %d %f\n' % (robot_id, 0))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, -math.pi))  # 负数表示顺时针旋转
                else:
                    sys.stdout.write('forward %d %f\n' % (robot_id, 0))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, math.pi))
        else:
            if abs(o-o_0) <= af:
                sys.stdout.write('rotate %d %f\n' % (robot_id, 0))
                sys.stdout.write('forward %d %f\n' % (robot_id, line_speed))
            elif abs(o-o_0) > af and abs(o-o_0) <= af*4:
                # 小于一定阈值转向减速防止震荡
                if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                    sys.stdout.write('forward %d %f\n' % (robot_id, 6))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, -self.rotateshake))  # 负数表示顺时针旋转
                else:
                    sys.stdout.write('forward %d %f\n' % (robot_id, 6))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, self.rotateshake))
            elif abs(o-o_0) > af*4 and abs(o-o_0) < 90/360*math.pi*2:
                if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                    sys.stdout.write('forward %d %f\n' % (robot_id, 6))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, -math.pi))  # 负数表示顺时针旋转
                else:
                    sys.stdout.write('forward %d %f\n' % (robot_id, 6))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, math.pi))
            elif abs(o-o_0) > 90/360*math.pi*2 and abs(o-o_0) < 100/360*math.pi*2:
                if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                    sys.stdout.write('forward %d %f\n' % (robot_id, 6))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, -math.pi/2))  # 负数表示顺时针旋转
                else:
                    sys.stdout.write('forward %d %f\n' % (robot_id, 6))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, math.pi/2))
            else:
                # 全力转向
                if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                    sys.stdout.write('forward %d %f\n' % (robot_id, cornerrotatespeed))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, -math.pi))  # 负数表示顺时针旋转
                else:
                    sys.stdout.write('forward %d %f\n' % (robot_id, cornerrotatespeed))
                    sys.stdout.write('rotate %d %f\n' % (robot_id, math.pi))

    def goTo_slow(self, robot_id, wb_destination, speed):
        x = float(self.robot[str(robot_id)][8]) # 机器人坐标
        y = float(self.robot[str(robot_id)][9])
        a = float(self.WB[wb_destination][1]) # 目标坐标
        b = float(self.WB[wb_destination][2])
        o_0= float(self.robot[str(robot_id)][7])  # 当前朝向
        o = math.atan2(b-y, a-x)  # 目标方向的角度
        af = math.atan(0.4/((a-x)**2 + (b-y)**2)**0.5)  # 满足0.4距离的偏移角度
        if abs(o-o_0) <= af:
            sys.stdout.write('rotate %d %f\n' % (robot_id, 0))
            sys.stdout.write('forward %d %f\n' % (robot_id, speed))
        else:
            if (o_0 > o and o_0-o < math.pi) or (o_0 < o and o-o_0 > math.pi):  #如果顺时针转更近则顺时针
                sys.stdout.write('forward %d %f\n' % (robot_id, 1))
                sys.stdout.write('rotate %d %f\n' % (robot_id, -1))  # 负数表示顺时针旋转
            else:
                sys.stdout.write('forward %d %f\n' % (robot_id, 1))
                sys.stdout.write('rotate %d %f\n' % (robot_id, 1))
    
    def goTo_avoid(self, robot_id, linespeed, rotatespeed):
        
        sys.stdout.write('rotate %d %f\n' % (robot_id, rotatespeed))
        sys.stdout.write('forward %d %f\n' % (robot_id, linespeed))
        
    def sampling(self):
        #计算4个机器人当前位置到最后一个节点之间的密集采样点
        dic = {}
        for rbid in range(4):
            if self.robot_stat[rbid] != 'free':
                lsi = []
                if self.robot[str(rbid)][1] != '0': #有货
                    road = 'big'
                else:                                   #没有货
                    road = 'small'
                roadlist = self.road_list[self.robot_start[rbid]+'to'+self.robot_stat[rbid]]['points_list_'+road]
                x = float(self.robot[str(rbid)][8]) # 机器人坐标
                y = float(self.robot[str(rbid)][9])
                a = float(roadlist[self.robot_step[rbid]][0]) # 当前step节点的目标坐标
                b = float(roadlist[self.robot_step[rbid]][1])
                distance_robot_wb = ((x - a)**2 + (y - b)**2)**0.5
                if distance_robot_wb < 1:
                    lsi.append(roadlist[self.robot_step[rbid]])
                else:
                    for i in range(int(distance_robot_wb*2)):#向下取整，得到采样个数
                        xi = x + (a-x)/2*i
                        yi = y + (b-y)/2*i
                        lsi.append([xi, yi])
                for i in range(self.robot_step[rbid]+1, len(roadlist)):
                    x = float(roadlist[i-1][0]) # 节点初始坐标
                    y = float(roadlist[i-1][1])
                    a = float(roadlist[i][0]) # 节点的结束坐标
                    b = float(roadlist[i][1])
                    distance_robot_wb = ((x - a)**2 + (y - b)**2)**0.5
                    if distance_robot_wb < 1:
                        lsi.append(roadlist[i])
                    else:
                        for i in range(int(distance_robot_wb)):#向下取整，得到采样个数
                            xi = x + (a-x)*i
                            yi = y + (b-y)*i
                            lsi.append([xi, yi])
                dic[rbid] = lsi
        return dic

    def avoid_long(self):
        v = [0, 0, 0, 0]
        dic = Control.sampling(self) # 输出采样点
        for i in range(4):
            for j in range(i+1, 4):
                if self.robot_stat[i] != 'free' and self.robot_stat[j] != 'free':
                    sampi = dic[i]
                    sampj = dic[j]
                    disi = 0
                    disj = 0
                    for t in range(1, 20 if len(sampi) > 20 and len(sampj) > 20 else int(min(len(sampi), len(sampj))/2)):
                        disi += ((sampi[t][0] - sampi[t-1][0])**2 + (sampi[t][1] - sampi[t-1][1])**2) ** 0.5
                        disj += ((sampj[t][0] - sampj[t-1][0])**2 + (sampj[t][1] - sampj[t-1][1])**2) ** 0.5
                        xi = sampi[t][0]
                        yi = sampi[t][1]
                        xj = sampj[t][0]
                        yj = sampj[t][1]
                        distance = ((xi - xj)**2 + (yi - yj)**2) ** 0.5
                        
                        if distance < 1.3:
                            if disi < disj:
                                v[j] = 0.1
                            else:
                                v[i] = 0.1
                                # dis_itokick = ((ix - xi)**2 + (iy - yi)**2) ** 0.5
                                # dis_jtokick = ((jx - xj)**2 + (jy - yj)**2) ** 0.5
                                # if dis_itokick <= dis_jtokick:
                                #     v[j] = self.Vt
                                # else:
                                #     v[i] = self.Vt
             
        # sys.stderr.write(str(w)+'\n')
        return v

    def avoid(self):
        w = [0, 0, 0, 0]
        v = [0, 0, 0, 0]
        for t in range(10):
            ls = []
            for i in range(4):
                x = float(self.robot[str(i)][8]) # x机器人坐标
                y = float(self.robot[str(i)][9])
                vx = float(self.robot[str(i)][5])
                vy = float(self.robot[str(i)][6])
                # 计算未来的位置
                xi = x + vx * self.N * t
                yi = y + vy * self.N * t
                ls.append([xi, yi])
            for i in range(4):
                for j in range(i+1, 4):
                    xi = ls[i][0]
                    yi = ls[i][1]
                    xj = ls[j][0]
                    yj = ls[j][1]
                    distance = ((xi - xj)**2 + (yi - yj)**2) ** 0.5
                    
                    ix = float(self.robot[str(i)][8]) # x机器人坐标
                    iy = float(self.robot[str(i)][9])
                    jx = float(self.robot[str(j)][8]) # y机器人坐标
                    jy = float(self.robot[str(j)][9]) 
                    # 角度差
                    a = abs(float(self.robot[str(i)][7]) - float(self.robot[str(j)][7]))
                    # 145<夹角<225度正撞，输出角速度偏移避让
                    if distance < 1.06 and a >= math.pi*3/4 and a <= math.pi*5/4:
                        o = math.atan2(jy-iy, jx-ix) # 机器人i指向机器人j的向量角度
                        if float(self.robot[str(i)][7]) - o <= 0:
                            w[i] = -self.Wt
                            w[j] = -self.Wt
                        elif float(self.robot[str(i)][7]) - o > 0:
                            w[i] = self.Wt
                            w[j] = self.Wt
                    # 45<夹角<135度,225<夹角<315度侧装，撞击者向被撞击者运动后方转向
                    elif distance < 1.06 and ((a >= math.pi/4 and a <= math.pi*3/4) or (a >= math.pi*5/4 and a <= math.pi*7/4)) and self.robot_stat[i] != self.robot_stat[j]:
                        
                        # dis_itokick = ((ix - xi)**2 + (iy - yi)**2) ** 0.5
                        # dis_jtokick = ((jx - xj)**2 + (jy - yj)**2) ** 0.5
                        # if dis_itokick <= dis_jtokick:
                        #     v[j] = self.Vt
                        # else:
                        #     v[i] = self.Vt
                        if float(self.robot[str(i)][7]) - float(self.robot[str(j)][7]) > 0:
                            w[i] = self.Wt
                            w[j] = self.Wt
                        elif float(self.robot[str(i)][7]) - float(self.robot[str(j)][7]) < 0:
                            w[i] = -self.Wt
                            w[j] = -self.Wt
                    # 0<夹角<45度,315<夹角<360度冲突，按距离目标距离避让
                    elif distance < 1.06:
                        if self.robot_stat[i] == self.robot_stat[j] and self.robot_stat[i] != 'free' and self.robot_stat[j] != 'free'\
                            and Control.testgoodcansell(self, self.robot[str(i)][1], self.robot_stat[i]) and Control.testgoodcansell(self, self.robot[str(j)][1], self.robot_stat[j]): # 目的地相同才会启动
                            # 手里货便宜的优先避让
                            # if int(self.robot[str(i)][1]) > int(self.robot[str(j)][1]) and self.robot[str(i)][1] != '0' and self.robot[str(j)][1] != '0':
                            #     v[j] = self.Vt
                            # elif int(self.robot[str(i)][1]) < int(self.robot[str(j)][1]) and self.robot[str(i)][1] != '0' and self.robot[str(j)][1] != '0':
                            #     v[i] = self.Vt
                            # 手里货一样，远的避让
                            # else:
                            
                            aim_i = self.robot_stat[i]
                            aim_j = self.robot_stat[j]
                            dis_i = self.distance_dic[i][aim_i[0]][int(aim_i[-1])]
                            dis_j = self.distance_dic[j][aim_j[0]][int(aim_j[-1])]
                            if dis_i >= dis_j:
                                v[i] = self.Vt
                            else:
                                v[j] = self.Vt
                    elif distance < 1.06 and self.robot_stat[i] != self.robot_stat[j] and self.robot_stat[i] != 'free' and self.robot_stat[j] != 'free':
                            dis_itokick = ((ix - xi)**2 + (iy - yi)**2) ** 0.5
                            dis_jtokick = ((jx - xj)**2 + (jy - yj)**2) ** 0.5
                            v[j] = self.Vt
                            # if dis_itokick <= dis_jtokick:
                            #     v[j] = self.Vt
                            # else:
                            #     v[i] = self.Vt
                       


                    
        # sys.stderr.write(str(w)+'\n')
        return w, v