def get_c_ore(self, in_can_info):
        # print "input", in_can_info
        # print in_can_info[0].pos
        t_c_order = []
        for ci in range(len(in_can_info)):
            tm_tar_pos = in_can_info[ci].pos
            tm_obs_pos = copy.deepcopy(self.obs_pos)
            tm_obs_pos.append(self.tar_pos)
            # tm_obs_pos.extend(self.obs_wall)
            tm_ob = len(tm_obs_pos)
            tm_obs_ori = []
            for i in range(tm_ob):
                tm_obs_ori.append([0.0, 0.0, 0.0])

            ore_order = NG_ore(
                tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos,
                self.ws_zero,
                [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
            # ore_order = TM_noplot(tm_ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall,self.rob_pos, self.rob_pos, self.d_max)
            while 1:
                tm_tar_pos = in_can_info[ci].pos
                tm_obs_pos = copy.deepcopy(self.obs_pos)
                tm_obs_pos.append(self.tar_pos)
                for i in ore_order:
                    if i != 'T':
                        tm_obs_pos[i] = [3.0, 3.0]

                tmp_order = NG_ore(
                    tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r,
                    self.rob_pos, self.ws_zero,
                    [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
                # tmp_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall,self.rob_pos, self.rob_pos, self.d_max)
                # print "after removing:", tmp_order
                if tmp_order[0] != 'T':
                    print "tricky environment for c_ore so extend", ore_order
                    ore_order.pop()
                    print "delete target", ore_order
                    ore_order.extend(tmp_order)
                    print "to", ore_order
                else:
                    if tmp_order[0] == 'T':
                        print "ok", ore_order
                        break
            ore_order.pop()

            if len(tm_obs_pos) in ore_order:
                print "\n\nThere is target!! warning!!!\n\n"
                t_c_order = []
            else:
                t_c_order.append(ore_order)
        return t_c_order
class EnvInfo:

    def __init__(self, rob_pos, ws_width, ws_depth, ws_cen, grid_size, wall_r):
        self.rob_pos = rob_pos
        self.GRID_SIZE = grid_size
        self.ws_w = ws_width
        self.ws_d = ws_depth
        self.ws_cen = ws_cen
        self.ws_zero = [round(self.ws_cen[0] - ws_width * self.GRID_SIZE * 0.5, 2), round(self.ws_cen[1] - ws_depth * self.GRID_SIZE * 0.5, 2)]
        self.obs_wall = self.get_obs_wall(OBJ_R=wall_r)

        self.order_error_flag = 1
        self.d_max = 2.0
        self.eta = 30

        grid_act = np.zeros([ws_width, ws_depth])
        self.grid_act = CUF.mark_edge_grid(grid_act)

    def get_env(self, obs_r, tar_r, min_ore):
        while 1:
            self.obs_grid = []
            grid_tmp = copy.deepcopy(self.grid_act)
            self.obs_r = obs_r
            self.tar_r = tar_r
            for ri in self.obs_r:
                grid_tmp, obs_center_tmp = CUF.place_circle_object_ig(grid_tmp, ri, 2)
                self.obs_grid.append(obs_center_tmp)
            grid_tmp, tar_tmp = CUF.place_circle_object_ig(grid_tmp, self.tar_r, 4)
            self.tar_grid = copy.deepcopy(tar_tmp)

            self.obs_pos = []
            for i in self.obs_grid:
                xi, yi = i
                self.obs_pos.append([round(xi * self.GRID_SIZE + self.ws_zero[0], 2), round(yi * self.GRID_SIZE + self.ws_zero[1], 2)])
            self.tar_pos = [round(self.tar_grid[0] * self.GRID_SIZE + self.ws_zero[0], 2), round(self.tar_grid[1] * self.GRID_SIZE + self.ws_zero[1], 2)]  # target object!

            self.d_max = 2.0
            tm_tar_pos = copy.deepcopy(self.tar_pos)
            tm_tar_ori = [0.0, 0.0, 0.0]
            tm_obs_pos = copy.deepcopy(self.obs_pos)
            # tm_obs_pos.extend(self.obs_wall)
            ob = len(tm_obs_pos)
            tm_obs_ori = []
            for i in range(ob):
                tm_obs_ori.append([0.0, 0.0, 0.0])

            ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
            # ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.rob_pos, self.rob_pos, self.d_max)
            # ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall, self.rob_pos, self.rob_pos, self.d_max)
            # TM_noplot()
            if len(ore_order) > min_ore:
                # print"before rearrangemet: ", ore_order
                tm_tar_pos = copy.deepcopy(self.tar_pos)
                tm_obs_pos = copy.deepcopy(self.obs_pos)
                # tm_obs_pos.extend(self.obs_wall)
                # ob = len(tm_obs_pos)
                self.ore_order = ore_order
                # self.ore_order = TM_plot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.rob_pos, self.rob_pos, self.d_max)

                # check if the obstacles are rearranged then target is reachable
                tm_tar_pos = copy.deepcopy(self.tar_pos)
                tm_obs_pos = copy.deepcopy(self.obs_pos)
                tm_ore_pos = []
                for i in self.ore_order:
                    if i != 'T':
                        tm_obs_pos.remove(self.obs_pos[i])
                # tm_obs_pos.extend(self.obs_wall)
                ob = len(tm_obs_pos)
                tmp_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
                # tmp_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall,self.rob_pos, self.rob_pos, self.d_max)
                # print "after removing:", tmp_order
                if tmp_order[0] == 'T':
                    # print"environment setting OK"
                    break
            # else:
                # print "SHORT...min:", min_ore, "ours:", len(ore_order)
        '''
        with out additional rearrangement
        '''
        if len(self.ore_order) > min_ore:
            self.ore_grid = []
            self.ore_pos = []
            self.ore_r = []

            self.obs_re_grid = copy.deepcopy(self.obs_grid)
            self.obs_re_pos = copy.deepcopy(self.obs_pos)
            self.obs_re_r = copy.deepcopy(self.obs_r)

            for i in self.ore_order:
                if i != 'T':
                    self.ore_grid.append(self.obs_re_grid[i])
                    self.ore_pos.append(self.obs_re_pos[i])
                    self.ore_r.append(self.obs_re_r[i])
            for i in self.ore_order:
                if i != 'T':
                    self.obs_re_grid.remove(self.obs_grid[i])
                    self.obs_re_pos.remove(self.obs_pos[i])
                    self.obs_re_r.remove(self.obs_r[i])

            self.grid_ori = copy.deepcopy(self.grid_act)
            for i in range(len(self.obs_r)):
                self.grid_ori = CUF.obstacle_circle(self.grid_ori, [round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2), self.obs_r[i]], 2)
            self.grid_ori = CUF.obstacle_circle(self.grid_ori, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4)  # target

            self.grid_del = copy.deepcopy(self.grid_act)
            for i in range(len(self.obs_re_r)):
                self.grid_del = CUF.obstacle_circle(self.grid_del, [round(self.obs_re_grid[i][0], 2), round(self.obs_re_grid[i][1], 2), self.obs_re_r[i]], 2)
            self.grid_del = CUF.obstacle_circle(self.grid_del, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4)  # target
            self.ore_order.pop()

    def get_env_case1(self):
        while 1:
            obs_r = [0.035, 0.035, 0.035, 0.035, 0.035, 0.035, 0.035, 0.035]
            tar_r = 0.035
            min_ore = 2
            self.obs_grid = []
            grid_tmp = copy.deepcopy(self.grid_act)
            self.obs_r = obs_r
            self.tar_r = tar_r

            self.obs_grid = [[10, 70], [10, 50], [10, 10],
                             [20, 70], [20, 50], [19, 31], [24, 25], [20, 10]]
            self.tar_grid = [30, 30]

            # self.grid_ori = copy.deepcopy(self.grid_act)
            # for i in range(len(self.obs_r)):
            #     print "obs", i, round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2)
            #     self.grid_ori = CUF.obstacle_circle(self.grid_ori, [round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2), self.obs_r[i]], 2)
            # self.grid_ori = CUF.obstacle_circle(self.grid_ori, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4)  # target
            # break
            # for ri in self.obs_r:
            #     grid_tmp, obs_center_tmp = CUF.place_circle_object_ig(grid_tmp, ri, 2)
            #     self.obs_grid.append(obs_center_tmp)
            # grid_tmp, tar_tmp = CUF.place_circle_object_ig(grid_tmp, self.tar_r, 4)
            # self.tar_grid = copy.deepcopy(tar_tmp)

            self.obs_pos = []
            for i in self.obs_grid:
                xi, yi = i
                self.obs_pos.append([round(xi * self.GRID_SIZE + self.ws_zero[0], 2), round(yi * self.GRID_SIZE + self.ws_zero[1], 2)])
            self.tar_pos = [round(self.tar_grid[0] * self.GRID_SIZE + self.ws_zero[0], 2), round(self.tar_grid[1] * self.GRID_SIZE + self.ws_zero[1], 2)]  # target object!

            self.d_max = 2.0
            tm_tar_pos = copy.deepcopy(self.tar_pos)
            tm_tar_ori = [0.0, 0.0, 0.0]
            tm_obs_pos = copy.deepcopy(self.obs_pos)
            # tm_obs_pos.extend(self.obs_wall)
            ob = len(tm_obs_pos)
            tm_obs_ori = []
            for i in range(ob):
                tm_obs_ori.append([0.0, 0.0, 0.0])

            # ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall, self.rob_pos, self.rob_pos, self.d_max)
            ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
            if len(ore_order) > min_ore:
                # print"environment setting OK"
                # print"target", self.tar_pos
                # print"obstacles", self.obs_pos
                # print"remove", ore_order, "th obstacles"
                tm_tar_pos = copy.deepcopy(self.tar_pos)
                tm_obs_pos = copy.deepcopy(self.obs_pos)
                # tm_obs_pos.extend(self.obs_wall)
                ob = len(tm_obs_pos)
                # self.ore_order = ore_order
                self.ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall, self.rob_pos, self.rob_pos, self.d_max)

                # check if the obstacles are rearranged then target is reachable
                tm_tar_pos = copy.deepcopy(self.tar_pos)
                tm_obs_pos = copy.deepcopy(self.obs_pos)
                tm_ore_pos = []
                for i in self.ore_order:
                    if i != 'T':
                        tm_ore_pos.append(self.obs_pos[i])
                for i in self.ore_order:
                    if i != 'T':
                        tm_obs_pos.remove(self.obs_pos[i])
                # tm_obs_pos.extend(self.obs_wall)
                ob = len(tm_obs_pos)

                # tmp_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
                tmp_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall, self.rob_pos, self.rob_pos, self.d_max)
                if len(tmp_order) == 1:
                    print"environment setting OK"
                    break
        '''
        # with out additional rearrangement
        '''
        if len(self.ore_order) > min_ore:
            self.ore_grid = []
            self.ore_pos = []
            self.ore_r = []

            self.obs_re_grid = copy.deepcopy(self.obs_grid)
            self.obs_re_pos = copy.deepcopy(self.obs_pos)
            self.obs_re_r = copy.deepcopy(self.obs_r)

            for i in self.ore_order:
                if i != 'T':
                    self.ore_grid.append(self.obs_re_grid[i])
                    self.ore_pos.append(self.obs_re_pos[i])
                    self.ore_r.append(self.obs_re_r[i])
            for i in self.ore_order:
                if i != 'T':
                    self.obs_re_grid.remove(self.obs_grid[i])
                    self.obs_re_pos.remove(self.obs_pos[i])
                    self.obs_re_r.remove(self.obs_r[i])

            self.grid_ori = copy.deepcopy(self.grid_act)
            for i in range(len(self.obs_r)):
                self.grid_ori = CUF.obstacle_circle(self.grid_ori, [round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2), self.obs_r[i]], 2)
            self.grid_ori = CUF.obstacle_circle(self.grid_ori, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4)  # target

            self.grid_del = copy.deepcopy(self.grid_act)
            for i in range(len(self.obs_re_r)):
                self.grid_del = CUF.obstacle_circle(self.grid_del, [round(self.obs_re_grid[i][0], 2), round(self.obs_re_grid[i][1], 2), self.obs_re_r[i]], 2)
            self.grid_del = CUF.obstacle_circle(self.grid_del, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4)  # target
            self.ore_order.pop()

    def get_max_can_case1(self):
        # [19, 35], [24, 25]
        self.can_grid = [[30, 70], [30, 50], [14, 25], [30, 10]]
        self.can_pos = []
        circle_r = max(self.ore_r)+0.005
        for i in self.can_grid:
            xi, yi = i
            self.can_pos.append([self.ws_zero[0] + xi * self.GRID_SIZE, self.ws_zero[1] + yi * self.GRID_SIZE])

        self.grid_max_can = copy.deepcopy(self.grid_act)
        for i in range(len(self.obs_r)):
            self.grid_max_can = CUF.obstacle_circle(self.grid_max_can, [round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2), self.obs_r[i]], 2)
        self.grid_max_can = CUF.obstacle_circle(self.grid_max_can, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4)  # target
        for i in range(len(self.can_grid)):
            self.grid_max_can = CUF.obstacle_circle(self.grid_max_can, [self.can_grid[i][0], self.can_grid[i][1], circle_r], 3)

    def update_env(self, in_obs_pos, in_obs_grid):
        self.d_max = 2.0
        tm_tar_pos = copy.deepcopy(self.tar_pos)
        tm_tar_ori = [0.0, 0.0, 0.0]
        tm_obs_pos = copy.deepcopy(in_obs_pos)
        # tm_obs_pos.extend(self.obs_wall)
        ob = len(tm_obs_pos)
        tm_obs_ori = []
        for i in range(ob):
            tm_obs_ori.append([0.0, 0.0, 0.0])

        self.ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
        # self.ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall, self.rob_pos, self.rob_pos, self.d_max)

        # check if the obstacles are rearranged then target is reachable
        while 1:
            tm_tar_pos = copy.deepcopy(self.tar_pos)
            tm_obs_pos = copy.deepcopy(self.obs_pos)
            tm_ore_pos = []
            for i in self.ore_order:
                if i != 'T':
                    tm_obs_pos[i] = [5.0, 0.0]
                    # tm_obs_pos.remove(self.obs_pos[i])
            # tm_obs_pos.extend(self.obs_wall)

            tmp_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
            # tmp_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall,self.rob_pos, self.rob_pos, self.d_max)
            # print "after removing:", tmp_order
            if tmp_order[0] != 'T':
                # print "tricky environment so extend", self.ore_order
                self.ore_order.pop()
                # print "delete target", self.ore_order
                self.ore_order.extend(tmp_order)
                # print "to", self.ore_order
            else:
                if tmp_order[0] == 'T':
                    # print "ok", self.ore_order
                    break

        self.ore_grid = []
        self.ore_pos = []
        self.ore_r = []

        self.obs_re_grid = copy.deepcopy(in_obs_grid)
        self.obs_re_pos = copy.deepcopy(in_obs_pos)
        self.obs_re_r = copy.deepcopy(self.obs_r)

        for i in self.ore_order:
            if i != 'T':
                self.ore_grid.append(self.obs_re_grid[i])
                self.ore_pos.append(self.obs_re_pos[i])
                self.ore_r.append(self.obs_re_r[i])
        for i in self.ore_order:
            if i != 'T':
                self.obs_re_grid.remove(self.obs_grid[i])
                self.obs_re_pos.remove(self.obs_pos[i])
                self.obs_re_r.remove(self.obs_r[i])

        self.grid_ori = copy.deepcopy(self.grid_act)
        for i in range(len(self.obs_r)):
            self.grid_ori = CUF.obstacle_circle(self.grid_ori, [round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2), self.obs_r[i]], 2)
        self.grid_ori = CUF.obstacle_circle(self.grid_ori, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4)  # target

        self.grid_del = copy.deepcopy(self.grid_act)
        for i in range(len(self.obs_re_r)):
            self.grid_del = CUF.obstacle_circle(self.grid_del, [round(self.obs_re_grid[i][0], 2), round(self.obs_re_grid[i][1], 2), self.obs_re_r[i]], 2)
        self.grid_del = CUF.obstacle_circle(self.grid_del, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4)  # target
        self.ore_order.pop()
        # else:
        #     self.order_error_flag = 0

    def get_max_can(self, input_grid, bt_num, trial_num, ):
        bt_circle = []
        circle_r = max(self.ore_r)+0.03
        for bt in range(bt_num):
            can_grid = []
            grid_can = copy.deepcopy(input_grid)  # get original scene from the grid_set
            empt_grid, occu_grid = CUF.getEmpOcc(grid_can)
            for i in range(trial_num):
                pick_cen = np.random.randint(0, len(empt_grid))
                check_sum = 0
                for oc in range(len(occu_grid)):
                    d_w = empt_grid[pick_cen][0] - occu_grid[oc][0]
                    d_d = empt_grid[pick_cen][1] - occu_grid[oc][1]
                    d_c = (d_w * d_w + d_d * d_d) ** 0.5 * self.GRID_SIZE
                    if d_c <= circle_r:
                        check_sum = 1

                if check_sum == 0:
                    can_grid.append(empt_grid[pick_cen])
                    for em in range(len(empt_grid)):
                        d_w = empt_grid[pick_cen][0] - empt_grid[em][0]
                        d_d = empt_grid[pick_cen][1] - empt_grid[em][1]
                        d_c = (d_w * d_w + d_d * d_d) ** 0.5 * self.GRID_SIZE
                        if d_c <= circle_r:
                            grid_can[empt_grid[em][0]][empt_grid[em][1]] = 3
                            grid_can[empt_grid[pick_cen][0]][empt_grid[pick_cen][1]] = 3
                            occu_grid.append([empt_grid[em][0], empt_grid[em][1]])
            bt_circle.append([can_grid, grid_can])

        max_cir_num = []
        for i in range(len(bt_circle)):
            max_cir_num.append([len(bt_circle[i][0])])

        # print(max_cir_num.index(max(max_cir_num)))
        max_trial = max_cir_num.index(max(max_cir_num))

        self.grid_max_can = copy.deepcopy(bt_circle[max_trial][1])
        t_can_grid = bt_circle[max_trial][0]

        can_dist = []
        for i in t_can_grid:
            can_dist.append(np.sqrt((self.tar_pos[0] - i[0]) ** 2 + (self.tar_pos[1] - i[1]) ** 2))
        sort_can_dist = copy.deepcopy(can_dist)
        sort_can_dist.sort(reverse=True)
        can_grid = []
        for i in range(len(can_dist)):
            can_grid.append(t_can_grid[can_dist.index(sort_can_dist[i])])
        self.can_grid = can_grid

        self.can_pos = []
        for i in self.can_grid:
            xi, yi = i
            self.can_pos.append([self.ws_zero[0] + xi * self.GRID_SIZE, self.ws_zero[1] + yi * self.GRID_SIZE])

    def get_obs_wall(self, OBJ_R):
        ws_side = []
        ws_side.append(
            [self.ws_cen[0] - self.ws_w * self.GRID_SIZE * 0.5, self.ws_cen[1] - self.ws_d * self.GRID_SIZE * 0.5 - OBJ_R])  # left low point
        ws_side.append([self.ws_cen[0] + self.ws_w * self.GRID_SIZE * 0.5 + OBJ_R,
                        self.ws_cen[1] - self.ws_d * self.GRID_SIZE * 0.5 - OBJ_R])  # right low point
        ws_side.append([self.ws_cen[0] + self.ws_w * self.GRID_SIZE * 0.5 + OBJ_R,
                        self.ws_cen[1] + self.ws_d * self.GRID_SIZE * 0.5 + OBJ_R])  # right high point
        ws_side.append(
            [self.ws_cen[0] - self.ws_w * self.GRID_SIZE * 0.5, self.ws_cen[1] + self.ws_d * self.GRID_SIZE * 0.5 + OBJ_R])  # left high point

        obs_wall = []
        obs_wall.extend(CUF.linspace2D(ws_side[0], ws_side[1], round(self.ws_w * self.GRID_SIZE / OBJ_R)))
        obs_wall.extend(CUF.linspace2D(ws_side[1], ws_side[2], round(self.ws_d * self.GRID_SIZE / OBJ_R)))
        obs_wall.extend(CUF.linspace2D(ws_side[2], ws_side[3], round(self.ws_w * self.GRID_SIZE / OBJ_R)))
        return obs_wall

    def get_can_info(self, in_can_info, in_obs_pos, in_obs_re_pos, in_ore_order, in_tar_pos):
        tmp_can_info = []
        for i in range(len(in_ore_order)):
            tmp_can_info.append(copy.deepcopy(in_can_info))
        tmp_obs_pos = copy.deepcopy(in_obs_pos)
        tmp_obs_re_pos = copy.deepcopy(in_obs_re_pos)
        tmp_ore_order = copy.deepcopy(in_ore_order)
        tmp_tar_pos = copy.deepcopy(in_tar_pos)
        # print("\nCheck if candidate blocks the target")
        for step_i in range(len(tmp_ore_order)):
            for i in range(len(tmp_can_info[step_i])):
                vfh_obs_pos = copy.deepcopy(tmp_obs_re_pos)
                vfh_obs_pos.append(tmp_can_info[step_i][i].pos)
                vfh_obs_pos.extend(self.obs_wall)
                vfh_tar_pos = copy.deepcopy(tmp_tar_pos)
                ob = len(vfh_obs_pos)
                vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta)
                # vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.obs_r, self.tar_r)
                if vfh[3] == 0:
                    tmp_can_info[step_i][i].BT = 1  # BT == 1 : The candidate blocks the target.
                else:                       # BT == 0 : The candidate does not block the target.
                    tmp_can_info[step_i][i].BT = 0

            # print("\nCheck if the candidate is accessible.")
            for i in range(len(tmp_can_info[step_i])):
                vfh_tar_pos = copy.deepcopy(tmp_can_info[step_i][i].pos)
                vfh_obs_pos = copy.deepcopy(tmp_obs_pos)
                for si in range(step_i+1):
                    # print "\nstep", si, "\nbefore", vfh_obs_pos
                    vfh_obs_pos.remove(tmp_obs_pos[tmp_ore_order[si]])
                    # print "after", vfh_obs_pos
                vfh_obs_pos.append(tmp_tar_pos)
                vfh_obs_pos.extend(self.obs_wall)
                ob = len(vfh_obs_pos)
                vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta)
                if vfh[3] == 0:                      # A == 1 : The candidate is accessible.
                    tmp_can_info[step_i][i].A = 0    # A == 0 : The candidate is not accessible.
                else:                   #
                    tmp_can_info[step_i][i].A = 1

            # print("\nCheck the candidate ORC.")
            for i in range(len(tmp_can_info[step_i])):
                vfh_tar_pos = copy.deepcopy(tmp_can_info[step_i][i].pos)
                vfh_obs_pos = copy.deepcopy(tmp_obs_pos)
                vfh_obs_pos.append(tmp_tar_pos)
                vfh_obs_pos.extend(self.obs_wall)
                ob = len(vfh_obs_pos)
                vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta)
                if vfh[3] == 0:              # A == 1 : The candidate is accessible.
                    tmp_can_info[step_i][i].A = 0    # A == 0 : The candidate is not accessible.
                    tm_tar_pos = copy.deepcopy(vfh_tar_pos)
                    tm_tar_ori = [0.0, 0.0, 0.0]
                    tm_obs_pos = copy.deepcopy(tmp_obs_pos)
                    # tm_obs_pos.extend(self.obs_wall)
                    ob = len(tm_obs_pos)
                    tm_obs_ori = []
                    for obs_ori_i in range(ob):
                        tm_obs_ori.append([0.0, 0.0, 0.0])

                    ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
                    # ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.rob_pos, self.rob_pos, self.d_max)
                    ore_order.pop()  # The last order is always the target so we need to pop the last element.
                    tmp_can_info[step_i][i].ORC = ore_order
                else:                   #
                    tmp_can_info[step_i][i].A = 1

        return tmp_can_info

    def get_can_A(self, in_can_info, in_obs_pos, in_tar_pos):
        tmp_can_info = copy.deepcopy(in_can_info)
        # print("\nCheck if the candidate is accessible.")
        for ci in range(len(tmp_can_info)):
            vfh_tar_pos = copy.deepcopy(tmp_can_info[ci].pos)
            vfh_obs_pos = copy.deepcopy(in_obs_pos)
            vfh_obs_pos.append(copy.deepcopy(in_tar_pos))
            vfh_obs_pos.extend(self.obs_wall)
            ob = len(vfh_obs_pos)
            vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta)
            if vfh[3] == 0:  # A == 1 : The candidate is accessible.
                tmp_can_info[ci].A = 0  # A == 0 : The candidate is not accessible.
            else:  #
                tmp_can_info[ci].A = 1
        return tmp_can_info

    def init_BT(self, in_can_info):
        for ci in in_can_info:
            ci.BT = 0
        return in_can_info

    def get_can_BT(self, in_can_info, in_obs_pos, in_tar_pos):
        tmp_can_info = copy.deepcopy(in_can_info)
        # print("\nCheck if candidate blocks the target")
        for ci in range(len(tmp_can_info)):
            if tmp_can_info[ci].BT == 0:
                vfh_obs_pos = copy.deepcopy(in_obs_pos)
                vfh_obs_pos.append(tmp_can_info[ci].pos)
                vfh_obs_pos.extend(self.obs_wall)
                vfh_tar_pos = copy.deepcopy(in_tar_pos)
                ob = len(vfh_obs_pos)
                vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta)
                if vfh[3] == 0:
                    tmp_can_info[ci].BT = 1  # BT == 1 : The candidate blocks the target.
                else:                        # BT == 0 : The candidate does not block the target.
                    tmp_can_info[ci].BT = 0
                    # print "can", ci, "bt=0"

        return tmp_can_info

    def get_cf(self, in_can_info):
        tmp_cf = []
        tmp_cf_index = []
        tmp_can_info = copy.deepcopy(in_can_info)

        # print("\nCheck the candidate ORC.")
        for ci in range(len(tmp_can_info)):
            # print "\ncan ", ci, "th has A, BT :", tmp_can_info[ci].A, tmp_can_info[ci].BT
            if tmp_can_info[ci].A == 1 and tmp_can_info[ci].BT == 0:
                tmp_cf.append(tmp_can_info[ci])
                tmp_cf_index.append(ci)
        return tmp_cf, tmp_cf_index

    def get_cf_b(self, in_cf, in_obs_pos):
        tmp_cf = copy.deepcopy(in_cf)
        tmp_obs_pos = copy.deepcopy(in_obs_pos)
        tmp_b = []
        for cb in range(len(tmp_cf)):  # cb: The candidate that will check the b value
            b = 0
            for ci in range(len(tmp_cf)):  # ci: Other candidates for checking the b value
                if cb != ci:
                    # print "\ntar", tmp_cf_pos[ci]
                    # print "obs", tmp_obs_pos
                    vfh_tar_pos = copy.deepcopy(tmp_cf[ci].pos)
                    vfh_obs_pos = copy.deepcopy(tmp_obs_pos)
                    vfh_obs_pos.append(tmp_cf[cb].pos)
                    vfh_obs_pos.extend(self.obs_wall)
                    ob = len(vfh_obs_pos)
                    vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta)
                    if vfh[3] == 0:
                        b = b + 1
            tmp_b.append(b)
        return tmp_b

    def get_cp(self, in_can_info):
        tmp_cp = []
        tmp_cp_index = []
        tmp_can_info = copy.deepcopy(in_can_info)

        # print("\nCheck the candidate ORC.")
        for ci in range(len(tmp_can_info)):
            # print "\ncan ", ci, "th has A, BT :", tmp_can_info[ci].A, tmp_can_info[ci].BT
            if tmp_can_info[ci].A == 0:
                tmp_cp.append(tmp_can_info[ci])
                tmp_cp_index.append(ci)
        return tmp_cp, tmp_cp_index

    # def get_can_A(self, in_can_info, in_obs_pos, in_tar_pos):
    #     tmp_can_info = copy.deepcopy(in_can_info)
    #     # print("\nCheck if the candidate is accessible.")
    #     for ci in range(len(tmp_can_info)):
    #         vfh_tar_pos = copy.deepcopy(tmp_can_info[ci].pos)
    #         vfh_obs_pos = copy.deepcopy(in_obs_pos)
    #         vfh_obs_pos.append(copy.deepcopy(in_tar_pos))
    #         vfh_obs_pos.extend(self.obs_wall)
    #         ob = len(vfh_obs_pos)
    #         vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta)
    #         if vfh[3] == 0:  # A == 1 : The candidate is accessible.
    #             tmp_can_info[ci].A = 0  # A == 0 : The candidate is not accessible.
    #             # print "c:", ci, ".A = 0"
    #         else:  #
    #             tmp_can_info[ci].A = 1
    #             # print "c:", ci, ".A = 1"
    #     return tmp_can_info
    #
    # def get_can_BT(self, in_can_info, in_obs_pos, in_tar_pos):
    #     tmp_can_info = copy.deepcopy(in_can_info)
    #     # print("\nCheck if candidate blocks the target")
    #     for ci in range(len(tmp_can_info)):
    #         vfh_obs_pos = copy.deepcopy(in_obs_pos)
    #         vfh_obs_pos.append(tmp_can_info[ci].pos)
    #         vfh_obs_pos.extend(self.obs_wall)
    #         vfh_tar_pos = copy.deepcopy(in_tar_pos)
    #         ob = len(vfh_obs_pos)
    #         vfh = influence(ob, vfh_tar_pos, vfh_obs_pos, self.rob_pos, self.d_max, self.eta)
    #         if vfh[3] == 0:
    #             tmp_can_info[ci].BT = 1  # BT == 1 : The candidate blocks the target.
    #             # print "c:", ci, ".BT = 1"
    #         # else:                        # BT == 0 : The candidate does not block the target.
    #         #     tmp_can_info[ci].BT = 0
    #         #     print "c:", ci, ".BT = 0"
    #
    #     return tmp_can_info

    def get_c_ore(self, in_can_info):
        # print "input", in_can_info
        # print in_can_info[0].pos
        t_c_order = []
        for ci in range(len(in_can_info)):
            tm_tar_pos = in_can_info[ci].pos
            tm_obs_pos = copy.deepcopy(self.obs_pos)
            tm_obs_pos.append(self.tar_pos)
            obs_r = []
            for i in tm_obs_pos:
                obs_r.append(0.035)
            ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
            while 1:
                tm_tar_pos = in_can_info[ci].pos
                tm_obs_pos = copy.deepcopy(self.obs_pos)
                tm_obs_pos.append(self.tar_pos)
                obs_r = []
                for i in tm_obs_pos:
                    obs_r.append(0.035)
                for i in ore_order:
                    if i != 'T':
                        tm_obs_pos[i] = [4.0, 0.0]

                tmp_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
                # tmp_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall,self.rob_pos, self.rob_pos, self.d_max)
                # print "after removing:", tmp_order
                if tmp_order[0] != 'T':
                    # print "not ok"
                    t_c_order.append([])
                    break
                    # if tmp_order[0] == -1:
                    #     print "no path"
                    #     t_c_order.append([])
                    #     break
                    # if len(ore_order) > len(self.obs_pos):
                    #     print "no path"
                    #     t_c_order.append([])
                    #     break
                    # print "tricky environment for c_ore so extend", ore_order
                    # ore_order.pop()
                    # print "delete target", ore_order
                    # ore_order.extend(tmp_order)
                    # print "to", ore_order
                else:
                    if tmp_order[0] == 'T':
                        # print "ok", ore_order
                        ore_order.pop()

                        if len(tm_obs_pos) in ore_order:
                            # print "\n\nThere is target!! warning!!!\n\n"
                            t_c_order.append([])
                        else:
                            t_c_order.append(ore_order)
                        break
        return t_c_order