Maze path finding algorithm (Not working properly)