package edu.toronto.csc301.warehouse;

import edu.toronto.csc301.grid.GridCell;
import edu.toronto.csc301.grid.IGrid;
import edu.toronto.csc301.robot.IGridRobot;

import java.util.*;

public class GridBreadthFirstSearch {
    private IGrid<Rack> grid;
    public GridBreadthFirstSearch(IGrid<Rack> grid) {
        this.grid = grid;
    }

    /**
     * Returns the next step direction on a shortest path from source to destination.
     *
     * NOTE: There may be more than one next step direction that can yield a particular
     * path distance. This method returns an arbitrary direction, specifically the first
     * direction attempted in the BFS yielding this path distance.
     *
     * @param source Source cell in the grid.
     * @param destination Destination cell in the grid.
     * @param otherRobotPositions Positions of other robots on the grid.
     * @return Map of path distances to possible next step.
     */
    public IGridRobot.Direction search(GridCell source, GridCell destination, List<GridCell> otherRobotPositions) {
        Queue<GridCell> unvisited = new LinkedList<GridCell>();
        Queue<GridCell> visited = new LinkedList<GridCell>();

        Map<GridCell, IGridRobot.Direction> gridCell2firstStep = new HashMap<>();
        Boolean firstStepDirectionSet = false;

        gridCell2firstStep.put(source, null);
        unvisited.add(source);

        while (!unvisited.isEmpty()) {
            GridCell currCell = unvisited.poll();
            visited.add(currCell);
            for (IGridRobot.Direction direction: IGridRobot.Direction.values()) {
                GridCell neighbour = this.getCellNeighbour(currCell, direction);
                if (this.grid.hasCell(neighbour) && !visited.contains(neighbour) && !unvisited.contains(neighbour) && !otherRobotPositions.contains(neighbour)) {
                    // Check if neighbour is destination.
                    if (neighbour.equals(destination)) {
                        // If this is the first iteration from source, return the current direction.
                        // Otherwise, return the direction of the first step.
                        if (firstStepDirectionSet) {
                            return gridCell2firstStep.get(currCell);
                        } else {
                            return direction;
                        }
                    }
                    unvisited.add(neighbour);
                    if (firstStepDirectionSet) {
                        gridCell2firstStep.put(neighbour, gridCell2firstStep.get(currCell));
                    } else {
                        gridCell2firstStep.put(neighbour, direction);
                    }
                }
            }
            if (!firstStepDirectionSet) { firstStepDirectionSet = true; }
        }
        return null;
    }

    private GridCell getCellNeighbour(GridCell cell, IGridRobot.Direction direction) {
        GridCell neighbour;
        switch(direction) {
            case NORTH:
                neighbour = GridCell.at(cell.x, cell.y + 1);
                break;
            case EAST:
                neighbour = GridCell.at(cell.x + 1, cell.y);
                break;
            case SOUTH:
                neighbour = GridCell.at(cell.x, cell.y - 1);
                break;
            case WEST:
                neighbour = GridCell.at(cell.x - 1, cell.y);
                break;
            default:
                throw new IllegalArgumentException("Invalid direction.");
        }
        return neighbour;
    }
}