csc450-chris

        public static List<NavigationNode> Navigate(RoadLink start, RoadLink finish, DateTime time)
        {
            List<NavigationNode> closed = new List<NavigationNode>();

            // build a priority queue of the paths we've found previously
            // sorted by the time it has currently spent traveling
            PriorityQueue<float, List<NavigationNode>> qq
                = new PriorityQueue<float, List<NavigationNode>>();

            // add the starting point as the beginning
            qq.Push(LinkFitness(start, time),
                NavigationNode.MakePath(start,time));

            while (qq.Count > 0)
            {
                // take the first path off the list, this has the lowest total
                // travel time so far
                float length = qq.PeekPriority();
                List<NavigationNode> path = qq.Pop();

                // check the last node in the path
                NavigationNode lastLink = path[path.Count-1];

                // if we've visited this node previously, we're making a cycle
                if (closed.Contains(lastLink))
                    continue; // skip to ignore cycles

                // if we've reached the destination, add the destination link
                // and return
                if (lastLink.LeavingLink == finish)
                {
                    path.Add(new NavigationNode(finish, null, null, time));
                    return path;
                }

                // mark visited
                closed.Add(lastLink);

                // create new paths based on where we can go to from here
                foreach (NavigationNode next in lastLink.Leave(time))
                {
                    // calculate the new length
                    float newLength = length
                        + TransitionFitness(next.LinkIntersection,
                            next.EnteringLink, next.LeavingLink, time)
                        + LinkFitness(next.LeavingLink, time);

                    //TODO: calculate the new time to add to the end node

                    // create the new path
                    List<NavigationNode> newList
                        = new List<NavigationNode>(path);
                    newList.Add(next);

                    // add it to the priority queue for consideration
                    qq.Push(newLength, newList);
                }
            }

            // you can't get there from here... no really... you can't
            return null;
        }

        public static float LinkFitness(RoadLink link, DateTime time)
        {
            return (link.Distance / link.SpeedLimit);
        }

        public static float TransitionFitness(Intersection intersection,
            RoadLink inLink, RoadLink outLink, DateTime time)
        {
            // 20m is the guessed transition distance
            //HACK: lol we should quantify this
            return (0.0124274f) /
                ((inLink.SpeedLimit + outLink.SpeedLimit)/2.0f);
        }