Fix bug in heap code

[crisdosaygo]

Also simplify heap implementation! :p :) xx ;p