def light_veh_pattern5(self, veh_num, current, origin, destination, speed, current_time):
        new_position = current
        time = 0

        # To light up grid(270, 270)
        check_first = False

        # Initiate intersection grid
        if self.time_step == 0:
            self.init_intersec_grid(self.t_ahead)

        if current_time > self.time_step:
            self.update_intersec_grid(current_time, self.time_step, veh_num)
            print('Pattern5')

        # Before veh get out of the intersection
        while new_position[0] < destination[0]:

            # Check if all parts of veh have been in intersection
            if new_position[1] - speed >= origin[0] + 2:
                if not self.intersec_grid[time][(270, 270)]:
                    print('firstgrid')
                    return False
                else:
                    new_position = (new_position[0] + speed, new_position[1])
                    check_first = True
            else:
                # All parts of veh have been in intersection
                # Calculate rotation angle
                if ((new_position[1] - 270 + speed) / 60) * 90 > 15:
                    self.r[veh_num] = ((new_position[1] - 270 + 3) / 60) * 90
                else:
                    self.r[veh_num] = 0

                if ((new_position[1] - 270 + speed) / 60) * 90 > 90:
                    self.r[veh_num] = 90

                # Calculate trajectory by using Bezier Curve
                x = pow(1 - (self.beze_t[veh_num] / 60), 2) * 270 + 2 * (self.beze_t[veh_num] / 60) * (
                    1 - self.beze_t[veh_num] / 60) * 330 + pow(
                    self.beze_t[veh_num] / 60, 2) * 330
                y = pow(1 - (self.beze_t[veh_num] / 60), 2) * 273 + 2 * (self.beze_t[veh_num] / 60) * (
                    1 - self.beze_t[veh_num] / 60) * 273 + pow(
                    self.beze_t[veh_num] / 60, 2) * 330

                self.beze_t[veh_num] += 2
                # if x >= 328:
                #     new_position = (x, y + speed)
                new_position = (x, y)
                # print('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%')
                # print(new_position)

                # Calculate the big Square's coordinate
                self.up_left_x[veh_num] = rec_funcs.W2S_up_left_x(new_position[0], self.r[veh_num])
                self.up_left_y[veh_num] = rec_funcs.W2S_up_left_y(new_position[1])
                self.down_left_x[veh_num] = rec_funcs.W2S_down_left_x(new_position[0], self.r[veh_num])
                self.down_left_y[veh_num] = rec_funcs.W2S_down_left_y(new_position[1], self.r[veh_num])
                self.up_right_x[veh_num] = rec_funcs.W2S_up_right_x(new_position[0], self.r[veh_num])
                self.up_right_y[veh_num] = rec_funcs.W2S_up_right_y(new_position[1])
                self.down_right_x[veh_num] = rec_funcs.W2S_down_right_x(new_position[0], self.r[veh_num])
                self.down_right_y[veh_num] = rec_funcs.W2S_down_right_y(new_position[1], self.r[veh_num])

                # Up left
                if not self.collision(veh_num, self.up_left_x[veh_num], self.up_left_y[veh_num], time):
                    print("upleft, pattern1")
                    return False

                # Up right
                if not self.collision(veh_num, self.up_right_x[veh_num], self.up_right_y[veh_num], time):
                    print("upright, pattern1")
                    return False

                # Down left
                if not self.collision(veh_num, self.down_left_x[veh_num], self.down_left_y[veh_num], time):
                    print("downleft, pattern1")
                    return False

                # Down right
                if ((self.down_right_x[veh_num]) // 10 * 10, (self.down_right_y[veh_num]) // 10 * 10) in \
                        self.intersec_grid[time]:
                    if self.intersec_grid[time][
                        ((self.down_right_x[veh_num]) // 10 * 10, (self.down_right_y[veh_num]) // 10 * 10)] == False:
                            print("downright, pattern1")
                            self.beze_t[veh_num] = 2
                            return False
                    else:
                        self.intersec_grid[time][
                            (self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] = False
                        self.intersec_grid[time][
                            ((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10)] = False
                        self.intersec_grid[time][
                            (self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10)] = False
                        self.intersec_grid[time][
                            ((self.down_right_x[veh_num]) // 10 * 10, (self.down_right_y[veh_num]) // 10 * 10)] = False
                # Situation that down_right is out of the region of inter_sec grid
                else:
                    if (self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10) in self.intersec_grid[
                        time]:
                        self.intersec_grid[time][
                            (self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] = False
                    if ((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10) in \
                            self.intersec_grid[time]:
                        self.intersec_grid[time][
                            ((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10)] = False
                    if (self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10) in \
                            self.intersec_grid[time]:
                        self.intersec_grid[time][
                            (self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10)] = False
                    # situation that middle grid exists
                    # x coordinate is the reason
                    if abs(self.up_left_x[veh_num] - self.up_right_x[veh_num]) > 10:
                        if ((self.up_left_x[veh_num] + 10) // 10 * 10, self.up_left_y[veh_num] // 10 * 10) in \
                                self.intersec_grid[time]:
                            self.intersec_grid[time][
                                ((self.up_left_x[veh_num] + 10) // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] = False
                        if ((self.up_left_x[veh_num] + 10) // 10 * 10, self.down_left_y[veh_num] // 10 * 10) in \
                                self.intersec_grid[time]:
                            self.intersec_grid[time][
                                ((self.up_left_x[veh_num] + 10) // 10 * 10,
                                 self.down_left_y[veh_num] // 10 * 10)] = False
                    # y coordinate is the reason
                    if abs(self.up_left_y[veh_num] - self.down_left_y[veh_num]) > 10:
                        if (self.up_left_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10) in \
                                self.intersec_grid[time]:
                            self.intersec_grid[time][
                                (self.up_left_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10)] = False
                        if (self.up_right_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10) in \
                                self.intersec_grid[time]:
                            self.intersec_grid[time][
                                (
                                self.up_right_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10)] = False

            if check_first:
                self.intersec_grid[time][(270, 270)] = False
            check_first = False

            # print("check p1 current_time", current_time)
            # print('time', time)
            # print(self.beze_t[veh_num])
            # print('new_position', new_position, 'r', self.r)
            # print(self.up_left_x[veh_num], self.up_left_y[veh_num])
            # print(self.up_right_x[veh_num], self.up_right_y[veh_num])
            # print(self.down_left_x[veh_num], self.down_left_y[veh_num])
            # print(self.down_right_x[veh_num], self.down_right_y[veh_num])
            # print(self.intersec_grid[time])
            time += 1

        # Initiate beze_t
        self.beze_t[veh_num] = 2

        return True
    def light_veh_pattern1(self, veh_num, current, origin, destination, speed, current_time):
        new_position = current
        time = 0
        # Avoid situation that grid is lighted up by same vehicle
        check_grid = []
        # To light up grid(270, 270)
        check_first = False

        # Initiate intersection grid
        if self.time_step == 0:
            for k in range(self.t_ahead):
                for i in range(270, 330, 10):
                    for j in range(270, 330, 10):
                        self.intersec_grid[k][(i, j)] = True

        if current_time > self.time_step:
            sa = current_time - self.time_step
            # print('sa', sa)
            self.time_step = current_time

            # Update intersec_grid for right times
            for i in range(sa):
                self.intersec_grid.append(copy.deepcopy(self.grid))
                del self.intersec_grid[0]
            print('Pattern1')

            for i in range(veh_num):
                self.r[i] = 0

        # Before veh get out of the intersection
        while new_position[1] < destination[1]:

            # Check if all parts of veh have been in intersection
            if new_position[0] + speed <= origin[0] + 2:
                if not self.intersec_grid[time][(270, 270)]:
                    print('firstgrid')
                    return False
                else:
                    new_position = (new_position[0] + speed, new_position[1])
                    check_first = True
            else:
                # All parts of veh have been in intersection
                # Calculate rotation angle
                if ((new_position[1] - 270 + speed) / 60) * 90 > 15:
                    self.r[veh_num] = ((new_position[1] - 270 + 3) / 60) * 90
                else:
                    self.r[veh_num] = 0

                if ((new_position[1] - 270 + speed) / 60) * 90 > 90:
                    self.r[veh_num] = 90

                # Calculate trajectory by using Bezier Curve
                x = pow(1 - (self.beze_t[veh_num] / 60), 2) * 273 + 2 * (self.beze_t[veh_num] / 60) * (
                    1 - self.beze_t[veh_num] / 60) * 330 + pow(
                    self.beze_t[veh_num] / 60, 2) * 330
                y = pow(1 - (self.beze_t[veh_num] / 60), 2) * 273 + 2 * (self.beze_t[veh_num] / 60) * (
                    1 - self.beze_t[veh_num] / 60) * 273 + pow(
                    self.beze_t[veh_num] / 60, 2) * 330

                self.beze_t[veh_num] += 2
                new_position = (x, y)

                # Calculate the big Square's coordinate
                self.up_left_x[veh_num] = rec_funcs.W2S_up_left_x(new_position[0], self.r[veh_num])
                self.up_left_y[veh_num] = rec_funcs.W2S_up_left_y(new_position[1])
                self.down_left_x[veh_num] = rec_funcs.W2S_down_left_x(new_position[0], self.r[veh_num])
                self.down_left_y[veh_num] = rec_funcs.W2S_down_left_y(new_position[1], self.r[veh_num])
                self.up_right_x[veh_num] = rec_funcs.W2S_up_right_x(new_position[0], self.r[veh_num])
                self.up_right_y[veh_num] = rec_funcs.W2S_up_right_y(new_position[1])
                self.down_right_x[veh_num] = rec_funcs.W2S_down_right_x(new_position[0], self.r[veh_num])
                self.down_right_y[veh_num] = rec_funcs.W2S_down_right_y(new_position[1], self.r[veh_num])

                # Up left
                # print(time)
                if (self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10) in self.intersec_grid[time]:
                    if self.intersec_grid[time][
                        (self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] == False:
                        print('upleft')
                        self.beze_t[veh_num] = 2
                        return False
                    else:
                        check_grid.append((self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10))

                # Up right
                if ((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10) in self.intersec_grid[
                    time]:
                    if self.intersec_grid[time][
                        ((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10)] == False:
                        if ((self.up_right_x[veh_num]) // 10 * 10,
                            self.up_right_y[veh_num] // 10 * 10) not in check_grid:
                            print('upright')
                            self.beze_t[veh_num] = 2
                            return False
                    else:
                        check_grid.append(((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10))

                # Down left
                if (self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10) in self.intersec_grid[
                    time]:
                    if self.intersec_grid[time][
                        (self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10)] == False:
                        if (self.down_left_x[veh_num] // 10 * 10,
                            (self.down_left_y[veh_num]) // 10 * 10) not in check_grid:
                            print('Downleft')
                            self.beze_t[veh_num] = 2
                            return False
                    else:
                        check_grid.append(
                            (self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10))

                # Down right
                if ((self.down_right_x[veh_num]) // 10 * 10, (self.down_right_y[veh_num]) // 10 * 10) in \
                        self.intersec_grid[time]:
                    if self.intersec_grid[time][
                        ((self.down_right_x[veh_num]) // 10 * 10, (self.down_right_y[veh_num]) // 10 * 10)] == False:
                        if ((self.down_right_x[veh_num]) // 10 * 10,
                            (self.down_right_y[veh_num]) // 10 * 10) not in check_grid:
                            print('downright')
                            self.beze_t[veh_num] = 2
                            return False
                    else:
                        self.intersec_grid[time][
                            (self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] = False
                        self.intersec_grid[time][
                            ((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10)] = False
                        self.intersec_grid[time][
                            (self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10)] = False
                        self.intersec_grid[time][
                            ((self.down_right_x[veh_num]) // 10 * 10, (self.down_right_y[veh_num]) // 10 * 10)] = False
                # Situation that down_right is out of the region of inter_sec grid
                else:
                    if (self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10) in self.intersec_grid[
                        time]:
                        self.intersec_grid[time][
                            (self.up_left_x[veh_num] // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] = False
                    if ((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10) in \
                            self.intersec_grid[time]:
                        self.intersec_grid[time][
                            ((self.up_right_x[veh_num]) // 10 * 10, self.up_right_y[veh_num] // 10 * 10)] = False
                    if (self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10) in \
                            self.intersec_grid[time]:
                        self.intersec_grid[time][
                            (self.down_left_x[veh_num] // 10 * 10, (self.down_left_y[veh_num]) // 10 * 10)] = False
                    # situation that middle grid exists
                    # x coordinate is the reason
                    if abs(self.up_left_x[veh_num] - self.up_right_x[veh_num]) > 10:
                        if ((self.up_left_x[veh_num] + 10) // 10 * 10, self.up_left_y[veh_num] // 10 * 10) in \
                                self.intersec_grid[time]:
                            self.intersec_grid[time][
                                ((self.up_left_x[veh_num] + 10) // 10 * 10, self.up_left_y[veh_num] // 10 * 10)] = False
                        if ((self.up_left_x[veh_num] + 10) // 10 * 10, self.down_left_y[veh_num] // 10 * 10) in \
                                self.intersec_grid[time]:
                            self.intersec_grid[time][
                                ((self.up_left_x[veh_num] + 10) // 10 * 10,
                                 self.down_left_y[veh_num] // 10 * 10)] = False
                    # y coordinate is the reason
                    if abs(self.up_left_y[veh_num] - self.down_left_y[veh_num]) > 10:
                        if (self.up_left_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10) in \
                                self.intersec_grid[time]:
                            self.intersec_grid[time][
                                (self.up_left_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10)] = False
                        if (self.up_right_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10) in \
                                self.intersec_grid[time]:
                            self.intersec_grid[time][
                                (
                                self.up_right_x[veh_num] // 10 * 10, (self.up_left_y[veh_num] + 10) // 10 * 10)] = False

            if check_first:
                self.intersec_grid[time][(270, 270)] = False
            check_first = False

            print("check p1 current_time", current_time)
            check_grid = []
            print('time', time)
            # print(self.beze_t[veh_num])
            print('new_position', new_position, 'r', self.r)
            print(self.up_left_x[veh_num], self.up_left_y[veh_num])
            print(self.up_right_x[veh_num], self.up_right_y[veh_num])
            print(self.down_left_x[veh_num], self.down_left_y[veh_num])
            print(self.down_right_x[veh_num], self.down_right_y[veh_num])
            print(self.intersec_grid[time])
            time += 1

        # Initiate beze_t
        self.beze_t[veh_num] = 2

        return True