Fix some bugs and extend algorithm.