Vector nodes;
Vector paths;
A_Star routing;

public class IVector extends java.util.Vector {
  public IVector() {

  }
  public IVector(IVector cop) {
    super.addAll(cop);
  }
  public void add(int node) {
    super.add(new Integer(node));
  }
  public boolean has(int node) {
    return super.contains(new Integer(node));
  }

  public int at(int index) {
    Integer elt = (Integer)super.elementAt(index);
    return elt.intValue();
  }
};


boolean jmp = false;

int fnode(int x,int y) {
  for(int i = 0 ; i < nodes.size() ; i++) {
    Node n = (Node)nodes.elementAt(i);
    float nx = n.mX;
    float ny = n.mY;

    if(nx-10 <= x && x <= nx+10 && ny-10 <= y && y <= ny+10) {
      return i;
    }
  }

  return -1;
}

class Node {
  public float mX,mY;
  public IVector links;
  public boolean hub;
  
  public Node(float x,float y) {
    mX = x;
    mY = y;

    links=new IVector();  
  }
  public void createLink(int node) {
    links.add(node);
  }
  public float distance(Node n) {
    if(n.hub && hub) {
      return sqrt((mX-n.mX)*(mX-n.mX) + (mY-n.mY)*(mY-n.mY));
    } else {
      return sqrt((mX-n.mX)*(mX-n.mX) + (mY-n.mY)*(mY-n.mY))*3;
    }
  }
  public void draw() {
    ellipseMode(CENTER);
    if(hub) {
      ellipse(mX,mY,12,12);
    } else {
      ellipse(mX,mY,9,9);
    }
  }
};

float pathLength(IVector path) {
  float d = 0;
  int index;
  index = (int)path.at(0);
  Node c,l=(Node)nodes.elementAt(index);
  for(int i = 1 ; i < path.size() ; i++) {
    index = (int)path.at(i);
    c = (Node)nodes.elementAt(index);
    d += sqrt((c.mX-l.mX)*(c.mX-l.mX) + (c.mY-l.mY)*(c.mY-l.mY));
    l = c;
  }
  return d;
}

void keyPressed() {
  if(key == ' ') {
    if(routing == null) {
      routing = new A_Star(nodes,start,end);
      redraw();
    } 

    routing.step();
    redraw();
  }
  if(key == 'r'){
    if(routing == null) {
      nodes = new Vector();
      paths = new Vector();
      firstRun = true;
    } else {
      routing = null;
    }
  }
  
  redraw();
}

PFont myFont;

void setup() 
{
  size(400, 300);
  framerate(15);
  noLoop();
  
  smooth();

  myFont = loadFont("TrebuchetMS-16.vlw");
  textFont(myFont,16);

  nodes = new Vector();
  paths = new Vector();
  routing = null;
  start=end=0;
  snode=-1;
}

int start,end;
int snode;

int downButton;

void mousePressed() {
firstRun = false;
  if(routing != null) { return; }
  downButton = mouseButton;
  if(mouseButton == LEFT || mouseButton == RIGHT) {
    snode = fnode(mouseX,mouseY);
  }
}

void mouseMoved() {

  redraw();
}

void mouseDragged() {

  redraw();
}

void mouseReleased() {
  if(routing != null) {
    return;
  }

  int nnode = fnode(mouseX,mouseY);
 
  if(snode < 0 && downButton == LEFT) {
    nodes.add(new Node(mouseX,mouseY));
  } else if(snode < 0 && downButton == RIGHT) { 
    nodes.add(new Node(mouseX,mouseY));
    end = nodes.size()-1;
  } else if(snode == nnode && downButton == RIGHT) {
    end = nnode;
    //Node n = (Node)nodes.elementAt(nnode);
    //n.hub = !n.hub;
  } else if(snode == nnode && downButton == LEFT) {
    Node n = (Node)nodes.elementAt(nnode);
    n.hub = !n.hub;
  } else if(snode != nnode) {
    if(nnode != -1 && nnode != snode) {
      Node start = (Node)nodes.elementAt(snode);
      Node end = (Node)nodes.elementAt(nnode);
      start.createLink(nnode);
      if(downButton == LEFT) {
        end.createLink(snode);
      }
    }
  }
  
  snode = -1;
  downButton = 0;
  
  redraw();
}

boolean firstRun = true;
int topac = 255;

void draw() 
{
  noLoop();
  background(241);
  
  if(firstRun || topac > 0) {
    fill(0,0,0,topac);
    if(!firstRun) {
      if(topac > 0) {
      topac -= 5; }
    } else {
      if(topac < 255) {
      topac += 5; }
    }
  
    textAlign(CENTER);
    text("Click to create nodes and hubs\nRight-click to specify end-node\nDrag to create paths between nodes\nSpacebar steps through routefinding\nR key resets",200,100);
    loop();
  }
  
  smooth();

  stroke(121,121,104);
  noFill();
  rect(0,0,width,height);

  for(int i = 0 ; i < nodes.size() ; i++) {
    Node n = (Node)nodes.elementAt(i);
    stroke(0,0,0);
    fill(0,0,0);
    for(int j = 0 ; j < n.links.size() ; j++) {
      Node c = (Node)nodes.elementAt(n.links.at(j));
      
      
      float dX = n.mY - c.mY;
      float dY = c.mX - n.mX;
      float d = sqrt(dX*dX + dY*dY);
      dX /= d;
      dY /= d;

      if(c.hub && n.hub) {
        stroke(0);
        line(n.mX+dX*3,n.mY+dY*3,c.mX+dX*3,c.mY+dY*3);
        //stroke(100);
        //line(n.mX+dX*1,n.mY+dY*1,c.mX+dX*1,c.mY+dY*1);
      } else {
        stroke(0);
        line(n.mX+dX*2,n.mY+dY*2,c.mX+dX*2,c.mY+dY*2);
      }
      stroke(0);
      
      strokeWeight(1);
      
    }
  }

  for(int i = 0 ; i < nodes.size() ; i++) {
    Node n = (Node)nodes.elementAt(i);

    stroke(0,0,0);
    if(i == start) {
      fill(100,255,100);
    } 
    else if(i == end) {
      fill(255,100,100);
    } 
    else {
      fill(255,255,255);
    }
    n.draw();
  }

  if(routing != null) {
    routing.draw();
  }

  if(snode >= 0) {
    Node beg = (Node)nodes.elementAt(snode);
    stroke(0,0,0);
    line(beg.mX,beg.mY,mouseX,mouseY);
  }
}