Changed the routines based on Ryan, Dav and peer feedback. Also added test cases, all test pass.

Changed the routines based on Ryan Grove, Dav Glass and peer feedback. Also added test cases, all test pass. Please review the implementation. I have also signed the CLA.