/// <summary>
        /// Plan given that we are starting on a road
        /// </summary>
        /// <param name="currentLane"></param>
        /// <param name="currentPosition"></param>
        /// <param name="goal"></param>
        /// <returns></returns>
        public RoadPlan PlanNavigableArea(INavigableTravelArea currentArea, Coordinates currentPosition, INavigableNode goal, List <ArbiterWaypoint> ignorable)
        {
            // get all downstream points of interest as well as the goal point of interest
            List <DownstreamPointOfInterest> downstreamPoints = currentArea.Downstream(currentPosition, ignorable, goal);

            // so, for each exit downstream we need to plan from the end of each interconnect to the goal
            this.RouteTimes(downstreamPoints, goal);

            // get road plan
            RoadPlan rp = this.GetRoadPlan(downstreamPoints);

            #region Output

            // update arbiter information
            List <RouteInformation> routeInfo = rp.RouteInformation(currentPosition);

            // make sure we're in a road state
            if (CoreCommon.CorePlanningState == null || CoreCommon.CorePlanningState is UrbanChallenge.Arbiter.Core.Common.State.TravelState)
            {
                // check route 1
                if (routeInfo.Count > 0)
                {
                    RouteInformation ri = routeInfo[0];
                    CoreCommon.CurrentInformation.Route1     = ri;
                    CoreCommon.CurrentInformation.Route1Time = ri.RouteTimeCost.ToString("F6");
                    CoreCommon.CurrentInformation.Route1Wp   = ri.Waypoint;
                }
            }

            #endregion

            // return road plan
            return(rp);
        }
        /// <summary>
        /// Gets the road plan
        /// </summary>
        /// <param name="pointsOfInterest"></param>
        /// <returns></returns>
        private RoadPlan GetRoadPlan(List <DownstreamPointOfInterest> pointsOfInterest)
        {
            // road plan
            RoadPlan rp = new RoadPlan(new Dictionary <ArbiterLaneId, LanePlan>());

            // lane assigned points
            Dictionary <ArbiterLaneId, List <DownstreamPointOfInterest> > lanePoints = new Dictionary <ArbiterLaneId, List <DownstreamPointOfInterest> >();

            // assign dpoi to lanes
            foreach (DownstreamPointOfInterest dpoi in pointsOfInterest)
            {
                if (lanePoints.ContainsKey(dpoi.PointOfInterest.Lane.LaneId))
                {
                    lanePoints[dpoi.PointOfInterest.Lane.LaneId].Add(dpoi);
                }
                else
                {
                    lanePoints.Add(dpoi.PointOfInterest.Lane.LaneId, new List <DownstreamPointOfInterest>());
                    lanePoints[dpoi.PointOfInterest.Lane.LaneId].Add(dpoi);
                }
            }

            // find the best in each lane
            foreach (KeyValuePair <ArbiterLaneId, List <DownstreamPointOfInterest> > pairs in lanePoints)
            {
                List <DownstreamPointOfInterest> dpois = pairs.Value;
                dpois.Sort();

                // choose best and add
                rp.LanePlans.Add(pairs.Key, new LanePlan(dpois[0]));
            }

            // return
            return(rp);
        }
        /// <summary>
        /// Plans over an intersection
        /// </summary>
        /// <param name="exitWaypoint"></param>
        /// <param name="goal"></param>
        /// <returns></returns>
        public IntersectionPlan PlanIntersection(ITraversableWaypoint exitWaypoint, INavigableNode goal)
        {
            // road plan if the itnersection has a road available to take from it
            RoadPlan rp = null;

            // check if road waypoint
            if (exitWaypoint is ArbiterWaypoint)
            {
                // get exit
                ArbiterWaypoint awp = (ArbiterWaypoint)exitWaypoint;

                // check if it has lane partition moving outwards
                if (awp.NextPartition != null)
                {
                    // road plan ignoring exit
                    List <ArbiterWaypoint> iws = RoadToolkit.WaypointsClose(awp.Lane.Way, awp.Position, awp);
                    rp = this.PlanNavigableArea(awp.Lane, awp.Position, goal, iws);
                }
            }

            // get exit plan
            IntersectionPlan ip = this.GetIntersectionExitPlan(exitWaypoint, goal);

            // add road plan if exists
            ip.SegmentPlan = rp;

            // return the plan
            return(ip);
        }
        /// <summary>
        /// Gets the road plan
        /// </summary>
        /// <param name="pointsOfInterest"></param>
        /// <returns></returns>
        private RoadPlan GetRoadPlan(List <DownstreamPointOfInterest> pointsOfInterest, ArbiterWay way)
        {
            // road plan
            RoadPlan rp = new RoadPlan(new Dictionary <ArbiterLaneId, LanePlan>());

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

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

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

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

            // return
            return(rp);
        }
        /// <summary>
        /// Plans the forward maneuver and secondary maneuver
        /// </summary>
        /// <param name="arbiterLane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="p"></param>
        /// <param name="blockage"></param>
        /// <returns></returns>
        public Maneuver ForwardManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, RoadPlan roadPlan,
            List<ITacticalBlockage> blockages)
        {
            // get primary maneuver
            Maneuver primary = this.OpposingForwardMonitor.PrimaryManeuver(arbiterLane, closestGood, vehicleState, blockages);

            // return primary for now
            return primary;
        }
        /// <summary>
        /// Get a road plan while setting partition costs very high
        /// </summary>
        /// <param name="partition"></param>
        /// <param name="goal"></param>
        /// <param name="blockAdjacent"></param>
        /// <param name="c"></param>
        /// <returns></returns>
        public RoadPlan PlanRoadOppositeWithoutPartition(ArbiterLanePartition partition, ArbiterLanePartition opposite, IArbiterWaypoint goal, bool blockAdjacent, Coordinates c, bool sameWay)
        {
            KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes;

            this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();
            RoadPlan rp = null;

            if (!blockAdjacent)
            {
                NavigationBlockage nb  = partition.Blockage;
                NavigationBlockage tmp = new NavigationBlockage(double.MaxValue);
                partition.Blockage = tmp;
                rp = this.PlanNavigableArea(partition.Lane, c, goal, new List <ArbiterWaypoint>());
                partition.Blockage = nb;
            }
            else
            {
                // save
                List <KeyValuePair <ArbiterLanePartition, NavigationBlockage> > savedBlockages = new List <KeyValuePair <ArbiterLanePartition, NavigationBlockage> >();

                // set
                savedBlockages.Add(new KeyValuePair <ArbiterLanePartition, NavigationBlockage>(partition, partition.Blockage));
                // create new
                NavigationBlockage anewerBlockage = new NavigationBlockage(Double.MaxValue);
                anewerBlockage.BlockageExists = true;
                partition.Blockage            = anewerBlockage;

                foreach (ArbiterLanePartition alp in partition.NonLaneAdjacentPartitions)
                {
                    if (alp.IsInside(c) && (!sameWay || (sameWay && partition.Lane.Way.Equals(alp.Lane.Way))))
                    {
                        savedBlockages.Add(new KeyValuePair <ArbiterLanePartition, NavigationBlockage>(alp, alp.Blockage));

                        // create new
                        NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
                        newerBlockage.BlockageExists = true;
                        alp.Blockage = newerBlockage;
                    }
                }

                // plan
                rp = this.PlanNavigableArea(opposite.Lane, c, goal, new List <ArbiterWaypoint>());

                // restore
                foreach (KeyValuePair <ArbiterLanePartition, NavigationBlockage> saved in savedBlockages)
                {
                    saved.Key.Blockage = saved.Value;
                }
            }

            // restore
            this.currentTimes = tmpCurrentTimes;

            // return
            return(rp);
        }
        /// <summary>
        /// Plan given that we are starting on a road
        /// </summary>
        /// <param name="currentLane"></param>
        /// <param name="currentPosition"></param>
        /// <param name="goal"></param>
        /// <returns></returns>
        public RoadPlan PlanRoads(ArbiterLane currentLane, Coordinates currentPosition, INavigableNode goal, List <ArbiterWaypoint> ignorable)
        {
            // get all downstream points of interest
            List <DownstreamPointOfInterest> downstreamPoints = new List <DownstreamPointOfInterest>();

            // get exits downstream from this current position in the way
            downstreamPoints.AddRange(this.Downstream(currentPosition, currentLane.Way, true, ignorable));

            // determine if goal is downstream in a specific lane, add to possible route times to consider
            DownstreamPointOfInterest goalDownstream = this.IsGoalDownStream(currentLane.Way, currentPosition, goal);

            // add goal to points downstream if it exists
            if (goalDownstream != null)
            {
                downstreamPoints.Add(goalDownstream);
            }

            // so, for each exit downstream we need to plan from the end of each interconnect to the goal
            this.DetermineDownstreamPointRouteTimes(downstreamPoints, goal, currentLane.Way);

            // get road plan
            RoadPlan rp = this.GetRoadPlan(downstreamPoints, currentLane.Way);

            // update arbiter information
            List <RouteInformation> routeInfo = rp.RouteInformation(currentPosition);

            // make sure we're in a road state
            if (CoreCommon.CorePlanningState == null ||
                CoreCommon.CorePlanningState is TravelState ||
                CoreCommon.CorePlanningState is TurnState)
            {
                // check route 1
                if (routeInfo.Count > 0)
                {
                    RouteInformation ri = routeInfo[0];
                    CoreCommon.CurrentInformation.Route1     = ri;
                    CoreCommon.CurrentInformation.Route1Time = ri.RouteTimeCost.ToString("F6");
                    CoreCommon.CurrentInformation.Route1Wp   = ri.Waypoint;
                }

                // check route 2
                if (routeInfo.Count > 1)
                {
                    RouteInformation ri = routeInfo[1];
                    CoreCommon.CurrentInformation.Route2     = ri;
                    CoreCommon.CurrentInformation.Route2Time = ri.RouteTimeCost.ToString("F6");
                    CoreCommon.CurrentInformation.Route2Wp   = ri.Waypoint;
                }
            }

            // return road plan
            return(rp);
        }
        /// <summary>
        /// Secondary maneuver outside minimum cap
        /// </summary>
        /// <param name="arbiterLane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="roadPlan"></param>
        /// <param name="blockages"></param>
        /// <param name="ignorable"></param>
        /// <returns></returns>
        public Maneuver? AdvancedSecondaryOutsideMinCap(ArbiterLane lane, ArbiterWaypoint laneGoal, VehicleState vehicleState, RoadPlan roadPlan,
            List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
        {
            // get proper reasoning component
            List<LateralReasoning> adjacentReasonings = new List<LateralReasoning>();
            if (bestTask != TypeOfTasks.Left)
            {
                if (this.rightLateralReasoning is LateralReasoning && this.rightLateralReasoning.Exists && this.rightLateralReasoning.ExistsExactlyHere(vehicleState))
                    adjacentReasonings.Add((LateralReasoning)this.rightLateralReasoning);
                else if (this.leftLateralReasoning is LateralReasoning && this.leftLateralReasoning.Exists && this.leftLateralReasoning.ExistsExactlyHere(vehicleState))
                    adjacentReasonings.Add((LateralReasoning)this.leftLateralReasoning);
            }
            else
            {
                if (this.leftLateralReasoning is LateralReasoning && this.leftLateralReasoning.Exists && this.leftLateralReasoning.ExistsExactlyHere(vehicleState))
                    adjacentReasonings.Add((LateralReasoning)this.leftLateralReasoning);
                else if (this.rightLateralReasoning is LateralReasoning && this.rightLateralReasoning.Exists && this.rightLateralReasoning.ExistsExactlyHere(vehicleState))
                    adjacentReasonings.Add((LateralReasoning)this.rightLateralReasoning);
            }

            // loop through possible
            foreach(LateralReasoning adjacentReasoning in adjacentReasonings)
            {
                // check if adjacent reasoning exists
                if (adjacentReasoning != null && adjacentReasoning.Exists)
                {
                    // update adjacent reasoning
                    adjacentReasoning.ForwardMonitor.Primary(adjacentReasoning.LateralLane, vehicleState, roadPlan, blockages, ignorable, false);

                    // otherwise check if forward vehicle slow
                    LaneChangeInformation forwardVehicleSecondary;

                    // check if should pass the forward vehicle
                    if (this.ForwardMonitor.ForwardVehicle.ShouldPass(out forwardVehicleSecondary, lane))
                    {
                        // check if forward vehicle failed
                        if (forwardVehicleSecondary.Reason == LaneChangeReason.FailedForwardVehicle)
                        {
                            ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " failed");

                            // make sure the failed vehicle is not within 50m of the goal
                            double vehicleDistanceToGoal = lane.DistanceBetween(ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, laneGoal.Position);
                            ArbiterOutput.Output("Failed FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", DistGoal: " + vehicleDistanceToGoal.ToString("f2") + ", speed: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f1"));
                            if ((vehicleDistanceToGoal > 50))
                            {
                                // check if adjacent has no forward vehicle
                                if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker)
                                {
                                    // plan the lane change
                                    return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, forwardVehicleSecondary);
                                }
                                // otherwise a forward vehicle exists
                                else
                                {
                                    // otherwise check if forward vehicle slow
                                    LaneChangeInformation lateralVehicleInformation;

                                    // check if lateral vehicle fine
                                    if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldPass(out lateralVehicleInformation, adjacentReasoning.LateralLane))
                                    {
                                        // plan the lane change
                                        return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
                                    }
                                    // check if lateral vehicle failed or slow
                                    else
                                    {
                                        // check distance to lateral > distance to forward + 25
                                        double distToAdjacent = lane.DistanceBetween(vehicleState.Front, adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
                                        double distToForward = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
                                        if (distToAdjacent > distToForward + 25.0)
                                        {
                                            // plan the lane change
                                            return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
                                        }
                                    }
                                }
                            }
                        }
                        // otherwise check if they are slow
                        else if (forwardVehicleSecondary.Reason == LaneChangeReason.SlowForwardVehicle)
                        {
                            ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " slow");

                            // make sure the slow vehicle is not within 50m of the goal if velocity is > 5mph
                            double vehicleDistanceToGoal = lane.DistanceBetween(ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, laneGoal.Position);
                            ArbiterOutput.Output("Slow FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", DistGoal: " + vehicleDistanceToGoal.ToString("f2") + ", speed: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f1"));
                            if ((vehicleDistanceToGoal > 50 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 2.24) ||
                                (vehicleDistanceToGoal > 75 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 4.48) ||
                                (vehicleDistanceToGoal > 100 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 6.72) ||
                                (vehicleDistanceToGoal > 125 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 8.96))
                            {
                                // check if adjacent has no forward vehicle
                                if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker)
                                {
                                    // plan the lane change
                                    ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: No vehicle in adjacent");
                                    return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
                                }
                                // otherwise a forward vehicle exists
                                else
                                {
                                    // otherwise check if forward vehicle slow
                                    LaneChangeInformation lateralVehicleInformation;

                                    // check if lateral vehicle fine
                                    if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldPass(out lateralVehicleInformation, adjacentReasoning.LateralLane))
                                    {
                                        // check distance to lateral > distance to forward + 25
                                        double distToAdjacent = lane.DistanceBetween(vehicleState.Front, adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
                                        double distToForward = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
                                        ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: Normal vehicle in adjacent: " + adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + "FV Dist: " + distToForward.ToString("f2") + ", Adj Dist: " + adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString());

                                        // check distance greater and adjacent speed greater than forward speed
                                        if (distToAdjacent > distToForward + 25.0 &&
                                            adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed > this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed)
                                        {
                                            // plan the lane change
                                            return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
                                        }

                                        // plan the lane change
                                        //return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
                                    }
                                    else if (
                                        lane.DistanceBetween(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition,
                                        adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) > 65.0 &&
                                        (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped ||
                                        (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed + 4.48 < adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed)))
                                    {
                                        // plan the lane change
                                        return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
                                    }
                                }
                            }
                        }
                    }
                }
            }

            // normal secondary parameterization as a fall through on the naviagation lane changes
            return this.SecondaryManeuver(lane, vehicleState, roadPlan, blockages, ignorable, bestTask);
        }
 /// <summary>
 /// Resets values held over time
 /// </summary>
 public void Reset()
 {
     currentLane = null;
     navigationPlan = null;
 }
        /// <summary>
        /// Sets the supra road plan
        /// </summary>
        /// <param name="rp"></param>
        /// <param name="current"></param>
        public void SetSupraRoadPlan(RoadPlan rp, SupraLane current)
        {
            this.navigationPlan = rp;

            // left
            List<LanePlan> left = new List<LanePlan>();

            // straight
            List<LanePlan> straight = new List<LanePlan>();

            // right
            List<LanePlan> right = new List<LanePlan>();

            Dictionary<ArbiterLaneId, LanePlan> plans = rp.LanePlans;

            // straight
            if (plans.ContainsKey(current.Initial.LaneId))
            {
                straight.Add(plans[current.Initial.LaneId]);
            }
            if (plans.ContainsKey(current.Final.LaneId))
            {
                straight.Add(plans[current.Final.LaneId]);
            }

            // create tasks
            tasks = new Dictionary<TypeOfTasks, List<LanePlan>>();
            tasks.Add(TypeOfTasks.Left, left);
            tasks.Add(TypeOfTasks.Straight, straight);
            tasks.Add(TypeOfTasks.Right, right);

            //s
            /*Dictionary<ArbiterLaneId, LanePlan> plans = rp.LanePlans;

            // left
            List<LanePlan> left = new List<LanePlan>();

            // straight
            List<LanePlan> straight = new List<LanePlan>();

            // right
            List<LanePlan> right = new List<LanePlan>();

            // tmp
            ArbiterLane temp = current.Initial.LaneOnLeft;

            // left
            while (temp != null && (temp.Way.Equals(current.Initial.Way) || temp.Way.Equals(current.Final.Way)))
            {
                if (plans.ContainsKey(temp.LaneId))
                {
                    left.Add(plans[temp.LaneId]);
                }

                temp = temp.LaneOnLeft;
            }

            // right
            temp = current.Initial.LaneOnRight;
            while (temp != null && (temp.Way.Equals(current.Initial.Way) || temp.Way.Equals(current.Final.Way)))
            {
                if (plans.ContainsKey(temp.LaneId))
                {
                    right.Add(plans[temp.LaneId]);
                }

                temp = temp.LaneOnRight;
            }

            // straight
            if (plans.ContainsKey(current.Initial.LaneId))
            {
                straight.Add(plans[current.Initial.LaneId]);
            }
            if (plans.ContainsKey(current.Final.LaneId))
            {
                straight.Add(plans[current.Final.LaneId]);
            }

            // create tasks
            tasks = new Dictionary<TypeOfTasks, List<LanePlan>>();
            tasks.Add(TypeOfTasks.Left, left);
            tasks.Add(TypeOfTasks.Straight, straight);
            tasks.Add(TypeOfTasks.Right, right);*/
        }
        /// <summary>
        /// Distinctly want to make lane change, parameters for doing so
        /// </summary>
        /// <param name="arbiterLane"></param>
        /// <param name="left"></param>
        /// <param name="vehicleState"></param>
        /// <param name="roadPlan"></param>
        /// <param name="blockages"></param>
        /// <param name="ignorable"></param>
        /// <returns></returns>
        public Maneuver? AdvancedDesiredLaneChangeManeuver(ArbiterLane lane, bool left, ArbiterWaypoint goal, RoadPlan rp, VehicleState vehicleState,
            List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, LaneChangeInformation laneChangeInformation, Maneuver? secondary,
            out LaneChangeParameters parameters)
        {
            // set aprams
            parameters = new LaneChangeParameters();

            // get final maneuver
            Maneuver? final = null;

            // check partition is not a startup chute
            if (lane.GetClosestPartition(vehicleState.Front).Type != PartitionType.Startup)
            {
                // get the lane goal distance
                double distanceToLaneGoal = lane.DistanceBetween(vehicleState.Front, goal.Position);

                // check if our distance is less than 50m to the goal
                if (distanceToLaneGoal < 50.0)
                {
                    // use old
                    final = this.LaneChangeManeuver(lane, left, goal, vehicleState, blockages, ignorable, laneChangeInformation, secondary, out parameters);

                    try
                    {
                        // check final null
                        if (final == null)
                        {
                            // check for checkpoint within 4VL of front of failed vehicle
                            ArbiterCheckpoint acCurrecnt = CoreCommon.Mission.MissionCheckpoints.Peek();
                            if (acCurrecnt.WaypointId is ArbiterWaypointId)
                            {
                                // get waypoint
                                ArbiterWaypoint awCheckpoint = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[acCurrecnt.WaypointId];

                                // check way
                                if (awCheckpoint.Lane.Way.Equals(lane.Way))
                                {
                                    // distance to wp
                                    double distToWp = lane.DistanceBetween(vehicleState.Front, awCheckpoint.Position);

                                    // check close to waypoint and stopped
                                    if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.1 && distToWp < TahoeParams.VL * 1.0)
                                    {
                                        ArbiterOutput.Output("Removing checkpoint: " + acCurrecnt.WaypointId.ToString() + " Stopped next to it");
                                        CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                        return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                                    }
                                }
                            }
                        }
                    }
                    catch (Exception) { }
                }
                // no forward vehicle
                else if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle == null)
                {
                    // adjacent monitor
                    LateralReasoning adjacent = null;
                    if (left && this.leftLateralReasoning is LateralReasoning && this.leftLateralReasoning.Exists)
                    {
                        // update
                        adjacent = (LateralReasoning)this.leftLateralReasoning;
                    }
                    else if (!left && this.rightLateralReasoning is LateralReasoning && this.rightLateralReasoning.Exists)
                    {
                        // update
                        adjacent = (LateralReasoning)this.rightLateralReasoning;
                    }

                    // check adj
                    if (adjacent != null)
                    {
                        // update
                        adjacent.ForwardMonitor.Primary(adjacent.LateralLane, vehicleState, rp, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), false);

                        if (adjacent.ForwardMonitor.ForwardVehicle.CurrentVehicle == null && adjacent.AdjacentAndRearClear(vehicleState))
                        {
                            // use old
                            final = this.LaneChangeManeuver(lane, left, goal, vehicleState, blockages, ignorable, laneChangeInformation, secondary, out parameters);
                        }
                    }
                }
            }

            if (!final.HasValue)
            {
                if (!secondary.HasValue)
                {
                    List<TravelingParameters> falloutParams = new List<TravelingParameters>();
                    TravelingParameters t1 = this.ForwardMonitor.ParameterizationHelper(lane, lane, goal.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
                    falloutParams.Add(t1);
                    falloutParams.Add(this.ForwardMonitor.LaneParameters);
                    if (this.ForwardMonitor.FollowingParameters.HasValue)
                        falloutParams.Add(this.ForwardMonitor.FollowingParameters.Value);
                    falloutParams.Sort();
                    TravelingParameters tpCatch = falloutParams[0];

                    return new Maneuver(tpCatch.Behavior, tpCatch.NextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                }
                else
                {
                    return secondary;
                }
            }
            else
            {
                return final;
            }
        }
        /// <summary>
        /// Set the road plan and populate the tasks
        /// </summary>
        /// <param name="rp"></param>
        /// <param name="current"></param>
        public void SetRoadPlan(RoadPlan rp, ArbiterLane current)
        {
            Dictionary<ArbiterLaneId, LanePlan> plans = rp.LanePlans;

            // left
            List<LanePlan> left = new List<LanePlan>();

            // straight
            List<LanePlan> straight = new List<LanePlan>();

            // right
            List<LanePlan> right = new List<LanePlan>();

            // tmp
            ArbiterLane temp = current.LaneOnLeft;

            // left
            while (temp != null && temp.Way.Equals(current.Way))
            {
                if (plans.ContainsKey(temp.LaneId))
                {
                    left.Add(plans[temp.LaneId]);
                }

                temp = temp.LaneOnLeft;
            }

            // right
            temp = current.LaneOnRight;
            while (temp != null && temp.Way.Equals(current.Way))
            {
                if (plans.ContainsKey(temp.LaneId))
                {
                    right.Add(plans[temp.LaneId]);
                }

                temp = temp.LaneOnRight;
            }

            // straight
            if (plans.ContainsKey(current.LaneId))
            {
                straight.Add(plans[current.LaneId]);
            }

            // create tasks
            tasks = new Dictionary<TypeOfTasks, List<LanePlan>>();
            tasks.Add(TypeOfTasks.Left, left);
            tasks.Add(TypeOfTasks.Straight, straight);
            tasks.Add(TypeOfTasks.Right, right);
        }
        /// <summary>
        /// Gets the road plan
        /// </summary>
        /// <param name="pointsOfInterest"></param>
        /// <returns></returns>
        private RoadPlan GetRoadPlan(List<DownstreamPointOfInterest> pointsOfInterest)
        {
            // road plan
            RoadPlan rp = new RoadPlan(new Dictionary<ArbiterLaneId, LanePlan>());

            // lane assigned points
            Dictionary<ArbiterLaneId, List<DownstreamPointOfInterest>> lanePoints = new Dictionary<ArbiterLaneId, List<DownstreamPointOfInterest>>();

            // assign dpoi to lanes
            foreach (DownstreamPointOfInterest dpoi in pointsOfInterest)
            {
                if (lanePoints.ContainsKey(dpoi.PointOfInterest.Lane.LaneId))
                {
                    lanePoints[dpoi.PointOfInterest.Lane.LaneId].Add(dpoi);
                }
                else
                {
                    lanePoints.Add(dpoi.PointOfInterest.Lane.LaneId, new List<DownstreamPointOfInterest>());
                    lanePoints[dpoi.PointOfInterest.Lane.LaneId].Add(dpoi);
                }
            }

            // find the best in each lane
            foreach (KeyValuePair<ArbiterLaneId, List<DownstreamPointOfInterest>> pairs in lanePoints)
            {
                List<DownstreamPointOfInterest> dpois = pairs.Value;
                dpois.Sort();

                // choose best and add
                rp.LanePlans.Add(pairs.Key, new LanePlan(dpois[0]));
            }

            // return
            return rp;
        }
        /// <summary>
        /// Gets the road plan
        /// </summary>
        /// <param name="pointsOfInterest"></param>
        /// <returns></returns>
        private RoadPlan GetRoadPlan(List<DownstreamPointOfInterest> pointsOfInterest, ArbiterWay way)
        {
            // road plan
            RoadPlan rp = new RoadPlan(new Dictionary<ArbiterLaneId, LanePlan>());

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

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

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

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

            // return
            return rp;
        }
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="waypoint"></param>
 /// <param name="entries"></param>
 public IntersectionPlan(ITraversableWaypoint waypoint, List<PlanableInterconnect> entries, RoadPlan roadPlan)
 {
     this.ExitWaypoint = waypoint;
     this.PossibleEntries = entries;
     this.SegmentPlan = roadPlan;
 }
		/// <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, RoadPlan navigationalPlan, VehicleState vehicleState,
			SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, 
			List<ITacticalBlockage> blockages, double vehicleSpeed)
		{
			// assign vehicles to their lanes
			this.roadMonitor.Assign(vehicles);

			// navigation tasks
			this.taskReasoning.navigationPlan = navigationalPlan;

			#region Stay in lane

			// maneuver given we are in a lane
			if (planningState is StayInLaneState)
			{
				// get state
				StayInLaneState sils = (StayInLaneState)planningState;

				// check reasoning if needs to be different
				if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(sils.Lane))
				{
					if (sils.Lane.LaneOnLeft == null)
						this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver);
					else if (sils.Lane.LaneOnLeft.Way.Equals(sils.Lane.Way))
						this.leftLateralReasoning = new LateralReasoning(sils.Lane.LaneOnLeft, SideObstacleSide.Driver);
					else
						this.leftLateralReasoning = new OpposingLateralReasoning(sils.Lane.LaneOnLeft, SideObstacleSide.Driver);

					if (sils.Lane.LaneOnRight == null)
						this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger);
					else if (sils.Lane.LaneOnRight.Way.Equals(sils.Lane.Way))
						this.rightLateralReasoning = new LateralReasoning(sils.Lane.LaneOnRight, SideObstacleSide.Passenger);
					else
						this.rightLateralReasoning = new OpposingLateralReasoning(sils.Lane.LaneOnRight, SideObstacleSide.Passenger);
					
					this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, sils.Lane);
				}

				// populate navigation with road plan
				taskReasoning.SetRoadPlan(navigationalPlan, sils.Lane);

				// as penalties for lane changes already taken into account, can just look at
				// best lane plan to figure out what to do
				TypeOfTasks bestTask = taskReasoning.Best;

				// get the forward lane plan
				Maneuver forwardManeuver = forwardReasoning.ForwardManeuver(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore);

				// get the secondary
				Maneuver? secondaryManeuver = forwardReasoning.AdvancedSecondary(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore, bestTask);  //forwardReasoning.SecondaryManeuver(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore, bestTask);

				// check behavior type for uturn
				if (secondaryManeuver.HasValue && secondaryManeuver.Value.PrimaryBehavior is UTurnBehavior)
					return secondaryManeuver.Value;

				// check if we wish to change lanes here
				if (bestTask != TypeOfTasks.Straight)
				{
					// parameters
					LaneChangeParameters parameters;
					secondaryManeuver = this.forwardReasoning.AdvancedDesiredLaneChangeManeuver(sils.Lane, bestTask == TypeOfTasks.Left ? true : false, navigationalPlan.BestPlan.laneWaypointOfInterest.PointOfInterest,
						navigationalPlan, vehicleState, blockages, sils.WaypointsToIgnore, new LaneChangeInformation(LaneChangeReason.Navigation, this.forwardReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle), secondaryManeuver, out parameters);
				}

				// final maneuver
				Maneuver finalManeuver = secondaryManeuver.HasValue ? secondaryManeuver.Value : forwardManeuver;

				// set opposing vehicle flag
				if (false && this.leftLateralReasoning != null && this.leftLateralReasoning is OpposingLateralReasoning && finalManeuver.PrimaryBehavior is StayInLaneBehavior)
				{
					StayInLaneBehavior silb = (StayInLaneBehavior)finalManeuver.PrimaryBehavior;
					OpposingLateralReasoning olr = (OpposingLateralReasoning)this.leftLateralReasoning;
					olr.ForwardMonitor.ForwardVehicle.Update(olr.lane, vehicleState);
					if (olr.ForwardMonitor.ForwardVehicle.CurrentVehicle != null)
					{
						ForwardVehicleTrackingControl fvtc = olr.ForwardMonitor.ForwardVehicle.GetControl(olr.lane, vehicleState);
						BehaviorDecorator[] bds = new BehaviorDecorator[finalManeuver.PrimaryBehavior.Decorators.Count];
						finalManeuver.PrimaryBehavior.Decorators.CopyTo(bds);
						finalManeuver.PrimaryBehavior.Decorators = new List<BehaviorDecorator>(bds);
						silb.Decorators.Add(new OpposingLaneDecorator(fvtc.xSeparation, olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed));
						ArbiterOutput.Output("Added Opposing Lane Decorator: " + fvtc.xSeparation.ToString("F2") + "m, " + olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f2") + "m/s");
					}
					finalManeuver.PrimaryBehavior = silb;					
				}

				// return the final
				return finalManeuver;
			}

			#endregion

			#region Stay in supra lane

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

				// check reasoning
				if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(sisls.Lane))
				{
					if (sisls.Lane.Initial.LaneOnLeft == null)
						this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver);
					else if (sisls.Lane.Initial.LaneOnLeft.Way.Equals(sisls.Lane.Initial.Way))
						this.leftLateralReasoning = new LateralReasoning(sisls.Lane.Initial.LaneOnLeft, SideObstacleSide.Driver);
					else
						this.leftLateralReasoning = new OpposingLateralReasoning(sisls.Lane.Initial.LaneOnLeft, SideObstacleSide.Driver);

					if (sisls.Lane.Initial.LaneOnRight == null)
						this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger);
					else if (sisls.Lane.Initial.LaneOnRight.Way.Equals(sisls.Lane.Initial.Way))
						this.rightLateralReasoning = new LateralReasoning(sisls.Lane.Initial.LaneOnRight, SideObstacleSide.Passenger);
					else
						this.rightLateralReasoning = new OpposingLateralReasoning(sisls.Lane.Initial.LaneOnRight, SideObstacleSide.Passenger);

					this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, sisls.Lane);
				}

				// populate navigation with road plan
				taskReasoning.SetSupraRoadPlan(navigationalPlan, sisls.Lane);

				// as penalties for lane changes already taken into account, can just look at
				// best lane plan to figure out what to do
				// TODO: NOTE THAT THIS BEST TASK SHOULD BE IN THE SUPRA LANE!! (DO WE NEED THIS)
				TypeOfTasks bestTask = taskReasoning.Best;

				// get the forward lane plan
				Maneuver forwardManeuver = forwardReasoning.ForwardManeuver(sisls.Lane, vehicleState, navigationalPlan, blockages, sisls.WaypointsToIgnore);

				// get hte secondary
				Maneuver? secondaryManeuver = forwardReasoning.AdvancedSecondary(sisls.Lane, vehicleState, navigationalPlan, blockages, new List<ArbiterWaypoint>(), bestTask);//forwardReasoning.SecondaryManeuver(sisls.Lane, vehicleState, navigationalPlan, blockages, sisls.WaypointsToIgnore, bestTask);

				// final maneuver
				Maneuver finalManeuver = secondaryManeuver.HasValue ? secondaryManeuver.Value : forwardManeuver;

				// check if stay in lane
				if (false && this.leftLateralReasoning != null && this.leftLateralReasoning is OpposingLateralReasoning && finalManeuver.PrimaryBehavior is SupraLaneBehavior)
				{					
					SupraLaneBehavior silb = (SupraLaneBehavior)finalManeuver.PrimaryBehavior;
					OpposingLateralReasoning olr = (OpposingLateralReasoning)this.leftLateralReasoning;
					olr.ForwardMonitor.ForwardVehicle.Update(olr.lane, vehicleState);
					if (olr.ForwardMonitor.ForwardVehicle.CurrentVehicle != null)
					{
						ForwardVehicleTrackingControl fvtc = olr.ForwardMonitor.ForwardVehicle.GetControl(olr.lane, vehicleState);
						BehaviorDecorator[] bds = new BehaviorDecorator[finalManeuver.PrimaryBehavior.Decorators.Count];
						finalManeuver.PrimaryBehavior.Decorators.CopyTo(bds);
						finalManeuver.PrimaryBehavior.Decorators = new List<BehaviorDecorator>(bds);
						silb.Decorators.Add(new OpposingLaneDecorator(fvtc.xSeparation, olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed));
						ArbiterOutput.Output("Added Opposing Lane Decorator: " + fvtc.xSeparation.ToString("F2") + "m, " + olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f2") + "m/s");
					}
					finalManeuver.PrimaryBehavior = silb;
				}

				// return the final
				return finalManeuver;

				// notify
				/*if (secondaryManeuver.HasValue)
					ArbiterOutput.Output("Secondary Maneuver");

				// check for forward's secondary maneuver for desired behavior other than going straight
				if (secondaryManeuver.HasValue)
				{
					// return the secondary maneuver
					return secondaryManeuver.Value;
				}
				// otherwise our default behavior and posibly desired is going straight
				else
				{
					// return default forward maneuver
					return forwardManeuver;
				}*/
			}

			#endregion

			#region Change Lanes State

			// maneuver given we are changing lanes
			else if (planningState is ChangeLanesState)
			{
				// get state
				ChangeLanesState cls = (ChangeLanesState)planningState;
				LaneChangeReasoning lcr = new LaneChangeReasoning();
				Maneuver final = lcr.PlanLaneChange(cls, vehicleState, navigationalPlan, blockages, new List<ArbiterWaypoint>());
				
				#warning need to filter through waypoints to ignore so don't get stuck by a stop line
				//Maneuver final = new Maneuver(cls.Resume(vehicleState, vehicleSpeed), cls, cls.DefaultStateDecorators, vehicleState.Timestamp);

				// return the final planned maneuver
				return final;

				/*if (!cls.parameters..TargetIsOnComing)
				{
					// check reasoning
					if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(cls.TargetLane))
					{
						if (cls.TargetLane.LaneOnLeft.Way.Equals(cls.TargetLane.Way))
							this.leftLateralReasoning = new LateralReasoning(cls.TargetLane.LaneOnLeft);
						else
							this.leftLateralReasoning = new OpposingLateralReasoning(cls.TargetLane.LaneOnLeft);

						if (cls.TargetLane.LaneOnRight.Way.Equals(cls.TargetLane.Way))
							this.rightLateralReasoning = new LateralReasoning(cls.TargetLane.LaneOnRight);
						else
							this.rightLateralReasoning = new OpposingLateralReasoning(cls.TargetLane.LaneOnRight);

						this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, cls.TargetLane);
					}

					
					// get speed command
					double speed;
					double distance;
					this.forwardReasoning.ForwardMonitor.StoppingParams(new ArbiterWaypoint(cls.TargetUpperBound.pt, null), cls.TargetLane, vehicleState.Front, vehicleState.ENCovariance, out speed, out distance);
					SpeedCommand sc = new ScalarSpeedCommand(Math.Max(speed, 0.0));
					cls.distanceLeft = distance;

					// get behavior
					ChangeLaneBehavior clb = new ChangeLaneBehavior(cls.InitialLane.LaneId, cls.TargetLane.LaneId, cls.InitialLane.LaneOnLeft != null && cls.InitialLane.LaneOnLeft.Equals(cls.TargetLane),
						distance, sc, new List<int>(), cls.InitialLane.PartitionPath, cls.TargetLane.PartitionPath, cls.InitialLane.Width, cls.TargetLane.Width);
					
					// plan over the target lane
					//Maneuver targetManeuver = forwardReasoning.ForwardManeuver(cls.TargetLane, vehicleState, !cls.TargetIsOnComing, blockage, cls.InitialLaneState.IgnorableWaypoints);

					// plan over the initial lane
					//Maneuver initialManeuver = forwardReasoning.ForwardManeuver(cls.InitialLane, vehicleState, !cls.InitialIsOncoming, blockage, cls.InitialLaneState.IgnorableWaypoints);

					// generate the change lanes command
					//Maneuver final = laneChangeReasoning.PlanLaneChange(cls, initialManeuver, targetManeuver);
					
				}
				else
				{
					throw new Exception("Change lanes into oncoming not supported yet by road tactical");
				}*/
			}

			#endregion

			#region Opposing Lanes State

			// maneuver given we are in an opposing lane
			else if (planningState is OpposingLanesState)
			{
				// get state
				OpposingLanesState ols = (OpposingLanesState)planningState;
				ArbiterWayId opposingWay = ols.OpposingWay;

				ols.SetClosestGood(vehicleState);
				ols.ResetLaneAgent = false;

				// check reasoning
				if (this.opposingReasoning == null || !this.opposingReasoning.Lane.Equals(ols.OpposingLane))
				{
					if (ols.OpposingLane.LaneOnRight == null)
						this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver);
					else if (!ols.OpposingLane.LaneOnRight.Way.Equals(ols.OpposingLane.Way))
						this.leftLateralReasoning = new LateralReasoning(ols.OpposingLane.LaneOnRight, SideObstacleSide.Driver);
					else
						this.leftLateralReasoning = new OpposingLateralReasoning(ols.OpposingLane.LaneOnRight, SideObstacleSide.Driver);

					if (ols.OpposingLane.LaneOnLeft == null)
						this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger);
					else if (!ols.OpposingLane.LaneOnLeft.Way.Equals(ols.OpposingLane.Way))
						this.rightLateralReasoning = new LateralReasoning(ols.OpposingLane.LaneOnLeft, SideObstacleSide.Passenger);
					else
						this.rightLateralReasoning = new OpposingLateralReasoning(ols.OpposingLane.LaneOnLeft, SideObstacleSide.Passenger);

					this.opposingReasoning = new OpposingReasoning(this.leftLateralReasoning, this.rightLateralReasoning, ols.OpposingLane);
				}

				// get the forward lane plan
				Maneuver forwardManeuver = this.opposingReasoning.ForwardManeuver(ols.OpposingLane, ols.ClosestGoodLane, vehicleState, navigationalPlan, blockages);

				// get the secondary maneuver
				Maneuver? secondaryManeuver = null;
				if(ols.ClosestGoodLane != null)
					secondaryManeuver = this.opposingReasoning.SecondaryManeuver(ols.OpposingLane, ols.ClosestGoodLane, vehicleState, blockages, ols.EntryParameters);

				// check for reasonings secondary maneuver for desired behavior other than going straight
				if (secondaryManeuver != null)
				{
					// return the secondary maneuver
					return secondaryManeuver.Value;
				}
				// otherwise our default behavior and posibly desired is going straight
				else
				{
					// return default forward maneuver
					return forwardManeuver;
				}
			}

			#endregion

			#region not imp
			/*
			#region Uturn

			// we are making a uturn
			else if (planningState is uTurnState)
			{
				// get the uturn state
				uTurnState uts = (uTurnState)planningState;

				// get the final lane we wish to be in
				ArbiterLane targetLane = uts.TargetLane;

				// get operational state
				Type operationalBehaviorType = CoreCommon.Communications.GetCurrentOperationalBehavior();

				// check if we have completed the uturn
				bool complete = operationalBehaviorType == typeof(StayInLaneBehavior);

				// default next behavior
				Behavior nextBehavior = new StayInLaneBehavior(targetLane.LaneId, new ScalarSpeedCommand(CoreCommon.OperationalStopSpeed), new List<int>());
				nextBehavior.Decorators = TurnDecorators.NoDecorators;

				// check if complete
				if (complete)
				{
					// stay in lane
					List<ArbiterLaneId> aprioriLanes = new List<ArbiterLaneId>();
					aprioriLanes.Add(targetLane.LaneId);
					return new Maneuver(nextBehavior, new StayInLaneState(targetLane), null, null, aprioriLanes, true);
				}
				// otherwise keep same
				else
				{
					// set abort behavior
					((StayInLaneBehavior)nextBehavior).SpeedCommand = new ScalarSpeedCommand(0.0);

					// maneuver
					return new Maneuver(uts.DefaultBehavior, uts, nextBehavior, new StayInLaneState(targetLane));
				}
			}

			#endregion*/

			#endregion

			#region Unknown

			// unknown state
			else
			{
				throw new Exception("Unknown Travel State type: planningState: " + planningState.ToString() + "\n with type: " + planningState.GetType().ToString());
			}

			#endregion
		}
        /// <summary>
        /// Makes new parameterization for nav
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="lanePlan"></param>
        /// <param name="speed"></param>
        /// <param name="distance"></param>
        /// <param name="stopType"></param>
        /// <returns></returns>
        public TravelingParameters NavStopParameterization(IFQMPlanable lane, RoadPlan roadPlan, double speed, double distance, 
            ArbiterWaypoint stopWaypoint, StopType stopType, VehicleState state)
        {
            // get min dist
            double distanceCutOff = stopType == StopType.StopLine ? CoreCommon.OperationslStopLineSearchDistance : CoreCommon.OperationalStopDistance;

            #region Get Decorators

            // turn direction default
            ArbiterTurnDirection atd = ArbiterTurnDirection.Straight;
            List<BehaviorDecorator> decorators = TurnDecorators.NoDecorators;

            // check if need decorators
            if (lane is ArbiterLane &&
                stopWaypoint.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest) &&
                roadPlan.BestPlan.laneWaypointOfInterest.IsExit &&
                distance < 40.0)
            {
                if (roadPlan.BestPlan.laneWaypointOfInterest.BestExit == null)
                    ArbiterOutput.Output("NAV BUG: lanePlan.laneWaypointOfInterest.BestExit: FQM NavStopParameterization");
                else
                {
                    switch (roadPlan.BestPlan.laneWaypointOfInterest.BestExit.TurnDirection)
                    {
                        case ArbiterTurnDirection.Left:
                            decorators = TurnDecorators.LeftTurnDecorator;
                            atd = ArbiterTurnDirection.Left;
                            break;
                        case ArbiterTurnDirection.Right:
                            atd = ArbiterTurnDirection.Right;
                            decorators = TurnDecorators.RightTurnDecorator;
                            break;
                        case ArbiterTurnDirection.Straight:
                            atd = ArbiterTurnDirection.Straight;
                            decorators = TurnDecorators.NoDecorators;
                            break;
                        case ArbiterTurnDirection.UTurn:
                            atd = ArbiterTurnDirection.UTurn;
                            decorators = TurnDecorators.LeftTurnDecorator;
                            break;
                    }
                }
            }
            else if (lane is SupraLane)
            {
                SupraLane sl = (SupraLane)lane;
                double distToInterconnect = sl.DistanceBetween(state.Front, sl.Interconnect.InitialGeneric.Position);

                if ((distToInterconnect > 0 && distToInterconnect < 40.0) || sl.ClosestComponent(state.Front) == SLComponentType.Interconnect)
                {
                    switch (sl.Interconnect.TurnDirection)
                    {
                        case ArbiterTurnDirection.Left:
                            decorators = TurnDecorators.LeftTurnDecorator;
                            atd = ArbiterTurnDirection.Left;
                            break;
                        case ArbiterTurnDirection.Right:
                            atd = ArbiterTurnDirection.Right;
                            decorators = TurnDecorators.RightTurnDecorator;
                            break;
                        case ArbiterTurnDirection.Straight:
                            atd = ArbiterTurnDirection.Straight;
                            decorators = TurnDecorators.NoDecorators;
                            break;
                        case ArbiterTurnDirection.UTurn:
                            atd = ArbiterTurnDirection.UTurn;
                            decorators = TurnDecorators.LeftTurnDecorator;
                            break;
                    }
                }
            }

            #endregion

            #region Get Maneuver

            Maneuver m = new Maneuver();
            bool usingSpeed = true;
            SpeedCommand sc = new StopAtDistSpeedCommand(distance);

            #region Distance Cutoff

            // check if distance is less than cutoff
            if (distance < distanceCutOff && stopType != StopType.EndOfLane)
            {
                // default behavior
                Behavior b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtDistSpeedCommand(distance), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true));

                // stopping so not using speed param
                usingSpeed = false;

                // exit is next
                if (stopType == StopType.Exit)
                {
                    // exit means stopping at a good exit in our current lane
                    IState nextState = new StoppingAtExitState(stopWaypoint.Lane, stopWaypoint, atd, true, roadPlan.BestPlan.laneWaypointOfInterest.BestExit, state.Timestamp, state.Front);
                    m = new Maneuver(b, nextState, decorators, state.Timestamp);
                }

                // stop line is left
                else if (stopType == StopType.StopLine)
                {
                    // determine if hte stop line is the best exit
                    bool isNavExit = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Equals(stopWaypoint);

                    // get turn direction
                    atd = isNavExit ? atd : ArbiterTurnDirection.Straight;

                    // predetermine interconnect if best exit
                    ArbiterInterconnect desired = null;
                    if (isNavExit)
                        desired = roadPlan.BestPlan.laneWaypointOfInterest.BestExit;
                    else if (stopWaypoint.NextPartition != null && state.Front.DistanceTo(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > 25)
                        desired = stopWaypoint.NextPartition.ToInterconnect;

                    // set decorators
                    decorators = isNavExit ? decorators : TurnDecorators.NoDecorators;

                    // stop at the stop
                    IState nextState = new StoppingAtStopState(stopWaypoint.Lane, stopWaypoint, atd, isNavExit, desired);
                    b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtLineSpeedCommand(), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true));
                    m = new Maneuver(b, nextState, decorators, state.Timestamp);
                    sc = new StopAtLineSpeedCommand();
                }
                else if(stopType == StopType.LastGoal)
                {
                    // stop at the last goal
                    IState nextState = new StayInLaneState(stopWaypoint.Lane, CoreCommon.CorePlanningState);
                    m = new Maneuver(b, nextState, decorators, state.Timestamp);
                }
            }

            #endregion

            #region Outisde Distance Envelope

            // not inside distance envalope
            else
            {
                // set speed
                sc = new ScalarSpeedCommand(speed);

                // check if lane
                if (lane is ArbiterLane)
                {
                    // get lane
                    ArbiterLane al = (ArbiterLane)lane;

                    // default behavior
                    Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));

                    // standard behavior is fine for maneuver
                    m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), decorators, state.Timestamp);
                }
                // check if supra lane
                else if (lane is SupraLane)
                {
                    // get lane
                    SupraLane sl = (SupraLane)lane;

                    // get sl state
                    StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;

                    // get default beheavior
                    Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List<int>());

                    // standard behavior is fine for maneuver
                    m = new Maneuver(b, sisls, decorators, state.Timestamp);
                }
            }

            #endregion

            #endregion

            #region Parameterize

            // create new params
            TravelingParameters tp = new TravelingParameters();
            tp.Behavior = m.PrimaryBehavior;
            tp.Decorators = m.PrimaryBehavior.Decorators;
            tp.DistanceToGo = distance;
            tp.NextState = m.PrimaryState;
            tp.RecommendedSpeed = speed;
            tp.Type = TravellingType.Navigation;
            tp.UsingSpeed = usingSpeed;
            tp.SpeedCommand = sc;
            tp.VehiclesToIgnore = new List<int>();

            // return navigation params
            return tp;

            #endregion
        }
        /// <summary>
        /// Makes use of parameterizations made from the initial forward maneuver plan
        /// </summary>
        /// <param name="arbiterLane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="roadPlan"></param>
        /// <param name="blockages"></param>
        /// <param name="ignorable"></param>
        /// <returns></returns>
        public Maneuver? SecondaryManeuver(IFQMPlanable arbiterLane, VehicleState vehicleState, RoadPlan roadPlan,
            List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
        {
            // check if we might be able to pass here
            bool validArea = arbiterLane is ArbiterLane || (((SupraLane)arbiterLane).ClosestComponent(vehicleState.Front) == SLComponentType.Initial);
            ArbiterLane ourForwardLane = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial;

            // check if the forward vehicle exists and we're in a valid area
            if (this.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker && validArea)
            {
                // check if we should pass the vehicle ahead
                LaneChangeInformation lci;
                bool sp = this.ForwardMonitor.ForwardVehicle.ShouldPass(out lci);

                // make sure we should do something before processing extras
                if(sp)
                {
                    // available parameterizations for the lane change
                    List<LaneChangeParameters> changeParams = new List<LaneChangeParameters>();

                    // get lane
                    ArbiterLane al = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial;

                    // get the location we need to return by
                    Coordinates absoluteUpperBound = arbiterLane is ArbiterLane ?
                        roadPlan.LanePlans[al.LaneId].laneWaypointOfInterest.PointOfInterest.Position :
                        ((SupraLane)arbiterLane).Interconnect.InitialGeneric.Position;

                    #region Failed Forward

                    // if failed, parameterize ourselved if we're following them
                    if (lci.Reason == LaneChangeReason.FailedForwardVehicle && this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle)
                    {
                        // notify
                        ArbiterOutput.Output("Failed Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.VehicleId.ToString());

                        // get traveling params from FQM to make sure we stopped for vehicle, behind vehicle
                        double v = CoreCommon.Communications.GetVehicleSpeed().Value;
                        TravelingParameters fqmParams = this.ForwardMonitor.CurrentParameters;
                        double d = this.ForwardMonitor.ForwardVehicle.DistanceToVehicle(arbiterLane, vehicleState.Front);
                        Coordinates departUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d - 3.0).Location;

                        // check stopped behing failed forward
                        try
                        {
                            if (fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
                            {
                                // check for checkpoint within 4VL of front of failed vehicle
                                ArbiterCheckpoint acCurrecnt = CoreCommon.Mission.MissionCheckpoints.Peek();
                                if (acCurrecnt.WaypointId.AreaSubtypeId.Equals(al.LaneId))
                                {
                                    // check distance
                                    ArbiterWaypoint awCheckpoint = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[acCurrecnt.WaypointId];
                                    double cpDistacne = Lane.DistanceBetween(vehicleState.Front, awCheckpoint.Position);
                                    if (cpDistacne < d || cpDistacne - d < TahoeParams.VL * 4.5)
                                    {
                                        ArbiterOutput.Output("Removing checkpoint: " + acCurrecnt.WaypointId.ToString() + " as failed vehicle over it");
                                        CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                        return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                                    }
                                }
                            }
                        }catch (Exception) { }

                        #region Right Lateral Reasoning Forwards

                        // check right lateral reasoning for existence, if so parametrize
                        if (rightLateralReasoning.Exists && fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
                        {
                            // get lane
                            ArbiterLane lane = al;

                            // determine failed vehicle lane change distance params
                            Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location;
                            Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.0)).Location;
                            Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location;

                            // get params for lane change
                            LaneChangeParameters? lcp = this.LaneChangeParameterization(
                                new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
                                lane, lane.LaneOnRight, false, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
                                departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable,
                                vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);

                            // check if exists to generate full param
                            if (lcp.HasValue)
                            {
                                // get param
                                LaneChangeParameters tp = lcp.Value;

                                // notify
                                ArbiterOutput.WriteToLog("Failed Forward: Right Lateral Reasoning Forwards: Available: " + tp.Available.ToString() + ", Feasable: " + tp.Feasible.ToString());

                                // get behavior
                                ChangeLaneBehavior clb = new ChangeLaneBehavior(
                                    al.LaneId, al.LaneOnRight.LaneId, false, al.DistanceBetween(vehicleState.Front, departUpperBound),
                                    new ScalarSpeedCommand(tp.Parameters.RecommendedSpeed), tp.Parameters.VehiclesToIgnore,
                                    al.LanePath(), al.LaneOnRight.LanePath(), al.Width, al.LaneOnRight.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
                                tp.Behavior = clb;
                                tp.Decorators = TurnDecorators.RightTurnDecorator;

                                // next state
                                ChangeLanesState cls = new ChangeLanesState(tp);
                                tp.NextState = cls;

                                // add parameterization to possible
                                changeParams.Add(tp);
                            }
                        }

                        #endregion

                        #region Left Lateral Reasoning

                        // check left lateral reasoning
                        if(leftLateralReasoning.Exists)
                        {
                            #region Left Lateral Opposing

                            // check opposing
                            ArbiterLane closestOpposingLane = this.GetClosestOpposing(ourForwardLane, vehicleState);
                            if(closestOpposingLane != null && (leftLateralReasoning.IsOpposing || !closestOpposingLane.Equals(leftLateralReasoning.LateralLane)))
                            {
                                // check room of initial
                                bool enoughRoom =
                                    (arbiterLane is ArbiterLane && (!roadPlan.BestPlan.laneWaypointOfInterest.IsExit || ((ArbiterLane)arbiterLane).DistanceBetween(vehicleState.Front, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > TahoeParams.VL * 5.0)) ||
                                    (arbiterLane is SupraLane && ((SupraLane)arbiterLane).DistanceBetween(vehicleState.Front, ((SupraLane)arbiterLane).Interconnect.InitialGeneric.Position) > TahoeParams.VL * 5.0);

                                // check opposing enough room
                                bool oppEnough = closestOpposingLane.DistanceBetween(closestOpposingLane.LanePath().StartPoint.Location, vehicleState.Front) > TahoeParams.VL * 5.0;

                                // check if enough room
                                if (enoughRoom && oppEnough)
                                {
                                    // check if we're stopped and the current trav params were for a vehicle and we're close to the vehicle
                                    bool stoppedBehindFV = fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle;

                                    // check that we're stopped behind forward vehicle before attempting to change lanes
                                    if (stoppedBehindFV)
                                    {
                                        #region Check Segment Blockage

                                        // check need to make uturn (hack)
                                        bool waitForUTurnCooldown;
                                        BlockageTactical bt = CoreCommon.BlockageDirector;
                                        StayInLaneBehavior tmpBlockBehavior = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(2.0), new List<int>(), al.LanePath(), al.Width, 0, 0);
                                        ITacticalBlockage itbTmp = new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, TahoeParams.VL, BlockageType.Static, -1, false, tmpBlockBehavior.GetType()));
                                        Maneuver tmpBlockManeuver = bt.LaneRecoveryManeuver(al, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value, roadPlan,
                                            new BlockageRecoveryState(tmpBlockBehavior,
                                            new StayInLaneState(al, CoreCommon.CorePlanningState), new StayInLaneState(al, CoreCommon.CorePlanningState), BlockageRecoveryDEFCON.REVERSE,
                                            new EncounteredBlockageState(itbTmp, CoreCommon.CorePlanningState, BlockageRecoveryDEFCON.REVERSE, SAUDILevel.None), BlockageRecoverySTATUS.ENCOUNTERED), true, out waitForUTurnCooldown);
                                        if (!waitForUTurnCooldown && tmpBlockManeuver.PrimaryBehavior is UTurnBehavior)
                                            return tmpBlockManeuver;
                                        else if (waitForUTurnCooldown)
                                            return null;

                                        #endregion

                                        // distance to forward vehicle too small
                                        double distToForwards = al.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
                                        double distToReverse = Math.Max(1.0, 8.0 - distToForwards);
                                        if (distToForwards < 8.0)
                                        {
                                            // notify
                                            ArbiterOutput.WriteToLog("Secondary: NOT Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " distance: " + distToForwards.ToString("f2"));

                                            this.RearMonitor = new RearQuadrantMonitor(al, SideObstacleSide.Driver);
                                            this.RearMonitor.Update(vehicleState);
                                            if (this.RearMonitor.CurrentVehicle != null)
                                            {
                                                double distToRearVehicle = al.DistanceBetween(this.RearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Position) - TahoeParams.RL;
                                                double distNeedClear = distToReverse + 2.0;
                                                if (distToRearVehicle < distNeedClear)
                                                {
                                                    // notify
                                                    ArbiterOutput.Output("Secondary: Rear: Not enough room to clear in rear: " + distToRearVehicle.ToString("f2") + " < " + distNeedClear.ToString("f2"));
                                                    return null;
                                                }
                                            }

                                            double distToLaneStart = al.DistanceBetween(al.LanePath().StartPoint.Location, vehicleState.Position) - TahoeParams.RL;
                                            if (distToReverse > distToLaneStart)
                                            {
                                                // notify
                                                ArbiterOutput.Output("Secondary: Rear: Not enough room in lane to reverse in rear: " + distToLaneStart.ToString("f2") + " < " + distToReverse.ToString("f2"));
                                                return null;
                                            }
                                            else
                                            {
                                                // notify
                                                ArbiterOutput.Output("Secondary: Reversing to pass Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " reversing distance: " + distToReverse.ToString("f2"));
                                                StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(distToReverse, true);
                                                StayInLaneBehavior silb = new StayInLaneBehavior(al.LaneId, sadsc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
                                                return new Maneuver(silb, CoreCommon.CorePlanningState, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
                                            }
                                        }
                                        else
                                        {
                                            // notify
                                            ArbiterOutput.WriteToLog("Secondary: Left Lateral Opposing: Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString());

                                            // determine failed vehicle lane change distance params
                                            Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location;
                                            Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.5)).Location;
                                            Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location;

                                            // check if enough room
                                            if (al.DistanceBetween(vehicleState.Front, defaultReturnUpperBound) >= d + TahoeParams.VL * 4.5)
                                            {
                                                // get hte closest oppoing
                                                ArbiterLane closestOpposing = this.GetClosestOpposing(al, vehicleState);

                                                // check exists
                                                if (closestOpposing != null)
                                                {
                                                    // set/check secondary
                                                    if (this.secondaryLeftLateralReasoning == null || !this.secondaryLeftLateralReasoning.LateralLane.Equals(closestOpposing))
                                                        this.secondaryLeftLateralReasoning = new OpposingLateralReasoning(closestOpposing, SideObstacleSide.Driver);

                                                    // check the state of hte lanes next to us
                                                    if (this.leftLateralReasoning.LateralLane.Equals(closestOpposing) && this.leftLateralReasoning.ExistsExactlyHere(vehicleState))
                                                    {
                                                        #region Plan

                                                        // need to make sure that we wait for 3 seconds with the blinker on (resetting with pause)
                                                        if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds > 3000)
                                                        {
                                                            // notify
                                                            ArbiterOutput.Output("Scondary: Left Lateral Opposing: Wait Timer DONE");

                                                            // get parameterization
                                                            LaneChangeParameters? tp = this.LaneChangeParameterization(
                                                                new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.ForwardMonitor.ForwardVehicle.CurrentVehicle),
                                                                al,
                                                                leftLateralReasoning.LateralLane,
                                                                true,
                                                                roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
                                                                departUpperBound,
                                                                defaultReturnLowerBound,
                                                                minimumReturnComplete,
                                                                defaultReturnUpperBound,
                                                                blockages,
                                                                ignorable,
                                                                vehicleState,
                                                                CoreCommon.Communications.GetVehicleSpeed().Value);

                                                            // check if available and feasible
                                                            if (tp.HasValue && tp.Value.Available && tp.Value.Feasible)
                                                            {
                                                                // notify
                                                                ArbiterOutput.Output("Scondary: Left Lateral Opposing: AVAILABLE & FEASIBLE");

                                                                LaneChangeParameters lcp = tp.Value;
                                                                lcp.Behavior = this.ForwardMonitor.CurrentParameters.Behavior;
                                                                lcp.Decorators = TurnDecorators.LeftTurnDecorator;
                                                                lcp.Behavior.Decorators = lcp.Decorators;

                                                                // next state
                                                                ChangeLanesState cls = new ChangeLanesState(tp.Value);
                                                                lcp.NextState = cls;

                                                                // add parameterization to possible
                                                                changeParams.Add(lcp);
                                                            }
                                                            // check if not available now but still feasible
                                                            else if (tp.HasValue && !tp.Value.Available && tp.Value.Feasible)
                                                            {
                                                                // notify
                                                                ArbiterOutput.Output("Scondary: Left Lateral Opposing: NOT Available, Still FEASIBLE, WAITING");

                                                                // wait and blink maneuver
                                                                TravelingParameters tp2 = this.ForwardMonitor.CurrentParameters;
                                                                tp2.Decorators = TurnDecorators.LeftTurnDecorator;
                                                                tp2.Behavior.Decorators = tp2.Decorators;

                                                                // create parameterization
                                                                LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, al.LaneOnLeft,
                                                                    true, true, tp2.Behavior, 0.0, CoreCommon.CorePlanningState, tp2.Decorators, tp2, new Coordinates(),
                                                                    new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle);

                                                                // add parameterization to possible
                                                                changeParams.Add(lcp);
                                                            }
                                                        }
                                                        // otherwise timer not running or not been long enough
                                                        else
                                                        {
                                                            // check if timer running
                                                            if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.IsRunning)
                                                                this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Start();

                                                            double waited = (double)(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds / 1000.0);
                                                            ArbiterOutput.Output("Waited for failed forwards: " + waited.ToString("F2") + " seconds");

                                                            // wait and blink maneuver
                                                            TravelingParameters tp = this.ForwardMonitor.CurrentParameters;
                                                            tp.Decorators = TurnDecorators.LeftTurnDecorator;
                                                            tp.Behavior.Decorators = tp.Decorators;

                                                            // create parameterization
                                                            LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, al.LaneOnLeft,
                                                                true, true, tp.Behavior, 0.0, CoreCommon.CorePlanningState, tp.Decorators, tp, new Coordinates(),
                                                                new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle);

                                                            // add parameterization to possible
                                                            changeParams.Add(lcp);
                                                        }

                                                        #endregion
                                                    }
                                                    else if (!this.leftLateralReasoning.LateralLane.Equals(closestOpposing) && !this.leftLateralReasoning.ExistsRelativelyHere(vehicleState))
                                                    {
                                                        // set and notify
                                                        ArbiterOutput.Output("superceeded left lateral reasoning with override for non adjacent left lateral reasoning");
                                                        ILateralReasoning tmpReasoning = this.leftLateralReasoning;
                                                        this.leftLateralReasoning = this.secondaryLeftLateralReasoning;

                                                        try
                                                        {
                                                            #region Plan

                                                            // need to make sure that we wait for 3 seconds with the blinker on (resetting with pause)
                                                            if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds > 3000)
                                                            {
                                                                // notify
                                                                ArbiterOutput.Output("Scondary: Left Lateral Opposing: Wait Timer DONE");

                                                                // get parameterization
                                                                LaneChangeParameters? tp = this.LaneChangeParameterization(
                                                                    new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.ForwardMonitor.ForwardVehicle.CurrentVehicle),
                                                                    al,
                                                                    leftLateralReasoning.LateralLane,
                                                                    true,
                                                                    roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
                                                                    departUpperBound,
                                                                    defaultReturnLowerBound,
                                                                    minimumReturnComplete,
                                                                    defaultReturnUpperBound,
                                                                    blockages,
                                                                    ignorable,
                                                                    vehicleState,
                                                                    CoreCommon.Communications.GetVehicleSpeed().Value);

                                                                // check if available and feasible
                                                                if (tp.HasValue && tp.Value.Available && tp.Value.Feasible)
                                                                {
                                                                    // notify
                                                                    ArbiterOutput.Output("Scondary: Left Lateral Opposing: AVAILABLE & FEASIBLE");

                                                                    LaneChangeParameters lcp = tp.Value;
                                                                    lcp.Behavior = this.ForwardMonitor.CurrentParameters.Behavior;
                                                                    lcp.Decorators = TurnDecorators.LeftTurnDecorator;
                                                                    lcp.Behavior.Decorators = TurnDecorators.LeftTurnDecorator;

                                                                    // next state
                                                                    ChangeLanesState cls = new ChangeLanesState(tp.Value);
                                                                    lcp.NextState = cls;

                                                                    // add parameterization to possible
                                                                    changeParams.Add(lcp);
                                                                }
                                                                // check if not available now but still feasible
                                                                else if (tp.HasValue && !tp.Value.Available && tp.Value.Feasible)
                                                                {
                                                                    // notify
                                                                    ArbiterOutput.Output("Scondary: Left Lateral Opposing: NOT Available, Still FEASIBLE, WAITING");

                                                                    // wait and blink maneuver
                                                                    TravelingParameters tp2 = this.ForwardMonitor.CurrentParameters;
                                                                    tp2.Decorators = TurnDecorators.LeftTurnDecorator;
                                                                    tp2.Behavior.Decorators = tp2.Decorators;

                                                                    // create parameterization
                                                                    LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, this.leftLateralReasoning.LateralLane,
                                                                        true, true, tp2.Behavior, 0.0, CoreCommon.CorePlanningState, tp2.Decorators, tp2, new Coordinates(),
                                                                        new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle);

                                                                    // add parameterization to possible
                                                                    changeParams.Add(lcp);
                                                                }
                                                            }
                                                            // otherwise timer not running or not been long enough
                                                            else
                                                            {
                                                                // check if timer running
                                                                if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.IsRunning)
                                                                    this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Start();

                                                                double waited = (double)(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds / 1000.0);
                                                                ArbiterOutput.Output("Waited for failed forwards: " + waited.ToString("F2") + " seconds");

                                                                // wait and blink maneuver
                                                                TravelingParameters tp = this.ForwardMonitor.CurrentParameters;
                                                                tp.Decorators = TurnDecorators.LeftTurnDecorator;
                                                                tp.Behavior.Decorators = tp.Decorators;

                                                                // create parameterization
                                                                LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, this.leftLateralReasoning.LateralLane,
                                                                    true, true, tp.Behavior, 0.0, CoreCommon.CorePlanningState, tp.Decorators, tp, new Coordinates(),
                                                                    new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle);

                                                                // add parameterization to possible
                                                                changeParams.Add(lcp);
                                                            }

                                                            #endregion
                                                        }
                                                        catch (Exception ex)
                                                        {
                                                            ArbiterOutput.Output("Core intelligence thread caught exception in forward reasoning secondary maneuver when non-standard adjacent left: " + ex.ToString());
                                                        }

                                                        // restore
                                                        this.leftLateralReasoning = tmpReasoning;
                                                    }
                                                }
                                                else
                                                {
                                                    // do nuttin
                                                    ArbiterOutput.Output("no opposing adjacent");
                                                }
                                            }
                                            else
                                            {
                                                // notify
                                                ArbiterOutput.Output("Secondary: LeftLatOpp: Stopped Behind FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", but not enough room to pass");
                                            }
                                        }
                                    }
                                    else
                                    {
                                        this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop();
                                        this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset();

                                        // notify
                                        ArbiterOutput.Output("Secondary: Left Lateral Opposing: NOT Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString());
                                    }
                                }
                                else
                                {
                                    ArbiterOutput.Output("Secondary Opposing: enough room to pass opposing: initial: " + enoughRoom.ToString() + ", opposing: " + oppEnough.ToString());
                                }
                            }

                            #endregion

                            #region Left Lateral Forwards

                            // otherwise parameterize
                            else if(fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
                            {
                                // get lane
                                ArbiterLane lane = al;

                                // determine failed vehicle lane change distance params
                                Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location;
                                Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.0)).Location;
                                Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location;

                                // get params for lane change
                                LaneChangeParameters? lcp = this.LaneChangeParameterization(
                                    new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
                                    lane, lane.LaneOnLeft, false, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
                                    departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound,
                                    blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);

                                // check if exists to generate full param
                                if (lcp.HasValue)
                                {
                                    // set param
                                    LaneChangeParameters tp = lcp.Value;

                                    // notify
                                    ArbiterOutput.Output("Secondary Failed Forward Reasoning Forwards: Available: " + tp.Available.ToString() + ", Feasible: " + tp.Feasible.ToString());

                                    // get behavior
                                    ChangeLaneBehavior clb = new ChangeLaneBehavior(
                                        al.LaneId, al.LaneOnLeft.LaneId, true, al.DistanceBetween(vehicleState.Front, departUpperBound),
                                        new ScalarSpeedCommand(tp.Parameters.RecommendedSpeed), tp.Parameters.VehiclesToIgnore,
                                        al.LanePath(), al.LaneOnLeft.LanePath(), al.Width, al.LaneOnLeft.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
                                    tp.Behavior = clb;
                                    tp.Decorators = TurnDecorators.LeftTurnDecorator;

                                    // next state
                                    ChangeLanesState cls = new ChangeLanesState(tp);
                                    tp.NextState = cls;

                                    // add parameterization to possible
                                    changeParams.Add(tp);
                                }
                            }

                            #endregion
                        }

                        #endregion
                    }

                    #endregion

                    #region Slow Forward

                    // if pass, determine if should pass in terms or vehicles adjacent and in front then call lane change function for maneuver
                    else if (lci.Reason == LaneChangeReason.SlowForwardVehicle)
                    {
                        // if left exists and is not opposing, parameterize
                        if (leftLateralReasoning.Exists && !leftLateralReasoning.IsOpposing)
                        {
                            throw new Exception("slow forward vehicle pass not implemented yet");
                        }

                        // if right exists and is not opposing, parameterize
                        if (rightLateralReasoning.Exists && !rightLateralReasoning.IsOpposing)
                        {
                            throw new Exception("slow forward vehicle pass not implemented yet");
                        }
                    }

                    #endregion

                    #region Parameterize

                    // check params to see if any are good and available
                    if(changeParams != null && changeParams.Count > 0)
                    {
                        // sort the parameterizations
                        changeParams.Sort();

                        // get first
                        LaneChangeParameters final = changeParams[0];

                        // notify
                        ArbiterOutput.Output("Secondary Reasoning Final: Available: " + final.Available.ToString() + ", Feasible: " + final.Feasible.ToString());

                        // make sure ok
                        if (final.Available && final.Feasible)
                        {
                            // return best param
                            return new Maneuver(changeParams[0].Behavior, changeParams[0].NextState, changeParams[0].Decorators, vehicleState.Timestamp);
                        }
                    }
                    #endregion
                }
            }

            // fallout is null
            return null;
        }
        /// <summary>
        /// Secondary maneuver when current lane is the desired lane
        /// </summary>
        /// <param name="arbiterLane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="roadPlan"></param>
        /// <param name="blockages"></param>
        /// <param name="ignorable"></param>
        /// <returns></returns>
        public Maneuver? AdvancedSecondaryNextCheck(ArbiterLane lane, ArbiterWaypoint laneGoal, VehicleState vehicleState, RoadPlan roadPlan,
            List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
        {
            // check if the forward vehicle exists
            if (this.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.PassedLongDelayedBirth)
            {
                // distance to forward vehicle
                double distanceToForwardVehicle = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);

                #region Distance From Forward Not Enough

                // check distance to forward vehicle
                if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed &&
                    distanceToForwardVehicle < 8.0 && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
                {
                    // set name
                    ArbiterLane al = lane;

                    // distance to forward vehicle too small
                    double distToForwards = al.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
                    double distToReverse = Math.Max(1.0, 8.0 - distToForwards);
                    if (distToForwards < 8.0)
                    {
                        // notify
                        ArbiterOutput.Output("Secondary: NOT Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " distance: " + distToForwards.ToString("f2"));

                        this.RearMonitor = new RearQuadrantMonitor(al, SideObstacleSide.Driver);
                        this.RearMonitor.Update(vehicleState);
                        if (this.RearMonitor.CurrentVehicle != null)
                        {
                            double distToRearVehicle = al.DistanceBetween(this.RearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Position) - TahoeParams.RL;
                            double distNeedClear = distToReverse + 2.0;
                            if (distToRearVehicle < distNeedClear)
                            {
                                // notify
                                ArbiterOutput.Output("Secondary: Rear: Not enough room to clear in rear: " + distToRearVehicle.ToString("f2") + " < " + distNeedClear.ToString("f2"));
                                return null;
                            }
                        }

                        double distToLaneStart = al.DistanceBetween(al.LanePath().StartPoint.Location, vehicleState.Position) - TahoeParams.RL;
                        if (distToReverse > distToLaneStart)
                        {
                            // notify
                            ArbiterOutput.Output("Secondary: Rear: Not enough room in lane to reverse in rear: " + distToLaneStart.ToString("f2") + " < " + distToReverse.ToString("f2"));
                            return null;
                        }
                        else
                        {
                            // notify
                            ArbiterOutput.Output("Secondary: Reversing to pass Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " reversing distance: " + distToReverse.ToString("f2"));
                            StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(distToReverse, true);
                            StayInLaneBehavior silb = new StayInLaneBehavior(al.LaneId, sadsc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
                            return new Maneuver(silb, CoreCommon.CorePlanningState, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
                        }
                    }
                }

                #endregion

                // get distance to next lane major
                List<WaypointType> wts = new List<WaypointType>(new WaypointType[] { WaypointType.Stop, WaypointType.End });
                ArbiterWaypoint nextWaypoint = lane.GetNext(lane.GetClosestPartition(vehicleState.Position).Final, wts, new List<ArbiterWaypoint>());

                // check if the vehicle occurs before the next major thing
                double distanceToNextMajor = lane.DistanceBetween(vehicleState.Front, nextWaypoint.Position);

                // check if vehicle occurs before the next lane major
                if (distanceToForwardVehicle < distanceToNextMajor)
                {
                    // check if forward vehicle exists in this lane and is closer then the lane goal
                    if (TacticalDirector.VehicleAreas.ContainsKey(lane) &&
                        TacticalDirector.VehicleAreas[lane].Contains(this.ForwardMonitor.ForwardVehicle.CurrentVehicle) &&
                        lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) < lane.DistanceBetween(vehicleState.Front, laneGoal.Position))
                    {
                        // some constants
                        double desiredDistance = 50.0;

                        // get the lane goal distance
                        double distanceToLaneGoal = lane.DistanceBetween(vehicleState.Front, laneGoal.Position);

                        // check if necessary to be in current lane
                        if (distanceToLaneGoal <= desiredDistance)
                        {
                            // default secondary maneuver
                            ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal < 50: " + desiredDistance.ToString("f2"));
                            return this.SecondaryManeuver(lane, vehicleState, roadPlan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), bestTask);
                        }
                        // otherwise would be good but not necessary
                        else
                        {
                            // return the maneuver from the vehicle being outside the min cap
                            ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal > 50: " + desiredDistance.ToString("f2"));
                            return this.AdvancedSecondaryOutsideMinCap(lane, laneGoal, vehicleState, roadPlan, blockages, ignorable, bestTask);
                        }
                    }
                }
            }

            // no secondary
            return null;
        }
 /// <summary>
 /// Secondary maneuver inside our minimum cap
 /// </summary>
 /// <param name="arbiterLane"></param>
 /// <param name="vehicleState"></param>
 /// <param name="roadPlan"></param>
 /// <param name="blockages"></param>
 /// <param name="ignorable"></param>
 /// <returns></returns>
 public Maneuver? AdvancedSecondaryInsideMinCap(ArbiterLane lane, ArbiterWaypoint laneGoal, VehicleState vehicleState, RoadPlan roadPlan,
     List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
 {
     return null;
 }
        /// <summary>
        /// Secondary maneuver when current lane is the desired lane
        /// </summary>
        /// <param name="arbiterLane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="roadPlan"></param>
        /// <param name="blockages"></param>
        /// <param name="ignorable"></param>
        /// <returns></returns>
        public Maneuver? AdvancedSecondary(IFQMPlanable arbiterLane, VehicleState vehicleState, RoadPlan roadPlan,
            List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
        {
            // check if we might be able to pass here
            bool validArea = arbiterLane is ArbiterLane || (((SupraLane)arbiterLane).ClosestComponent(vehicleState.Front) == SLComponentType.Initial);

            // check normal valid area
            if (validArea)
            {
                // get lane we are in
                ArbiterLane ourForwardLane = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial;

                // check sl
                if (arbiterLane is SupraLane)
                {
                    Dictionary<ArbiterLaneId, LanePlan> tmpPlans = new Dictionary<ArbiterLaneId,LanePlan>();
                    tmpPlans.Add(ourForwardLane.LaneId, roadPlan.LanePlans[ourForwardLane.LaneId]);
                    RoadPlan tmp = new RoadPlan(tmpPlans);
                    roadPlan = tmp;
                }

                // return the advanced plan
                ArbiterWaypoint goalWp = arbiterLane is SupraLane ? (ArbiterWaypoint)((SupraLane)arbiterLane).Interconnect.InitialGeneric : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest;
                return this.AdvancedSecondaryNextCheck(ourForwardLane, goalWp, vehicleState, roadPlan, blockages, ignorable, bestTask);
            }

            // fall through
            return null;
        }
        /// <summary>
        /// Plan a lane change
        /// </summary>
        /// <param name="cls"></param>
        /// <param name="initialManeuver"></param>
        /// <param name="targetManeuver"></param>
        /// <returns></returns>
        public Maneuver PlanLaneChange(ChangeLanesState cls, VehicleState vehicleState, RoadPlan roadPlan, 
            List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable)
        {
            // check blockages
            if (blockages != null && blockages.Count > 0 && blockages[0] is LaneChangeBlockage)
            {
                // create the blockage state
                EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);

                // go to a blockage handling tactical
                return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
            }

            // lanes of the lane change
            ArbiterLane initial = cls.Parameters.Initial;
            ArbiterLane target = cls.Parameters.Target;

            #region Initial Forwards

            if (!cls.Parameters.InitialOncoming)
            {
                ForwardReasoning initialReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), initial);

                #region Target Forwards

                if (!cls.Parameters.TargetOncoming)
                {
                    // target reasoning
                    ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target);

                    #region Navigation

                    if (cls.Parameters.Reason == LaneChangeReason.Navigation)
                    {
                        // parameters to follow
                        List<TravelingParameters> tps = new List<TravelingParameters>();

                        // vehicles to ignore
                        List<int> ignorableVehicles = new List<int>();

                        // params for forward lane
                        initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
                        TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
                            CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
                            initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
                            vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle);

                        ArbiterOutput.Output("initial dist to go: " + initialParams.DistanceToGo.ToString("f3"));

                        if (initialParams.Type == TravellingType.Vehicle && !initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
                        {
                            tps.Add(initialParams);
                        }
                        else
                            tps.Add(initialReasoning.ForwardMonitor.NavigationParameters);

                        ignorableVehicles.AddRange(initialParams.VehiclesToIgnore);

                        // get params for the final lane
                        targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List<ArbiterWaypoint>());
                        TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters;
                        tps.Add(targetParams);
                        ignorableVehicles.AddRange(targetParams.VehiclesToIgnore);

                        try
                        {
                            if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.1 &&
                                targetParams.Type == TravellingType.Vehicle &&
                                targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle != null &&
                                targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed)
                            {
                                return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                            }
                        }
                        catch (Exception) { }

                        ArbiterOutput.Output("target dist to go: " + targetParams.DistanceToGo.ToString("f3"));

                        // decorators
                        List<BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;

                        // distance
                        double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
                        cls.Parameters.DistanceToDepartUpperBound = distanceToGo;

                        // check if need to modify distance to go
                        if (initialParams.Type == TravellingType.Vehicle && initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
                        {
                            double curDistToUpper = cls.Parameters.DistanceToDepartUpperBound;
                            double newVhcDistToUpper = initial.DistanceBetween(vehicleState.Front, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) - 2.0;

                            if (curDistToUpper > newVhcDistToUpper)
                            {
                                distanceToGo = newVhcDistToUpper;
                            }
                        }

                        // get final
                        tps.Sort();

                        // get the proper speed command
                        ScalarSpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed);
                        if (sc.Speed < 8.84)
                            sc = new ScalarSpeedCommand(Math.Min(targetParams.RecommendedSpeed, 8.84));

                        // continue the lane change with the proper speed command
                        ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo,
                            sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
                            initial.NumberOfLanesRight(vehicleState.Front, true));

                        // standard maneuver
                        return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp);
                    }

                    #endregion

                    #region Failed Forwards

                    else if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
                    {
                        // parameters to follow
                        List<TravelingParameters> tps = new List<TravelingParameters>();

                        // vehicles to ignore
                        List<int> ignorableVehicles = new List<int>();

                        // params for forward lane
                        initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
                        TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
                            CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
                            initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
                            vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
                        tps.Add(initialParams);
                        ignorableVehicles.AddRange(initialParams.VehiclesToIgnore);

                        // get params for the final lane
                        targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List<ArbiterWaypoint>());
                        TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters;
                        tps.Add(targetParams);
                        ignorableVehicles.AddRange(targetParams.VehiclesToIgnore);

                        // decorators
                        List<BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;

                        // distance
                        double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
                        cls.Parameters.DistanceToDepartUpperBound = distanceToGo;

                        // get final
                        tps.Sort();

                        // get the proper speed command
                        SpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed);

                        // continue the lane change with the proper speed command
                        ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo,
                            sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
                            initial.NumberOfLanesRight(vehicleState.Front, true));

                        // standard maneuver
                        return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp);
                    }

                    #endregion

                    #region Slow

                    else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }

                    #endregion

                    else
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }
                }

                #endregion

                #region Target Oncoming

                else
                {
                    OpposingReasoning targetReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), target);

                    #region Failed Forward

                    if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
                    {
                        // parameters to follow
                        List<TravelingParameters> tps = new List<TravelingParameters>();

                        // ignore the forward vehicle but keep params for forward lane
                        initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
                        TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
                            CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
                            initial.WaypointList[initial.WaypointList.Count-1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
                        tps.Add(initialParams);

                        // get params for the final lane
                        targetReasoning.ForwardManeuver(target, initial, vehicleState, roadPlan, blockages);
                        TravelingParameters targetParams = targetReasoning.OpposingForwardMonitor.CurrentParamters.Value;
                        tps.Add(targetParams);

                        // decorators
                        List<BehaviorDecorator> decorators = cls.Parameters.ToLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;

                        // distance
                        double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
                        cls.Parameters.DistanceToDepartUpperBound = distanceToGo;

                        // get final
                        tps.Sort();

                        // get the proper speed command
                        SpeedCommand sc = new ScalarSpeedCommand(Math.Min(tps[0].RecommendedSpeed, 2.24));

                        // check final for stopped failed opposing
                        VehicleAgent forwardVa = targetReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle;
                        if (forwardVa != null)
                        {
                            // dist between
                            double distToFV = -targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition);

                            // check stopped
                            bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5;

                            // check distance
                            bool distOk = distToFV < 2.5 * TahoeParams.VL;

                            // check failed
                            bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed;

                            // notify
                            ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString());

                            // check all for failed
                            if (stopped && distOk && failed)
                            {
                                // check inside target
                                if (target.LanePolygon.IsInside(vehicleState.Front))
                                {
                                    // blockage recovery
                                    StayInLaneState sils = new StayInLaneState(initial, CoreCommon.CorePlanningState);
                                    StayInLaneBehavior silb = new StayInLaneBehavior(initial.LaneId, new StopAtDistSpeedCommand(TahoeParams.VL * 2.0, true), new List<int>(), initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(vehicleState.Front, false), initial.NumberOfLanesRight(vehicleState.Front, false));
                                    BlockageRecoveryState brs = new BlockageRecoveryState(silb, sils, sils, BlockageRecoveryDEFCON.REVERSE,
                                        new EncounteredBlockageState(new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, 4.0, BlockageType.Static, -1, true, silb.GetType())), sils, BlockageRecoveryDEFCON.INITIAL, SAUDILevel.None),
                                        BlockageRecoverySTATUS.EXECUTING);
                                    return new Maneuver(silb, brs, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
                                }
                                // check which lane we are in
                                else
                                {
                                    // return to forward lane
                                    return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(initial, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                                }
                            }
                        }

                        // continue the lane change with the proper speed command
                        ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo,
                            sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.ReversePath, initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
                            initial.NumberOfLanesRight(vehicleState.Front, true));

                        // standard maneuver
                        return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp);
                    }

                    #endregion

                    #region Other

                    else if (cls.Parameters.Reason == LaneChangeReason.Navigation)
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }
                    else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }
                    else
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }

                    #endregion
                }

                #endregion
            }

            #endregion

            #region Initial Oncoming

            else
            {
                OpposingReasoning initialReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), initial);

                #region Target Forwards

                if (!cls.Parameters.TargetOncoming)
                {
                    ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target);

                    if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }

                    #region Navigation

                    else if (cls.Parameters.Reason == LaneChangeReason.Navigation)
                    {
                        // parameters to follow
                        List<TravelingParameters> tps = new List<TravelingParameters>();

                        // distance to the upper bound of the change
                        double distanceToGo = target.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
                        cls.Parameters.DistanceToDepartUpperBound = distanceToGo;

                        // get params for the initial lane
                        initialReasoning.ForwardManeuver(initial, target, vehicleState, roadPlan, blockages);

                        // current params of the fqm
                        TravelingParameters initialParams = initialReasoning.OpposingForwardMonitor.CurrentParamters.Value;

                        if (initialParams.Type == TravellingType.Vehicle)
                        {
                            if(!initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
                                tps.Add(initialParams);
                            else
                            {
                                tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters);
                                distanceToGo = initial.DistanceBetween(initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, vehicleState.Front) - TahoeParams.VL;
                            }
                        }
                        else
                            tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters);

                        // get params for forward lane
                        targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, ignorable);
                        TravelingParameters targetParams = targetReasoning.ForwardMonitor.ParameterizationHelper(target, target,
                            CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
                            target.WaypointList[target.WaypointList.Count-1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
                            vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle);
                        tps.Add(targetParams);

                        // ignoring vehicles add
                        List<int> ignoreVehicles = initialParams.VehiclesToIgnore;
                        ignoreVehicles.AddRange(targetParams.VehiclesToIgnore);

                        // decorators
                        List<BehaviorDecorator> decorators = !cls.Parameters.ToLeft ? TurnDecorators.RightTurnDecorator : TurnDecorators.LeftTurnDecorator;

                        // get final
                        tps.Sort();

                        // get the proper speed command
                        SpeedCommand sc = tps[0].SpeedCommand;

                        if (sc is StopAtDistSpeedCommand)
                        {
                            sc = new ScalarSpeedCommand(0.0);
                        }

                        // check final for stopped failed opposing
                        VehicleAgent forwardVa = targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle;
                        if (forwardVa != null)
                        {
                            // dist between
                            double distToFV = targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition);

                            // check stopped
                            bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5;

                            // check distance
                            bool distOk = distToFV < 2.5 * TahoeParams.VL;

                            // check failed
                            bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed;

                            // notify
                            ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString());

                            // check all for failed
                            if (stopped && distOk && failed)
                            {
                                // check which lane we are in
                                if (initial.LanePolygon.IsInside(vehicleState.Front))
                                {
                                    // return to opposing lane
                                    return new Maneuver(new HoldBrakeBehavior(), new OpposingLanesState(initial, true, CoreCommon.CorePlanningState, vehicleState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                                }
                                else
                                {
                                    // lane state
                                    return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                                }
                            }
                        }

                        // continue the lane change with the proper speed command
                        ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo,
                            sc, ignoreVehicles, initial.ReversePath, target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, false),
                            initial.NumberOfLanesRight(vehicleState.Front, false));

                        // standard maneuver
                        return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp);
                    }

                    #endregion

                    else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }
                    else
                    {
                        // fallout exception
                        throw new Exception("currently unsupported lane change type");
                    }
                }

                #endregion

                else
                {
                    // fallout exception
                    throw new Exception("currently unsupported lane change type");
                }
            }

            #endregion
        }
        /// <summary>
        /// Plans the forward maneuver and secondary maneuver
        /// </summary>
        /// <param name="arbiterLane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="p"></param>
        /// <param name="blockage"></param>
        /// <returns></returns>
        public Maneuver ForwardManeuver(IFQMPlanable arbiterLane, VehicleState vehicleState, RoadPlan roadPlan, 
            List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable)
        {
            // get primary parameterization
            TravelingParameters primary = this.ForwardMonitor.Primary(arbiterLane, vehicleState, roadPlan, blockages, ignorable, true);

            // return primary for now
            return new Maneuver(primary.Behavior, primary.NextState, primary.Decorators, vehicleState.Timestamp);
        }
        /// <summary>
        /// Behavior given we stay in the current lane
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="state"></param>
        /// <param name="downstreamPoint"></param>
        /// <returns></returns>
        public TravelingParameters Primary(IFQMPlanable lane, VehicleState state, RoadPlan roadPlan, 
            List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, bool log)
        {
            // possible parameterizations
            List<TravelingParameters> tps = new List<TravelingParameters>();

            #region Lane Major Parameterizations with Current Lane Goal Params, If Best Goal Exists in Current Lane

            // check if the best goal is in the current lane
            ArbiterWaypoint lanePoint = null;
            if (lane.AreaComponents.Contains(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Lane))
                lanePoint = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest;

            // get the next thing we need to stop at no matter what and parameters for stopping at it
            ArbiterWaypoint laneNavStop;
            double laneNavStopSpeed;
            double laneNavStopDistance;
            StopType laneNavStopType;
            this.NextNavigationalStop(lane, lanePoint, state.Front, state.ENCovariance, ignorable,
                out laneNavStopSpeed, out laneNavStopDistance, out laneNavStopType, out laneNavStop);

            // create parameterization of the stop
            TravelingParameters laneNavParams = this.NavStopParameterization(lane, roadPlan, laneNavStopSpeed, laneNavStopDistance, laneNavStop, laneNavStopType, state);
            this.navigationParameters = laneNavParams;
            this.laneParameters = laneNavParams;
            tps.Add(laneNavParams);

            #region Log
            if (log)
            {
                // add to current parames to arbiter information
                CoreCommon.CurrentInformation.FQMBehavior = laneNavParams.Behavior.ToShortString();
                CoreCommon.CurrentInformation.FQMBehaviorInfo = laneNavParams.Behavior.ShortBehaviorInformation();
                CoreCommon.CurrentInformation.FQMSpeedCommand = laneNavParams.Behavior.SpeedCommandString();
                CoreCommon.CurrentInformation.FQMDistance = laneNavParams.DistanceToGo.ToString("F6");
                CoreCommon.CurrentInformation.FQMSpeed = laneNavParams.RecommendedSpeed.ToString("F6");
                CoreCommon.CurrentInformation.FQMState = laneNavParams.NextState.ShortDescription();
                CoreCommon.CurrentInformation.FQMStateInfo = laneNavParams.NextState.StateInformation();
                CoreCommon.CurrentInformation.FQMStopType = laneNavStopType.ToString();
                CoreCommon.CurrentInformation.FQMWaypoint = laneNavStop.ToString();
                CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = lane.CurrentMaximumSpeed(state.Position).ToString("F1");
            }
            #endregion

            #endregion

            #region Forward Vehicle Parameterization

            // forward vehicle update
            this.ForwardVehicle.Update(lane, state);

            // clear current params
            this.followingParameters = null;

            // check not in a sparse area
            bool sparseArea = lane is ArbiterLane ?
                ((ArbiterLane)lane).GetClosestPartition(state.Front).Type == PartitionType.Sparse :
                ((SupraLane)lane).ClosestComponent(state.Front) == SLComponentType.Initial && ((SupraLane)lane).Initial.GetClosestPartition(state.Front).Type == PartitionType.Sparse;

            // exists forward vehicle
            if (!sparseArea && this.ForwardVehicle.ShouldUseForwardTracker)
            {
                // get forward vehicle params, set lane decorators
                TravelingParameters vehicleParams = this.ForwardVehicle.Follow(lane, state, ignorable);
                vehicleParams.Behavior.Decorators.AddRange(this.laneParameters.Decorators);
                this.FollowingParameters = vehicleParams;

                #region Log
                if (log)
                {
                    // add to current parames to arbiter information
                    CoreCommon.CurrentInformation.FVTBehavior = vehicleParams.Behavior.ToShortString();
                    CoreCommon.CurrentInformation.FVTSpeed = this.ForwardVehicle.FollowingParameters.RecommendedSpeed.ToString("F3");
                    CoreCommon.CurrentInformation.FVTSpeedCommand = vehicleParams.Behavior.SpeedCommandString();
                    CoreCommon.CurrentInformation.FVTDistance = vehicleParams.DistanceToGo.ToString("F2");
                    CoreCommon.CurrentInformation.FVTState = vehicleParams.NextState.ShortDescription();
                    CoreCommon.CurrentInformation.FVTStateInfo = vehicleParams.NextState.StateInformation();

                    // set xSeparation
                    CoreCommon.CurrentInformation.FVTXSeparation = this.ForwardVehicle.ForwardControl.xSeparation.ToString("F0");
                }
                #endregion

                // check if we're stopped behind fv, allow wait timer if true, stop wait timer if not behind fv
                bool forwardVehicleStopped = this.ForwardVehicle.CurrentVehicle.IsStopped;
                bool forwardSeparationGood = this.ForwardVehicle.ForwardControl.xSeparation < TahoeParams.VL * 2.5;
                bool wereStopped = CoreCommon.Communications.GetVehicleSpeed().Value < 0.1;
                bool forwardDistanceToGo = vehicleParams.DistanceToGo < 3.5;
                if (forwardVehicleStopped && forwardSeparationGood && wereStopped && forwardDistanceToGo)
                    this.ForwardVehicle.StoppedBehindForwardVehicle = true;
                else
                {
                    this.ForwardVehicle.StoppedBehindForwardVehicle = false;
                    this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop();
                    this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset();
                }

                // add vehicle param
                tps.Add(vehicleParams);
            }
            else
            {
                // no forward vehicle
                this.followingParameters = null;
                this.ForwardVehicle.StoppedBehindForwardVehicle = false;
            }

            #endregion

            #region Sparse Waypoint Parameterization

            // check for sparse waypoints downstream
            bool sparseDownstream;
            bool sparseNow;
            double sparseDistance;
            lane.SparseDetermination(state.Front, out sparseDownstream, out sparseNow, out sparseDistance);

            // check if sparse areas downstream
            if (sparseDownstream)
            {
                // set the distance to the sparse area
                if (sparseNow)
                    sparseDistance = 0.0;

                // get speed
                double speed = SpeedTools.GenerateSpeed(sparseDistance, 2.24, lane.CurrentMaximumSpeed(state.Front));

                // maneuver
                Maneuver m = new Maneuver();
                bool usingSpeed = true;
                SpeedCommand sc = new ScalarSpeedCommand(speed);

                #region Parameterize Given Speed Command

                // check if lane
                if (lane is ArbiterLane)
                {
                    // get lane
                    ArbiterLane al = (ArbiterLane)lane;

                    // default behavior
                    Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
                    b.Decorators = this.laneParameters.Decorators;

                    // standard behavior is fine for maneuver
                    m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), this.laneParameters.Decorators, state.Timestamp);
                }
                // check if supra lane
                else if (lane is SupraLane)
                {
                    // get lane
                    SupraLane sl = (SupraLane)lane;

                    // get sl state
                    StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;

                    // get default beheavior
                    Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List<int>());
                    b.Decorators = this.laneParameters.Decorators;

                    // standard behavior is fine for maneuver
                    m = new Maneuver(b, sisls, this.laneParameters.Decorators, state.Timestamp);
                }

                #endregion

                #region Parameterize

                // create new params
                TravelingParameters tp = new TravelingParameters();
                tp.Behavior = m.PrimaryBehavior;
                tp.Decorators = this.laneParameters.Decorators;
                tp.DistanceToGo = Double.MaxValue;
                tp.NextState = m.PrimaryState;
                tp.RecommendedSpeed = speed;
                tp.Type = TravellingType.Navigation;
                tp.UsingSpeed = usingSpeed;
                tp.SpeedCommand = sc;
                tp.VehiclesToIgnore = new List<int>();

                // return navigation params
                tps.Add(tp);

                #endregion
            }

            #endregion

            // sort params by most urgent
            tps.Sort();

            // set current params
            this.currentParameters = tps[0];

            // get behavior to check add vehicles to ignore
            if (this.currentParameters.Behavior is StayInLaneBehavior)
                ((StayInLaneBehavior)this.currentParameters.Behavior).IgnorableObstacles = this.ForwardVehicle.VehiclesToIgnore;

            // out of navigation, blockages, and vehicle following determine the actual primary parameters for this lane
            return tps[0];
        }
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="waypoint"></param>
 /// <param name="entries"></param>
 public IntersectionPlan(ITraversableWaypoint waypoint, List <PlanableInterconnect> entries, RoadPlan roadPlan)
 {
     this.ExitWaypoint    = waypoint;
     this.PossibleEntries = entries;
     this.SegmentPlan     = roadPlan;
 }