Clean the code and fix some bugs. Ready to be merged in master branch.