Dijkstra dijk;
int w = 10;
int h = 10;
int spacing;
PFont font;

void setup(){
  size(400, 400);
  spacing = width / w;
  smooth();
  dijk = new Dijkstra();
  dijk.nodes = MapMaker.makeMap(w, h);
  font = loadFont("ArialMT-10.vlw");
  textFont(font, 10);
}

void draw(){
  background(230);
  fill(255);
  int m = (mouseX/spacing) + (mouseY/spacing) * w;
  rect((m % w) * spacing, (m / w) * spacing, spacing, spacing);
  drawNodes();
}

void mousePressed(){
  int m = (mouseX/spacing) + (mouseY/spacing) * w;
  Node n = (Node)dijk.nodes.get(m);
  dijk.makeWeb(n);
}

void drawNodes(){
  for(int i = 0; i < dijk.nodes.size(); i++){
    Node n = (Node)dijk.nodes.get(i);
    Node p = null;
    int pnum = -1;
    if(n.parent != null){
      p = n.parent;
      pnum = dijk.nodes.indexOf(p);
    }
    int x = i % w;
    int y = i / w;
    int s = spacing / 2;
    fill(255);
    ellipse(s + x * spacing, s + y * spacing, 10, 10);
    fill(0);
    text(nf(n.g, 2, 1), 10 + x * spacing, 10 + y * spacing);
    if(p != null){
      int px = pnum % w;
      int py = pnum / w;
      line(s + x * spacing, s + y * spacing, s + lerp(x, px, .75) * spacing, s + lerp(y, py, .75) * spacing);
    }
  }
}

class Dijkstra{
  Vector open = new Vector();
  Vector closed = new Vector();
  Vector nodes = new Vector();
  Dijkstra(){
  }
  void setNodes(Vector nodes){
    this.nodes = nodes;
  }
  void makeWeb(Node start){
    open.clear();
    closed.clear();
    for(int i = 0; i < nodes.size(); i++){
      Node n = (Node)nodes.get(i);
      n.reset();
    }
    open.add(start);
    start.g = 0;
    while(open.size() > 0){
      Node current = (Node)open.remove(0);
      closed.add(current);
      for(int i = 0; i < current.links.size(); i++){
        Connector a = (Connector)current.links.get(i);
        Node adjacent = a.n;
        if(!closed.contains(adjacent)){
          if(!open.contains(adjacent)){
            open.add(adjacent);
            adjacent.parent = current;
            adjacent.setG(a);
          } 
          else {
            if(adjacent.g > current.g + a.d){
              adjacent.parent = current;
            adjacent.setG(a);
            }
          }
        }
      }
    }
  }
}

static class Connector{
  Node n;
  float d;
  Connector(Node n, float d){
    this.n = n;
    this.d = d;
  }
}

static class Node{
  Node parent;
  Vector links = new Vector();
  float g;
  Node(){
  }
  void connect(Node n, float d){
    links.add(new Connector(n, d));
  }
  void reset(){
    parent = null;
    g = 0;
  }
  void setG(Connector o){
    g = parent.g + o.d;
  }
}

static class MapMaker{
  static Vector nodes = new Vector();
  MapMaker(){
  }
  static Vector makeMap(int w, int h){
    for(int i = 0; i < w * h; i++){
      Node n = new Node();
      nodes.add(n);
    }
    for(int i = 0; i < w * h; i++){
      Node n = (Node)nodes.get(i);
      int x = i % w;
      int y = i / h;
      for(int ax = -1; ax < 2; ax++){
        for(int ay = -1; ay < 2; ay++){
          int nx = ax + x;
          int ny = ay + y;
          if(nx > -1 && ny > -1 && nx < w && ny < h && !(nx == x && ny == y)){
            Node o = (Node)nodes.get(nx + ny * w);
            float d = dist(x, y, nx, ny);
            println(d);
            n.connect(o, d);
          }
        }
      }
    }
    return nodes;
  }
}