Algo optimization: part 3.

Boost: around 50% in part 3.
Refactor code.

Almost done.