Code refactoring and various algorithm fixing