Lots of fixes & rewrite unit tests