Unit tests started

Have added some unit tests and started refactoring.