Fix heuristic value on first node

First node f value was 0 not g + h.