working goTo function with pathfinding

CitizenClass.pde

@@ -1,3 +1,4 @@
+import java.lang.Double;
 
 class Citizen {
   int i_xSpawn, i_ySpawn, i_xPos, i_yPos, i_diameter;
@@ -6,15 +7,6 @@ class Citizen {
   int i_home;
   String S_name;
 
-
-  GraphNode startNode, endNode;
-  int startNodeID, endNodeID;
-  GraphNode[] rNodes;
-  //GraphEdge[] exploredEdges;
-
-  IGraphSearch pathFinder;
-  int pathAlgorithm = i_pathAlgorithm;
-
   //0-100:
   //motivation (bei arbeiter montag ~40, freitag ~80 - 100)
   //7 emotionen?
@@ -24,7 +16,6 @@ class Citizen {
 
   // identität: gelbhemd, lokal, regional, national, international, fremd, 
 
-  //names of pathfinding algorithms: greedy best first search, uniform cost search, a star search
 
   Citizen(int home, String name) {
     i_home = home;
@@ -50,12 +41,33 @@ class Citizen {
   }
 
   void update() {
-    if(b_moving) { //if rNodes.length > 0 
+    if (b_moving) { //if rNodes.length > 0 
-     println(rNodes[0].x());
+      if (rNodes.length > nextNode) {
-    }
+        //println(rNodes[nextNode].x());
-    if (rNodes.length == 0){ //rNodes needs to be an array list 
+        //if (i_xPos != int(rNodes[nextNode].xf()) || i_yPos != int(rNodes[nextNode].yf())) {
+        //println("arrived at nextnode!");
+        i_xPos = int(rNodes[nextNode].xf());
+        i_yPos = int(rNodes[nextNode].yf());
+        nextNode++;
+        // if (rNodes.length > nextNode) {
+        //   nextNode++;
+        // } else {
+        //   nextNode = 0;
+        //   b_moving = false;
+        // }
+        //} else {
+        //println("hmm");
+        //}
+      } else {
         b_moving = false;
+        nextNode = 0;
+        goTo(int(random(0,width)),int(random(0,height)));
       }
+      println(b_moving);
+    }
+    //if (rNodes.length == 0) { //rNodes needs to be an array list ?
+    //  b_moving = false;
+    //}
   }
 
   void display() {
@@ -66,11 +78,13 @@ class Citizen {
     //draw Route
     if (rNodes.length >= 2) {
       pushStyle();
+      if (b_drawPathLine) {
         stroke(color(100, 100, 100));
         strokeWeight(1);
         noFill();
         for (int i = 1; i < rNodes.length; i++)
           line(rNodes[i-1].xf(), rNodes[i-1].yf(), rNodes[i].xf(), rNodes[i].yf());
+      }
       stroke(160, 0, 0);
       fill(255, 0, 0);
       ellipse(rNodes[rNodes.length-1].xf(), rNodes[rNodes.length-1].yf(), nodeSize, nodeSize); 
@@ -78,6 +92,14 @@ class Citizen {
     }
   }
 
+  GraphNode startNode, endNode;
+  int startNodeID, endNodeID;
+  GraphNode[] rNodes;
+  ArrayList<GraphNode> rNodesList = new ArrayList<GraphNode>();
+  int nextNode;
+
+  IGraphSearch pathFinder;
+  int pathAlgorithm = i_pathAlgorithm;
 
   void goTo(int x, int y) {
     //add: go to node? go to house?
@@ -91,21 +113,23 @@ class Citizen {
       nodeDistance++;
     }
     endNodeID = gs.getNodeAt(x, y, 0, nodeDistance).id();
-    //usePathFinder(startNodeID, endNodeID);
-    
 
     pathFinder.search(startNodeID, endNodeID, true); //returns null if no route is found
 
     rNodes = pathFinder.getRoute();
 
-    if(rNodes.length > 0) b_moving = true;
+    if (rNodes.length > 0) {
+      nextNode = 0;
+      b_moving = true;
 
+      for (int i = 0; i < rNodes.length; i++) {
+        rNodesList.add(rNodes[i]);
+      }
+    } else {
+      //println("bloop");
+        goTo(int(random(0,width)),int(random(0,height)));
+      //b_moving = false; //this might be problematic if i want movement controlled by different things than goto / nodes
+    }
     //exploredEdges = pathFinder.getExaminedEdges();
   }
-
-  //void usePathFinder(int startNodeID, int endNodeID) {
-  //  pathFinder.search(startNodeID, endNodeID, true);
-  //  rNodes = pathFinder.getRoute();
-  //  exploredEdges = pathFinder.getExaminedEdges();
-  //}
 }

citizen.pde

@@ -1,9 +1,10 @@
-Citizen[] citizen = new Citizen[3];
+Citizen[] citizen = new Citizen[9];
 
 //settings
 boolean b_smallerImage = true; //use a cropped, smaller version of the 1920x1080 image for faster development
 boolean b_loadSun = false; //load sun from web-api instead of local json file (local file is adressed to january 4th 2019. might implement a check that downloads today's information only once...
 boolean b_drawNodes = false;
+boolean b_drawPathLine = false;
 float f_nodeResolution = 0.2; //defines density of path-finding nodes, multiplied with resolution
 int i_pathAlgorithm = 3;
 
@@ -30,9 +31,15 @@ void setup() {
   
   surface.setSize(img_houses.width, img_houses.height);
 
-  citizen[0] = new Citizen(59, "Pe");
+  citizen[0] = new Citizen(59, "Paul");
-  citizen[1] = new Citizen(121, "Paul");
+  citizen[1] = new Citizen(121, "Matze");
   citizen[2] = new Citizen(18, "Freddy");
+  citizen[3] = new Citizen(59, "Korbi");
+  citizen[4] = new Citizen(121, "Lotte");
+  citizen[5] = new Citizen(18, "Reifi");
+  citizen[6] = new Citizen(59, "Grasso");
+  citizen[7] = new Citizen(121, "Nico");
+  citizen[8] = new Citizen(18, "Victor");
 
   for (int i = 0; i < citizen.length; i++) {
     citizen[i].spawn(int(random(0, houses.size())));