Fixed bugs. Added main for test. Seems to work. Propper unit tests needs
to be written.