/// <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
        }
        /// <summary>
        /// Maneuver for recovering from a lane blockage, used for lane changes as well
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="vehicleSpeed"></param>
        /// <param name="defcon"></param>
        /// <param name="saudi"></param>
        /// <returns></returns>
        public Maneuver LaneRecoveryManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, 
            INavigationalPlan plan, BlockageRecoveryState brs, bool failedVehiclesPersistentlyOnly, out bool waitForUTurn)
        {
            // get the blockage
            ITacticalBlockage laneBlockageReport = brs.EncounteredState.TacticalBlockage;

            // get the turn state
            StayInLaneState sils = new StayInLaneState(lane, CoreCommon.CorePlanningState);

            // set wait false
            waitForUTurn = false;

            #region Reverse

            // check the state of the recovery for being the initial state
            if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL)
            {
                // determine if the reverse behavior is recommended
                if (laneBlockageReport.BlockageReport.ReverseRecommended)
                {
                    // notify
                    ArbiterOutput.Output("Initial encounter, reverse recommended, reversing");

                    // get reverse behavior
                    StayInLaneBehavior reverseRecovery = this.LaneReverseRecovery(lane, vehicleState, vehicleSpeed, brs.EncounteredState.TacticalBlockage.BlockageReport);
                    reverseRecovery.TimeStamp = vehicleState.Timestamp;

                    // get recovery state
                    BlockageRecoveryState brsT = new BlockageRecoveryState(
                        reverseRecovery, null, new StayInLaneState(lane, CoreCommon.CorePlanningState),
                        brs.Defcon = BlockageRecoveryDEFCON.REVERSE, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
                    brsT.CompletionState = brsT;

                    // reset blockages
                    BlockageHandler.SetDefaultBlockageCooldown();

                    // maneuver
                    return new Maneuver(reverseRecovery, brsT, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
                }
            }

            #endregion

            #region uTurn

            // check if uturn is available
            ArbiterLane opp = this.tacticalUmbrella.roadTactical.forwardReasoning.GetClosestOpposing(lane, vehicleState);

            // resoning
            OpposingLateralReasoning olrTmp = new OpposingLateralReasoning(opp, SideObstacleSide.Driver);

            if (opp != null && olrTmp.ExistsExactlyHere(vehicleState) && opp.IsInside(vehicleState.Front) && opp.LanePath().GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front) < TahoeParams.VL * 3.0)
            {
                // check possible to reach goal given block partition way
                RoadPlan uTurnNavTest = CoreCommon.Navigation.PlanRoadOppositeWithoutPartition(
                    lane.GetClosestPartition(vehicleState.Front),
                    opp.GetClosestPartition(vehicleState.Front),
                    CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
                    true,
                    vehicleState.Front,
                    true);

                // flag to try the uturn
                bool uturnTry = false;

                // all polygons to include
                List<Polygon> allPolys = BlockageTactical.AllForwardPolygons(vehicleState, failedVehiclesPersistentlyOnly);// .AllObstacleAndStoppedVehiclePolygons(vehicleState);

                // test blockage takes up segment
                double numLanesLeft = lane.NumberOfLanesLeft(vehicleState.Front, true);
                double numLanesRight = lane.NumberOfLanesRight(vehicleState.Front, true);
                LinePath leftBound = lane.LanePath().ShiftLateral((lane.Width * numLanesLeft) + (lane.Width / 2.0));
                LinePath rightBound = lane.LanePath().ShiftLateral((lane.Width * numLanesRight) + (lane.Width / 2.0));
                bool segmentBlockage = BlockageTester.TestBlockage(allPolys, leftBound, rightBound);
                uturnTry = segmentBlockage;

                // check if we should try the uturn
                if(uturnTry)
                {
                    // check uturn timer
                    if(uTurnTimer.ElapsedMilliseconds / 1000.0 > 2.0)
                    {
                        #region Determine Checkpoint

                        // check route feasible
                        if (uTurnNavTest.BestPlan.laneWaypointOfInterest.RouteTime < double.MaxValue - 1
                            && uTurnNavTest.BestPlan.laneWaypointOfInterest.TimeCostToPoint < double.MaxValue - 1)
                        {
                            ArbiterOutput.Output("valid route to checkpoint by rerouting, uturning");
                        }
                        // otherwise remove the next checkpoint
                        else
                        {
                            // notiify
                            ArbiterOutput.Output("NO valid route to checkpoint by rerouting");

                            // get the next cp
                            ArbiterCheckpoint cp1 = CoreCommon.Mission.MissionCheckpoints.Peek();

                            // get distance to blockage
                            double blockageDistance = 15.0;

                            // check cp is in our lane
                            if (cp1.WaypointId.AreaSubtypeId.Equals(lane.LaneId))
                            {
                                // check distance to cp1
                                double distanceToCp1 = lane.DistanceBetween(vehicleState.Front, CoreCommon.RoadNetwork.ArbiterWaypoints[cp1.WaypointId].Position);

                                // check that this is an already inserted waypoint
                                if (cp1.Type == CheckpointType.Inserted)
                                {
                                    // remove cp
                                    ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                    ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as inserted type of checkpoint");

                                    ArbiterCheckpoint ac2 = CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                    ArbiterOutput.Output("removed checkpoint: " + ac2.WaypointId.ToString() + " as blocked type of checkpoint");
                                }
                                // closer to us than the blockage
                                else if (distanceToCp1 < blockageDistance)
                                {
                                    // remove cp
                                    ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                    ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as between us and the blockage ~ 15m away");
                                }
                                // very close to blockage on the other side
                                else if (distanceToCp1 < blockageDistance + 5.0)
                                {
                                    // remove cp
                                    ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                    ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as on the other side of blockage ~ 15m away");
                                }
                                // further away from the blockage
                                else
                                {
                                    // check over all checkpoints if there exists a checkpoint cp2 in the opposing lane within 5m along our lane
                                    ArbiterCheckpoint cp2 = null;
                                    foreach (ArbiterWaypoint oppAw in opp.WaypointList)
                                    {
                                        // check checkpoint
                                        if (oppAw.IsCheckpoint)
                                        {
                                            // distance between us and that waypoint
                                            double distanceToCp2 = lane.DistanceBetween(vehicleState.Front, oppAw.Position);

                                            // check along distance < 5.0m
                                            if (Math.Abs(distanceToCp1 - distanceToCp2) < 5.0)
                                            {
                                                // set cp
                                                cp2 = new ArbiterCheckpoint(oppAw.CheckpointId, oppAw.WaypointId, CheckpointType.Inserted);
                                            }
                                        }
                                    }

                                    // check close cp exists
                                    if (cp2 != null)
                                    {
                                        // remove cp1 and replace with cp2
                                        ArbiterOutput.Output("inserting checkpoint: " + cp2.WaypointId.ToString() + " before checkpoint: " + cp1.WaypointId.ToString() + " as can be replaced with adjacent opposing cp2");
                                        //CoreCommon.Mission.MissionCheckpoints.Dequeue();

                                        // get current checkpoints
                                        ArbiterCheckpoint[] updatedAcs = new ArbiterCheckpoint[CoreCommon.Mission.MissionCheckpoints.Count + 1];
                                        updatedAcs[0] = cp2;
                                        CoreCommon.Mission.MissionCheckpoints.CopyTo(updatedAcs, 1);
                                        updatedAcs[1].Type = CheckpointType.Blocked;
                                        CoreCommon.Mission.MissionCheckpoints = new Queue<ArbiterCheckpoint>(new List<ArbiterCheckpoint>(updatedAcs));
                                    }
                                    // otherwise remove cp1
                                    else
                                    {
                                        // remove cp
                                        ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                        ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane");
                                    }
                                }
                            }
                            // otherwise we remove the checkpoint
                            else
                            {
                                // remove cp
                                ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane");
                            }
                        }

                        #endregion

                        #region Plan Uturn

                        // notify
                        ArbiterOutput.Output("Segment blocked, uturn available");

                        // nav penalties
                        ArbiterLanePartition alpClose = lane.GetClosestPartition(vehicleState.Front);
                        CoreCommon.Navigation.AddHarshBlockageAcrossSegment(alpClose, vehicleState.Front);

                        // uturn
                        LinePath lpTmp = new LinePath(new Coordinates[] { vehicleState.Front, opp.LanePath().GetClosestPoint(vehicleState.Front).Location });
                        Coordinates vector = lpTmp[1] - lpTmp[0];
                        lpTmp[1] = lpTmp[1] + vector.Normalize(opp.Width / 2.0);
                        lpTmp[0] = lpTmp[0] - vector.Normalize(lane.Width / 2.0);
                        LinePath lb = lpTmp.ShiftLateral(15.0);
                        LinePath rb = lpTmp.ShiftLateral(-15.0);
                        List<Coordinates> uturnPolyCOords = new List<Coordinates>();
                        uturnPolyCOords.AddRange(lb);
                        uturnPolyCOords.AddRange(rb);
                        uTurnState uts = new uTurnState(opp, Polygon.GrahamScan(uturnPolyCOords));

                        // reset blockages
                        BlockageHandler.SetDefaultBlockageCooldown();

                        // reset the timers
                        this.Reset();

                        // go to uturn
                        return new Maneuver(uts.Resume(vehicleState, 2.0), uts, TurnDecorators.NoDecorators, vehicleState.Timestamp);

                        #endregion
                    }
                    // uturn timer not enough
                    else
                    {
                        // check timer running
                        if(!uTurnTimer.IsRunning)
                        {
                            uTurnTimer.Stop();
                            uTurnTimer.Reset();
                            uTurnTimer.Start();
                        }

                        // if gets here, need to wait
                        double time = uTurnTimer.ElapsedMilliseconds / 1000.0;
                        ArbiterOutput.Output("uTurn behavior evaluated to success, cooling down for: " + time.ToString("f2") + " out of 2");
                        waitForUTurn = true;
                        return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }
                else
                {
                    waitForUTurn = false;
                    this.Reset();
                }
            }
            else
            {
                waitForUTurn = false;
                this.Reset();
            }

            #endregion

            #region Recovery Escalation

            // plan forward reasoning
            this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardManeuver(lane, vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>());

            // check clear on right, or if it does nto exist here
            ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning;
            bool rightClear = !rightLateral.Exists || !rightLateral.ExistsExactlyHere(vehicleState) ||
                rightLateral.AdjacentAndRearClear(vehicleState);

            // check clear on left, or if it does nto exist here
            ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning;
            bool leftClear = !leftLateral.Exists || !leftLateral.ExistsExactlyHere(vehicleState) ||
                leftLateral.AdjacentAndRearClear(vehicleState);
            if(leftClear && leftLateral is OpposingLateralReasoning)
                leftClear = leftLateral.ForwardClear(vehicleState, TahoeParams.VL * 3.0, 2.24,
                    new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
                    lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), TahoeParams.VL * 3.0).Location);

            // check both clear to widen
            bool widenOk = rightClear && leftClear;

            // Notify
            ArbiterOutput.Output("Blockage tactical recovery escalation: rightClear: " + rightClear.ToString() + ", leftCler: " + leftClear.ToString());

            // if we can't widen for some reason just go through with no widen
            StayInLaneBehavior silb = this.LaneRecoveryBehavior(lane, vehicleState, vehicleSpeed, plan, brs, brs.EncounteredState);
            silb.TimeStamp = vehicleState.Timestamp;

            // check widen
            if (widenOk)
            {
                // output
                ArbiterOutput.Output("Lane Blockage Recovery: Adjacent areas clear");

                // mini icrease width
                silb.LaneWidth = silb.LaneWidth + TahoeParams.T;

                // check execute none saudi
                CompletionReport l0Cr;
                bool l0TestOk = CoreCommon.Communications.TestExecute(silb, out l0Cr);

                // check mini ok
                if (l0TestOk)
                {
                    // notify
                    ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen ok");

                    // update the current state
                    BlockageRecoveryState brsL0 = new BlockageRecoveryState(
                        silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
                    return new Maneuver(silb, brsL0, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                }
                else
                {
                    // notify
                    ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen failed, moving to large widen");

                    #region Change Lanes

                    // check not in change lanes
                    if (brs.Defcon != BlockageRecoveryDEFCON.CHANGELANES_FORWARD && brs.Defcon != BlockageRecoveryDEFCON.WIDENBOUNDS)
                    {
                        // check normal change lanes reasoning
                        bool shouldWait;
                        IState laneChangeCompletionState;
                        ChangeLaneBehavior changeLanesBehavior = this.ChangeLanesRecoveryBehavior(lane, vehicleState, out shouldWait, out laneChangeCompletionState);

                        // check change lanes behaviore exists
                        if (changeLanesBehavior != null)
                        {
                            ArbiterOutput.Output("Lane Blockage Recovery: Found adjacent lane available change lanes beahvior: " + changeLanesBehavior.ToShortString() + ", " + changeLanesBehavior.ShortBehaviorInformation());

                            // update the current state
                            BlockageRecoveryState brsCL = new BlockageRecoveryState(
                                changeLanesBehavior, laneChangeCompletionState, sils, BlockageRecoveryDEFCON.CHANGELANES_FORWARD, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
                            return new Maneuver(changeLanesBehavior, brsCL, changeLanesBehavior.ChangeLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, vehicleState.Timestamp);
                        }
                        // check should wait
                        else if (shouldWait)
                        {
                            ArbiterOutput.Output("Lane Blockage Recovery: Should wait for the forward lane, waiting");
                            return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                    }

                    #endregion

                    // notify
                    ArbiterOutput.Output("Lane Blockage Recovery: Fell Through Forward Change Lanes");

                    // increase width
                    silb.LaneWidth = silb.LaneWidth * 3.0;

                    // check execute l1
                    CompletionReport l1Cr;
                    bool l1TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr);

                    // check ok
                    if (l1TestOk)
                    {
                        // notify
                        ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen ok");

                        // update the current state
                        BlockageRecoveryState brsL1 = new BlockageRecoveryState(
                            silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
                        return new Maneuver(silb, brsL1, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    // go to l2 for all the marbles
                    else
                    {
                        // notify
                        ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen failed, moving to 3LW, L1 Saudi");

                        ShutUpAndDoItDecorator saudi2 = new ShutUpAndDoItDecorator(SAUDILevel.L1);
                        List<BehaviorDecorator> saudi2bds = new List<BehaviorDecorator>(new BehaviorDecorator[] { saudi2 });
                        silb.Decorators = saudi2bds;
                        BlockageRecoveryState brsL2 = new BlockageRecoveryState(
                            silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
                        return new Maneuver(silb, brsL2, saudi2bds, vehicleState.Timestamp);
                    }
                }
            }

            #endregion

            // fallout goes back to lane state
            return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
        }
        /// <summary>
        /// Plans the next maneuver
        /// </summary>
        /// <param name="roads"></param>
        /// <param name="mission"></param>
        /// <param name="vehicleState"></param>
        /// <param name="CoreCommon.CorePlanningState"></param>
        /// <param name="observedVehicles"></param>
        /// <param name="observedObstacles"></param>
        /// <param name="coreState"></param>
        /// <param name="carMode"></param>
        /// <returns></returns>
        public Maneuver Plan(VehicleState vehicleState, double vehicleSpeed,
            SceneEstimatorTrackedClusterCollection observedVehicles, SceneEstimatorUntrackedClusterCollection observedObstacles,
            CarMode carMode, INavigableNode goal)
        {
            // set blockages
            List<ITacticalBlockage> blockages = this.blockageHandler.DetermineBlockages(CoreCommon.CorePlanningState);

            #region Travel State

            if (CoreCommon.CorePlanningState is TravelState)
            {
                #region Stay in Lane State

                if (CoreCommon.CorePlanningState is StayInLaneState)
                {
                    // get lane state
                    StayInLaneState sils = (StayInLaneState)CoreCommon.CorePlanningState;

                    #region Blockages

                    // check blockages
                    if (blockages != null && blockages.Count > 0 && blockages[0] is LaneBlockage)
                    {
                        // create the blockage state
                        EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);

                        // check not from a dynamicly moving vehicle
                        if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
                        {
                            // go to a blockage handling tactical
                            return new Maneuver(new HoldBrakeBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else
                            ArbiterOutput.Output("Lane blockage reported for moving vehicle, ignoring");
                    }

                    #endregion

                    // update the total time ignorable have been seen
                    sils.UpdateIgnoreList();

                    // nav plan to find poi
                    RoadPlan rp = navigation.PlanNavigableArea(sils.Lane, vehicleState.Position, goal, sils.WaypointsToIgnore);

                    // check for unreachable route
                    if (rp.BestPlan.laneWaypointOfInterest.BestRoute != null &&
                        rp.BestPlan.laneWaypointOfInterest.BestRoute.Count == 0 &&
                        rp.BestPlan.laneWaypointOfInterest.RouteTime >= Double.MaxValue - 1.0)
                    {
                        ArbiterOutput.Output("Removed Unreachable Checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString());
                        CoreCommon.Mission.MissionCheckpoints.Dequeue();
                        return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    else if (rp.BestPlan.laneWaypointOfInterest.TimeCostToPoint >= Double.MaxValue - 1.0)
                    {
                        ArbiterOutput.Output("Best Lane Waypoint of Interest is END OF LANE WITH NO INTERCONNECTS, LEADING NOWHERE");
                        ArbiterOutput.Output("Removed Unreachable Checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString());
                        CoreCommon.Mission.MissionCheckpoints.Dequeue();
                        return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }

                    #region Check Supra Lane Availability

                    // if the poi is at the end of this lane, is not stop, leads to another lane, and has no overlapping lanes
                    // or if the poi's best exit is an exit in this lane, is not a stop, has no overlapping lanes and leads to another lane
                    // create supralane

                    // check if navigation is corrent in saying we want to continue on the current lane and we're far enough along the lane, 30m for now
                    if(rp.BestPlan.Lane.Equals(sils.Lane.LaneId))
                    {
                        // get navigation poi
                        DownstreamPointOfInterest dpoi = rp.BestPlan.laneWaypointOfInterest;

                        // check that the poi is not stop and is not the current checkpoint
                        if(!dpoi.PointOfInterest.IsStop && !(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(dpoi.PointOfInterest.WaypointId)))
                        {
                            // get the best exit or the poi
                            ArbiterInterconnect ai = dpoi.BestExit;

                            // check if exit goes into a lane and not a uturn
                            if(ai != null && ai.FinalGeneric is ArbiterWaypoint && ai.TurnDirection != ArbiterTurnDirection.UTurn)
                            {
                                // final lane or navigation poi interconnect
                                ArbiterLane al = ((ArbiterWaypoint)ai.FinalGeneric).Lane;

                                // check not same lane
                                if (!al.Equals(sils.Lane))
                                {
                                    // check if enough room to start
                                    bool enoughRoom = !sils.Lane.Equals(al) || sils.Lane.LanePath(sils.Lane.WaypointList[0].Position, vehicleState.Front).PathLength > 30;
                                    if (enoughRoom)
                                    {
                                        // try to get intersection associated with the exit
                                        ArbiterIntersection aInter = CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(dpoi.PointOfInterest.WaypointId) ?
                                            CoreCommon.RoadNetwork.IntersectionLookup[dpoi.PointOfInterest.WaypointId] : null;

                                        // check no intersection or no overlapping lanes
                                        if (aInter == null || !aInter.PriorityLanes.ContainsKey(ai) || aInter.PriorityLanes[ai].Count == 0)
                                        {
                                            // create the supra lane
                                            SupraLane sl = new SupraLane(sils.Lane, ai, al);

                                            // switch to the supra lane state
                                            StayInSupraLaneState sisls = new StayInSupraLaneState(sl, CoreCommon.CorePlanningState);
                                            sisls.UpdateState(vehicleState.Front);

                                            // set
                                            return new Maneuver(new NullBehavior(), sisls, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                                        }
                                    }
                                }
                            }
                        }
                    }

                    #endregion

                    // plan final tactical maneuver
                    Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return final maneuver
                    return final;
                }

                #endregion

                #region Stay in Supra Lane State

                else if (CoreCommon.CorePlanningState is StayInSupraLaneState)
                {
                    // state
                    StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;

                    #region Blockages

                    // check blockages
                    if (blockages != null && blockages.Count > 0 && blockages[0] is LaneBlockage)
                    {
                        // create the blockage state
                        EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);

                        // check not from a dynamicly moving vehicle
                        if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
                        {
                            // go to a blockage handling tactical
                            return new Maneuver(new HoldBrakeBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else
                            ArbiterOutput.Output("Lane blockage reported for moving vehicle, ignoring");
                    }

                    #endregion

                    // check if we are in the final lane
                    if (sisls.Lane.ClosestComponent(vehicleState.Position) == SLComponentType.Final)
                    {
                        // go to stay in lane
                        return new Maneuver(new NullBehavior(), new StayInLaneState(sisls.Lane.Final, sisls), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }

                    // update ignorable
                    sisls.UpdateIgnoreList();

                    // nav plan to find points
                    RoadPlan rp = navigation.PlanNavigableArea(sisls.Lane, vehicleState.Position, goal, sisls.WaypointsToIgnore);

                    // check for unreachable route
                    if (rp.BestPlan.laneWaypointOfInterest.BestRoute != null &&
                        rp.BestPlan.laneWaypointOfInterest.BestRoute.Count == 0 &&
                        rp.BestPlan.laneWaypointOfInterest.RouteTime >= Double.MaxValue - 1.0)
                    {
                        ArbiterOutput.Output("Removed Unreachable Checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString());
                        CoreCommon.Mission.MissionCheckpoints.Dequeue();
                        return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }

                    // plan
                    Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // update current state
                    sisls.UpdateState(vehicleState.Front);

                    // return final maneuver
                    return final;
                }

                #endregion

                #region Stopping At Stop State

                else if (CoreCommon.CorePlanningState is StoppingAtStopState)
                {
                    // get state
                    StoppingAtStopState sass = (StoppingAtStopState)CoreCommon.CorePlanningState;

                    // check to see if we're stopped
                    // check if in other lane
                    if (CoreCommon.Communications.HasCompleted((new StayInLaneBehavior(null, null, null)).GetType()))
                    {
                        // update intersection monitor
                        if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(sass.waypoint.AreaSubtypeWaypointId))
                        {
                            // nav plan
                            IntersectionPlan ip = navigation.PlanIntersection(sass.waypoint, goal);

                            // update intersection monitor
                            this.tactical.Update(observedVehicles, vehicleState);
                            IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
                                sass.waypoint,
                                CoreCommon.RoadNetwork.IntersectionLookup[sass.waypoint.AreaSubtypeWaypointId],
                                vehicleState, ip.BestOption);
                        }
                        else
                        {
                            IntersectionTactical.IntersectionMonitor = null;
                        }

                        // check if we've hit goal if stop is cp
                        if (sass.waypoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
                        {
                            ArbiterOutput.Output("Stopped at current goal: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + ", Removing");
                            CoreCommon.Mission.MissionCheckpoints.Dequeue();

                            if (CoreCommon.Mission.MissionCheckpoints.Count == 0)
                            {
                                return new Maneuver(new HoldBrakeBehavior(), new NoGoalsLeftState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                            }
                        }

                        // move to the intersection
                        IState next = new WaitingAtIntersectionExitState(sass.waypoint, sass.turnDirection, new IntersectionDescription(), sass.desiredExit);
                        Behavior b = new HoldBrakeBehavior();
                        return new Maneuver(b, next, sass.DefaultStateDecorators, vehicleState.Timestamp);
                    }
                    else
                    {
                        // otherwise update the stop parameters
                        Behavior b = sass.Resume(vehicleState, vehicleSpeed);
                        return new Maneuver(b, CoreCommon.CorePlanningState, sass.DefaultStateDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region Change Lanes State

                else if (CoreCommon.CorePlanningState is ChangeLanesState)
                {
                    // get state
                    ChangeLanesState cls = (ChangeLanesState)CoreCommon.CorePlanningState;

                    #region Blockages

                    // check blockages
                    if (blockages != null && blockages.Count > 0 && blockages[0] is LaneChangeBlockage)
                    {
                        // create the blockage state
                        EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);

                        // check not from a dynamicly moving vehicle
                        if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
                        {
                            // go to a blockage handling tactical
                            return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else
                            ArbiterOutput.Output("Lane Change blockage reported for moving vehicle, ignoring");
                    }

                    #endregion

                    // get a good lane
                    ArbiterLane goodLane = null;
                    if(!cls.Parameters.InitialOncoming)
                        goodLane = cls.Parameters.Initial;
                    else if(!cls.Parameters.TargetOncoming)
                        goodLane = cls.Parameters.Target;
                    else
                        throw new Exception("not going from or to good lane");

                    // nav plan to find poi
                    #warning make goal better if there is none come to stop
                    RoadPlan rp = navigation.PlanNavigableArea(goodLane, vehicleState.Front,
                        CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], new List<ArbiterWaypoint>());

                    // check current behavior type
                    bool done = CoreCommon.Communications.HasCompleted((new ChangeLaneBehavior(null, null, false, 0, null, null)).GetType());

                    if (done)
                    {
                        if (cls.Parameters.TargetOncoming)
                            return new Maneuver(
                                new StayInLaneBehavior(cls.Parameters.Target.LaneId,
                                    new ScalarSpeedCommand(cls.Parameters.Parameters.RecommendedSpeed),
                                    cls.Parameters.Parameters.VehiclesToIgnore,
                                    cls.Parameters.Target.ReversePath,
                                    cls.Parameters.Target.Width,
                                    cls.Parameters.Target.NumberOfLanesRight(vehicleState.Front, !cls.Parameters.InitialOncoming),
                                    cls.Parameters.Target.NumberOfLanesLeft(vehicleState.Front, !cls.Parameters.InitialOncoming)),
                                new OpposingLanesState(cls.Parameters.Target, true, cls, vehicleState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        else
                            return new Maneuver(
                                new StayInLaneBehavior(cls.Parameters.Target.LaneId,
                                    new ScalarSpeedCommand(cls.Parameters.Parameters.RecommendedSpeed),
                                    cls.Parameters.Parameters.VehiclesToIgnore,
                                    cls.Parameters.Target.LanePath(),
                                    cls.Parameters.Target.Width,
                                    cls.Parameters.Target.NumberOfLanesLeft(vehicleState.Front, !cls.Parameters.InitialOncoming),
                                    cls.Parameters.Target.NumberOfLanesRight(vehicleState.Front, !cls.Parameters.InitialOncoming)),
                                new StayInLaneState(cls.Parameters.Target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    else
                    {
                        return tactical.Plan(cls, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
                    }
                }

                #endregion

                #region Opposing Lanes State

                else if (CoreCommon.CorePlanningState is OpposingLanesState)
                {
                    // get state
                    OpposingLanesState ols = (OpposingLanesState)CoreCommon.CorePlanningState;
                    ols.SetClosestGood(vehicleState);

                    #region Blockages

                    // check blockages
                    if (blockages != null && blockages.Count > 0 && blockages[0] is OpposingLaneBlockage)
                    {
                        // create the blockage state
                        EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);

                        // check not from a dynamicly moving vehicle
                        if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
                        {
                            // go to a blockage handling tactical
                            return new Maneuver(new HoldBrakeBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else
                            ArbiterOutput.Output("Opposing Lane blockage reported for moving vehicle, ignoring");
                    }

                    #endregion

                    // check closest good null
                    if (ols.ClosestGoodLane != null)
                    {
                        // nav plan to find poi
                        RoadPlan rp = navigation.PlanNavigableArea(ols.ClosestGoodLane, vehicleState.Position, goal, new List<ArbiterWaypoint>());

                        // plan final tactical maneuver
                        Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                    // otherwise need to make a uturn
                    else
                    {
                        ArbiterOutput.Output("in opposing lane with no closest good, making a uturn");
                        ArbiterLanePartition alp = ols.OpposingLane.GetClosestPartition(vehicleState.Front);
                        Coordinates c1 = vehicleState.Front + alp.Vector().Normalize(8.0);
                        Coordinates c2 = vehicleState.Front - alp.Vector().Normalize(8.0);
                        LinePath lpTmp = new LinePath(new Coordinates[] { c1, c2 });
                        List<Coordinates> pCoords = new List<Coordinates>();
                        pCoords.AddRange(lpTmp.ShiftLateral(ols.OpposingLane.Width)); //* 1.5));
                        pCoords.AddRange(lpTmp.ShiftLateral(-ols.OpposingLane.Width));// / 2.0));
                        Polygon uturnPoly = Polygon.GrahamScan(pCoords);
                        uTurnState uts = new uTurnState(ols.OpposingLane, uturnPoly, true);
                        uts.Interconnect = alp.ToInterconnect;

                        // plan final tactical maneuver
                        Maneuver final = new Maneuver(new NullBehavior(), uts, TurnDecorators.LeftTurnDecorator, vehicleState.Timestamp);

                        // return final maneuver
                        return final;
                    }
                }

                #endregion

                #region Starting up off of chute state

                else if (CoreCommon.CorePlanningState is StartupOffChuteState)
                {
                    // cast the type
                    StartupOffChuteState socs = (StartupOffChuteState)CoreCommon.CorePlanningState;

                    // check if in lane part of chute
                    if (CoreCommon.Communications.HasCompleted((new TurnBehavior(null, null, null, null, null, null)).GetType()))
                    {
                        // go to lane state
                        return new Maneuver(new NullBehavior(), new StayInLaneState(socs.Final.Lane, new Probability(0.8, 0.2), true, socs), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    // otherwise continue
                    else
                    {
                        // simple maneuver generation
                        TurnBehavior tb = (TurnBehavior)socs.Resume(vehicleState, 1.4);

                        // add bounds to observable
                        CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(tb.LeftBound, ArbiterInformationDisplayObjectType.leftBound));
                        CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(tb.RightBound, ArbiterInformationDisplayObjectType.rightBound));

                        // final maneuver
                        return new Maneuver(tb, socs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region Unknown

                else
                {
                    // non-handled state
                    throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
                }

                #endregion
            }

            #endregion

            #region Intersection State

            else if (CoreCommon.CorePlanningState is IntersectionState)
            {
                #region Waiting at Intersection Exit State

                if (CoreCommon.CorePlanningState is WaitingAtIntersectionExitState)
                {
                    // get state
                    WaitingAtIntersectionExitState waies = (WaitingAtIntersectionExitState)CoreCommon.CorePlanningState;

                    // nav plan
                    IntersectionPlan ip = navigation.PlanIntersection(waies.exitWaypoint, goal);

                    // plan
                    Maneuver final = tactical.Plan(waies, ip, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return final maneuver
                    return final;
                }

                #endregion

                #region Stopping At Exit State

                else if (CoreCommon.CorePlanningState is StoppingAtExitState)
                {
                    // get state
                    StoppingAtExitState saes = (StoppingAtExitState)CoreCommon.CorePlanningState;

                    // check to see if we're stopped
                    if (CoreCommon.Communications.HasCompleted((new StayInLaneBehavior(null, null, null)).GetType()))
                    {
                        // check if we've hit goal if stop is cp
                        if (saes.waypoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
                        {
                            ArbiterOutput.Output("Stopped at current goal: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + ", Removing");
                            CoreCommon.Mission.MissionCheckpoints.Dequeue();

                            if (CoreCommon.Mission.MissionCheckpoints.Count == 0)
                            {
                                return new Maneuver(new HoldBrakeBehavior(), new NoGoalsLeftState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                            }
                        }

                        // move to the intersection
                        IState next = new WaitingAtIntersectionExitState(saes.waypoint, saes.turnDirection, new IntersectionDescription(), saes.desiredExit);
                        Behavior b = new HoldBrakeBehavior();
                        return new Maneuver(b, next, saes.DefaultStateDecorators, vehicleState.Timestamp);
                    }
                    else
                    {
                        // nav plan
                        IntersectionPlan ip = navigation.PlanIntersection(saes.waypoint, goal);

                        // update the intersection monitor
                        if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(saes.waypoint.AreaSubtypeWaypointId))
                        {
                            IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
                                saes.waypoint,
                                CoreCommon.RoadNetwork.IntersectionLookup[saes.waypoint.AreaSubtypeWaypointId],
                                vehicleState, ip.BestOption);
                        }
                        else
                            IntersectionTactical.IntersectionMonitor = null;

                        // plan final tactical maneuver
                        Maneuver final = tactical.Plan(saes, ip, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                }

                #endregion

                #region Turn State

                else if (CoreCommon.CorePlanningState is TurnState)
                {
                    // get state
                    TurnState ts = (TurnState)CoreCommon.CorePlanningState;

                    // check if in other lane
                    if (CoreCommon.Communications.HasCompleted((new TurnBehavior(null, null, null, null, null, null)).GetType()))
                    {
                        if (ts.Interconnect.FinalGeneric is ArbiterWaypoint)
                        {
                            // get final wp, and if next cp, remove
                            ArbiterWaypoint final = (ArbiterWaypoint)ts.Interconnect.FinalGeneric;
                            if (CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(final.AreaSubtypeWaypointId))
                                CoreCommon.Mission.MissionCheckpoints.Dequeue();

                            // stay in target lane
                            IState nextState = new StayInLaneState(ts.TargetLane, new Probability(0.8, 0.2), true, CoreCommon.CorePlanningState);
                            Behavior b = new NullBehavior();
                            return new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else if (ts.Interconnect.FinalGeneric is ArbiterPerimeterWaypoint)
                        {
                            // stay in target lane
                            IState nextState = new ZoneTravelingState(((ArbiterPerimeterWaypoint)ts.Interconnect.FinalGeneric).Perimeter.Zone, (INavigableNode)ts.Interconnect.FinalGeneric);
                            Behavior b = new NullBehavior();
                            return new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else
                            throw new Exception("unhandled unterconnect final wp type");
                    }

                    // get interconnect
                    if (ts.Interconnect.FinalGeneric is ArbiterWaypoint)
                    {
                        // nav plan
                        IntersectionPlan ip = navigation.PlanIntersection((ITraversableWaypoint)ts.Interconnect.InitialGeneric, goal);

                        // plan
                        Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, ip, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                    // else to zone
                    else if (ts.Interconnect.FinalGeneric is ArbiterPerimeterWaypoint)
                    {
                        // plan
                        Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                    else
                    {
                        throw new Exception("method not imp");
                    }
                }

                #endregion

                #region uTurn State

                else if (CoreCommon.CorePlanningState is uTurnState)
                {
                    // get state
                    uTurnState uts = (uTurnState)CoreCommon.CorePlanningState;

                    // plan over the target segment, ignoring the initial waypoint of the target lane
                    ArbiterWaypoint initial = uts.TargetLane.GetClosestPartition(vehicleState.Position).Initial;
                    List<ArbiterWaypoint> iws = RoadToolkit.WaypointsClose(initial.Lane.Way, vehicleState.Front, initial);
                    RoadPlan rp = navigation.PlanRoads(uts.TargetLane, vehicleState.Front, goal, iws);

                    // plan
                    Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return final maneuver
                    return final;
                }

                #endregion

                #region Intersection Startup State

                else if (CoreCommon.CorePlanningState is IntersectionStartupState)
                {
                    // get startup state
                    IntersectionStartupState iss = (IntersectionStartupState)CoreCommon.CorePlanningState;

                    // get intersection
                    ArbiterIntersection ai = iss.Intersection;

                    // get plan
                    IEnumerable<ITraversableWaypoint> entries = ai.AllEntries.Values;
                    IntersectionStartupPlan isp = navigation.PlanIntersectionStartup(entries, goal);

                    // plan tac
                    Maneuver final = tactical.Plan(iss, isp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return
                    return final;
                }

                #endregion

                #region Unknown

                else
                {
                    // non-handled state
                    throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
                }

                #endregion
            }

            #endregion

            #region Zone State

            else if (CoreCommon.CorePlanningState is ZoneState)
            {
                #region Zone Travelling State

                if (CoreCommon.CorePlanningState is ZoneTravelingState)
                {
                    // set state
                    ZoneTravelingState zts = (ZoneTravelingState)CoreCommon.CorePlanningState;

                    // check to see if we're stopped
                    if (CoreCommon.Communications.HasCompleted((new ZoneTravelingBehavior(null, null, new Polygon[0], null, null, null, null)).GetType()))
                    {
                        // plan over state and zone
                        ZonePlan zp = this.navigation.PlanZone(zts.Zone, zts.Start, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]);

                        if (zp.ZoneGoal is ArbiterParkingSpotWaypoint)
                        {
                            // move to parking state
                            ParkingState ps = new ParkingState(zp.Zone, ((ArbiterParkingSpotWaypoint)zp.ZoneGoal).ParkingSpot);
                            return new Maneuver(new HoldBrakeBehavior(), ps, TurnDecorators.NoDecorators, vehicleState.Timestamp);

                        }
                        else if(zp.ZoneGoal is ArbiterPerimeterWaypoint)
                        {
                            // get plan
                            IntersectionPlan ip = navigation.GetIntersectionExitPlan((ITraversableWaypoint)zp.ZoneGoal, goal);

                            // move to exit
                            WaitingAtIntersectionExitState waies = new WaitingAtIntersectionExitState((ITraversableWaypoint)zp.ZoneGoal, ip.BestOption.ToInterconnect.TurnDirection, new IntersectionDescription(), ip.BestOption.ToInterconnect);
                            return new Maneuver(new HoldBrakeBehavior(), waies, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                    }
                    else
                    {
                        // plan over state and zone
                        ZonePlan zp = this.navigation.PlanZone(zts.Zone, zts.Start, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]);

                        // plan
                        Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, zp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                }

                #endregion

                #region Parking State

                else if (CoreCommon.CorePlanningState is ParkingState)
                {
                    // set state
                    ParkingState ps = (ParkingState)CoreCommon.CorePlanningState;

                    // check to see if we're stopped
                    if (CoreCommon.Communications.HasCompleted((new ZoneParkingBehavior(null, null, new Polygon[0], null, null, null, null, null, 0.0)).GetType()))
                    {
                        if (ps.ParkingSpot.Checkpoint.CheckpointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber))
                        {
                            ArbiterOutput.Output("Reached Goal, cp: " + ps.ParkingSpot.Checkpoint.CheckpointId.ToString());
                            CoreCommon.Mission.MissionCheckpoints.Dequeue();
                        }

                        // pull out of the space
                        PullingOutState pos = new PullingOutState(ps.Zone, ps.ParkingSpot);
                        return new Maneuver(new HoldBrakeBehavior(), pos, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    else
                    {
                        // plan
                        Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                }

                #endregion

                #region Pulling Out State

                else if (CoreCommon.CorePlanningState is PullingOutState)
                {
                    // set state
                    PullingOutState pos = (PullingOutState)CoreCommon.CorePlanningState;

                    // plan over state and zone
                    ZonePlan zp = this.navigation.PlanZone(pos.Zone, pos.ParkingSpot.Checkpoint, goal);

                    // check to see if we're stopped
                    if (CoreCommon.Communications.HasCompleted((new ZoneParkingPullOutBehavior(null, null, new Polygon[0], null, null, null, null, null, null, null, null)).GetType()))
                    {
                        // maneuver to next place to go
                        return new Maneuver(new HoldBrakeBehavior(), new ZoneTravelingState(pos.Zone, pos.ParkingSpot.Checkpoint), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    else
                    {
                        // plan
                        Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, zp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                }

                #endregion

                #region Zone Startup State

                else if (CoreCommon.CorePlanningState is ZoneStartupState)
                {
                    // feed through the plan from the zone tactical
                    Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return final maneuver
                    return final;
                }

                #endregion

                #region Zone Orientation

                else if (CoreCommon.CorePlanningState is ZoneOrientationState)
                {
                    ZoneOrientationState zos = (ZoneOrientationState)CoreCommon.CorePlanningState;

                    // add bounds to observable
                    LinePath lp = new LinePath(new Coordinates[] { zos.final.Start.Position, zos.final.End.Position });
                    CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lp.ShiftLateral(TahoeParams.T), ArbiterInformationDisplayObjectType.leftBound));
                    CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lp.ShiftLateral(-TahoeParams.T), ArbiterInformationDisplayObjectType.rightBound));

                    // check to see if we're stopped
                    //if (CoreCommon.Communications.HasCompleted((new UTurnBehavior(null, null, null, null)).GetType()))
                    //{
                        // maneuver to next place to go
                        return new Maneuver(new HoldBrakeBehavior(), new ZoneTravelingState(zos.Zone, zos.final.End), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    //}
                    // not stopped doing hte maneuver
                    //else
                    //	return new Maneuver(zos.Resume(vehicleState, 1.4), zos, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                }

                #endregion

                #region Unknown

                else
                {
                    // non-handled state
                    throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
                }

                #endregion
            }

            #endregion

            #region Other State

            else if (CoreCommon.CorePlanningState is OtherState)
            {
                #region Start Up State

                if (CoreCommon.CorePlanningState is StartUpState)
                {
                    // get state
                    StartUpState sus = (StartUpState)CoreCommon.CorePlanningState;

                    // make a new startup agent
                    StartupReasoning sr = new StartupReasoning(this.laneAgent);

                    // get final state
                    IState nextState = sr.Startup(vehicleState, carMode);

                    // return no op ad zero all decorators
                    Behavior nextBehavior = sus.Resume(vehicleState, vehicleSpeed);

                    // return maneuver
                    return new Maneuver(nextBehavior, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                }

                #endregion

                #region Paused State

                else if (CoreCommon.CorePlanningState is PausedState)
                {
                    // if switch back to run
                    if (carMode == CarMode.Run)
                    {
                        // get state
                        PausedState ps = (PausedState)CoreCommon.CorePlanningState;

                        // get what we were previously doing
                        IState previousState = ps.PreviousState();

                        // check if can resume
                        if (previousState != null && previousState.CanResume())
                        {
                            // resume state is next
                            return new Maneuver(new HoldBrakeBehavior(), new ResumeState(previousState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        // otherwise go to startup
                        else
                        {
                            // next state is startup
                            IState nextState = new StartUpState();

                            // return no op
                            Behavior nextBehavior = new HoldBrakeBehavior();

                            // return maneuver
                            return new Maneuver(nextBehavior, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                    }
                    // otherwise stay stopped
                    else
                    {
                        // stay stopped and paused
                        return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region Human State

                else if (CoreCommon.CorePlanningState is HumanState)
                {
                    // change to startup
                    if (carMode == CarMode.Run)
                    {
                        // next is startup
                        IState next = new StartUpState();

                        // next behavior just stay iin place for now
                        Behavior behavior = new HoldBrakeBehavior();

                        // return startup maneuver
                        return new Maneuver(behavior, next, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    // in human mode still
                    else
                    {
                        // want to remove old behavior stuff
                        return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region Resume State

                else if (CoreCommon.CorePlanningState is ResumeState)
                {
                    // get state
                    ResumeState rs = (ResumeState)CoreCommon.CorePlanningState;

                    // make sure can resume (this is simple action)
                    if (rs.StateToResume != null && rs.StateToResume.CanResume())
                    {
                        // return old behavior
                        Behavior nextBehavior = rs.Resume(vehicleState, vehicleSpeed);

                        // return maneuver
                        return new Maneuver(nextBehavior, rs.StateToResume, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    // otherwise just startup
                    else
                    {
                        // startup
                        return new Maneuver(new HoldBrakeBehavior(), new StartUpState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region No Goals Left State

                else if (CoreCommon.CorePlanningState is NoGoalsLeftState)
                {
                    // check if goals available
                    if (CoreCommon.Mission.MissionCheckpoints.Count > 0)
                    {
                        // startup
                        return new Maneuver(new HoldBrakeBehavior(), new StartUpState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    else
                    {
                        // stay paused
                        return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region eStopped State

                else if (CoreCommon.CorePlanningState is eStoppedState)
                {
                    // change to startup
                    if (carMode == CarMode.Run)
                    {
                        // next is startup
                        IState next = new StartUpState();

                        // next behavior just stay iin place for now
                        Behavior behavior = new HoldBrakeBehavior();

                        // return startup maneuver
                        return new Maneuver(behavior, next, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    // in human mode still
                    else
                    {
                        // want to remove old behavior stuff
                        return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region Unknown

                else
                {
                    // non-handled state
                    throw new ArgumentException("Unknown OtherState type", "CoreCommon.CorePlanningState");
                }

                #endregion
            }

            #endregion

            #region Blockage State

            else if (CoreCommon.CorePlanningState is BlockageState)
            {
                #region Blockage State

                // something is blocked, in the encountered state we want to filter to base components of state
                if (CoreCommon.CorePlanningState is EncounteredBlockageState)
                {
                    // cast blockage state
                    EncounteredBlockageState bs = (EncounteredBlockageState)CoreCommon.CorePlanningState;

                    // plan through the blockage state with no road plan as just a quick filter
                    Maneuver final = tactical.Plan(bs, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return the final maneuver
                    return final;
                }

                #endregion

                #region Blockage Recovery State

                // recover from blockages
                else if (CoreCommon.CorePlanningState is BlockageRecoveryState)
                {
                    // get the blockage recovery state
                    BlockageRecoveryState brs = (BlockageRecoveryState)CoreCommon.CorePlanningState;

                    #region Check Various Statuses of Completion

                    // check successful completion report of behavior
                    if (brs.RecoveryBehavior != null && CoreCommon.Communications.HasCompleted(brs.RecoveryBehavior.GetType()))
                    {
                        // set updated status
                        ArbiterOutput.Output("Successfully received completion of behavior: " + brs.RecoveryBehavior.ToShortString() + ", " + brs.RecoveryBehavior.ShortBehaviorInformation());
                        brs.RecoveryStatus = BlockageRecoverySTATUS.COMPLETED;

                        // move to the tactical plan
                        return this.tactical.Plan(brs, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
                    }
                    // check operational startup
                    else if (CoreCommon.Communications.HasCompleted((new OperationalStartupBehavior()).GetType()))
                    {
                        // check defcon types
                        if (brs.Defcon == BlockageRecoveryDEFCON.REVERSE)
                        {
                            // abort maneuver as operational has no state information
                            return new Maneuver(new NullBehavior(), brs.AbortState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                    }

                    #endregion

                    #region Information

                    // set recovery information
                    CoreCommon.CurrentInformation.FQMState = brs.EncounteredState.ShortDescription();
                    CoreCommon.CurrentInformation.FQMStateInfo = brs.EncounteredState.StateInformation();
                    CoreCommon.CurrentInformation.FQMBehavior = brs.RecoveryBehavior != null ? brs.RecoveryBehavior.ToShortString() : "NONE";
                    CoreCommon.CurrentInformation.FQMBehaviorInfo = brs.RecoveryBehavior != null ? brs.RecoveryBehavior.ShortBehaviorInformation() : "NONE";
                    CoreCommon.CurrentInformation.FQMSpeed = brs.RecoveryBehavior != null ? brs.RecoveryBehavior.SpeedCommandString() : "NONE";

                    #endregion

                    #region Blocked

                    if (brs.RecoveryStatus == BlockageRecoverySTATUS.BLOCKED)
                    {
                        if (brs.RecoveryBehavior is ChangeLaneBehavior)
                        {
                            brs.RecoveryStatus = BlockageRecoverySTATUS.ENCOUNTERED;
                            brs.Defcon = BlockageRecoveryDEFCON.CHANGELANES_FORWARD;
                            return new Maneuver(new HoldBrakeBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else
                        {
                            ArbiterOutput.Output("Recovery behavior blocked, reverting to abort state: " + brs.AbortState.ToString());
                            return new Maneuver(new HoldBrakeBehavior(), brs.AbortState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                    }

                    #endregion

                    #region Navigational Plan

                    // set navigational plan
                    INavigationalPlan navPlan = null;

                    #region Encountered

                    // blockage
                    if (brs.RecoveryStatus == BlockageRecoverySTATUS.ENCOUNTERED)
                    {
                        // get state
                        if (brs.AbortState is StayInLaneState)
                        {
                            // lane state
                            StayInLaneState sils = (StayInLaneState)brs.AbortState;
                            navPlan = navigation.PlanNavigableArea(sils.Lane, vehicleState.Position, goal, sils.WaypointsToIgnore);
                        }
                    }

                    #endregion

                    #region Completion

                    // blockage
                    if (brs.RecoveryStatus == BlockageRecoverySTATUS.COMPLETED)
                    {
                        // get state
                        if (brs.CompletionState is StayInLaneState)
                        {
                            // lane state
                            StayInLaneState sils = (StayInLaneState)brs.CompletionState;
                            navPlan = navigation.PlanNavigableArea(sils.Lane, vehicleState.Position, goal, sils.WaypointsToIgnore);
                        }
                    }

                    #endregion

                    #endregion

                    // move to the tactical plan
                    Maneuver final = this.tactical.Plan(brs, navPlan, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return the final maneuver
                    return final;
                }

                #endregion
            }

            #endregion

            #region Unknown

            else
            {
                // non-handled state
                throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
            }

            // for now, return null
            return new Maneuver();

            #endregion
        }