Problems with my waypoints code

I have been working on a waypoint function that gives the route between two points in the form of an array and it was working fine until I added an extra waypoint. Now it ignores the waypoint when finding routes even if it is clearly part of a shorter route and if it is the goal unity crashes all together. I have looked through my code for the problem and I'm yet to find anything and I was wondering if anyone had an insight.

 static var nodes = new Array ();  //will contain nodes coordinates
 static var joins = new Array ();  //will contain nodes joined to each node
 
 var cube : GameObject;  //Temporary, used as graphical indicator of points
 
 nodes[0] = Vector3(-5,0,5);
 nodes[1] = Vector3(10,0,10);
 nodes[2] = Vector3(5,0,-10);
 nodes[3] = Vector3(5,0,0);
 nodes[4] = Vector3(-10,0,5);
 nodes[5] = Vector3(10,0,5);  //Set's up the nodes coordinates
 nodes[6] = Vector3(0,0,5);
 
 joins[0] = new Array(2,3,4);
 joins[1] = new Array(3,5);
 joins[2] = new Array(0,5,3);
 joins[3] = new Array(1,2,0);
 joins[4] = new Array();
 joins[4][0] = 0;
 joins[5] = new Array(1,2);  //sets up nodes neighbours.
 joins[6] = new Array(1,0);
 
 function Start () 
 {
     for(i=0; i<nodes.length; i++)
     {
         Instantiate(cube, nodes[i], Quaternion.identity);
     }
 }  //Temporarry, places graphical indicators at node points
 
 
 function Update ()
 {
     for(i=0; i<joins.length; i++)
     {
         for(x=0; x<joins[i].length; x++)
             Debug.DrawLine(nodes[i], nodes[joins[i][x]], Color.red);
     }
     //Draws debug Lines between all neighbour nodes
 }
 
 static function route(start : int, end : int)
 {
     if(start==end)
         return(Array(start,end));
     var x=0;
     var openNodes = new Array (); //Will contain any nodes that haven't reached a dead end
     var reached = new Array ();  //Will contain [0]the previous node in it's chain and [1]if it has been reached in the search
     openNodes[0] = new Array (start, Vector3.Distance(nodes[start], nodes[end]), 0); //Sets initial openNodes value to being search at start
     var reachedGoal=false; //Variable used to exit while loop
     for(i=0; i<nodes.length; i++)
     {
         reached[i] = new Array(null,false);
     }  //Initialises all values at null and false for the reached array
     reached[start][1]=true;// sets the start node to be 'reached'
     while(!reachedGoal)
     {
         var smallest : float;
         var activeNode : int;
         var activeTravelled : float;
         var newNode : int;
         var newTravelled : float;
         var activeOpen : int;
         var firstCheck=true;
         var newNodePrevious : int; //Various variables used inside the while loop
         
         
         
         for(i=0; i<openNodes.length; i++) //For each entry in openNodes...
         {
             activeNode=openNodes[i][0]; //Set the selected entry as active Node...
             activeTravelled=openNodes[i][2];  //And the distance traveled to reach it from the start node as active travelled...
             for(n=0; n<joins[activeNode].length; n++)  //Then search each neighbour node...
             {
                 var distanceCheck=activeTravelled+Vector3.Distance(nodes[activeNode],nodes[joins[activeNode][n]]);  //and calculate the distance it will take to reach that node using the distance tavelled for the previous node +the distance between the two nodes
                 if((firstCheck||distanceCheck+Vector3.Distance(nodes[joins[activeNode][n]],nodes[end])<smallest)&&!reached[joins[activeNode][n]][1]) //To check if this is either the first comparison or if it is the shortest path yet, taking into account the straight line distance to the end node...
                 {
                     newNode=joins[activeNode][n];
                     smallest=distanceCheck;
                     newTravelled=distanceCheck;
                     firstCheck=false;
                     newNodePrevious=activeNode;  //If it is, set it to be the new node, as well as some variables used in the next step
                 }
             }
         }
         reached[newNode][1]=true;//Set the final new node to be 'reached'
         reached[newNode][0]=newNodePrevious;//And set which node was before it in the chain
         openNodes[openNodes.length]=new Array(newNode, Vector3.Distance(nodes[newNode],nodes[end]), newTravelled);//Finally create a new entry for the newNode in the open Nodes array
         if(newNode==end)
             reachedGoal=true; // If the new node is the goal break the loop, if not repeat until the goal is reached
     }
     var lastNode=end; //Set a variable to hold the last checked loop in the chain(which should be the goal)
     var routePath=new Array(); //Create an array to hold the path
     routePath[0]=lastNode;//Set the first result to be the end
     while(lastNode!=start)
     {
         lastNode=reached[lastNode][0];
         routePath[routePath.length]=lastNode;
     }//Work bacwords using the previous node information to fill the routePath with the (reversed) start to end path
     return(routePath);//Return the path information
 }

Above is my script for the function itself and below is how I call it

         var firstCheck=true;
         var endFirstCheck=true;
         var shortest=0.0;
         var endShortest=0.0;
         for(i=0; i<waypoints.nodes.length; i++)
         {
             var distanceCheck=Vector3.Distance(transform.position, waypoints.nodes[i]);
             if(firstCheck||distanceCheck<shortest)
             {
                 startNode=i;
                 shortest=distanceCheck;
                 firstCheck=false;
             }
             distanceCheck=Vector3.Distance(GameObject.Find("player").transform.position, waypoints.nodes[i]);
             if(endFirstCheck||distanceCheck<endShortest)
             {
                 endNode=i;
                 endShortest=distanceCheck;
                 endFirstCheck=false;
             }
         }//Checks for the closest node and sets it as the start point
         path=waypoints.route(startNode,endNode);// runs the routing script and sets up the pth array