Fixed some bugs, now the simple algorithm should work