Add more test

update implementation
Improve solution.

1. Code style fixes.
2. Drop GNU includes.
3. Rewrite loop for better performance.