AStar: Implement findPath