I'm trying to register every Nth point within and array to an arraylist so I can later include those to a sorted array and compare distance, in some sort of a flocking behaviour. The first array determines a tail for the agent, the arraylist is the collected info and the second array is the sorted list. Here's the code part of it:

2. //Every Nth frames
3.     if (frameCount % TrailInterval == 0 && TrailNodeCount < maxTrailNodes) {
4.       // Register the Flocker's position
5.       PVector TrailNodeLoc = Floc.get();
6.       TrailNode T = new TrailNode(TrailNodeLoc);
7.       // Add the position to a local nodes list
8.       Trail.add(T);
9.       // Register 3 nodes per trail
10.       if (TrailNodeCount==0 || TrailNodeCount ==int(maxTrailNodes/2) || TrailNodeCount==maxTrailNodes-1 ) TrailsCollection.add(T);
11.       // If the trail is longer than the specified range
12.       if (Trail.size() == maxTrailNodes) {
13.         // Get the first node (that is the older one)
14.         TrailNode X = (TrailNode)Trail.get(0);
15.         // Get the index of the node within the global node list (will make sense later)
16.         int index = TrailsCollection.indexOf(X);
17.         if (index >=0) {
18.           // And remove it from the global node list (this is to avoid overflooding)
19.           TrailsCollection.remove(index);
20.         }
21.         // Remove it from the local trail list
22.         Trail.remove(0);
23.       }
24.       //Find the cell containing the Flocker
25.       int XCell = floor(T.Tloc.x/gridSize);
26.       int YCell = floor(T.Tloc.y/gridSize);
27.       int ZCell = floor(T.Tloc.z/gridSize);
28.       //Check the range and correct it if necessary
29.       if (XCell > Rows) XCell = Rows;
30.       if (YCell > Cols) YCell = Cols;
31.       if (ZCell > Stacks) ZCell = Stacks;
32.       if (XCell < 1) XCell = 0;
33.       if (YCell < 1) YCell = 0;
34.       if (ZCell < 1) ZCell = 0;
35.       //Enumerate the corresponding cell (-1 because it counts from zero)
36.       int Cell = (XCell + (YCell*Rows) + (ZCell*Cols*Rows))-1;
37.       if (Cell < 0) Cell = 0;
38.       if (Cell >= Cells) Cell = Cells-1;
39.       // Add the node to its Cell
40.       TrailCollection[Cell].add(T);
41.     }
42.     // And draw a polyline through the list of local nodes list, from first to last
43.     beginShape(); 
44.     for (int t=0; t<Trail.size(); t++) {
45.       TrailNode T = (TrailNode)Trail.get(t);
46.       MakeTrail[t] = T.Tloc;
47.       stroke(150, 20, 20);
48.       vertex(MakeTrail[t].x, MakeTrail[t].y, MakeTrail[t].z);
49.     }
50.     vertex(Floc.x, Floc.y, Floc.z);
51.     endShape();
52.   }

Somehow, I'm having strange behaviours with it and I can't sort it out.

Could you check it and see if you find the error here?