from agentState import AgentState
from typing import Dict, Tuple
from gridCellType import GridCellType
from agentActionType import AgentActionType
from agentOrientation import AgentOrientation
from queue import Queue
from turnCar import turn_left_orientation, turn_right_orientation

class Succ:
    state: AgentState
    action: AgentActionType
    cost: int

    def __init__(self, state: AgentState, action: AgentActionType) -> None:
        self.state = state
        self.action = action
        self.cost = cost

def find_path_to_nearest_can(startState: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> list[AgentActionType]:
    q: Queue[list[Succ]] = Queue()
    visited: list[AgentState] = []
    startStates: list[Succ] = [Succ(startState, AgentActionType.UNKNOWN)]
    q.put(startStates)
    while not q.empty():
        currently_checked = q.get()
        visited.append(currently_checked[-1].state)
        if is_state_success(currently_checked[-1].state, grid):
            return extract_actions(currently_checked)
        successors = succ(currently_checked[-1].state)
        for s in successors:
            already_visited = False
            for v in visited:
                if v.position[0] == s.state.position[0] and v.position[1] == s.state.position[1] and s.state.orientation == v.orientation:
                    already_visited = True
                    break
            if already_visited:
                continue
            if is_state_valid(s.state, grid):
                new_list = currently_checked.copy()
                new_list.append(s)
                q.put(new_list)
    return []

        
        
def extract_actions(successors: list[Succ]) -> list[AgentActionType]:
    output: list[AgentActionType] = []
    for s in successors:
        if s.action != AgentActionType.UNKNOWN:
            output.append(s.action)
    return output

def succ(state: AgentState) -> list[Succ]:
    result: list[Succ] = []
    result.append(Succ(AgentState(state.position, turn_left_orientation(state.orientation)), AgentActionType.TURN_LEFT))
    result.append(Succ(AgentState(state.position, turn_right_orientation(state.orientation)), AgentActionType.TURN_RIGHT))
    state_succ = move_forward_succ(state)
    if state_succ != None:
        result.append(move_forward_succ(state))
    return result

def move_forward_succ(state: AgentState) -> Succ:
    position = get_next_cell(state)
    if position == None:
        return None
    return Succ(AgentState(position, state.orientation), AgentActionType.MOVE_FORWARD)


def get_next_cell(state: AgentState) -> Tuple[int, int]:
    if state.orientation == AgentOrientation.UP:
        if state.position[1] - 1 < 1:
            return None
        return (state.position[0], state.position[1] - 1)
    if state.orientation == AgentOrientation.DOWN:
        if state.position[1] + 1 > 27:
            return None
        return (state.position[0], state.position[1] + 1)
    if state.orientation == AgentOrientation.LEFT:
        if state.position[0] - 1 < 1:
            return None
        return (state.position[0] - 1, state.position[1])
    if state.position[0] + 1 > 27:
        return None
    return (state.position[0] + 1, state.position[1])

def is_state_success(state: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> bool:
    next_cell = get_next_cell(state)
    try:
        return grid[next_cell] == GridCellType.GARBAGE_CAN
    except:
        return False

def get_cost_for_action(action: AgentActionType, cell_type: GridCellType) -> int:
    if action == AgentActionType.TURN_LEFT or action == AgentActionType.TURN_RIGHT:
        return 1
    if cell_type == GridCellType.SPEED_BUMP:
        if action == AgentActionType.MOVE_FORWARD:
            return 10
    if action == AgentActionType.MOVE_FORWARD:
        return 3


def is_state_valid(state: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> bool:
    try: 
        return grid[state.position] == GridCellType.STREET_HORIZONTAL or grid[state.position] == GridCellType.STREET_VERTICAL or grid[state.position] == GridCellType.SPEED_BUMP
    except:
        return False