Implemented basic pathfinding