fill in ComputeHeuristcCost and tested