Some code rearrangement, Readme update (bug found)