/// <summary>
        /// Write partition informaton
        /// </summary>
        /// <param name="sw"></param>
        private void WritePartitionInformation(StreamWriter sw)
        {
            // get list of all partitions that connect waypoints
            List <IConnectAreaWaypoints> icaws = new List <IConnectAreaWaypoints>();

            #region Populate partitions

            // get lane partitions
            foreach (ArbiterSegment asg in roadNetwork.ArbiterSegments.Values)
            {
                foreach (ArbiterLane al in asg.Lanes.Values)
                {
                    foreach (ArbiterLanePartition alp in al.Partitions)
                    {
                        icaws.Add(alp);
                    }
                }
            }

            // get interconnects
            foreach (ArbiterInterconnect ai in roadNetwork.ArbiterInterconnects.Values)
            {
                icaws.Add(ai);
            }

            // zones (holy stuff what a hack)
            foreach (ArbiterZone az in roadNetwork.ArbiterZones.Values)
            {
                icaws.Add(new SceneZonePartition(az));
            }

            #endregion

            // notify
            sw.WriteLine("NumberOfPartitions" + "\t" + icaws.Count.ToString());
            string completionPercent = "";

            #region Create Partitions in Road Graph

            // write each
            for (int i = 0; i < icaws.Count; i++)
            {
                IConnectAreaWaypoints icaw = icaws[i];
                sw.WriteLine("Partition");

                string id = "";
                if (icaw is SceneZonePartition)
                {
                    id = ("PartitionId" + "\t" + ((SceneZonePartition)icaw).Zone.ToString());
                }
                else
                {
                    id = ("PartitionId" + "\t" + icaw.ConnectionId.ToString());
                }
                sw.WriteLine(id);

                // notify
                double percent = ((double)i) / ((double)icaws.Count) * 100.0;
                string tmpP    = percent.ToString("F0") + "% Complete";
                if (tmpP != completionPercent)
                {
                    completionPercent = tmpP;
                    EditorOutput.WriteLine(completionPercent);
                }

                #region Interconnect

                if (icaw is ArbiterInterconnect)
                {
                    ArbiterInterconnect ai = (ArbiterInterconnect)icaw;
                    sw.WriteLine("PartitionType" + "\t" + "Interconnect");
                    sw.WriteLine("Sparse" + "\t" + "False");
                    sw.WriteLine("FitType" + "\t" + "Line");

                    Coordinates c = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
                    sw.WriteLine("FitParameters" + "\t" + c.ArcTan.ToString("F6"));
                    sw.WriteLine("LeftBoundary" + "\t" + "None");
                    sw.WriteLine("RightBoundary" + "\t" + "None");
                    sw.WriteLine("NumberOfPoints" + "\t" + "2");
                    sw.WriteLine("Points");
                    sw.WriteLine(ai.InitialGeneric.ToString());
                    sw.WriteLine(ai.FinalGeneric.ToString());
                    sw.WriteLine("End_Points");

                    List <ArbiterWaypoint> aws = this.GetNearbyStops(ai);
                    sw.WriteLine("NumberOfNearbyStoplines" + "\t" + aws.Count);
                    if (aws.Count != 0)
                    {
                        sw.WriteLine("NearbyStoplines");
                        foreach (ArbiterWaypoint aw in aws)
                        {
                            sw.WriteLine(aw.ToString());
                        }
                        sw.WriteLine("End_NearbyStoplines");
                    }

                    #region Adjacent

                    List <string> adjacentPartitions = new List <string>();

                    // add current
                    adjacentPartitions.Add(ai.ToString());

                    #region Initial

                    if (icaw.InitialGeneric is ArbiterWaypoint)
                    {
                        // wp
                        ArbiterWaypoint aw = (ArbiterWaypoint)icaw.InitialGeneric;

                        // prev
                        if (aw.PreviousPartition != null)
                        {
                            adjacentPartitions.Add(aw.PreviousPartition.ToString());
                        }

                        // next
                        if (aw.NextPartition != null)
                        {
                            adjacentPartitions.Add(aw.NextPartition.ToString());
                        }

                        // exits
                        if (aw.IsExit)
                        {
                            foreach (ArbiterInterconnect ais in aw.Exits)
                            {
                                if (!ais.Equals(ai))
                                {
                                    adjacentPartitions.Add(ais.ToString());
                                }
                            }
                        }

                        if (aw.IsEntry)
                        {
                            foreach (ArbiterInterconnect ais in aw.Entries)
                            {
                                if (!ais.Equals(ai))
                                {
                                    adjacentPartitions.Add(ais.ToString());
                                }
                            }
                        }
                    }
                    else if (icaw.InitialGeneric is ArbiterPerimeterWaypoint)
                    {
                        adjacentPartitions.Add((new SceneZonePartition(((ArbiterPerimeterWaypoint)icaw.InitialGeneric).Perimeter.Zone)).ToString());
                    }

                    #endregion

                    #region Final

                    if (icaw.FinalGeneric is ArbiterWaypoint)
                    {
                        // wp
                        ArbiterWaypoint aw = (ArbiterWaypoint)icaw.FinalGeneric;

                        // prev
                        if (aw.PreviousPartition != null)
                        {
                            adjacentPartitions.Add(aw.PreviousPartition.ToString());
                        }

                        // next
                        if (aw.NextPartition != null)
                        {
                            adjacentPartitions.Add(aw.NextPartition.ToString());
                        }

                        // exits
                        if (aw.IsExit)
                        {
                            foreach (ArbiterInterconnect ais in aw.Exits)
                            {
                                adjacentPartitions.Add(ais.ToString());
                            }
                        }

                        if (aw.IsEntry)
                        {
                            foreach (ArbiterInterconnect ais in aw.Entries)
                            {
                                if (!ais.Equals(ai))
                                {
                                    adjacentPartitions.Add(ais.ToString());
                                }
                            }
                        }
                    }
                    else if (icaw.FinalGeneric is ArbiterPerimeterWaypoint)
                    {
                        adjacentPartitions.Add((new SceneZonePartition(((ArbiterPerimeterWaypoint)icaw.FinalGeneric).Perimeter.Zone)).ToString());
                    }

                    #endregion

                    sw.WriteLine("NumberOfLaneAdjacentPartitions" + "\t" + adjacentPartitions.Count.ToString());
                    if (adjacentPartitions.Count != 0)
                    {
                        sw.WriteLine("LaneAdjacentPartitions");
                        foreach (string s in adjacentPartitions)
                        {
                            sw.WriteLine(s);
                        }
                        sw.WriteLine("End_LaneAdjacentPartitions");
                    }

                    #endregion

                    sw.WriteLine("NumberOfLeftLaneAdjacentPartitions" + "\t" + "0");
                    sw.WriteLine("NumberOfRightLaneAdjacentPartitions" + "\t" + "0");

                    List <IConnectAreaWaypoints> nearby = this.GetNearbyPartitions(ai, icaws);
                    sw.WriteLine("NumberOfNearbyPartitions" + "\t" + nearby.Count.ToString());
                    if (nearby.Count != 0)
                    {
                        sw.WriteLine("NearbyPartitions");
                        foreach (IConnectAreaWaypoints tmp in nearby)
                        {
                            sw.WriteLine(tmp.ToString());
                        }
                        sw.WriteLine("End_NearbyPartitions");
                    }

                    sw.WriteLine("End_Partition");
                }

                #endregion

                #region Zone

                else if (icaw is SceneZonePartition)
                {
                    SceneZonePartition szp = (SceneZonePartition)icaw;
                    sw.WriteLine("PartitionType" + "\t" + "Zone");
                    sw.WriteLine("Sparse" + "\t" + "False");
                    sw.WriteLine("FitType" + "\t" + "Polygon");

                    string count = szp.Zone.Perimeter.PerimeterPoints.Count.ToString();
                    string wps   = "";
                    foreach (ArbiterPerimeterWaypoint apw in szp.Zone.Perimeter.PerimeterPoints.Values)
                    {
                        wps = wps + "\t" + apw.Position.X.ToString("f6") + "\t" + apw.Position.Y.ToString("f6");
                    }
                    sw.WriteLine("FitParameters" + "\t" + count + wps);

                    sw.WriteLine("LeftBoundary" + "\t" + "None");
                    sw.WriteLine("RightBoundary" + "\t" + "None");
                    sw.WriteLine("NumberOfPoints" + "\t" + szp.Zone.Perimeter.PerimeterPoints.Count.ToString());
                    sw.WriteLine("Points");
                    foreach (ArbiterPerimeterWaypoint apw in szp.Zone.Perimeter.PerimeterPoints.Values)
                    {
                        sw.WriteLine(apw.WaypointId.ToString());
                    }
                    sw.WriteLine("End_Points");

                    List <ArbiterWaypoint> aws = this.GetNearbyStops(szp);
                    sw.WriteLine("NumberOfNearbyStoplines" + "\t" + aws.Count);
                    if (aws.Count != 0)
                    {
                        sw.WriteLine("NearbyStoplines");
                        foreach (ArbiterWaypoint aw in aws)
                        {
                            sw.WriteLine(aw.ToString());
                        }
                        sw.WriteLine("End_NearbyStoplines");
                    }

                    #region Adjacent

                    List <string> adjacentStrings = new List <string>();

                    // add current
                    adjacentStrings.Add(szp.ToString());

                    foreach (ArbiterPerimeterWaypoint apw in szp.Zone.Perimeter.PerimeterPoints.Values)
                    {
                        if (apw.IsExit)
                        {
                            foreach (ArbiterInterconnect ai in apw.Exits)
                            {
                                adjacentStrings.Add(ai.ToString());
                            }
                        }

                        if (apw.IsEntry)
                        {
                            foreach (ArbiterInterconnect ais in apw.Entries)
                            {
                                adjacentStrings.Add(ais.ToString());
                            }
                        }
                    }

                    sw.WriteLine("NumberOfLaneAdjacentPartitions" + "\t" + adjacentStrings.Count.ToString());
                    if (adjacentStrings.Count != 0)
                    {
                        sw.WriteLine("LaneAdjacentPartitions");
                        foreach (string s in adjacentStrings)
                        {
                            sw.WriteLine(s);
                        }
                        sw.WriteLine("End_LaneAdjacentPartitions");
                    }


                    #endregion

                    sw.WriteLine("NumberOfLeftLaneAdjacentPartitions" + "\t" + "0");
                    sw.WriteLine("NumberOfRightLaneAdjacentPartitions" + "\t" + "0");

                    List <IConnectAreaWaypoints> nearby = this.GetNearbyPartitions(szp, icaws);
                    sw.WriteLine("NumberOfNearbyPartitions" + "\t" + nearby.Count.ToString());
                    if (nearby.Count != 0)
                    {
                        sw.WriteLine("NearbyPartitions");
                        foreach (IConnectAreaWaypoints tmp in nearby)
                        {
                            sw.WriteLine(tmp.ToString());
                        }
                        sw.WriteLine("End_NearbyPartitions");
                    }

                    sw.WriteLine("End_Partition");
                }

                #endregion

                #region Lane

                else if (icaw is ArbiterLanePartition)
                {
                    ArbiterLanePartition alp = (ArbiterLanePartition)icaw;
                    sw.WriteLine("PartitionType" + "\t" + "Lane");
                    string sparseString = alp.Type == PartitionType.Sparse ? "True" : "False";
                    sw.WriteLine("Sparse" + "\t" + sparseString);

                    if (alp.Type != PartitionType.Sparse)                    //alp.UserPartitions.Count <= 1)
                    {
                        sw.WriteLine("FitType" + "\t" + "Line");
                        sw.WriteLine("FitParameters" + "\t" + alp.Vector().ArcTan.ToString("F6"));
                    }
                    else
                    {
                        sw.WriteLine("FitType" + "\t" + "Polygon");

                        /*List<Coordinates> polyCoords = new List<Coordinates>();
                         * polyCoords.Add(alp.Initial.Position);
                         * polyCoords.AddRange(alp.NotInitialPathCoords());
                         * LinePath lpr = (new LinePath(polyCoords)).ShiftLateral(-TahoeParams.VL * 3.0);
                         * LinePath lpl = (new LinePath(polyCoords)).ShiftLateral(TahoeParams.VL * 3.0);
                         * List<Coordinates> finalCoords = new List<Coordinates>(polyCoords.ToArray());
                         * finalCoords.AddRange(lpr);
                         * finalCoords.AddRange(lpl);
                         * Polygon p = Polygon.GrahamScan(finalCoords);*/

                        if (alp.SparsePolygon == null)
                        {
                            alp.SetDefaultSparsePolygon();
                        }

                        string coordinateString = "";
                        foreach (Coordinates c in alp.SparsePolygon)
                        {
                            coordinateString = coordinateString + "\t" + c.X.ToString("F6") + "\t" + c.Y.ToString("F6");
                        }

                        sw.WriteLine("FitParameters" + "\t" + alp.SparsePolygon.Count.ToString() + coordinateString);
                    }

                    sw.WriteLine("LaneWidth" + "\t" + alp.Lane.Width.ToString("F6"));
                    sw.WriteLine("LeftBoundary" + "\t" + alp.Lane.BoundaryLeft.ToString());
                    sw.WriteLine("RightBoundary" + "\t" + alp.Lane.BoundaryRight.ToString());
                    sw.WriteLine("NumberOfPoints" + "\t" + "2");
                    sw.WriteLine("Points");
                    sw.WriteLine(alp.InitialGeneric.ToString());
                    sw.WriteLine(alp.FinalGeneric.ToString());
                    sw.WriteLine("End_Points");

                    List <ArbiterWaypoint> aws = this.GetNearbyStops(alp);
                    sw.WriteLine("NumberOfNearbyStoplines" + "\t" + aws.Count);
                    if (aws.Count != 0)
                    {
                        sw.WriteLine("NearbyStoplines");
                        foreach (ArbiterWaypoint aw in aws)
                        {
                            sw.WriteLine(aw.ToString());
                        }
                        sw.WriteLine("End_NearbyStoplines");
                    }

                    #region Adjacent

                    List <string> adjacentPartitions = new List <string>();

                    // add current
                    adjacentPartitions.Add(alp.ToString());

                    #region Initial

                    if (icaw.InitialGeneric is ArbiterWaypoint)
                    {
                        // wp
                        ArbiterWaypoint aw = (ArbiterWaypoint)icaw.InitialGeneric;

                        // prev
                        if (aw.PreviousPartition != null)
                        {
                            adjacentPartitions.Add(aw.PreviousPartition.ToString());
                        }

                        // next
                        if (aw.NextPartition != null && !aw.NextPartition.Equals(alp))
                        {
                            adjacentPartitions.Add(aw.NextPartition.ToString());
                        }

                        // exits
                        if (aw.IsExit)
                        {
                            foreach (ArbiterInterconnect ais in aw.Exits)
                            {
                                adjacentPartitions.Add(ais.ToString());
                            }
                        }

                        if (aw.IsEntry)
                        {
                            foreach (ArbiterInterconnect ais in aw.Entries)
                            {
                                adjacentPartitions.Add(ais.ToString());
                            }
                        }
                    }

                    #endregion

                    #region Final

                    if (icaw.FinalGeneric is ArbiterWaypoint)
                    {
                        // wp
                        ArbiterWaypoint aw = (ArbiterWaypoint)icaw.FinalGeneric;

                        // prev
                        if (aw.PreviousPartition != null && !aw.PreviousPartition.Equals(alp))
                        {
                            adjacentPartitions.Add(aw.PreviousPartition.ToString());
                        }

                        // next
                        if (aw.NextPartition != null)
                        {
                            adjacentPartitions.Add(aw.NextPartition.ToString());
                        }

                        // exits
                        if (aw.IsExit)
                        {
                            foreach (ArbiterInterconnect ais in aw.Exits)
                            {
                                adjacentPartitions.Add(ais.ToString());
                            }
                        }

                        if (aw.IsEntry)
                        {
                            foreach (ArbiterInterconnect ais in aw.Entries)
                            {
                                adjacentPartitions.Add(ais.ToString());
                            }
                        }
                    }

                    #endregion

                    sw.WriteLine("NumberOfLaneAdjacentPartitions" + "\t" + adjacentPartitions.Count.ToString());
                    if (adjacentPartitions.Count != 0)
                    {
                        sw.WriteLine("LaneAdjacentPartitions");
                        foreach (string s in adjacentPartitions)
                        {
                            sw.WriteLine(s);
                        }
                        sw.WriteLine("End_LaneAdjacentPartitions");
                    }

                    #endregion

                    List <string> leftAlps  = new List <string>();
                    List <string> rightAlps = new List <string>();

                    foreach (ArbiterLanePartition tmpAlp in alp.NonLaneAdjacentPartitions)
                    {
                        if (tmpAlp.Lane.Equals(alp.Lane.LaneOnLeft))
                        {
                            leftAlps.Add(tmpAlp.ToString());
                        }
                        else
                        {
                            rightAlps.Add(tmpAlp.ToString());
                        }
                    }

                    sw.WriteLine("NumberOfLeftLaneAdjacentPartitions" + "\t" + leftAlps.Count.ToString());
                    if (leftAlps.Count != 0)
                    {
                        sw.WriteLine("LeftLaneAdjacentPartitions");
                        foreach (string s in leftAlps)
                        {
                            sw.WriteLine(s);
                        }
                        sw.WriteLine("End_LeftLaneAdjacentPartitions");
                    }

                    sw.WriteLine("NumberOfRightLaneAdjacentPartitions" + "\t" + rightAlps.Count.ToString());
                    if (rightAlps.Count != 0)
                    {
                        sw.WriteLine("RightLaneAdjacentPartitions");
                        foreach (string s in rightAlps)
                        {
                            sw.WriteLine(s);
                        }
                        sw.WriteLine("End_RightLaneAdjacentPartitions");
                    }

                    List <IConnectAreaWaypoints> nearby = this.GetNearbyPartitions(alp, icaws);
                    sw.WriteLine("NumberOfNearbyPartitions" + "\t" + nearby.Count.ToString());
                    if (nearby.Count != 0)
                    {
                        sw.WriteLine("NearbyPartitions");
                        foreach (IConnectAreaWaypoints tmp in nearby)
                        {
                            sw.WriteLine(tmp.ToString());
                        }
                        sw.WriteLine("End_NearbyPartitions");
                    }

                    sw.WriteLine("End_Partition");
                }

                #endregion
            }

            #endregion
        }
        /// <summary>
        /// Resets the interconnect we wish to take
        /// </summary>
        /// <param name="reset"></param>
        public void ResetDesired(ArbiterInterconnect desired)
        {
            // create monitors
            this.AllMonitors = this.IntersectionPriorityQueue;
            this.NonStopPriorityAreas = new List<IDominantMonitor>();
            this.PreviouslyWaiting = new List<IDominantMonitor>();
            this.cooldownTimer = new Stopwatch();

            #region Priority Areas Over Interconnect

            // check contains priority lane for this
            if (this.Intersection.PriorityLanes.ContainsKey(desired.ToInterconnect))
            {
                // loop through all other priority monitors over this interconnect
                foreach (IntersectionInvolved ii in this.Intersection.PriorityLanes[desired.ToInterconnect])
                {
                    // check area type if lane
                    if (ii.Area is ArbiterLane)
                    {
                        // add lane to non stop priority areas
                        this.NonStopPriorityAreas.Add(new DominantLaneEntryMonitor(this, ii));
                    }
                    // otherwise is zone
                    else if (ii.Area is ArbiterZone)
                    {
                        // add lane to non stop priority areas
                        this.NonStopPriorityAreas.Add(
                            new DominantZoneEntryMonitor((ArbiterPerimeterWaypoint)ii.Exit, ((ArbiterInterconnect)desired), false, this, ii));
                    }
                    else
                    {
                        throw new ArgumentException("unknown intersection involved area", "ii");
                    }
                }
            }
            // otherwise be like, wtf?
            else
            {
                ArbiterOutput.Output("Exception: intersection: " + this.Intersection.ToString() + " priority lanes does not contain interconnect: " + desired.ToString() + " returning can go");
                //ArbiterOutput.Output("Error in intersection monitor!!!!!!!!!@Q!!, desired interconnect: " + desired.ToInterconnect.ToString() + " not found in intersection: " + ai.ToString() + ", not setting any dominant monitors");
            }

            #endregion

            #region Entry Area

            if (desired.FinalGeneric is ArbiterWaypoint)
            {
                this.EntryAreaMonitor = new LaneEntryAreaMonitor((ArbiterWaypoint)desired.FinalGeneric);
            }
            else if (desired.FinalGeneric is ArbiterPerimeterWaypoint)
            {
                this.EntryAreaMonitor = new ZoneAreaEntryMonitor((ArbiterPerimeterWaypoint)desired.FinalGeneric, (ArbiterInterconnect)desired,
                    false, this, new IntersectionInvolved(this.OurMonitor.Waypoint, ((ArbiterPerimeterWaypoint)desired.FinalGeneric).Perimeter.Zone,
                    ((ArbiterInterconnect)desired).TurnDirection));
            }
            else
            {
                throw new Exception("unhandled desired interconnect final type");
            }

            #endregion

            // add to all
            foreach (IIntersectionQueueable iiq in this.NonStopPriorityAreas)
                this.AllMonitors.Add(iiq);
            this.AllMonitors.Add(this.EntryAreaMonitor);

            // update all
            this.Update(CoreCommon.Communications.GetVehicleState());
        }
        /// <summary>
        /// Determine the involved lanes with any interconnect
        /// </summary>
        /// <param name="exits"></param>
        /// <param name="incoming"></param>
        /// <returns></returns>
        /// <remarks>TODO: implement</remarks>
        private Dictionary<ArbiterInterconnect, List<IntersectionInvolved>> DetermineInvolved(IEnumerable<ITraversableWaypoint> exits, Dictionary<ArbiterLane, LinePath.PointOnPath> incoming)
        {
            // final mapping of interconnects to priority lanes that list of priority areas over each interconnect
            Dictionary<ArbiterInterconnect, List<IntersectionInvolved>> priority = new Dictionary<ArbiterInterconnect, List<IntersectionInvolved>>();

            // 1. Get list of all lanes incoming to the intersection
            List<ArbiterLane> als = new List<ArbiterLane>();
            foreach (ArbiterLane al in incoming.Keys)
            {
                als.Add(al);
            }

            // 2. Get all exits for each area
            Dictionary<IVehicleArea, List<ITraversableWaypoint>> exitLookup = new Dictionary<IVehicleArea, List<ITraversableWaypoint>>();
            foreach (ITraversableWaypoint aw in exits)
            {
                if (exitLookup.ContainsKey(aw.VehicleArea))
                {
                    exitLookup[aw.VehicleArea].Add(aw);
                }
                else
                {
                    // add all exits
                    List<ITraversableWaypoint> laneExits = new List<ITraversableWaypoint>();
                    laneExits.Add(aw);
                    exitLookup.Add(aw.VehicleArea, laneExits);
                }
            }

            // 3. loop through exits and determine priority areas above them
            foreach (ITraversableWaypoint aw in exits)
            {
                /*if (aw is ArbiterWaypoint && ((ArbiterWaypoint)aw).WaypointId.Equals(new ArbiterWaypointId(19, new ArbiterLaneId(2, new ArbiterWayId(2, new ArbiterSegmentId(6))))))
                {
                    Console.WriteLine("");
                }*/

                // 3.1 loop through interconnects from exits
                foreach (ArbiterInterconnect ai in aw.Exits)
                {
                    // exit priority areas
                    List<IntersectionInvolved> priorityAreas = new List<IntersectionInvolved>();

                    // add explicit interconnects
                    priorityAreas.AddRange(this.laneOverlaps(als, ai, exits));

                    // implicit intersections
                    List<IntersectionInvolved> implicitInvolved = this.nonStopOverlaps(exits, ai);
                    foreach (IntersectionInvolved ii in implicitInvolved)
                    {
                        // make sure not contained already
                        if (!priorityAreas.Contains(ii))
                        {
                            // add
                            priorityAreas.Add(ii);
                        }
                        // if already contained, replace
                        else if(ii.CompareTo(priorityAreas[priorityAreas.IndexOf(ii)]) == -1)
                        {
                            priorityAreas.Remove(ii);
                            priorityAreas.Add(ii);
                        }
                    }

                    // add the priority overlaps to the exits
                    priority.Add(ai, priorityAreas);
                }

                // 3.2 check continuation if exists
                if(aw.IsStop)
                {
                    // get waypoint
                    ArbiterWaypoint waypoint = (ArbiterWaypoint)aw;

                    // check if next partition exists
                    if(waypoint.NextPartition != null)
                    {
                        // exit priority areas
                        List<IntersectionInvolved> priorityAreas = new List<IntersectionInvolved>();

                        // fake interconnect
                        ArbiterInterconnect fakeAi = new ArbiterInterconnect(waypoint, waypoint.NextPartition.Final, ArbiterTurnDirection.Straight);

                        #region New

                        // list of next intersection ivolved
                        List<IntersectionInvolved> nextII = new List<IntersectionInvolved>();

                        // list of fake interconnects defining the continuation
                        List<ArbiterInterconnect> fakeAis = new List<ArbiterInterconnect>();

                        // add the next partition by default
                        fakeAis.Add(fakeAi);

                        // entries into lane of fake ai involved in the intersection
                        List<ArbiterWaypoint> laneEntries = new List<ArbiterWaypoint>();
                        foreach (ITraversableWaypoint itw in exits)
                        {
                            foreach (ArbiterInterconnect aiTmp in itw.Exits)
                            {
                                if (aiTmp.FinalGeneric is ArbiterWaypoint)
                                {
                                    ArbiterWaypoint awTmp = (ArbiterWaypoint)aiTmp.FinalGeneric;

                                    if (awTmp.Lane.Equals(waypoint.Lane) && !awTmp.Equals(waypoint.NextPartition.Final) && !laneEntries.Contains(awTmp))
                                    {
                                        ArbiterInterconnect tmpFake = new ArbiterInterconnect(waypoint, awTmp, ArbiterTurnDirection.Straight);
                                        if (!fakeAis.Contains(tmpFake))
                                        {
                                            laneEntries.Add(awTmp);
                                            fakeAis.Add(tmpFake);
                                        }
                                    }
                                }
                            }
                        }

                        // loop through fake ais adding ii
                        foreach (ArbiterInterconnect fake in fakeAis)
                        {
                            // explicit and explicit add to tmp
                            List<IntersectionInvolved> tmpIis = this.laneOverlaps(als, fake, exits);
                            tmpIis.AddRange(this.nonStopOverlaps(exits, fake));

                            // check and add
                            foreach (IntersectionInvolved tmpIi in tmpIis)
                            {
                                if (!nextII.Contains(tmpIi))
                                    nextII.Add(tmpIi);
                                else if (nextII[nextII.IndexOf(tmpIi)].Exit == null)
                                {
                                    nextII.Remove(tmpIi);
                                    nextII.Add(tmpIi);
                                }
                            }
                        }

                        // add to priority areas
                        priorityAreas.AddRange(nextII);

                        #endregion

                        #region Old

                        /*
                            // fake interconnect
                            ArbiterInterconnect fakeAi = new ArbiterInterconnect(waypoint, waypoint.NextPartition.Final);

                            // add explicit interconnects
                            priorityAreas.AddRange(this.laneOverlaps(als, fakeAi));

                            // add implicit interconnects
                            priorityAreas.AddRange(this.nonStopOverlaps(exits, fakeAi));

                            // add the priority overlaps to the exits
                            priority.Add(fakeAi, priorityAreas);
                        */

                        #endregion

                        // add the priority overlaps to the exits
                        if (priority.ContainsKey(fakeAi))
                            EditorOutput.WriteLine("Error adding interconnect: " + fakeAi.ToString() + " to priority areas as key already existed, check priorities");
                        else
                            priority.Add(fakeAi, priorityAreas);
                    }
                }
            }

            // return the final priorities
            return priority;
        }
