Working max heap algorithm