Added a simple heap implementation.
Extra test code.