Reorganized code and fixed minor bug