Finishing up step 6, a few bugs, added stuff to cmakelists and main to run it