fixed #543 : Add tests and fxing and clean up code