Edited some comments, fixed new algorythm again. I think.

I made sure all the x's and y's matched up, and I think it should work, but
there might be some other bugs, it seemed a little strange. I dunno, I'm getting
closer, I'm sure.