/* Initialise any variables. */
var NONODES = AB.randomIntAtoB (5, 15); 
var NOCONNECTIONS = NONODES * AB.randomIntAtoB(1, 2);
var graph;
var queue = [];

/* Initialise any constants. */
/* The "rest distance" between nodes. */
const springLength = 200;

/* Initialise a graph object. */
function Graph() {
    /* Store all of the nodes by label in this object. */
    this.graph = {};
  
    /* Iterate through all of the nodes using this array. */
    this.nodes = [];

    /* Initialise the start and end nodes. */
    this.start = null;
    this.end = null;
}

/* Set the start node. */
Graph.prototype.setStart = function(node) {
    this.start = node;
}

/* Set the end node. */
Graph.prototype.setEnd = function(node) {
    this.end = node;
}

/* Add a node to the graph. */
Graph.prototype.addNode = function(label) {
    /* Create a new node. */
    var node = new Node(label);
  
    /* Add this newly created node to the set of all nodes. */
    this.graph[label] = node;
    this.nodes.push(node);
  
    /* Return a reference to the node. */
    return node;
}

/* Draw the collection of nodes. */
Graph.prototype.show = function() {
    for (var i = 0; i < this.nodes.length; i++) {
        this.nodes[i].showEdges();
    }
  
    for (var i = 0; i < this.nodes.length; i++) {
        this.nodes[i].show();
    }
}

/* Simulate some physics by animating the graph. */
Graph.prototype.simulate = function() {
    /* Place the first node in the centre. */
    this.nodes[0].pos.set(width / 2, height / 2);

    /* Position every node with respect to every other node. */
    for (var i = 1; i < this.nodes.length; i++) {
        var node1 = this.nodes[i];
        for (var j = 0; j < this.nodes.length; j++) {
            if (i == j) continue;
            var node2 = this.nodes[j];

            /* Create a vector that points between the above nodes. */
            var force = p5.Vector.sub(node1.pos, node2.pos);
            var dist = force.mag();

            var spring = 0;
            var k = 0.06;
      
            if (node1.isConnected(node2) || node2.isConnected(node1)) {
                spring = k * (springLength - dist);
            }
      
            var separate = 1 / (dist * k);
            force.setMag(spring + separate)
            node1.vel.add(force);
            node1.vel.mult(0.95);
        }
    }

    /* Add a velocity for all nodes. */
    for (var i = 0; i < this.nodes.length; i++) {
        var node = this.nodes[i];
        node.pos.add(node.vel);
    }
}

function Node(label) {
    this.pos = createVector(random(width), random(height));
    this.vel = createVector();
    this.col = color(0);
    this.label = label;
    this.edges = [];
    this.parent = null;
    this.searched = false;
    this.onqueue = false;
}

/* Is the node in question connected to another node? */
Node.prototype.isConnected = function(neighbor) {
    var index = this.edges.indexOf(neighbor);
    return (index >= 0);
}

/* Draw the node in question. */
Node.prototype.show = function() {
    textAlign(CENTER);
    var w = textWidth(this.label);
    stroke(255);
    fill(this.col);
    ellipse(this.pos.x, this.pos.y, w * 2, w * 2);
    fill(255);
    noStroke();
    text(this.label, this.pos.x, this.pos.y);
}

/* Add colour to the node. */
Node.prototype.highlight = function() {
    this.col = color(0, 150, 0);
}

/* Draw any connections for this node. */
Node.prototype.showEdges = function() {
    noFill();
    stroke(255);
    for (var i = 0; i < this.edges.length; i++) {
        line(this.pos.x, this.pos.y, this.edges[i].pos.x, this.edges[i].pos.y);
    }
}


function connect(n, m) {
    if (!n.isConnected(m)) n.edges.push(m);
    if (!m.isConnected(n)) m.edges.push(n);
};

function setup() 
{
    createCanvas(ABWorld.fullwidth(), ABWorld.fullheight());
    graph = new Graph();
    
    var start = graph.addNode('START');
    var dest  = graph.addNode('DEST');
    var i,c,d;
  
    /* Add (NONODES-2) further random nodes. */
    for (i = 2; i <= NONODES-1; i++) 
    {
        var s = Math.random().toString(36).substring(9);
        graph.addNode(s);
    }

    /* Connect start and end to 2 random nodes. */
    for (i = 1; i <= 2; i++) {
        c = AB.randomIntAtoB(2, NONODES-1); 
        connect(graph.nodes[0], graph.nodes[c]);
    }

    for (i = 1; i <= 2; i++) {
        c = AB.randomIntAtoB(2, NONODES-1); 
        connect(graph.nodes[1], graph.nodes[c]);
    }
    
    /* Add some random other connections. */
    for (i = 1; i <= NOCONNECTIONS; i++) {
        c = AB.randomIntAtoB(2, NONODES-1); 
        d = AB.randomIntAtoB(2, NONODES-1); 
        connect(graph.nodes[c], graph.nodes[d]);
    }
    
    graph.setStart(start);
    graph.setEnd(dest);
    bfs();
}

function draw() {
    background(51);
    graph.simulate();
    graph.show();
}

function highlightPath(n) {
    var subclock = 0;
    n.highlight(); 
    
    while (n != graph.start) {
        if (subclock > 1000) break;
        subclock++;
            
        n = n.parent;
        n.highlight(); 
    }
}

function bfs() {
    var clock = 0;
    var i;
    var found = false;
 
    queue.push(graph.start);

    while (queue.length > 0) {
        if (clock > 10000) break;
        clock++;
      
        var person = queue.shift();
        if (!person.searched) {
            console.log("======");
            console.log("Start of search of " + person.label);
            if (person == graph.end) {
                console.log("Found goal node");
                highlightPath(person);
                found = true;
                break;
            } else {
                console.log("Checking neighbours of " + person.label);
                for (i = 0; i < person.edges.length; i++) {
                    var n = person.edges[i];
                    if (!n.onqueue) {
                        console.log("Adding neighbour " + n.label + " to queue");
                        queue.push(n);
                        n.parent = person;
                        n.onqueue = true;
                    }
                }
                
                person.searched = true;
                console.log("End of search of " + person.label);
            }
        }
    }
    
    if (!found) {
        console.log("No path found!");
    }
}