# Last updated: 27/2/2024

import time

FILL_CODES = "^>v<ABCDEFGHIJKLMNOPQRSTUVWXYZ1234567890"
DOWN = 2
UP = 0
LEFT = 3
RIGHT = 1
EVERYTHING_EXCEPT_DIRECTION = 252

NORTH = 32
SOUTH = 128
EAST = 64
WEST = 16
DIRECTIONS = [NORTH, EAST, SOUTH, WEST]

DONE = 8  # bitmap done flag in each cell
NOTDONE = 255 - DONE  # inverse of DONE


class Solver:
    def __init__(self, target_cell, start_pos=240, phase=0, direction=0, conn=False):
        """creates a maze solver object that can update a maze map and return instructions"""

        self.phase = phase  # phase of solving
        self.start_pos = start_pos  # this is where we started. This value will not be changed.
        self.end_pos = target_cell  # this is where we will ultimately finish. This value will not be changed.
        self.pos = start_pos  # where are we in the maze?
        self.target = target_cell  # where are we going?
        self.direction = direction  # the direction of the robot
        self.done = DONE  # currently is it done (8 or 0)

        # these next two lines were written by past malcolm and i have no idea what they do. hopefully ill find out...
        # Future malcolm here! I figured out what they do! These lines are for giving special commands before the robot
        # does the final solve. The robot will run all the phase2_to_3_commands just before the final solve. "-" means
        # reverse to the back of the cell but don't go forward again, so the robot is in the same position as when it
        # was first placed into the maze
        self.phase2_to_3_commands = ("-",)
        self.current_phase_change_command = 0

        self.maze = []
        for cell in range(256):  # create the maze array
            self.maze.append(0)

        # place the top, bottom, and side walls of the maze
        for cell in range(256):
            if cell < 16:  # the cells at the top of the maze
                self.maze[cell] |= NORTH

            if cell > 239:  # the cells at the bottom of the maze
                self.maze[cell] |= SOUTH

            if cell % 16 == 0:  # the cells at the sides of the maze
                self.maze[cell] |= WEST
                self.maze[cell + 15] |= EAST

        if conn:
            raise NotImplementedError(
                "Connectivity hasn't been implemented yet. Please set conn to False when creating the Solver object. Thank you!")

        else:
            self.conn = None

    def print_maze(self):
        """outputs a visual representation of the maze to stdout (usually the shell window)"""

        line1 = ""
        line2 = ""

        for cell in range(len(self.maze)):
            line1 += "+"

            if cell == self.pos:
                code = "&"  # the ampersand represents the position of the robot

            else:
                code = FILL_CODES[self.maze[cell] % 4]  # the code should be V, ^, <, or >.

            if self.maze[cell] & NORTH != 0 or (self.maze[cell - 16] & SOUTH != 0 and cell > 15):
                line1 += "-"  # if the cell has a north wall OR the one above has a south wall, add a -.
            else:
                line1 += " "

            if self.maze[cell] & WEST != 0 or self.maze[cell - 1] & EAST != 0:
                line2 += "|" + code
            else:
                line2 += " " + code

            if (cell + 1) % 16 == 0 and self.maze[cell] & EAST != 0:
                line2 += "|"

            if (cell + 1) % 16 == 0:
                line1 += "+"
                print(line1, line2, sep="\n")  # output the lines
                line1, line2 = "", ""  # reset the lines to nothing

        print("+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+")  # print the walls at the bottom of the maze

    def do_wall(self, wall, position=None):
        """places a wall into the maze at position. Defaults to self.pos (the robot's position).
The wall can be N, S, E, or W"""

        if position is None:
            position = self.pos

        if wall == "N":
            bit = NORTH
            other_pos = position - 16
            other_bit = SOUTH

        elif wall == "S":
            bit = SOUTH
            other_pos = position + 16
            other_bit = NORTH

        elif wall == "E":
            bit = EAST
            other_pos = self.pos + 1
            other_bit = WEST

        elif wall == "W":
            bit = WEST
            other_pos = self.pos - 1
            other_bit = EAST

        else:
            raise ValueError("Wrong wall: must be N, S, E, or W; not" + str(wall) + ".")

        self.maze[position] |= bit  # place the wall into the maze
        if 0 <= other_pos < 256:
            self.maze[other_pos] |= other_bit  # if applicable, place the wall in its other position into the maze

    def set_position(self, commands):
        """called setp in the PicOne script"""

        is_digit = False  # dig in PicOne script
        for c in range(len(commands)):
            char = commands[c].upper()
            if char in "0123456789":
                if not is_digit:
                    self.pos = int(char)
                else:
                    self.pos = self.pos * 10 + int(char)

                is_digit = True

            else:
                if is_digit:
                    print(self.pos)
                is_digit = False

                # now the command is either to move the robot or to place a wall in the maze
                if char == "U":  # up
                    self.pos -= 16
                elif char == "D":  # down
                    self.pos += 16
                elif char == "L":  # left
                    self.pos -= 1
                elif char == "R":  # right
                    self.pos += 1
                else:
                    self.do_wall(char)  # place a wall in the maze

    def clear_done(self):
        """clears the done flag in each of the cells of the maze"""

        for cell in range(256):
            self.maze[cell] &= NOTDONE

    def solve(self, complete_solve=False):
        """solves the maze; complete_solve is whether to do everything, not just until we've got to the robot"""

        self.clear_done()  # clear the done flag in each cell

        list1 = [self.target]  # add the maze centre to list1
        list2 = []  # empty list2

        self.maze[self.target] = self.maze[self.target] & NOTDONE + self.done
        solving = True
        while solving:  # solving is set to False when list2 is empty
            for i in range(len(list1)):
                pos = list1[i]
                if not self.maze[pos] & NORTH:
                    candidate_cell = pos - 16  # cell above
                    if candidate_cell >= 0 and self.maze[candidate_cell] & DONE != self.done:
                        self.maze[candidate_cell] = ((self.maze[
                                                          candidate_cell] & NOTDONE) + self.done & EVERYTHING_EXCEPT_DIRECTION) + DOWN
                        list2.append(candidate_cell)

                if not self.maze[pos] & SOUTH:
                    candidate_cell = pos + 16
                    if candidate_cell <= 255 and self.maze[candidate_cell] & DONE != self.done:
                        self.maze[candidate_cell] = ((self.maze[
                                                          candidate_cell] & NOTDONE) + self.done & EVERYTHING_EXCEPT_DIRECTION) + UP
                        list2.append(candidate_cell)

                if not self.maze[pos] & WEST:
                    candidate_cell = pos - 1
                    if candidate_cell >= 0 and self.maze[candidate_cell] & DONE != self.done:
                        self.maze[candidate_cell] = ((self.maze[
                                                          candidate_cell] & NOTDONE) + self.done & EVERYTHING_EXCEPT_DIRECTION) + RIGHT
                        list2.append(candidate_cell)

                if not self.maze[pos] & EAST:
                    candidate_cell = pos + 1
                    if candidate_cell <= 255 and self.maze[candidate_cell] & DONE != self.done:
                        self.maze[candidate_cell] = ((self.maze[
                                                          candidate_cell] & NOTDONE) + self.done & EVERYTHING_EXCEPT_DIRECTION) + LEFT
                        list2.append(candidate_cell)

                if pos == self.pos and not complete_solve:
                    solving = False
                    break  # if we've found the robot, we don't need to go any deeper

            list1 = list2
            list2 = []
            if not list1:
                solving = False  # list1 is empty, so solving has finished

    def wn(self, left, front, right):
        """what should the robot do based on the wall values?"""

        if self.pos == self.target:  # we've reached where we are going to for the moment
            instruction = "s"  # s = stop

            # this phase is done, so we can move onto the next solving phase
            self.phase += 1  # increment the phase by 1

            if self.phase == 1:
                self.target = self.start_pos  # go back to the start cell
                self.solve()
                return self.wn(left, front, right)  # this command assumes we are going to the start

            elif self.phase == 2:  # we're moving into the final phase!!
                if self.current_phase_change_command < len(self.phase2_to_3_commands):
                    # give a command to get the robot into the right position for the final solve
                    self.current_phase_change_command += 1
                    self.phase -= 1  # this tricks the solver into thinking it's the first phase
                    self.direction = (self.direction + 2) % 4  # facing the other way

                    return self.phase2_to_3_commands[self.current_phase_change_command - 1]

                else:
                    # get a list of commands to solve the maze
                    self.target = self.end_pos  # the one end of the maze
                    self.solve(complete_solve=True)  # resolve the maze with the new target
                    self.phase = 3
                    inst = ""  # instruction in the final solve
                    commands = []  # the list of commands for the final solve
                    while inst != "s":  # repeat until we've got to the end of the maze
                        inst = self.wn(0, 0, 0)  # get the next command
                        commands.append(inst)

                    # shorten the command list by grouping together long runs of +s (essentially run-length encoding)
                    new_commands = []
                    run = 0  # how long the current run of +s we're on is
                    for command in commands:
                        if command == "+":  # the command is a + (forwards)
                            run += 1

                        elif run > 0:  # the command is not a +, but we've just finished a run of +s
                            new_commands.append("+" + str(run))
                            new_commands.append(command)
                            run = 0

                        else:
                            new_commands.append(command)  # just add the command to the new list

                    return new_commands  # give the commands to Toby to complete the solve

            return instruction

        current_cell = self.maze[self.pos]

        if front:  # there's a wall in front
            if not current_cell & DIRECTIONS[self.direction]:
                self.do_wall("NESW"[self.direction])

        if left:  # there's a wall to the left
            
            if not current_cell & DIRECTIONS[(self.direction + 3) % 4]:
                self.do_wall("NESW"[(self.direction + 3) % 4])

        if right:  # there's a wall to the right
            if not current_cell & DIRECTIONS[(self.direction + 1) % 4]:
                self.do_wall("NESW"[(self.direction + 1) % 4])

        if left + front + right < 2:
            self.solve()  # resolve the maze because we have a choice
            current_cell = self.maze[self.pos]

        if self.phase == 3:  # final solve
            absolute_direction = current_cell % 4  # 0, 1, 2, or 3, meaning north, east, south or west
            if absolute_direction == self.direction:
                relative_direction = 0  # just go straight on

            else:
                cant_do_it = False  # can't go forward because of a wall or the edge of the maze
                next_cell = 0
                if self.direction == 0 and self.pos > 15 and not current_cell & NORTH:
                    next_cell = self.pos - 16
                elif self.direction == 1 and self.pos % 16 != 15 and not current_cell & EAST:
                    next_cell = self.pos + 1
                elif self.direction == 2 and self.pos < 240 and not current_cell & SOUTH:
                    next_cell = self.pos + 16
                elif self.direction == 3 and self.pos % 16 > 0 and not current_cell & WEST:
                    next_cell = self.pos - 1
                else:
                    cant_do_it = True

                if next_cell % 4 == self.direction and not cant_do_it:
                    relative_direction = 0
                else:
                    relative_direction = (absolute_direction - self.direction) % 4

            self.direction = (self.direction + relative_direction) % 4  # we'll be going this way now

        else:
            if left + front + right < 2:
                absolute_direction = current_cell % 4  # 0, 1, 2, or 3, meaning north, east, south or west
                relative_direction = (absolute_direction - self.direction) % 4
                self.direction = absolute_direction  # we'll be going this way now

            else:
                if left and right and not front:
                    relative_direction = 0  # straight on

                elif left and front and not right:
                    relative_direction = 1  # right

                elif front and right and not left:
                    relative_direction = 3  # left

                else:
                    relative_direction = 2  # U-turn

                self.direction = (self.direction + relative_direction) % 4

        # change the position of the robot based on the direction it will move
        if self.direction == 0:
            self.pos -= 16
        elif self.direction == 1:
            self.pos += 1
        elif self.direction == 2:
            self.pos += 16
        else:
            self.pos -= 1

        # work out which instruction to send back to Toby's code
        if relative_direction == 0:  # our direction is the same as the required one
            instruction = "+"

        elif relative_direction == 3:  # because relative_direction is modulus 4, -1 is equal to 3
            instruction = "<"

        elif relative_direction == 1:
            instruction = ">"

        else:
            instruction = "O"  # U-turn

        return instruction  # give back the instruction(s) to Toby's code


