Add improved heap code