Exemplo n.º 4
0
        /// <summary>
        /// Resets the interconnect we wish to take
        /// </summary>
        /// <param name="reset"></param>
        public void ResetDesired(ArbiterInterconnect desired)
        {
            // create monitors
            this.AllMonitors          = this.IntersectionPriorityQueue;
            this.NonStopPriorityAreas = new List <IDominantMonitor>();
            this.PreviouslyWaiting    = new List <IDominantMonitor>();
            this.cooldownTimer        = new Stopwatch();

            #region Priority Areas Over Interconnect

            // check contains priority lane for this
            if (this.Intersection.PriorityLanes.ContainsKey(desired.ToInterconnect))
            {
                // loop through all other priority monitors over this interconnect
                foreach (IntersectionInvolved ii in this.Intersection.PriorityLanes[desired.ToInterconnect])
                {
                    // check area type if lane
                    if (ii.Area is ArbiterLane)
                    {
                        // add lane to non stop priority areas
                        this.NonStopPriorityAreas.Add(new DominantLaneEntryMonitor(this, ii));
                    }
                    // otherwise is zone
                    else if (ii.Area is ArbiterZone)
                    {
                        // add lane to non stop priority areas
                        this.NonStopPriorityAreas.Add(
                            new DominantZoneEntryMonitor((ArbiterPerimeterWaypoint)ii.Exit, ((ArbiterInterconnect)desired), false, this, ii));
                    }
                    else
                    {
                        throw new ArgumentException("unknown intersection involved area", "ii");
                    }
                }
            }
            // otherwise be like, wtf?
            else
            {
                ArbiterOutput.Output("Exception: intersection: " + this.Intersection.ToString() + " priority lanes does not contain interconnect: " + desired.ToString() + " returning can go");
                //ArbiterOutput.Output("Error in intersection monitor!!!!!!!!!@Q!!, desired interconnect: " + desired.ToInterconnect.ToString() + " not found in intersection: " + ai.ToString() + ", not setting any dominant monitors");
            }

            #endregion

            #region Entry Area

            if (desired.FinalGeneric is ArbiterWaypoint)
            {
                this.EntryAreaMonitor = new LaneEntryAreaMonitor((ArbiterWaypoint)desired.FinalGeneric);
            }
            else if (desired.FinalGeneric is ArbiterPerimeterWaypoint)
            {
                this.EntryAreaMonitor = new ZoneAreaEntryMonitor((ArbiterPerimeterWaypoint)desired.FinalGeneric, (ArbiterInterconnect)desired,
                                                                 false, this, new IntersectionInvolved(this.OurMonitor.Waypoint, ((ArbiterPerimeterWaypoint)desired.FinalGeneric).Perimeter.Zone,
                                                                                                       ((ArbiterInterconnect)desired).TurnDirection));
            }
            else
            {
                throw new Exception("unhandled desired interconnect final type");
            }

            #endregion

            // add to all
            foreach (IIntersectionQueueable iiq in this.NonStopPriorityAreas)
            {
                this.AllMonitors.Add(iiq);
            }
            this.AllMonitors.Add(this.EntryAreaMonitor);

            // update all
            this.Update(CoreCommon.Communications.GetVehicleState());
        }
