Code cleanup + add unit tests