Fix ManhattanHeuristic
Now ManhattanHeuristic is correct