Fixed, documented and write some unit tests

Fixed, documented and write some unit tests