I think you might be getting some off by one errors in this but I can't be sure. You might want to do some refactoring to extract some of the for loops into methods. It could make this a little easier to follow.