Calculate path automatically
AStar: Refactored findPath()