Non-functional test
Fixed everything

Successful test and running driver

Needs: Heap implementation