After several bug fixes hopefully done.

More importantly after using the correct algorithm