Implementing a simple heap so I can optimize it