Working Heuristic function (need to test more)