def init_solver():
    """initializes the solver for the human interface"""

    global solver
    solver = Solver(target_cell=165)

    """
    fred = "240EUEUEUNRNSRNSRNSrnedewdewdws"
    solver.set_position(fred)
    fred2 = "128nrnrnrnrnrnrnrnrnrnrnrnrnrnrnrn"
    solver.set_position(fred2)
    solver.print_maze()
    solver.solve()
    solver.print_maze()
    """

    solver.set_position("240UUUUUNRNRNRNRNRNEDEDEDEDEDE")
    solver.set_position("240")
    solver.solve()


def human_interface():
    global solver
    instruction = ""
    old_values = []
    solver.print_maze()

    while instruction != "s":
        values = input("Enter wall values: ").strip()
        if values.lower() == "restart" or values.lower() == "r":
            print("Restarting...")
            init_solver()
            instruction = ""
            old_values = []
            solver.print_maze()
            continue

        elif values.lower() == "solve" or values.lower() == "s":
            print("Solving...")
            solver.solve()
            solver.print_maze()
            continue

        elif " " in values:
            values = values.split()
            for v in range(len(values) - 1, -1, -1):
                if values[v] == "":
                    del values[v]

                else:
                    values[v] = int(values[v])

        elif len(values) == 0:
            values = old_values

        else:
            walls = []
            for char in values:
                if char in "01":
                    walls.append(int(char))

            values = walls

        if len(values) != 3:
            print("Error: wrong number of walls")
            continue

        start = time.time_ns()
        instruction = solver.wn(*values)
        end = time.time_ns()
        print("Position: " + str(solver.pos))
        print("Direction: " + str(solver.direction))
        print("Time taken: " + str(end - start))
        print(instruction)

        old_values = values
        solver.print_maze()


if __name__ == "__main__":
    solver = ...
    init_solver()
    try:
        from pympler import asizeof
        print(asizeof.asizeof(solver))
    except ImportError:
        pass

    try:
        human_interface()

    except KeyboardInterrupt:
        print("Quitting...")

# 2998400ns taken by the old solver (~3ms)
# 0(???)ns taken by the new solver!
# this is an ∞ % improvement!!!
# this is an ∞ % improvement!!!
# +-+-+-+-+-+-+
# |4 3 2 1 0  |
# + + + + + + +
# |5 4 3 2 1 0|
# + + + + + + +
# |6 5 4 3 2 1|
# + + + +-+ + +
# |7 6 5|4 > 2|
# + +-+-+ +-+-+
# |8 9 A|5 6 7|
# +-+ + +-+-+ +
# |B A|B A 9 8|
# +-+-+-+-+-+-+