public void push(Waypoint w, float cost)
    {
        PNode node = new PNode (w, cost);
        if (count == 0) {
            head = node;
            tail = node;
        } else {
            // find the place to insert
            PNode curr = head;
            while (curr != null && curr.cost < node.cost) {
                curr = curr.next;
            }
            if (curr == null) {
                // we are inserting at the end
                tail.next = node;
                node.prev = tail;
                node.next = null;
                tail = node;
            } else if (curr == head) {
                // we are insterint at the beginning
                head.prev = node;
                node.next = head;
                node.prev = null;
                head = node;
            } else {
                // somewhere in the middle
                curr.prev.next = node;
                node.prev = curr.prev;
                node.next = curr;
                curr.prev = node;
            }

        }
        count++;
    }

        public static void CsvLoadTripRoutes(string filename, bool lngFirst)
        {
            // load trip routes
            Dictionary<string, LinkedList<Waypoint>> routes = new Dictionary<string, LinkedList<Waypoint>>();
            using (CsvFileReader reader = new CsvFileReader(filename))
            {
                CsvRow row = new CsvRow();
                while (reader.ReadRow(row, ','))
                {
                    string routeID = row[0];
                    double distance = 0;
                    double lat = Convert.ToDouble(lngFirst ? row[2] : row[1]);
                    double lng = Convert.ToDouble(lngFirst ? row[1] : row[2]);
                    if (routes.ContainsKey(routeID))
                        distance = routes[routeID].First.Value.GetDistance(new Location(lat, lng, "null"));
                    Waypoint waypoint = new Waypoint(lat, lng, TimeSpan.Parse(row[3]), distance, row[4].Replace("\"", ""));

                    // Scenario #1
                    if (!routes.ContainsKey(routeID))
                        routes[routeID] = new LinkedList<Waypoint>();
                    routes[routeID].AddLast(waypoint);

                }
            }
            foreach (LinkedList<Waypoint> w in routes.Values)
            {
                Route r = new Route(w.ToArray());
                string key = Route.GetKey(r.start, r.end);
                MapTools.routes.Add(key, r);
            }
        }

 public override void Start()
 {
     base.Start ();
     startRotation = -180f;
     activeWaypoint = Waypoint.startingWaypoint;
     player = GameObject.FindGameObjectWithTag("Player");
     enemies = GameObject.FindGameObjectsWithTag("Enemy");
     for (int i = 0; i < enemies.Length; i++)
         if (enemies[i] == this.gameObject)
             enemies[i] = player;
     obstacles = GameObject.FindGameObjectsWithTag("Terrain");
     timeToDestroy = 0.0f;
     rotation += startRotation;
     syncStartPosition = GetComponent<Rigidbody>().position;
     syncEndPosition = GetComponent<Rigidbody>().position;
     syncStartRotation = GetComponent<Rigidbody>().rotation;
     syncEndRotation = GetComponent<Rigidbody>().rotation;
     nextBurst = true;
     burstTime = 0.0f;
     flare = turboFlare.GetComponent<ParticleSystem> ();
     core = turboCore.GetComponent<ParticleSystem> ();
     flare.startSize = 2.0f;
     core.startSize = 2.0f;
     flare.startRotation = transform.eulerAngles.y *Mathf.Deg2Rad;
     core.startRotation = transform.eulerAngles.y *Mathf.Deg2Rad;
     turboFlare.SetActive (false);
     turboCore.SetActive (false);
 }

	void Update()
    {
        if (Input.GetMouseButtonDown(0))
        {
            Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
            RaycastHit hit;
            if (Physics.Raycast(ray, out hit))
            {
                Waypoint waypoint = hit.collider.GetComponent<Waypoint>();
                if (waypoint != null)
                {
                    path = graphManager.GetPath(currentWaypoint, waypoint);
                }
            }
        }

        if (path != null &&
            path.Count > 0 &&
            Vector3.Distance(transform.position, currentWaypoint.transform.position) < reachThreshold)
        {
            currentWaypoint = path[0];
            path.RemoveAt(0);
        }

        transform.position = Vector3.Lerp(transform.position, currentWaypoint.transform.position, lerpRatio * Time.deltaTime);
	}

 public Waypoint(long timeVal, Tile tileVal, Waypoint prevVal, List<UnitSelection> startVal)
 {
     time = timeVal;
     tile = tileVal;
     prev = prevVal;
     start = startVal;
 }

 public override void OnTriggerEnter(Collider trigger)
 {
     base.OnTriggerEnter (trigger);
     if (activeWaypoint.GetComponent<Collider>() == trigger) {
         activeWaypoint = activeWaypoint.getNextWaypoint();
     }
 }

    private string CreateMajorRoutes(string locationString)
    {
        string results = "";
        string key = "ArLeGdHOcc5h7j3L4W37oFGcU9E-LF3tAZi4o0DfhXbPJ8aiyTGbIDNHex08R2u7";
        MajorRoutesRequest majorRoutesRequest = new MajorRoutesRequest();

        // Set the credentials using a valid Bing Maps key
        majorRoutesRequest.Credentials = new RouteService.Credentials();
        majorRoutesRequest.Credentials.ApplicationId = key;

        // Set the destination of the routes from major roads
        Waypoint endPoint = new Waypoint();
        endPoint.Location = new RouteService.Location();
        string[] digits = locationString.Split(',');
        endPoint.Location.Latitude = double.Parse(digits[0].Trim());
        endPoint.Location.Longitude = double.Parse(digits[1].Trim());
        endPoint.Description = "Location";

        // Set the option to return full routes with directions
        MajorRoutesOptions options = new MajorRoutesOptions();
        options.ReturnRoutes = true;

        majorRoutesRequest.Destination = endPoint;
        majorRoutesRequest.Options = options;

        // Make the route-from-major-roads request
        RouteServiceClient routeService = new RouteServiceClient("BasicHttpBinding_IRouteService");

        // The result is an MajorRoutesResponse Object
        MajorRoutesResponse majorRoutesResponse = routeService.CalculateRoutesFromMajorRoads(majorRoutesRequest);

        Regex regex = new Regex("<[/a-zA-Z:]*>",
          RegexOptions.IgnoreCase | RegexOptions.Multiline);

        if (majorRoutesResponse.StartingPoints.Length > 0)
        {
            StringBuilder directions = new StringBuilder();

            for (int i = 0; i < majorRoutesResponse.StartingPoints.Length; i++)
            {
                directions.Append(String.Format("Coming from {1}\n", i + 1,
                    majorRoutesResponse.StartingPoints[i].Description));

                for (int j = 0;
                  j < majorRoutesResponse.Routes[i].Legs[0].Itinerary.Length; j++)
                {
                    //Strip tags
                    string step = regex.Replace(
                      majorRoutesResponse.Routes[i].Legs[0].Itinerary[j].Text, string.Empty);
                    directions.Append(String.Format("     {0}. {1}\n", j + 1, step));
                }
            }

            results = directions.ToString();
        }
        else
            results = "No Routes found";

        return results;
    }

    public List<Waypoint> ShortestPath(Waypoint a, Waypoint b)
    {
        Dictionary<Waypoint, Waypoint> cameFrom = new Dictionary<Waypoint, Waypoint> ();
        Dictionary<Waypoint, float> costSoFar = new Dictionary<Waypoint, float> ();
        PQueue frontier = new PQueue();
        cameFrom[a] = null;
        costSoFar [a] = 0f;
        frontier.push (a, 0);
        while (!frontier.empty ()) {
            Waypoint current = frontier.pop ();
            if (current == b)
                break;
            foreach (Waypoint next in current.connections) {
                float newCost = costSoFar [current] + Waypoint.distance (current, next);
                if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next]) {
                    costSoFar [next] = newCost;
                    frontier.push (next, newCost);
                    cameFrom [next] = current;
                }
            }
        }

        List<Waypoint> path = new List<Waypoint> ();
        Waypoint point = b;
        while (point != a) {
            path.Add (point);
            point = cameFrom[point];
        }
        path.Add (a);
        path.Reverse ();
        return path;
    }

        public long GetRoadTime(Point[] points)
        {
            DBC.Assert(points.Length == 2, "Calculating road time requires exactly 2 points. " + points.Length + " were provided.");

            var cacheResult = routeCache.GetTime(points[0].address, points[1].address);
            if (cacheResult != null)
            {
                Log(points, true);
                return cacheResult.Value;
            }
            Log(points, false);

            var router = new RouteServiceClient();
            var request = new RouteRequest();
            request.Credentials = new Credentials();
            request.Credentials.Token = GetToken();
            request.Waypoints = new Waypoint[points.Length];

            for (int i = 0; i < points.Length; i++)
            {
                var p = points[i];
                var waypoint = new Waypoint();
                waypoint.Description = p.address;
                waypoint.Location = new Location();
                waypoint.Location.Latitude = p.lat;
                waypoint.Location.Longitude = p.lon;
                request.Waypoints[i] = waypoint;
            }

            RouteResponse response = router.CalculateRoute(request);
            var seconds = response.Result.Summary.TimeInSeconds;
            routeCache.Add(points, seconds);

            return seconds;
        }

    void MoveToWaypoit(Waypoint waypoint)
    {
        //als de waypoint afstand kleiner is als 0.1 dan haal nieuwe waypoint op
        if (Vector2.Distance(waypoint.gameObject.transform.position, this.transform.position) < 0.1)
        {
            this.targetWaypoint = waypoint.NextWaypoint;
        }
        else
        {
            //reken de vector richting de waypoint uit
            Vector2 vectorRichtingWaypoint = waypoint.transform.position - this.transform.position;
            //normalize deze vector(dat betekend dat de lengte van de vector terug naar 1 gebracht word)
            vectorRichtingWaypoint.Normalize();
            //vermenigvuldig deze vector met de (speed*deltatime) die we willen hebben(dit betekend de lengte van de vector word nu zo groot als de speed*detltatime)
            vectorRichtingWaypoint *= Speed * Time.fixedDeltaTime;
            //voeg deze nieuwe vector aan onze positie en we bewegen richting de waypoint
            this.transform.position += new Vector3(vectorRichtingWaypoint.x, vectorRichtingWaypoint.y);

            //convert v3 naar v2
            Vector2 thispos = new Vector2(this.transform.position.x, this.transform.position.y);

            this.drawLineTo(this.movementLine, this.transform.position, thispos + vectorRichtingWaypoint.normalized * Speed);
            this.bewegingsVector = vectorRichtingWaypoint;
        }
    }

 public void AddRotationWaypoint()
 {
     GameObject go = new GameObject();
     go.AddComponent("RotationWaypoint");
     Waypoint w = go.GetComponent<Waypoint>();
     w.manager = this;
     if (count > 0) {
         if (last != null) {
             go.transform.position = last.transform.position;
             last.next = w;
             last = w;
         } else {
             last = first;
             while (last.next != null) {
                 last = last.next;
             }
             go.transform.position = last.transform.position;
             last.next = w;
             last = w;
         }
     } else {
         first = w;
         last = w;
         go.transform.position = transform.position;
     }
     count++;
     Selection.activeObject = go;
     go.name = "Waypoint " + count + " (rotation)";
     go.transform.parent = transform;
 }

 // Use this for initialization
 void Start()
 {
     animation.Play();
     myController = GetComponent<CharacterController>();
     initialPosition = transform.position;
     initialWaypoint = waypoint;
 }

 public void Reset()
 {
     m_isMoving = false;
     m_levelStartWp = null;
     m_levelEndWp = null;
     m_nextWaypoint = null;
 }

    public void Connect(Waypoint other)
    {
        if (other == null)
            throw new ArgumentNullException("other");

        this.AddNeighbour(other);
        other.AddNeighbour(this);
    }

 // Game freaks out without a default constructor. I never use it.
 public StationaryPointParameter()
 {
     targetBody = Planetarium.fetch.Home;
     longitude = 0.0;
     wp = new Waypoint();
     submittedWaypoint = false;
     this.successCounter = 0;
 }

	public override void ResumeMovement(Transform target)
	{
		if (target == null)
		{
			CurrWaypoint = waypointManager.GetWaypoint(this.gameObject);
			this.target = CurrWaypoint.transform;
		}		
	}

    private bool CanSeePlayer(Waypoint waypoint)
    {
        RaycastHit2D hit = MazeManager.RayCast(
            CurrentPosition, waypoint, waypoint.Position - CurrentPosition,
            VISION_ANGLE_LIMIT, PLAYER_LAYER_MASK);

        return hit.collider != null;
    }

        private void decideNextPatrolPoint()
        {
            // Keep track of the previous patrol point to prevent backtracking
            previousWaypoint = parent.CurrentWaypoint;

            // Set our target to go to
            parent.FinalTargetWaypoint = parent.CurrentWaypoint.GetRandomNeighbour(previousWaypoint);
        }

        private int waypointWriteOffset = 0; //location where it can write next waypoint in array.

        #endregion Fields

        #region Constructors

        public WalkingQueue(Player player)
        {
            this.player = player;
            for(int i = 0; i < MAX_WALKING_WAYPOINTS; i++)
                walkingQueue[i] = new Waypoint(0, 0, -1);
            this.lastDirection = 6;
            resetWalkingQueue();
        }

	float targInnaccuracy = 2f; // Extra innaccuray to simulate mouse hand shake or something

	// Use this for initialization
	void Start () {
		netChar = GetComponent<NetworkCharacter>();

		if(waypoints == null) {
			waypoints = GameObject.FindObjectsOfType<Waypoint>();
		}

		destination = GetClosestWaypoint();
	}