Adding tests, and fixes for main code