Add some test, some clean up and minor changes