Add unit testing, code cleanup