Add some unit testing

Written some unit tests and fixed minor bugs