/// <summary>
        /// Gets all exits downstream from a point
        /// </summary>
        /// <param name="position"></param>
        /// <param name="way"></param>
        /// <param name="exits">We are looking for exits</param>
        /// <returns></returns>
        private List <DownstreamPointOfInterest> Downstream(Coordinates position, ArbiterWay way, bool exits, List <ArbiterWaypoint> ignorable)
        {
            List <DownstreamPointOfInterest> waypoints = new List <DownstreamPointOfInterest>();

            foreach (ArbiterLane al in way.Lanes.Values)
            {
                LinePath.PointOnPath pop = al.GetClosestPoint(position);

                if (al.IsInside(position) || position.DistanceTo(al.LanePath().StartPoint.Location) < 1.0)
                {
                    ArbiterLanePartition currentPartition = al.GetClosestPartition(position);
                    ArbiterWaypoint      initial          = currentPartition.Final;
                    double initialCost = position.DistanceTo(currentPartition.Final.Position) / way.Segment.SpeedLimits.MaximumSpeed;

                    do
                    {
                        if (((exits && currentPartition.Final.IsExit) || (!exits && currentPartition.Final.IsEntry)) && !ignorable.Contains(currentPartition.Final))
                        {
                            double timeCost = initialCost + this.TimeCostInLane(initial, currentPartition.Final);
                            DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
                            dpoi.DistanceToPoint = al.LanePath().DistanceBetween(pop, al.GetClosestPoint(currentPartition.Final.Position));
                            dpoi.IsExit          = true;
                            dpoi.IsGoal          = false;
                            dpoi.PointOfInterest = currentPartition.Final;
                            dpoi.TimeCostToPoint = timeCost;
                            waypoints.Add(dpoi);
                        }

                        currentPartition = currentPartition.Final.NextPartition;
                    }while(currentPartition != null);
                }
            }

            return(waypoints);
        }
        /// <summary>
        /// Checks if a goal is downstream on the current road
        /// </summary>
        /// <param name="way"></param>
        /// <param name="currentPosition"></param>
        /// <param name="goal"></param>
        /// <returns></returns>
        private DownstreamPointOfInterest IsGoalDownStream(ArbiterWay way, Coordinates currentPosition, INavigableNode goal)
        {
            if (goal is ArbiterWaypoint)
            {
                ArbiterWaypoint goalWaypoint = (ArbiterWaypoint)goal;

                if (goalWaypoint.Lane.Way.Equals(way))
                {
                    foreach (ArbiterLane lane in way.Lanes.Values)
                    {
                        if (lane.Equals(goalWaypoint.Lane))
                        {
                            Coordinates current   = lane.GetClosest(currentPosition);
                            Coordinates goalPoint = lane.GetClosest(goal.Position);

                            if (lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0)
                            {
                                double distToGoal = lane.DistanceBetween(current, goalPoint);

                                if (distToGoal > 0)
                                {
                                    DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
                                    dpoi.DistanceToPoint = distToGoal;
                                    dpoi.IsGoal          = true;
                                    dpoi.IsExit          = false;
                                    dpoi.PointOfInterest = goalWaypoint;
                                    dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint);
                                    dpoi.RouteTime       = 0.0;
                                    return(dpoi);
                                }
                            }
                        }
                        else
                        {
                            Coordinates current   = lane.GetClosest(currentPosition);
                            Coordinates goalPoint = lane.GetClosest(goal.Position);

                            if ((lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0) && lane.IsInside(goalPoint))
                            {
                                double distToGoal = lane.DistanceBetween(current, goalPoint);

                                if (distToGoal > 0)
                                {
                                    DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
                                    dpoi.DistanceToPoint = distToGoal;
                                    dpoi.IsGoal          = true;
                                    dpoi.IsExit          = false;
                                    dpoi.PointOfInterest = goalWaypoint;
                                    dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint);
                                    dpoi.RouteTime       = 0.0;
                                    //return dpoi;
                                }
                            }
                        }
                    }
                }
            }

            return(null);
        }
        public bool HitGoal(VehicleState state, Coordinates goal, IAreaSubtypeWaypointId id)
        {
            // check if forced
            if (this.EntryParameters.HasValue && this.EntryParameters.Value.ForcedOpposing)
            {
                // get other way
                ArbiterWay other = this.OpposingLane.Way.WayId.Number == 1 ? this.OpposingLane.Way.Segment.Way2 : this.OpposingLane.Way.Segment.Way1;

                // check goal in other way
                if (id is ArbiterWaypointId && ((ArbiterWaypointId)id).LaneId.WayId.Equals(other.WayId))
                {
                    // center
                    Coordinates vehicleCenter = state.Front - state.Heading.Normalize(TahoeParams.VL / 2.0);

                    // check all lanes
                    foreach (ArbiterLane al in other.Lanes.Values)
                    {
                        // get closest point to the center of this vehicle
                        bool b = al.LanePath().GetClosestPoint(vehicleCenter).Location.DistanceTo(
                            al.LanePath().GetClosestPoint(goal).Location) < TahoeParams.VL / 2.0;

                        if (b)
                        {
                            return(true);
                        }
                    }
                }
            }

            return(false);
        }
        /// <summary>
        /// Waypoints to ignore
        /// </summary>
        /// <param name="way"></param>
        /// <param name="position"></param>
        /// <returns></returns>
        public static List <ArbiterWaypoint> WaypointsClose(ArbiterWay way, Coordinates position, ArbiterWaypoint ignoreSpecifically)
        {
            List <ArbiterWaypoint> waypoints = new List <ArbiterWaypoint>();

            foreach (ArbiterLane al in way.Lanes.Values)
            {
                ArbiterWaypoint aw = al.GetClosestWaypoint(position, TahoeParams.VL * 2.0);
                if (aw != null)
                {
                    waypoints.Add(aw);
                }
            }

            if (ignoreSpecifically != null && !waypoints.Contains(ignoreSpecifically))
            {
                waypoints.Add(ignoreSpecifically);
            }

            return(waypoints);
        }
        /// <summary>
        /// Route ploan for downstream exits
        /// </summary>
        /// <param name="downstreamPoints"></param>
        /// <param name="goal"></param>
        private void DetermineDownstreamPointRouteTimes(List <DownstreamPointOfInterest> downstreamPoints, INavigableNode goal, ArbiterWay aw)
        {
            // so, for each exit downstream we need to plan from the end of each interconnect to the goal
            foreach (DownstreamPointOfInterest dpoi in downstreamPoints)
            {
                // check if exit
                if (dpoi.IsExit)
                {
                    // current best time
                    double bestCurrent = Double.MaxValue;
                    List <INavigableNode> bestRoute        = null;
                    ArbiterInterconnect   bestInterconnect = null;

                    // fake node
                    FakeExitNode fen = new FakeExitNode(dpoi.PointOfInterest);

                    // init fields
                    double timeCost;
                    List <INavigableNode> routeNodes;

                    // plan
                    this.Plan(fen, goal, out routeNodes, out timeCost);

                    bestCurrent      = timeCost;
                    bestRoute        = routeNodes;
                    bestInterconnect = routeNodes.Count > 1 ? fen.GetEdge(routeNodes[1]) : null;

                    // plan from each interconnect to find the best time from exit

                    /*foreach (ArbiterInterconnect ai in dpoi.PointOfInterest.Exits)
                     * {
                     *      // init fields
                     *      double timeCost;
                     *      List<INavigableNode> routeNodes;
                     *
                     *      // plan
                     *      this.Plan(ai.End, goal, out routeNodes, out timeCost);
                     *
                     *      // check
                     *      if (timeCost < bestCurrent)
                     *      {
                     *              bestCurrent = timeCost;
                     *              bestRoute = routeNodes;
                     *              bestInterconnect = ai;
                     *      }
                     * }*/

                    // set best
                    dpoi.RouteTime = bestCurrent;
                    dpoi.BestRoute = bestRoute;
                    dpoi.BestExit  = bestInterconnect;
                }
            }
        }
        /// <summary>
        /// Gets the road plan
        /// </summary>
        /// <param name="pointsOfInterest"></param>
        /// <returns></returns>
        private RoadPlan GetRoadPlan(List <DownstreamPointOfInterest> pointsOfInterest, ArbiterWay way)
        {
            // road plan
            RoadPlan rp = new RoadPlan(new Dictionary <ArbiterLaneId, LanePlan>());

            // find the best in each lane
            foreach (ArbiterLane al in way.Lanes.Values)
            {
                // points in lane
                List <DownstreamPointOfInterest> lanePoints = new List <DownstreamPointOfInterest>();

                // loop over all points
                foreach (DownstreamPointOfInterest dpoi in pointsOfInterest)
                {
                    if (dpoi.PointOfInterest.Lane.Equals(al))
                    {
                        lanePoints.Add(dpoi);
                    }
                }

                // if exist any
                if (lanePoints.Count > 0)
                {
                    // sort
                    lanePoints.Sort();

                    // choose best and add
                    rp.LanePlans.Add(al.LaneId, new LanePlan(lanePoints[0]));
                }
            }

            // return
            return(rp);
        }