Al momento non ho tempo per guardare il tuo codice mi spiace.. l'unico consiglio che posso darti è debuggare su questa parte, ovvero testando parte per parte se fa quello che realmente ti dice.. prova a commentare un pezzo alla volta ed eseguire solo alcune parti, fatti stampare gli array ecc.. dovresti venirne a capo così!