Algorithm should work now. Debugging will come soon.