Added heuristic calculation
Added class for a*