Algorithm bugged

[szaghi]

The current implemented algorithm is bugged: it provides (apparently) wrong results (if compared with respect others codes).

Fix implementation for correctness.