/// <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 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
		}