Significant refactoring for thread support.

Fixed a few bugs in heap implementation.