Fixed bug (presumably).
I think I fixed the bug after a little bit of testing. Need to figure out a way to test this algorith better.