/// <summary>
        /// Checks if it is clear to go into the other lane
        /// </summary>
        /// <param name="state"></param>
        /// <returns></returns>
        public bool IsClear(VehicleState state, double vUs)
        {
            // temp get
            bool b = this.CheckClear(state, vUs);

            if (!b)
            {
                this.ResetTiming();
                return false;
            }

            // if none found, tiemr not running start timer
            if (!this.RearClearStopwatch.IsRunning)
            {
                this.ResetTiming();
                this.RearClearStopwatch.Start();
                ArbiterOutput.Output("Forward Rear: " + this.VehicleSide.ToString() + " Clear, starting cooldown");
                return false;
            }
            // enough time
            else if (this.RearClearStopwatch.IsRunning && this.RearClearStopwatch.ElapsedMilliseconds / 1000.0 > 0.5)
            {
                ArbiterOutput.Output("Forward Rear: " + this.VehicleSide.ToString() + " Clear, cooldown complete");
                return true;
            }
            // not enough time
            else
            {
                double timer = this.RearClearStopwatch.ElapsedMilliseconds / 1000.0;
                ArbiterOutput.Output("Forward Rear: " + this.VehicleSide.ToString() + " Clear, cooldown timer: " + timer.ToString("F2"));
                return false;
            }
        }
        /// <summary>
        /// Checks if hte opposing lane is clear to pass an opposing vehicle
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="state"></param>
        /// <returns></returns>
        public bool ClearForDisabledVehiclePass(ArbiterLane lane, VehicleState state, double vUs, Coordinates minReturn)
        {
            // update the forward vehicle
            this.ForwardVehicle.Update(lane, state);

            // check if the rear vehicle exists and is moving along with us
            if (this.ForwardVehicle.ShouldUseForwardTracker && this.ForwardVehicle.CurrentVehicle != null)
            {
                // distance from other to us
                double currentDistance = lane.DistanceBetween(this.ForwardVehicle.CurrentVehicle.ClosestPosition, state.Front) - (2 * TahoeParams.VL);
                double minChangeDist = lane.DistanceBetween(minReturn, state.Front);

                // check if he's within min return dist
                if(currentDistance > minChangeDist)
                {
                    // params
                    double vOther = this.ForwardVehicle.CurrentVehicle.StateMonitor.Observed.speedValid ? this.ForwardVehicle.CurrentVehicle.Speed : lane.Way.Segment.SpeedLimits.MaximumSpeed;

                    // get distance of envelope for him to slow to our speed
                    double xEnvelope = (Math.Pow(vUs, 2.0) - Math.Pow(vOther, 2.0)) / (2.0 * -0.5);

                    // check to see if vehicle is outside of the envelope to slow down for us after 3 seconds
                    double xSafe = currentDistance - minChangeDist - (xEnvelope + (vOther * 15.0));
                    return xSafe > 0 ? true : false;
                }
                else
                    return false;
            }
            else
                return true;
        }
        public void MessageArrived(string channelName, object message)
        {
            if (channelName == "RndfNetworkChannel")
            {
                this.rndfNetwork = (Common.RndfNetwork.RndfNetwork)message;
                Console.WriteLine("   > Rndf Received");
            }
            else if (channelName == "MdfChannel")
            {
                this.mdf = (Common.RndfNetwork.Mdf)message;
                Console.WriteLine("   > Mdf Received");
            }
            else if (channelName == "PositionChannel")
            {
                this.vehicleState = (VehicleState) message;
                Console.WriteLine("   > Position Received: " + this.vehicleState.ToString());
            }
            else if (channelName == "TestStringChannel")
            {
                Console.WriteLine("Received String On TestStringChannel: " + ((string)message));
            }
            else
            {
                throw new ArgumentException("Unknown Channel", channelName);

            }
        }
        /// <summary>
        /// Gets a default behavior for a path segment
        /// </summary>
        /// <param name="location"></param>
        /// <param name="vehicleState"></param>
        /// <param name="exit"></param>
        /// <param name="relative"></param>
        /// <param name="stopSpeed"></param>
        /// <param name="aMax"></param>
        /// <param name="dt">timestep in seconds</param>
        /// <returns></returns>
        public static PathFollowingBehavior DefaultStayInLaneBehavior(RndfLocation location, VehicleState vehicleState, 
            RndfWaypointID action, ActionType actionType, bool relative, double stopSpeed, double aMax, double dt,
            double maxSpeed, IPath path)
        {
            // get lane path
            //IPath path = RoadToolkit.LanePath(location.Partition.FinalWaypoint.Lane, vehicleState, relative);

            // check if the action is just a goal (note that exit and stop take precedence)
            if (actionType == ActionType.Goal)
            {
                // get maximum speed
                //double maxSpeed = location.Partition.FinalWaypoint.Lane.Way.Segment.SpeedInformation.MaxSpeed;
                //double maxSpeed = maxV;

                // generate path following behavior
                //return new PathFollowingBehavior(path, new ScalarSpeedCommand(maxSpeed));
                return null;
            }
            else
            {
                // get maximum speed
                //double maxSpeed = location.Partition.FinalWaypoint.Lane.Way.Segment.SpeedInformation.MaxSpeed;

                // get operational required distance to hand over to operational stop
                double distance = RoadToolkit.DistanceUntilOperationalStop(RoadToolkit.DistanceToWaypoint(location, action)-TahoeParams.FL);

                // get desired velocity
                double desiredSpeed = RoadToolkit.InferFinalSpeed(0, stopSpeed, distance, maxSpeed, aMax, dt);

                // generate path following behavior
                //return new PathFollowingBehavior(path, new ScalarSpeedCommand(desiredSpeed));
                return null;
            }
        }
 public UrbanChallenge.Behaviors.Behavior Resume(VehicleState currentState, double speed)
 {
     if (StateToResume != null)
         return StateToResume.Resume(currentState, speed);
     else
         return null;
 }
        /// <summary>
        /// Get partition of startup chute we might be upon
        /// </summary>
        /// <param name="vehicleState"></param>
        /// <returns></returns>
        public ArbiterLanePartition GetStartupChute(VehicleState vehicleState)
        {
            // current road network
            ArbiterRoadNetwork arn = CoreCommon.RoadNetwork;

            // get startup chutes
            List<ArbiterLanePartition> startupChutes = new List<ArbiterLanePartition>();
            foreach (ArbiterSegment asg in arn.ArbiterSegments.Values)
            {
                foreach (ArbiterLane al in asg.Lanes.Values)
                {
                    foreach (ArbiterLanePartition alp in al.Partitions)
                    {
                        if (alp.Type == PartitionType.Startup)
                            startupChutes.Add(alp);
                    }
                }
            }

            // figure out which startup chute we are in
            foreach (ArbiterLanePartition alp in startupChutes)
            {
                // check if within 40m of it
                if(vehicleState.Front.DistanceTo(alp.Initial.Position) < 40.0)
                {
                    // check orientation relative to lane
                    Coordinates laneVector = alp.Vector().Normalize();

                    // get our heading
                    Coordinates ourHeading = vehicleState.Heading.Normalize();

                    // check dist to each other to make sure forwards
                    if (laneVector.DistanceTo(ourHeading) < Math.Sqrt(2.0))
                    {
                        // figure out extension
                        Coordinates backC = alp.Initial.Position - alp.Vector().Normalize(40.0);

                        // generate new line path
                        LinePath lp = new LinePath(new Coordinates[] { backC, alp.Final.Position });
                        LinePath lpL = lp.ShiftLateral(alp.Lane.Width / 2.0);
                        LinePath lpR = lp.ShiftLateral(-alp.Lane.Width / 2.0);
                        List<Coordinates> poly = new List<Coordinates>();
                        poly.AddRange(lpL);
                        poly.AddRange(lpR);
                        Polygon chutePoly = Polygon.GrahamScan(poly);

                        // check if we're inside the chute
                        if (chutePoly.IsInside(vehicleState.Front))
                        {
                            // return the chute
                            return alp;
                        }
                    }
                }
            }

            // fallout
            return null;
        }
        /// <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>
 /// Constructor
 /// </summary>
 /// <param name="localRoute"></param>
 /// <param name="bestFullRoute"></param>
 /// <param name="vehicleState"></param>
 /// <param name="maneuver"></param>
 public AiRemoteListener(Routes localRoute, FullRoute bestFullRoute, VehicleState vehicleState, Behavior behavior,
     IPath path, DynamicObstacles dynamicObstacles, StaticObstacles staticObstacles)
 {
     this.LocalRoute = localRoute;
     this.BestFullRoute = bestFullRoute;
     this.VehicleState = vehicleState;
     this.behavior = behavior;
     this.path = path;
     this.DynamicObstacles = dynamicObstacles;
     this.StaticObstacles = staticObstacles;
 }
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="currentArbiterState"></param>
 /// <param name="internalCarMode"></param>
 /// <param name="fullRoute"></param>
 /// <param name="routeTime"></param>
 /// <param name="actionPoint"></param>
 /// <param name="currentGoal"></param>
 /// <param name="goals"></param>
 /// <param name="behavior"></param>
 public ArbiterInformation(string currentArbiterState, CarMode internalCarMode, FullRoute fullRoute, double routeTime,
     RndfWaypointID actionPoint, RndfWaypointID currentGoal, Queue<RndfWaypointID> goals, Behavior behavior, VehicleState planningState)
 {
     this.CurrentArbiterState = currentArbiterState;
     this.InternalCarMode = internalCarMode;
     this.FullRoute = fullRoute;
     this.RouteTime = routeTime;
     this.ActionPoint = actionPoint;
     this.CurrentGoal = currentGoal;
     this.Goals = goals;
     this.Behavior = behavior;
     this.PlanningState = planningState;
 }
        private void RenderRelativePath(VehicleState vs, Path transPath, Graphics g, WorldTransform t, bool isArbiterPath)
        {
            if((isArbiterPath && DrawingUtility.DrawArbiterLanePath) ||
                (!isArbiterPath && DrawingUtility.DrawOperationalLanePath))
            {
                // compute the rotation matrix to add in our vehicles rotation
                /*Matrix3 rotMatrix = new Matrix3(
                    Math.Cos(vs.heading.ArcTan), -Math.Sin(vs.heading.ArcTan), 0,
                    Math.Sin(vs.heading.ArcTan), Math.Cos(vs.heading.ArcTan), 0,
                    0, 0, 1);

                // compute the translation matrix to move our vehicle's location
                Matrix3 transMatrix = new Matrix3(
                    1, 0, vs.xyPosition.X,
                    0, 1, vs.xyPosition.Y,
                    0, 0, 1);

                // compute the combined transformation matrix
                Matrix3 m = rotMatrix * transMatrix;

                // clone, transform and add each segment to our path
                transPath.Transform(m);*/

                float nomPixelWidth = 2.0f;
                float penWidth = nomPixelWidth / t.Scale;
                Pen arbiterPen = new Pen(Color.FromArgb(100, DrawingUtility.ArbiterLanePath), penWidth);
                Pen operationalPen = new Pen(Color.FromArgb(100, DrawingUtility.OperationalLanePath), penWidth);

                // display path
                foreach (IPathSegment ps in transPath)
                {
                    if (ps is BezierPathSegment)
                    {
                        BezierPathSegment seg = (BezierPathSegment)ps;
                        CubicBezier cb = seg.cb;

                        if (isArbiterPath)
                        {
                            g.DrawBezier(arbiterPen, DrawingUtility.ToPointF(cb.P0), DrawingUtility.ToPointF(cb.P1), DrawingUtility.ToPointF(cb.P2), DrawingUtility.ToPointF(cb.P3));
                        }
                        else
                        {
                            g.DrawBezier(operationalPen, DrawingUtility.ToPointF(cb.P0), DrawingUtility.ToPointF(cb.P1), DrawingUtility.ToPointF(cb.P2), DrawingUtility.ToPointF(cb.P3));
                        }
                    }
                }
            }
        }
        public Behavior Resume(VehicleState currentState, double speed)
        {
            // decorators
            List<BehaviorDecorator> bds = new List<BehaviorDecorator>();

            // zero decorators
            bds.Add(new TurnSignalDecorator(TurnSignal.Off));

            // get no op
            Behavior b = new NullBehavior();

            // set decorators
            b.Decorators = bds;

            // return
            return b;
        }
        public OpposingLanesState(ArbiterLane opposingLane, bool resetLaneAgent, IState previous, VehicleState vs)
        {
            this.resetLaneAgent = resetLaneAgent;
            this.OpposingLane = opposingLane;
            this.OpposingWay = opposingLane.Way.WayId;
            this.ClosestGoodLane = null;

            this.SetClosestGood(vs);

            if (previous is ChangeLanesState)
            {
                EntryParameters = ((ChangeLanesState)previous).Parameters;
            }
            else if (previous is OpposingLanesState)
            {
                EntryParameters = ((OpposingLanesState)previous).EntryParameters;
            }
            else
            {
                EntryParameters = null;
            }
        }
        static void Main(string[] args)
        {
            // Initialize Communications
            Console.WriteLine("1. Initializing Communications...");
            Communicator communicator = new Communicator();
            Console.WriteLine("   > Communications Initialized\n");

            // Wait for Rndf,Mdf to continue
            Console.WriteLine("2. Waiting for Rndf, Mdf to continue...");
            communicator.InitializeTestServer();
            Console.WriteLine("   > Rndf, Mdf Received\n");

            // Wait for User continue
            Console.WriteLine("3. Ready To Start Testing, Press ENTER to Continue...");
            Console.ReadLine();

            // Wait for 1 second
            Thread.Sleep(1000);

            /*
            // Turn => Stopped at Stop
            // Create a fake vehicle state
            RndfWayPoint position = communicator.RndfNetwork.Goals[1].Waypoint;
            Console.WriteLine("  > Position: " + position.WaypointID.ToString());
            VehicleState vehicleState = new VehicleState();
            vehicleState.xyPosition = position.Position;
            LaneEstimate laneEstimate = new LaneEstimate(position.Lane.LaneID, position.Lane.LaneID, 1);
            List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
            laneEstimates.Add(laneEstimate);
            vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
            communicator.PublishVehicleState(vehicleState);
            Console.WriteLine("  > Published Position");*/

            /*
            // Road
            // Create a fake vehicle state
            RndfWaypointID tmpID = new RndfWaypointID(new LaneID(new WayID(new SegmentID(1), 1), 1), 1);
            RndfWayPoint position = communicator.RndfNetwork.Waypoints[tmpID];
            Console.WriteLine("  > Position: " + position.WaypointID.ToString());
            VehicleState vehicleState = new VehicleState();
            vehicleState.xyPosition = position.Position;
            LaneEstimate laneEstimate = new LaneEstimate(position.Lane.LaneID, position.Lane.LaneID, 1);
            List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
            laneEstimates.Add(laneEstimate);
            vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
            vehicleState.speed = 3;
            communicator.PublishVehicleState(vehicleState);
            Console.WriteLine("  > Published Position");*/

            /*
            // Intersection
            // Create a fake vehicle state
            RndfWaypointID tmpID = new RndfWaypointID(new LaneID(new WayID(new SegmentID(1), 1), 1), 2);
            RndfWaypointID tmpEndID = new RndfWaypointID(new LaneID(new WayID(new SegmentID(26), 2), 3), 1);
            InterconnectID testID = new InterconnectID(tmpID, tmpEndID);
            Interconnect testInter = communicator.RndfNetwork.Interconnects[testID];
            RndfWayPoint position = communicator.RndfNetwork.Waypoints[tmpID];
            VehicleState vehicleState = new VehicleState();
            vehicleState.xyPosition = position.Position + new UrbanChallenge.Common.Coordinates(4, 4);
            LaneEstimate laneEstimate = new LaneEstimate(tmpID.LaneID, tmpEndID.LaneID, 1);
            List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
            laneEstimates.Add(laneEstimate);
            vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
            vehicleState.speed = 3;
            communicator.PublishVehicleState(vehicleState);
            Console.WriteLine("  > Published Position");
             */

            // In Lane
            // Create a fake vehicle state
            RndfWayPoint position = communicator.RndfNetwork.Goals[2].Waypoint;
            Console.WriteLine("  > Position: " + position.WaypointID.ToString());
            VehicleState vehicleState = new VehicleState();
            vehicleState.xyPosition = position.Position;
            LaneEstimate laneEstimate = new LaneEstimate(position.Lane.LaneID, position.Lane.LaneID, 1);
            List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
            laneEstimates.Add(laneEstimate);
            vehicleState.speed = 0;
            vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
            communicator.PublishVehicleState(vehicleState);
            //Console.WriteLine("  > Published Position");

            /*// Read the configuration file.
            RemotingConfiguration.Configure("..\\..\\App.config", false);
            WellKnownServiceTypeEntry[] wkst = RemotingConfiguration.GetRegisteredWellKnownServiceTypes();

            // "Activate" the NameService singleton.
            ObjectDirectory objectDirectory = (ObjectDirectory)Activator.GetObject(typeof(ObjectDirectory), wkst[0].ObjectUri);

            // Receive the facades of components we want to use
            RndfEditorFacade editorFacade = (RndfEditorFacade)objectDirectory.Resolve("RndfEditor");

            // Get the RndfNetwork
            RndfNetwork rndfNetwork = editorFacade.RndfNetwork;

            // Create the Goals from the RndfNetwork
            Queue<Goal> goals = generateGoals(rndfNetwork);
            Console.WriteLine("Created Goals");

            // Create Speed Limits from the RndfNetwork
            List<SpeedInformation> speedLimits = generateSpeedLimits(rndfNetwork);
            Console.WriteLine("Created speed limits");

            // Create Mdf
            Mdf mdf = new Mdf(goals, speedLimits);
            Console.WriteLine("Created Mdf");

            // Create a fake vehicle state
            RndfWayPoint position = rndfNetwork.Goals[1].Waypoint;
            Console.WriteLine("Position: " + position.WaypointID.ToString());
            VehicleState vehicleState = new VehicleState();
            vehicleState.xyPosition = position.Position;
            LaneEstimate laneEstimate = new LaneEstimate(position.Lane.LaneID, position.Lane.LaneID, 1);
            List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
            laneEstimates.Add(laneEstimate);
            vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);

            // Test
            Console.WriteLine("Number of RndfNetwork Segments: " + rndfNetwork.Segments.Values.Count);

            // Bind the facades of components we implement.
            objectDirectory.Rebind(TestDataServerFacadeImpl.Instance(rndfNetwork, mdf, vehicleState), "TestServer");*/

            // Show that the navigation system is ready and waiting for input
            Console.WriteLine("");
            Console.WriteLine("4. Test Serve Complete, Waiting. Press ENTER to Shut Down");
            Console.ReadLine();
            Console.WriteLine("");
            communicator.ShutDown();
        }
 public UrbanChallenge.Behaviors.Behavior Resume(VehicleState currentState, double speed)
 {
     throw new Exception("The method or operation is not implemented.");
 }
        /// <summary>
        /// Updates this monitor
        /// </summary>
        public void Update(VehicleState ourState)
        {
            // make sure not our vehicle
            if (!isOurs)
            {
                // get a list of vehicles possibly in the exit
                List<VehicleAgent> stopVehicles = new List<VehicleAgent>();

                // check all valid vehicles
                foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
                {
                    // check if any of the point inside the polygon
                    for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
                    {
                        // get abs coord
                        Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);

                        // check if inside poly
                        if (this.stoppedExit.ExitPolygon.IsInside(c) && !stopVehicles.Contains(va))
                        {
                            // add vehicle
                            stopVehicles.Add(va);
                        }
                    }
                }

                // check occluded
                foreach (VehicleAgent va in TacticalDirector.OccludedVehicles.Values)
                {
                    // check if any of the point inside the polygon
                    for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
                    {
                        // get abs coord
                        Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);

                        // check if inside poly
                        if (this.stoppedExit.ExitPolygon.IsInside(c) && !stopVehicles.Contains(va))
                        {
                            // add vehicle
                            ArbiterOutput.Output("Added occluded vehicle: " + va.ToString() + " to SEM: " + this.stoppedExit.ToString());
                            stopVehicles.Add(va);
                        }
                    }
                }

                // check if we already have a vehicle we are referencing
                if (this.currentVehicle != null && !stopVehicles.Contains(this.currentVehicle))
                {
                    // get the polygon of the current vehicle	and inflate a tiny bit?
                    Polygon p = this.currentVehicle.GetAbsolutePolygon(ourState);
                    p = p.Inflate(1.0);

                    // Vehicle agent to switch to if exists
                    VehicleAgent newer = null;
                    double distance = Double.MaxValue;

                    // check for closest vehicle to exist in that polygon
                    foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
                    {
                        // check if vehicle is stopped
                        if (stopVehicles.Contains(va))
                        {
                            // get distance to stop
                            double stopLineDist;
                            va.ClosestPointTo(this.stoppedExit.Waypoint.Position, ourState, out stopLineDist);

                            // check all vehicle points
                            foreach (Coordinates rel in va.StateMonitor.Observed.relativePoints)
                            {
                                Coordinates abs = va.TransformCoordAbs(rel, ourState);
                                double tmpDist = stopLineDist;

                                // check for the clsoest with points inside vehicle
                                if (p.IsInside(abs) && (newer == null || tmpDist < distance))
                                {
                                    newer = va;
                                    distance = tmpDist;
                                }
                            }
                        }
                    }

                    // check if we can switch vehicles
                    if (newer != null)
                    {
                        ArbiterOutput.Output("For StopLine: " + this.stoppedExit.ToString() + " switched vehicleId: " + this.currentVehicle.VehicleId.ToString() + " for vehicleId: " + newer.VehicleId.ToString());
                        this.currentVehicle = newer;
                        this.NotFoundPrevious = notFoundDefault;
                    }
                    else if(NotFoundPrevious == 0)
                    {
                        // set this as not having a vehicle
                        this.currentVehicle = null;

                        // set not found before
                        this.NotFoundPrevious = this.notFoundDefault;

                        // stop timers
                        this.timer.Stop();
                        this.timer.Reset();
                    }
                    else
                    {
                        // set not found before
                        this.NotFoundPrevious--;
                        ArbiterOutput.Output("current not found, not found previous: " + this.NotFoundPrevious.ToString() + ", at stopped exit: " + this.stoppedExit.Waypoint.ToString());
                    }
                }
                // update the current vehicle
                else if (this.currentVehicle != null && stopVehicles.Contains(this.currentVehicle))
                {
                    // udpate current
                    this.currentVehicle = stopVehicles[stopVehicles.IndexOf(this.currentVehicle)];

                    // set as found previous
                    this.NotFoundPrevious = this.notFoundDefault;

                    // check if need to update timer
                    if (this.currentVehicle.IsStopped && !this.timer.IsRunning)
                    {
                        this.timer.Stop();
                        this.timer.Reset();
                        this.timer.Start();
                    }
                    // otherwise check if moving
                    else if (!this.currentVehicle.IsStopped && this.timer.IsRunning)
                    {
                        this.timer.Stop();
                        this.timer.Reset();
                    }
                }
                // otherwise if we have no vehicle and exist vehicles in stop area
                else if (this.currentVehicle == null && stopVehicles.Count > 0)
                {
                    // get closest vehicle to the stop
                    VehicleAgent toTrack = null;
                    double distance = Double.MaxValue;

                    // set as found previous
                    this.NotFoundPrevious = this.notFoundDefault;

                    // loop through
                    foreach (VehicleAgent va in stopVehicles)
                    {
                        // check if vehicle is stopped
                        if (va.IsStopped)
                        {
                            // distance of vehicle to stop line
                            double distanceToStop;
                            Coordinates closestToStop = va.ClosestPointTo(this.stoppedExit.Waypoint.Position, ourState, out distanceToStop).Value;

                            // check if we don't have one or tmp closer than tracked
                            if (toTrack == null || distanceToStop < distance)
                            {
                                toTrack = va;
                                distance = distanceToStop;
                            }
                        }
                    }

                    // check if we have one to track
                    if (toTrack != null)
                        this.currentVehicle = toTrack;
                }
            }
        }
 public UrbanChallenge.Behaviors.Behavior Resume(UrbanChallenge.Common.Vehicle.VehicleState currentState, double speed)
 {
     return(new StayInLaneBehavior(OpposingLane.LaneId, new ScalarSpeedCommand(0.0), new List <int>()));
 }
 public PathsDisplay(VehicleState updatedState, VehicleState arbiterState, IPath path)
 {
     this.updatedState = updatedState;
     this.arbiterState = arbiterState;
     this.path = path;
 }
        /// <summary>
        /// Helps with parameterizations for lateral reasoning
        /// </summary>
        /// <param name="referenceLane"></param>
        /// <param name="fqmLane"></param>
        /// <param name="goal"></param>
        /// <param name="vehicleFront"></param>
        /// <returns></returns>
        public TravelingParameters ParameterizationHelper(ArbiterLane referenceLane, ArbiterLane fqmLane,
            Coordinates goal, Coordinates vehicleFront, IState nextState, VehicleState state, VehicleAgent va)
        {
            // get traveling parameterized list
            List<TravelingParameters> tps = new List<TravelingParameters>();

            // get distance to the goal
            double goalDistance;
            double goalSpeed;
            this.StoppingParams(goal, referenceLane, vehicleFront, new double[] { }, out goalSpeed, out goalDistance);
            tps.Add(this.GetParameters(referenceLane, goalSpeed, goalDistance, state));

            // get next stop
            ArbiterWaypoint stopPoint;
            double stopSpeed;
            double stopDistance;
            StopType stopType;
            this.NextLaneStop(fqmLane, vehicleFront, new double[] { }, new List<ArbiterWaypoint>(),
                out stopPoint, out stopSpeed, out stopDistance, out stopType);
            this.StoppingParams(stopPoint.Position, referenceLane, vehicleFront, new double[] { },
                out stopSpeed, out stopDistance);
            tps.Add(this.GetParameters(referenceLane, stopSpeed, stopDistance, state));

            // get vehicle
            if (va != null)
            {
                VehicleAgent tmp = this.ForwardVehicle.CurrentVehicle;
                double tmpDist = this.ForwardVehicle.currentDistance;
                this.ForwardVehicle.CurrentVehicle = va;
                this.ForwardVehicle.currentDistance = referenceLane.DistanceBetween(state.Front, va.ClosestPosition);
                TravelingParameters tp = this.ForwardVehicle.Follow(referenceLane, state, new List<ArbiterWaypoint>());
                tps.Add(tp);
                this.ForwardVehicle.CurrentVehicle = tmp;
                this.ForwardVehicle.currentDistance = tmpDist;
            }

            // parameterize
            tps.Sort();
            return tps[0];
        }
        /// <summary>
        /// Gets parameterization
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="speed"></param>
        /// <param name="distance"></param>
        /// <param name="state"></param>
        /// <returns></returns>
        public TravelingParameters GetParameters(ArbiterLane lane, double speed, double distance, VehicleState state)
        {
            double distanceCutOff = CoreCommon.OperationslStopLineSearchDistance;
            Maneuver m = new Maneuver();
            SpeedCommand sc;
            bool usingSpeed = true;

            #region Distance Cutoff

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

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

                // standard behavior is fine for maneuver
                m = new Maneuver(b, new StayInLaneState(lane, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, state.Timestamp);
            }

            #endregion

            #region Outisde Distance Envelope

            // not inside distance envalope
            else
            {
                // get lane
                ArbiterLane al = lane;

                // default behavior
                sc = new ScalarSpeedCommand(speed);
                Behavior b = new StayInLaneBehavior(al.LaneId, sc, 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), TurnDecorators.NoDecorators, state.Timestamp);
            }

            #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>
 /// Updates vehicles
 /// </summary>
 /// <param name="vehicles"></param>
 /// <param name="state"></param>
 public void Update(SceneEstimatorTrackedClusterCollection vehicles, VehicleState state)
 {
     // vehicle population
     this.PopulateValidVehicles(vehicles);
     this.PopulateAreaVehicles();
     this.UpdateQueuingMonitors(state.Timestamp);
 }
