Fixed a simple bug, tests results match both implementations