Clean up some code and make a checkpoint after submit the paper rtss2014