Dijkstra's Algorithm Visualization

I would find it interesting to see a visualization on [Dijkstra's algorithm,](https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm) I have an implementation using Processing. I am new to processing and getting the nodes and edges to change color as the algorithm traverses over them is not working correctly, so I am not doing, or doing something wrong.

Here is what I tossed together:

``` java
//DijkstrasAlgorithm
int width = 600;
int height = 600;
ArrayList nodes = new ArrayList();
int c = 0;
Node Current;
int destNodeIndex;
Node destNode;
int nodeCount = 3;
int neighborCount = 3;

int totalNodes = -1;
double inf = 1000000;
int index = 0;
void setup() {
size(600, 600, P2D);
createNodes(nodeCount);

destNode = GetRandomDest();

frameRate(5);
}

void draw() {
background(50);

text("Total Nodes: " + totalNodes + ", Destination Node: " + destNode, 10, 20);

for(Node n : nodes) {
n.show();
}

if (!Finished() && index < nodeCount - 1) {
Current = VisitNode(index);
Current.show();
index++;
}

c++;
}

Node createNode(double dist) {
int x = floor(random(width));
int y = floor(random(height));
totalNodes++;
return new Node(x, y, dist, totalNodes);
}

void createNodes(int nodeCount) {
Node prev = createNode(0);
for (int i = 0; i < nodeCount-1; i++) {
Node n = createNode(inf).AddNeighbor(prev);
for (int j = 0; j < random(4); j++) {
Node neighbor = createNode(inf);
n.AddNeighbor(neighbor);
//nodes.add(neighbor);
}

nodes.add(n);
prev = n;
}
}

Node VisitNode(int index) {
Node n = nodes.get(index);
n.Visit();
return n;
}

ArrayList GetUnVisitedNodes() {
ArrayList unvisited = new ArrayList();
for (Node n : nodes) {
if (!n.GetVisited())
unvisited.add(n);
}
return unvisited;
}

double GetSmallestUnVisitedValue() {
ArrayList nodes = GetUnVisitedNodes();
double smallest = -1;
for (Node n : nodes) {
if (n.Distance < smallest)
smallest = n.Distance;
}
return smallest;
}

boolean Finished() {
return destNode.GetVisited() || GetSmallestUnVisitedValue() == inf;
}

Node GetRandomDest() {
int id = floor(random(totalNodes));
for (Node n : nodes) {
if (n.Id == id)
return n;
else
for (Node b : n.Neighbors) {
if (b.Id == id)
return b;
}
}
return null;
}
```

``` java
//Edge
public class Edge {
double Length;
Node A;
Node B;
boolean Crossed;
boolean Visit;
public Edge(Node a, Node b) {
A = a;
B = b;
Length = abs(dist(a.X, a.Y, b.X, b.Y));
Crossed = false;
}

public boolean Connects(Node a, Node b) {
Visit = true;
Crossed = (a == A || a == B) && (b == A || b == B);
Visit = false;
return Crossed;
}

public void show() {
pushMatrix();
if (Visit)
stroke(0,255,0);
else if (Crossed)
stroke(255,0,0);
else
stroke(0,0,255);

line(A.X, A.Y, B.X, B.Y);

popMatrix();
}

}
```

``` java
//Node
public class Node {
int X;
int Y;
double Distance;
boolean Visited;
int Id;
ArrayList Neighbors;
ArrayList Edges;

boolean current = false;

public Node(int x, int y, double distance, int id) {
X = x;
Y = y;
Distance = distance;
Id = id;
Visited = false;
Neighbors = new ArrayList();
Edges = new ArrayList();
}

public void SetDistance(double d) {
Distance = d;
}

public Node AddNeighbor(Node n) {
Neighbors.add(n);
Edge e = new Edge(this, n);
Edges.add(e);
return this;
}

public ArrayList GetNeighbors() {
return Neighbors;
}

public void Visit() {
current = true;
CalculateDistances();
Visited = true;
current = false;
}

public boolean GetVisited() {
return Visited;
}

public void CalculateDistances() {
for (Node n : Neighbors) {
if (!n.GetVisited()) {
Edge e = GetEdgeToNeighbor(n);
if (e != null) {
double currentDist = e.Length + n.Distance;
if (n.Distance < currentDist)
n.Distance = currentDist;
}
}
}
}

public Edge GetEdgeToNeighbor(Node n) {
for (Edge e : Edges) {
if (e.Connects(this, n)) {
return e;
}
}
return null;
}

public void show() {
//pushMatrix();
if (current) {
fill(0,255,0);
} else if (Visited) {
fill(255,0,0);
} else {
fill(0,0,255);
}
ellipse(X, Y, 15,15); //this

fill(255);
text(Id, X+10, Y+10);

//popMatrix();

for(int i = 0; i