bug fix, seems to work (same result as original python impl), still need to clear the code from debug messages and do more tests