Beispiel #21
0
 public bool IsCompletelyClear(UrbanChallenge.Common.Vehicle.VehicleState ourState)
 {
     this.Update(ourState);
     return(this.Clear(ourState));
 }
        /// <summary>
        /// Given vehicles and there locations determines if the lane adjacent to us is occupied adjacent to the vehicle
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="state"></param>
        /// <returns></returns>
        public bool Occupied(ArbiterLane lane, VehicleState state)
        {
            // check stopwatch time for proper elapsed
            if (this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 10)
                this.Reset();

            if (TacticalDirector.VehicleAreas.ContainsKey(lane))
            {
                // vehicles in the lane
                List<VehicleAgent> laneVehicles = TacticalDirector.VehicleAreas[lane];

                // position of the center of our vehicle
                Coordinates center = state.Front - state.Heading.Normalize(TahoeParams.VL / 2.0);

                // cutoff for allowing vehicles outside this range
                double dCutOff = TahoeParams.VL * 1.5;

                // loop through vehicles
                foreach (VehicleAgent va in laneVehicles)
                {
                    // vehicle high level distance
                    double d = Math.Abs(lane.DistanceBetween(center, va.ClosestPosition));

                    // check less than distance cutoff
                    if (d < dCutOff)
                    {
                        ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " filled with vehicle: " + va.ToString());
                        this.CurrentVehicle = va;
                        this.Reset();
                        return true;
                    }
                }
            }

            // now check the lateral sensor for being occupied
            if (this.SideSickObstacleDetected(lane, state))
            {
                this.CurrentVehicle = new VehicleAgent(true, true);
                this.Reset();
                ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " SIDE OBSTACLE DETECTED");
                return true;
            }

            // none found
            this.CurrentVehicle = null;

            // if none found, timer not running start timer
            if (!this.LateralClearStopwatch.IsRunning)
            {
                this.CurrentVehicle = new VehicleAgent(true, true);
                this.Reset();
                this.LateralClearStopwatch.Start();
                ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, starting cooldown");
                return true;
            }
            // check enough time
            else if (this.LateralClearStopwatch.IsRunning && this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 0.5)
            {
                this.CurrentVehicle = null;
                ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown complete");
                return false;
            }
            // not enough time
            else
            {
                this.CurrentVehicle = new VehicleAgent(true, true);
                double timer = this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0;
                ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown timer: " + timer.ToString("F2"));
                return true;
            }
        }
        /// <summary>
        /// Check if a side sick blocking obstacle is detected
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="state"></param>
        /// <returns></returns>
        private bool SideSickObstacleDetected(ArbiterLane lane, VehicleState state)
        {
            try
            {
                // get distance from vehicle to lane opp side
                Coordinates vec = state.Front - state.Position;
                vec = this.VehicleSide == SideObstacleSide.Driver ? vec.Rotate90() : vec.RotateM90();

                SideObstacles sobs = CoreCommon.Communications.GetSideObstacles(this.VehicleSide);
                if (sobs != null && sobs.obstacles != null)
                {
                    foreach (SideObstacle so in sobs.obstacles)
                    {
                        Coordinates cVec = state.Position + vec.Normalize(so.distance);
                        if (so.height > 0.7 && lane.LanePolygon.IsInside(cVec))
                            return true;
                    }
                }
            }
            catch (Exception ex)
            {
                ArbiterOutput.Output("side sick obstacle exception: " + ex.ToString());
            }

            return false;
        }
