The code works now!

Added some tests and finished up the code.