Fix some bugs and change sort algorithm

"Fix" code is in the comment
Clear some unused variables 36sec