Binary heap seems to work now