Improved code, tests and README changes