Maze v1

Added maze navigation algorithm v1