Using center of gravity to compute herustic func now. works fine but need to fix divide by zero bug