Beispiel #24
0
 public bool Clear(UrbanChallenge.Common.Vehicle.VehicleState ourState)
 {
     return(this.currentVehicle == null ||
            (this.currentVehicle.StateMonitor.Observed.speedValid && !this.currentVehicle.IsStopped && this.currentVehicle.Speed > 2.0));
 }
Beispiel #25
0
        public void Update(UrbanChallenge.Common.Vehicle.VehicleState ourState)
        {
            // get lane
            ArbiterLane lane = this.turnFinalWaypoint.Lane;

            // get the possible vehicles that could be in this stop
            if (TacticalDirector.VehicleAreas.ContainsKey(lane))
            {
                // get vehicles in the the lane of the exit
                List <VehicleAgent> possibleVehicles = TacticalDirector.VehicleAreas[lane];

                // get the point we are looking from
                LinePath.PointOnPath referencePoint = lane.LanePath().GetClosestPoint(this.turnFinalWaypoint.Position);

                // tmp
                VehicleAgent tmp     = null;
                double       tmpDist = Double.MaxValue;

                // check over all possible vehicles
                foreach (VehicleAgent possible in possibleVehicles)
                {
                    // get vehicle point on lane
                    LinePath.PointOnPath vehiclePoint = lane.LanePath().GetClosestPoint(possible.ClosestPosition);

                    // check if the vehicle is in front of the reference point
                    double vehicleDist = lane.LanePath().DistanceBetween(referencePoint, vehiclePoint);
                    if (vehicleDist >= 0 && vehicleDist < TahoeParams.VL * 2.0)
                    {
                        tmp     = possible;
                        tmpDist = vehicleDist;
                    }
                }

                if (tmp != null)
                {
                    if (this.currentVehicle == null || !this.currentVehicle.Equals(tmp))
                    {
                        this.timer.Stop();
                        this.timer.Reset();
                        this.currentVehicle = tmp;
                    }
                    else
                    {
                        this.currentVehicle = tmp;
                    }

                    if (this.currentVehicle.IsStopped && !this.timer.IsRunning)
                    {
                        this.timer.Stop();
                        this.timer.Reset();
                        this.timer.Start();
                    }
                    else if (!this.currentVehicle.IsStopped && this.timer.IsRunning)
                    {
                        this.timer.Stop();
                        this.timer.Reset();
                    }
                }
                else
                {
                    this.currentVehicle = null;
                    this.timer.Stop();
                    this.timer.Reset();
                }
            }
            else
            {
                this.timer.Stop();
                this.timer.Reset();
                this.currentVehicle = 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>
        /// Resume from pause
        /// </summary>
        /// <returns></returns>
        public Behavior Resume(VehicleState currentState, double speed)
        {
            // turn behavior into chute
            TurnBehavior b = new TurnBehavior(this.Final.Lane.LaneId, this.Final.PartitionPath,
                this.Final.PartitionPath.ShiftLateral(this.Final.Lane.Width / 2.0),
                this.Final.PartitionPath.ShiftLateral(-this.Final.Lane.Width / 2.0),
                new ScalarSpeedCommand(1.4), null);

            // return behavior
            return b;
        }
        public bool VehicleExistsInsidePolygon(VehicleAgent va, Polygon p, VehicleState ourState)
        {
            for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
            {
                Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);
                if (p.IsInside(c))
                    return true;
            }

            return false;
        }
        /// <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
        }
        public bool VehiclePassableInPolygon(ArbiterLane al, VehicleAgent va, Polygon p, VehicleState ourState, Polygon circ)
        {
            Polygon vehiclePoly = va.GetAbsolutePolygon(ourState);
            vehiclePoly = Polygon.ConvexMinkowskiConvolution(circ, vehiclePoly);
            List<Coordinates> pointsOutside = new List<Coordinates>();
            ArbiterLanePartition alp = al.GetClosestPartition(va.ClosestPosition);

            foreach(Coordinates c in vehiclePoly)
            {
                if (!p.IsInside(c))
                    pointsOutside.Add(c);
            }

            foreach (Coordinates m in pointsOutside)
            {
                foreach (Coordinates n in pointsOutside)
                {
                    if(!m.Equals(n))
                    {
                        if (GeneralToolkit.TriangleArea(alp.Initial.Position, m, alp.Final.Position) *
                            GeneralToolkit.TriangleArea(alp.Initial.Position, n, alp.Final.Position) < 0)
                        {
                            return false;
                        }
                    }
                }
            }

            return true;
        }
        /// <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>
 /// If this is clear or not at this current timestep
 /// </summary>
 public bool Clear(VehicleState ourState)
 {
     if (!isOurs)
         return this.currentVehicle == null;
     else
         return false;
 }
        /// <summary>
        /// Generic plan
        /// </summary>
        /// <param name="planningState"></param>
        /// <param name="navigationalPlan"></param>
        /// <param name="vehicleState"></param>
        /// <param name="vehicles"></param>
        /// <param name="obstacles"></param>
        /// <param name="blockage"></param>
        /// <returns></returns>
        public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState,
            SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles,
            List<ITacticalBlockage> blockages, double vehicleSpeed)
        {
            // update stuff
            this.Update(vehicles, vehicleState);

            #region Plan Roads

            if (planningState is TravelState)
            {
                Maneuver roadFinal = roadTactical.Plan(
                    planningState,
                    (RoadPlan)navigationalPlan,
                    vehicleState,
                    vehicles,
                    obstacles,
                    blockages,
                    vehicleSpeed);

                // return
                return roadFinal;
            }

            #endregion

            #region Plan Intersection

            else if (planningState is IntersectionState)
            {
                Maneuver intersectionFinal = intersectionTactical.Plan(
                    planningState,
                    navigationalPlan,
                    vehicleState,
                    vehicles,
                    obstacles,
                    blockages);

                // return
                return intersectionFinal;
            }

            #endregion

            #region Plan Zone

            else if (planningState is ZoneState)
            {
                Maneuver zoneFinal = zoneTactical.Plan(
                    planningState,
                    navigationalPlan,
                    vehicleState,
                    vehicles,
                    obstacles,
                    blockages);

                // return
                return zoneFinal;
            }

            #endregion

            #region Plan Blockage

            else if (planningState is BlockageState)
            {
                Maneuver blockageFinal = blockageTactical.Plan(
                    planningState,
                    vehicleState,
                    vehicleSpeed,
                    blockages,
                    navigationalPlan);

                return blockageFinal;
            }

            #endregion

            #region Unknown

            else
            {
                throw new Exception("Unknown planning state type: " + planningState.GetType().ToString());
            }

            #endregion
        }
        /// <summary>
        /// Checks if this area is completely clear of vehicles and no chance of one coming in soon
        /// </summary>
        /// <param name="ourState"></param>
        /// <returns></returns>
        public bool IsCompletelyClear(VehicleState ourState)
        {
            // amke sure not ours
            if (!isOurs)
            {
                // get the possible vehicles that could be in this stop
                if (TacticalDirector.VehicleAreas.ContainsKey(this.stoppedExit.Waypoint.Lane))
                {
                    // get vehicles in the the lane of the exit
                    List<VehicleAgent> possibleVehicles = TacticalDirector.VehicleAreas[this.stoppedExit.Waypoint.Lane];

                    // inflate the polygon
                    Polygon exitPolygon = this.stoppedExit.ExitPolygon.Inflate(TahoeParams.VL);

                    // check vehicles in lane
                    foreach (VehicleAgent va in possibleVehicles)
                    {
                        // check if vehicle inside inflated exit polygon
                        if (exitPolygon.IsInside(va.ClosestPosition))
                        {
                            return false;
                        }
                    }
                }

                // clear if made it this far
                return true;
            }
            else
            {
                // ours is always clear relative to us
                return true;
            }
        }