Exemplo n.º 5
0
 public string StateInformation()
 {
     return(Interconnect.ToString());
 }
        public void GenerateInterconnectPolygon(ArbiterInterconnect ai)
        {
            List<Coordinates> polyPoints = new List<Coordinates>();
            try
            {
                // width
                double width = 3.0;
                if (ai.InitialGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
                    width = width < aw.Lane.Width ? aw.Lane.Width : width;
                }
                if (ai.FinalGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
                    width = width < aw.Lane.Width ? aw.Lane.Width : width;
                }

                if (ai.TurnDirection == ArbiterTurnDirection.UTurn ||
                    ai.TurnDirection == ArbiterTurnDirection.Straight ||
                    !(ai.InitialGeneric is ArbiterWaypoint) ||
                    !(ai.FinalGeneric is ArbiterWaypoint))
                {
                    LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
                    LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
                    polyPoints.AddRange(lp);
                    polyPoints.AddRange(rp);
                    ai.TurnPolygon = Polygon.GrahamScan(polyPoints);

                    if (ai.TurnDirection == ArbiterTurnDirection.UTurn)
                    {
                        List<Coordinates> updatedPts = new List<Coordinates>();
                        LinePath interTmp = ai.InterconnectPath.Clone();
                        Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
                        interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0);
                        interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0);
                        lp = interTmp.ShiftLateral(TahoeParams.VL);
                        rp = interTmp.ShiftLateral(-TahoeParams.VL);
                        updatedPts.AddRange(lp);
                        updatedPts.AddRange(rp);
                        ai.TurnPolygon = Polygon.GrahamScan(updatedPts);
                    }
                }
                else
                {
                    // polygon points
                    List<Coordinates> interPoints = new List<Coordinates>();

                    // waypoint
                    ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
                    ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;

                    // left and right path
                    LinePath leftPath = new LinePath();
                    LinePath rightPath = new LinePath();

                    // some initial points
                    LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position });
                    LinePath il = initialPath.ShiftLateral(width / 2.0);
                    LinePath ir = initialPath.ShiftLateral(-width / 2.0);
                    leftPath.Add(il[1]);
                    rightPath.Add(ir[1]);

                    // some final points
                    LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position });
                    LinePath fl = finalPath.ShiftLateral(width / 2.0);
                    LinePath fr = finalPath.ShiftLateral(-width / 2.0);
                    leftPath.Add(fl[0]);
                    rightPath.Add(fr[0]);

                    // initial and final paths
                    Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position);
                    Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position);

                    // get where the paths intersect and vector to normal path
                    Coordinates c;
                    iPath.Intersect(fPath, out c);
                    Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c;
                    Coordinates center = c + vector.Normalize((vector.Length / 2.0));

                    // get width expansion
                    Coordinates iVec = awI.PreviousPartition != null ? awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0);
                    double iRot = -iVec.ArcTan;
                    Coordinates fVec = awF.NextPartition != null ? awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0);
                    fVec = fVec.Rotate(iRot);
                    double fDeg = fVec.ToDegrees();
                    double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
                    double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0;

                    // get inner point (small scale)
                    Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0);

                    // get outer
                    Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0);

                    if (ai.TurnDirection == ArbiterTurnDirection.Right)
                    {
                        rightPath.Insert(1, innerPoint);
                        ai.InnerCoordinates = rightPath;
                        leftPath.Reverse();
                        leftPath.Insert(1, outerPoint);
                        Polygon p = new Polygon(leftPath.ToArray());
                        p.AddRange(rightPath.ToArray());
                        ai.TurnPolygon = p;
                    }
                    else
                    {
                        leftPath.Insert(1, innerPoint);
                        ai.InnerCoordinates = leftPath;
                        rightPath.Reverse();
                        rightPath.Insert(1, outerPoint);
                        Polygon p = new Polygon(leftPath.ToArray());
                        p.AddRange(rightPath.ToArray());
                        ai.TurnPolygon = p;
                    }
                }
            }
            catch (Exception e)
            {
                Console.WriteLine("error generating turn polygon: " + ai.ToString());
                ai.TurnPolygon = ai.DefaultPoly();
            }
        }
        /// <summary>
        /// Generates interconnects into the road network
        /// </summary>
        /// <param name="arn"></param>
        /// <returns></returns>
        public ArbiterRoadNetwork GenerateInterconnects(ArbiterRoadNetwork arn)
        {
            // list of all exit entries in the xy rndf
            List<SimpleExitEntry> sees = new List<SimpleExitEntry>();

            // zones
            if(xyRndf.Zones != null)
            {
                // loop over zones
                foreach (SimpleZone sz in xyRndf.Zones)
                {
                    // add all ee's
                    sees.AddRange(sz.Perimeter.ExitEntries);
                }
            }

            // segments
            if (xyRndf.Segments != null)
            {
                // loop over segments
                foreach (SimpleSegment ss in xyRndf.Segments)
                {
                    // lanes
                    foreach (SimpleLane sl in ss.Lanes)
                    {
                        // add all ee's
                        sees.AddRange(sl.ExitEntries);
                    }
                }
            }

            // loop over ee's and create interconnects
            foreach (SimpleExitEntry see in sees)
            {
                IArbiterWaypoint initial = arn.LegacyWaypointLookup[see.ExitId];
                IArbiterWaypoint final = arn.LegacyWaypointLookup[see.EntryId];
                ArbiterInterconnect ai = new ArbiterInterconnect(initial, final);
                arn.ArbiterInterconnects.Add(ai.InterconnectId, ai);
                arn.DisplayObjects.Add(ai);

                if (initial is ITraversableWaypoint)
                {
                    ITraversableWaypoint initialWaypoint = (ITraversableWaypoint)initial;

                    initialWaypoint.IsExit = true;

                    if (initialWaypoint.Exits == null)
                        initialWaypoint.Exits = new List<ArbiterInterconnect>();

                    initialWaypoint.Exits.Add(ai);
                }
                else
                {
                    throw new Exception("initial wp of ee: " + see.ExitId + " is not ITraversableWaypoint");
                }

                if (final is ITraversableWaypoint)
                {
                    ITraversableWaypoint finalWaypoint = (ITraversableWaypoint)final;

                    finalWaypoint.IsEntry = true;

                    if (finalWaypoint.Entries == null)
                        finalWaypoint.Entries = new List<ArbiterInterconnect>();

                    finalWaypoint.Entries.Add(ai);
                }
                else
                {
                    throw new Exception("final wp of ee: " + see.EntryId + " is not ITraversableWaypoint");
                }

                // set the turn direction
                this.SetTurnDirection(ai);

                // interconnectp olygon stuff
                this.GenerateInterconnectPolygon(ai);
                if (ai.TurnPolygon.IsComplex)
                {
                    Console.WriteLine("Found complex polygon for interconnect: " + ai.ToString());
                    ai.TurnPolygon = ai.DefaultPoly();
                }
            }

            return arn;
        }
        /// <summary>
        /// Plans what maneuer we should take next
        /// </summary>
        /// <param name="planningState"></param>
        /// <param name="navigationalPlan"></param>
        /// <param name="vehicleState"></param>
        /// <param name="vehicles"></param>
        /// <param name="obstacles"></param>
        /// <param name="blockage"></param>
        /// <returns></returns>
        public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState,
                             SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, List <ITacticalBlockage> blockages)
        {
            #region Waiting At Intersection Exit

            if (planningState is WaitingAtIntersectionExitState)
            {
                // state
                WaitingAtIntersectionExitState waies = (WaitingAtIntersectionExitState)planningState;

                // get intersection plan
                IntersectionPlan ip = (IntersectionPlan)navigationalPlan;

                // nullify turn reasoning
                this.TurnReasoning = null;

                #region Intersection Monitor Updates

                // check correct intersection monitor
                if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(waies.exitWaypoint.AreaSubtypeWaypointId) &&
                    (IntersectionTactical.IntersectionMonitor == null ||
                     !IntersectionTactical.IntersectionMonitor.OurMonitor.Waypoint.Equals(waies.exitWaypoint)))
                {
                    // create new intersection monitor
                    IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
                        waies.exitWaypoint,
                        CoreCommon.RoadNetwork.IntersectionLookup[waies.exitWaypoint.AreaSubtypeWaypointId],
                        vehicleState,
                        ip.BestOption);
                }

                // update if exists
                if (IntersectionTactical.IntersectionMonitor != null)
                {
                    // update monitor
                    IntersectionTactical.IntersectionMonitor.Update(vehicleState);

                    // print current
                    ArbiterOutput.Output(IntersectionTactical.IntersectionMonitor.IntersectionStateString());
                }

                #endregion

                #region Desired Behavior

                // get best option from previously saved
                IConnectAreaWaypoints icaw = null;

                if (waies.desired != null)
                {
                    ArbiterInterconnect tmpInterconnect = waies.desired;
                    if (waies.desired.InitialGeneric is ArbiterWaypoint)
                    {
                        ArbiterWaypoint init = (ArbiterWaypoint)waies.desired.InitialGeneric;
                        if (init.NextPartition != null && init.NextPartition.Final.Equals(tmpInterconnect.FinalGeneric))
                        {
                            icaw = init.NextPartition;
                        }
                        else
                        {
                            icaw = waies.desired;
                        }
                    }
                    else
                    {
                        icaw = waies.desired;
                    }
                }
                else
                {
                    icaw          = ip.BestOption;
                    waies.desired = icaw.ToInterconnect;
                }

                #endregion

                #region Turn Feasibility Reasoning

                // check uturn
                if (waies.desired.TurnDirection == ArbiterTurnDirection.UTurn)
                {
                    waies.turnTestState = TurnTestState.Completed;
                }

                // check already determined feasible
                if (waies.turnTestState == TurnTestState.Unknown ||
                    waies.turnTestState == TurnTestState.Failed)
                {
                    #region Determine Behavior to Accomplish Turn

                    // get default turn behavior
                    TurnBehavior testTurnBehavior = this.DefaultTurnBehavior(icaw);

                    // set saudi decorator
                    if (waies.saudi != SAUDILevel.None)
                    {
                        testTurnBehavior.Decorators.Add(new ShutUpAndDoItDecorator(waies.saudi));
                    }

                    // set to ignore all vehicles
                    testTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });

                    #endregion

                    #region Check Turn Feasible

                    // check if we have completed
                    CompletionReport turnCompletionReport;
                    bool             completedTest = CoreCommon.Communications.TestExecute(testTurnBehavior, out turnCompletionReport);        //CoreCommon.Communications.AsynchronousTestHasCompleted(testTurnBehavior, out turnCompletionReport, true);

                    // if we have completed the test
                    if (completedTest || ((TrajectoryBlockedReport)turnCompletionReport).BlockageType != BlockageType.Dynamic)
                    {
                        #region Can Complete

                        // check success
                        if (turnCompletionReport.Result == CompletionResult.Success)
                        {
                            // set completion state of the turn
                            waies.turnTestState = TurnTestState.Completed;
                        }

                        #endregion

                        #region No Saudi Level, Found Initial Blockage

                        // otherwise we cannot do the turn, check if saudi is still none
                        else if (waies.saudi == SAUDILevel.None)
                        {
                            // notify
                            ArbiterOutput.Output("Increased Saudi Level of Turn to L1");

                            // up the saudi level, set as turn failed and no other option
                            waies.saudi         = SAUDILevel.L1;
                            waies.turnTestState = TurnTestState.Failed;
                        }

                        #endregion

                        #region Already at L1 Saudi

                        else if (waies.saudi == SAUDILevel.L1)
                        {
                            // notify
                            ArbiterOutput.Output("Turn with Saudi L1 Level failed");

                            // get an intersection plan without this interconnect
                            IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect(
                                waies.exitWaypoint,
                                CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
                                waies.desired);

                            // check that the plan exists
                            if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) &&
                                testPlan.BestRouteTime < double.MaxValue - 1.0)
                            {
                                // get the desired interconnect
                                ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect;

                                #region Check that the reset interconnect is feasible

                                // test the reset interconnect
                                TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset);

                                // set to ignore all vehicles
                                testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });

                                // check if we have completed
                                CompletionReport turnResetCompletionReport;
                                bool             completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport);

                                // check to see if this is feasible
                                if (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95)
                                {
                                    // notify
                                    ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to current interconnect: " + waies.desired.ToString());

                                    // set the interconnect as being blocked
                                    CoreCommon.Navigation.AddInterconnectBlockage(waies.desired);

                                    // reset all
                                    waies.desired       = reset;
                                    waies.turnTestState = TurnTestState.Completed;
                                    waies.saudi         = SAUDILevel.None;
                                    waies.useTurnBounds = true;
                                    IntersectionMonitor.ResetDesired(reset);
                                }

                                #endregion

                                #region No Lane Bounds

                                // otherwise try without lane bounds
                                else
                                {
                                    // notify
                                    ArbiterOutput.Output("Had to fallout to using no turn bounds");

                                    // up the saudi level, set as turn failed and no other option
                                    waies.saudi         = SAUDILevel.L1;
                                    waies.turnTestState = TurnTestState.Completed;
                                    waies.useTurnBounds = false;
                                }

                                #endregion
                            }

                            #region No Lane Bounds

                            // otherwise try without lane bounds
                            else
                            {
                                // up the saudi level, set as turn failed and no other option
                                waies.saudi         = SAUDILevel.L1;
                                waies.turnTestState = TurnTestState.Unknown;
                                waies.useTurnBounds = false;
                            }

                            #endregion
                        }

                        #endregion

                        // want to reset ourselves
                        return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
                    }

                    #endregion
                }

                #endregion

                #region Entry Monitor Blocked

                // checks the entry monitor vehicle for failure
                if (IntersectionMonitor != null && IntersectionMonitor.EntryAreaMonitor != null &&
                    IntersectionMonitor.EntryAreaMonitor.Vehicle != null && IntersectionMonitor.EntryAreaMonitor.Failed)
                {
                    ArbiterOutput.Output("Entry area blocked");

                    // get an intersection plan without this interconnect
                    IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect(
                        waies.exitWaypoint,
                        CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
                        waies.desired,
                        true);

                    // check that the plan exists
                    if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) &&
                        testPlan.BestRouteTime < double.MaxValue - 1.0)
                    {
                        // get the desired interconnect
                        ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect;

                        #region Check that the reset interconnect is feasible

                        // test the reset interconnect
                        TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset);

                        // set to ignore all vehicles
                        testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });

                        // check if we have completed
                        CompletionReport turnResetCompletionReport;
                        bool             completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport);

                        // check to see if this is feasible
                        if (reset.TurnDirection == ArbiterTurnDirection.UTurn || (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95))
                        {
                            // notify
                            ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to all possible turns into final");

                            // set all the interconnects to the final as being blocked
                            if (((ITraversableWaypoint)waies.desired.FinalGeneric).IsEntry)
                            {
                                foreach (ArbiterInterconnect toBlock in ((ITraversableWaypoint)waies.desired.FinalGeneric).Entries)
                                {
                                    CoreCommon.Navigation.AddInterconnectBlockage(toBlock);
                                }
                            }

                            // check if exists previous partition to block
                            if (waies.desired.FinalGeneric is ArbiterWaypoint)
                            {
                                ArbiterWaypoint finWaypoint = (ArbiterWaypoint)waies.desired.FinalGeneric;
                                if (finWaypoint.PreviousPartition != null)
                                {
                                    CoreCommon.Navigation.AddBlockage(finWaypoint.PreviousPartition, finWaypoint.Position, false);
                                }
                            }

                            // reset all
                            waies.desired       = reset;
                            waies.turnTestState = TurnTestState.Completed;
                            waies.saudi         = SAUDILevel.None;
                            waies.useTurnBounds = true;
                            IntersectionMonitor.ResetDesired(reset);

                            // want to reset ourselves
                            return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
                        }

                        #endregion
                    }
                    else
                    {
                        ArbiterOutput.Output("Entry area blocked, but no otehr valid route found");
                    }
                }

                #endregion

                // check if can traverse
                if (IntersectionTactical.IntersectionMonitor == null || IntersectionTactical.IntersectionMonitor.CanTraverse(icaw, vehicleState))
                {
                    #region If can traverse the intersection

                    // quick check not interconnect
                    if (!(icaw is ArbiterInterconnect))
                    {
                        icaw = icaw.ToInterconnect;
                    }

                    // get interconnect
                    ArbiterInterconnect ai = (ArbiterInterconnect)icaw;

                    // clear all old completion reports
                    CoreCommon.Communications.ClearCompletionReports();

                    // check if uturn
                    if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint && ai.TurnDirection == ArbiterTurnDirection.UTurn)
                    {
                        // go into turn
                        List <ArbiterLane> involvedLanes = new List <ArbiterLane>();
                        involvedLanes.Add(((ArbiterWaypoint)ai.InitialGeneric).Lane);
                        involvedLanes.Add(((ArbiterWaypoint)ai.FinalGeneric).Lane);
                        uTurnState nextState = new uTurnState(((ArbiterWaypoint)ai.FinalGeneric).Lane,
                                                              IntersectionToolkit.uTurnBounds(vehicleState, involvedLanes));
                        nextState.Interconnect = ai;

                        // hold brake
                        Behavior b = new HoldBrakeBehavior();

                        // return maneuver
                        return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
                    }
                    else
                    {
                        if (ai.FinalGeneric is ArbiterWaypoint)
                        {
                            ArbiterWaypoint finalWaypoint = (ArbiterWaypoint)ai.FinalGeneric;

                            // get turn params
                            LinePath finalPath;
                            LineList leftLL;
                            LineList rightLL;
                            IntersectionToolkit.TurnInfo(finalWaypoint, out finalPath, out leftLL, out rightLL);

                            // go into turn
                            IState nextState = new TurnState(ai, ai.TurnDirection, finalWaypoint.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds);

                            // hold brake
                            Behavior b = new HoldBrakeBehavior();

                            // return maneuver
                            return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
                        }
                        else
                        {
                            // final perimeter waypoint
                            ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric;

                            // get turn params
                            LinePath finalPath;
                            LineList leftLL;
                            LineList rightLL;
                            IntersectionToolkit.ZoneTurnInfo(ai, apw, out finalPath, out leftLL, out rightLL);

                            // go into turn
                            IState nextState = new TurnState(ai, ai.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds);

                            // hold brake
                            Behavior b = new HoldBrakeBehavior();

                            // return maneuver
                            return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
                        }
                    }

                    #endregion
                }
                // otherwise need to wait
                else
                {
                    IState next = waies;
                    return(new Maneuver(new HoldBrakeBehavior(), next, next.DefaultStateDecorators, vehicleState.Timestamp));
                }
            }

            #endregion

            #region Stopping At Exit

            else if (planningState is StoppingAtExitState)
            {
                // cast to exit stopping
                StoppingAtExitState saes = (StoppingAtExitState)planningState;
                saes.currentPosition = vehicleState.Front;

                // get intersection plan
                IntersectionPlan ip = (IntersectionPlan)navigationalPlan;

                // if has an intersection
                if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(saes.waypoint.AreaSubtypeWaypointId))
                {
                    // create new intersection monitor
                    IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
                        saes.waypoint,
                        CoreCommon.RoadNetwork.IntersectionLookup[saes.waypoint.AreaSubtypeWaypointId],
                        vehicleState,
                        ip.BestOption);

                    // update it
                    IntersectionTactical.IntersectionMonitor.Update(vehicleState);
                }
                else
                {
                    IntersectionTactical.IntersectionMonitor = null;
                }

                // otherwise update the stop parameters
                saes.currentPosition = vehicleState.Front;
                Behavior b = saes.Resume(vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);
                return(new Maneuver(b, saes, saes.DefaultStateDecorators, vehicleState.Timestamp));
            }

            #endregion

            #region In uTurn

            else if (planningState is uTurnState)
            {
                // get state
                uTurnState uts = (uTurnState)planningState;

                // check if in other lane
                if (CoreCommon.Communications.HasCompleted((new UTurnBehavior(null, null, null, null)).GetType()))
                {
                    // quick check
                    if (uts.Interconnect != null && uts.Interconnect.FinalGeneric is ArbiterWaypoint)
                    {
                        // get the closest partition to the new lane
                        ArbiterLanePartition alpClose = uts.TargetLane.GetClosestPartition(vehicleState.Front);

                        // waypoints
                        ArbiterWaypoint partitionInitial = alpClose.Initial;
                        ArbiterWaypoint uturnEnd         = (ArbiterWaypoint)uts.Interconnect.FinalGeneric;

                        // check initial past end
                        if (partitionInitial.WaypointId.Number > uturnEnd.WaypointId.Number)
                        {
                            // get waypoints inclusive
                            List <ArbiterWaypoint> inclusive = uts.TargetLane.WaypointsInclusive(uturnEnd, partitionInitial);
                            bool found = false;

                            // loop through
                            foreach (ArbiterWaypoint aw in inclusive)
                            {
                                if (!found && aw.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
                                {
                                    // notiofy
                                    ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as passed over in uturn");

                                    // remove
                                    CoreCommon.Mission.MissionCheckpoints.Dequeue();

                                    // set found
                                    found = true;
                                }
                            }
                        }
                        // default check
                        else if (uts.Interconnect.FinalGeneric.Equals(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]))
                        {
                            // notiofy
                            ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as end of uturn");

                            // remove
                            CoreCommon.Mission.MissionCheckpoints.Dequeue();
                        }
                    }
                    // check if the uturn is for a blockage
                    else if (uts.Interconnect == null)
                    {
                        // get final lane
                        ArbiterLane targetLane = uts.TargetLane;

                        // check has opposing
                        if (targetLane.Way.Segment.Lanes.Count > 1)
                        {
                            // check the final checkpoint is in our lane
                            if (CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.AreaSubtypeId.Equals(targetLane.LaneId))
                            {
                                // check that the final checkpoint is within the uturn polygon
                                if (uts.Polygon != null &&
                                    uts.Polygon.IsInside(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId].Position))
                                {
                                    // remove the checkpoint
                                    ArbiterOutput.Output("Found checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + " inside blockage uturn area, dequeuing");
                                    CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                }
                            }
                        }
                    }

                    // stay in target lane
                    IState   nextState = new StayInLaneState(uts.TargetLane, new Probability(0.8, 0.2), true, CoreCommon.CorePlanningState);
                    Behavior b         = new StayInLaneBehavior(uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0), new List <int>(), uts.TargetLane.LanePath(), uts.TargetLane.Width, uts.TargetLane.NumberOfLanesLeft(vehicleState.Front, true), uts.TargetLane.NumberOfLanesRight(vehicleState.Front, true));
                    return(new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
                }
                // otherwise continue uturn
                else
                {
                    // get polygon
                    Polygon p = uts.Polygon;

                    // add polygon to observable
                    CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(p, ArbiterInformationDisplayObjectType.uTurnPolygon));

                    // check the type of uturn
                    if (!uts.singleLaneUturn)
                    {
                        // get ending path
                        LinePath endingPath = uts.TargetLane.LanePath();

                        // next state is current
                        IState nextState = uts;

                        // behavior
                        Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0));

                        // maneuver
                        return(new Maneuver(b, nextState, null, vehicleState.Timestamp));
                    }
                    else
                    {
                        // get ending path
                        LinePath endingPath = uts.TargetLane.LanePath().Clone();
                        endingPath = endingPath.ShiftLateral(-2.0);                        //uts.TargetLane.Width);

                        // add polygon to observable
                        CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(endingPath, ArbiterInformationDisplayObjectType.leftBound));

                        // next state is current
                        IState nextState = uts;

                        // behavior
                        Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0));

                        // maneuver
                        return(new Maneuver(b, nextState, null, vehicleState.Timestamp));
                    }
                }
            }

            #endregion

            #region In Turn

            else if (planningState is TurnState)
            {
                // get state
                TurnState ts = (TurnState)planningState;

                // add bounds to observable
                if (ts.LeftBound != null && ts.RightBound != null)
                {
                    CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.LeftBound, ArbiterInformationDisplayObjectType.leftBound));
                    CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.RightBound, ArbiterInformationDisplayObjectType.rightBound));
                }

                // create current turn reasoning
                if (this.TurnReasoning == null)
                {
                    this.TurnReasoning = new TurnReasoning(ts.Interconnect,
                                                           IntersectionTactical.IntersectionMonitor != null ? IntersectionTactical.IntersectionMonitor.EntryAreaMonitor : null);
                }

                // get primary maneuver
                Maneuver primary = this.TurnReasoning.PrimaryManeuver(vehicleState, blockages, ts);

                // get secondary maneuver
                Maneuver?secondary = this.TurnReasoning.SecondaryManeuver(vehicleState, (IntersectionPlan)navigationalPlan);

                // return the manevuer
                return(secondary.HasValue ? secondary.Value : primary);
            }

            #endregion

            #region Itnersection Startup

            else if (planningState is IntersectionStartupState)
            {
                // state and plan
                IntersectionStartupState iss = (IntersectionStartupState)planningState;
                IntersectionStartupPlan  isp = (IntersectionStartupPlan)navigationalPlan;

                // initial path
                LinePath vehiclePath = new LinePath(new Coordinates[] { vehicleState.Rear, vehicleState.Front });
                List <ITraversableWaypoint> feasibleEntries = new List <ITraversableWaypoint>();

                // vehicle polygon forward of us
                Polygon vehicleForward = vehicleState.ForwardPolygon;

                // best waypoint
                ITraversableWaypoint best = null;
                double bestCost           = Double.MaxValue;

                // given feasible choose best, no feasible choose random
                if (feasibleEntries.Count == 0)
                {
                    foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values)
                    {
                        if (vehicleForward.IsInside(itw.Position))
                        {
                            feasibleEntries.Add(itw);
                        }
                    }

                    if (feasibleEntries.Count == 0)
                    {
                        foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values)
                        {
                            feasibleEntries.Add(itw);
                        }
                    }
                }

                // get best
                foreach (ITraversableWaypoint itw in feasibleEntries)
                {
                    if (isp.NodeTimeCosts.ContainsKey(itw))
                    {
                        KeyValuePair <ITraversableWaypoint, double> lookup = new KeyValuePair <ITraversableWaypoint, double>(itw, isp.NodeTimeCosts[itw]);

                        if (best == null || lookup.Value < bestCost)
                        {
                            best     = lookup.Key;
                            bestCost = lookup.Value;
                        }
                    }
                }

                // get something going to this waypoint
                ArbiterInterconnect interconnect = null;
                if (best.IsEntry)
                {
                    ArbiterInterconnect closest = null;
                    double closestDistance      = double.MaxValue;

                    foreach (ArbiterInterconnect ai in best.Entries)
                    {
                        double dist = ai.InterconnectPath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
                        if (closest == null || dist < closestDistance)
                        {
                            closest         = ai;
                            closestDistance = dist;
                        }
                    }

                    interconnect = closest;
                }
                else if (best is ArbiterWaypoint && ((ArbiterWaypoint)best).PreviousPartition != null)
                {
                    interconnect = ((ArbiterWaypoint)best).PreviousPartition.ToInterconnect;
                }

                // get state
                if (best is ArbiterWaypoint)
                {
                    // go to this turn state
                    LinePath finalPath;
                    LineList leftBound;
                    LineList rightBound;
                    IntersectionToolkit.TurnInfo((ArbiterWaypoint)best, out finalPath, out leftBound, out rightBound);
                    return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, ((ArbiterWaypoint)best).Lane,
                                                                               finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp));
                }
                else
                {
                    // go to this turn state
                    LinePath finalPath;
                    LineList leftBound;
                    LineList rightBound;
                    IntersectionToolkit.ZoneTurnInfo(interconnect, (ArbiterPerimeterWaypoint)best, out finalPath, out leftBound, out rightBound);
                    return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, null,
                                                                               finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp));
                }
            }

            #endregion

            #region Unknown

            else
            {
                throw new Exception("Unknown planning state in intersection tactical plan: " + planningState.ToString());
            }

            #endregion
        }