Add unit tests & imrpove some code
WIP