public bool ForwardClear(VehicleState state, double usDistanceToTravel, double usAvgSpeed, LaneChangeInformation information, Coordinates minReturn)
 {
     throw new Exception("The method or operation is not implemented.");
 }
 public bool ForwardClear(VehicleState state, double usDistanceToTravel, double usAvgSpeed, LaneChangeInformation information, Coordinates minReturn)
 {
     if (information.Reason == LaneChangeReason.FailedForwardVehicle)
         return this.ForwardMonitor.ClearForDisabledVehiclePass(lane, state, CoreCommon.Communications.GetVehicleSpeed().Value, minReturn);
     else
         throw new Exception("standard forward clear in opposing lateral reasoning not imp");
 }
        /// <summary>
        /// Distinctly want to make lane change, parameters for doing so
        /// </summary>
        /// <param name="arbiterLane"></param>
        /// <param name="left"></param>
        /// <param name="vehicleState"></param>
        /// <param name="roadPlan"></param>
        /// <param name="blockages"></param>
        /// <param name="ignorable"></param>
        /// <returns></returns>
        public Maneuver? LaneChangeManeuver(ArbiterLane lane, bool left, ArbiterWaypoint goal, VehicleState vehicleState,
            List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, LaneChangeInformation laneChangeInformation, Maneuver? secondary,
            out LaneChangeParameters parameters)
        {
            // distance until the change is complete
            double distanceToUpperBound = lane.DistanceBetween(vehicleState.Front, goal.Position);
            double neededDistance = (1.5 * TahoeParams.VL * Math.Max(Math.Abs(goal.Lane.LaneId.Number - lane.LaneId.Number), 1)) +
                (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));

            parameters = new LaneChangeParameters();
            if (distanceToUpperBound < neededDistance)
                return null;

            Coordinates upperBound = new Coordinates();
            Coordinates upperReturnBound = new Coordinates();
            Coordinates minimumReturnBound = new Coordinates();
            Coordinates defaultReturnBound = new Coordinates();

            if (laneChangeInformation.Reason == LaneChangeReason.FailedForwardVehicle)
            {
                double distToForwards = Math.Min(neededDistance, lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) - 2.0);
                upperBound = lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), distToForwards).Location;
                defaultReturnBound = lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition), TahoeParams.VL * 4.0).Location;
            }

            // get params for lane change
            LaneChangeParameters? changeParams = this.LaneChangeParameterization(
                laneChangeInformation,
                lane, left ? lane.LaneOnLeft : lane.LaneOnRight, false, goal.Position, upperBound,
                new Coordinates(), new Coordinates(), defaultReturnBound, blockages, ignorable,
                vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);

            // set lane change params
            parameters = changeParams.HasValue ? changeParams.Value : parameters = new LaneChangeParameters();

            // check if the lane change is available or recommended
            if (changeParams != null && changeParams.Value.Feasible)
            {
                // minimize parameterizations
                List<TravelingParameters> tps = new List<TravelingParameters>();

                tps.Add(this.ForwardMonitor.LaneParameters);
                tps.Add(changeParams.Value.Parameters);
                if(this.ForwardMonitor.FollowingParameters.HasValue)
                    tps.Add(this.ForwardMonitor.FollowingParameters.Value);

                tps.Sort();

                // check if possible to make lane change
                if (changeParams.Value.Available)
                {
                    // get traveling params from FQM to make sure we stopped for vehicle, behind vehicle
                    double v = CoreCommon.Communications.GetVehicleSpeed().Value;

                    // just use other params with shorted distance bound
                    TravelingParameters final = tps[0];

                    // final behavior
                    ChangeLaneBehavior clb = new ChangeLaneBehavior(
                        lane.LaneId,
                        parameters.Target.LaneId,
                        left,
                        final.DistanceToGo,
                        final.SpeedCommand,
                        final.VehiclesToIgnore,
                        lane.LanePath(),
                        parameters.Target.LanePath(),
                        lane.Width,
                        parameters.Target.Width,
                        lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true));

                    // final state
                    ChangeLanesState cls = new ChangeLanesState(changeParams.Value);

                    // return maneuver
                    return new Maneuver(clb, cls, left ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, vehicleState.Timestamp);
                }
                // otherwise plan for requirements of change coming up
                else
                {
                    // check if secondary exists
                    if (secondary != null)
                    {
                        return secondary;
                    }
                    // otherwise plan for upcoming
                    else
                    {
                        // get params
                        TravelingParameters final = tps[0];

                        // return maneuver
                        return new Maneuver(tps[0].Behavior, tps[0].NextState, this.ForwardMonitor.NavigationParameters.Decorators, vehicleState.Timestamp);
                    }
                }
            }

            // return null over fallout
            return null;
        }
        /// <summary>
        /// Generates the lane change parameterization
        /// </summary>
        /// <param name="information"></param>
        /// <param name="initial"></param>
        /// <param name="final"></param>
        /// <param name="goal"></param>
        /// <param name="departUpperBound"></param>
        /// <param name="defaultReturnLowerBound"></param>
        /// <param name="minimumReturnComplete"></param>
        /// <param name="defaultReturnUpperBound"></param>
        /// <param name="blockages"></param>
        /// <param name="ignorable"></param>
        /// <param name="state"></param>
        /// <param name="speed"></param>
        /// <returns></returns>
        public LaneChangeParameters? LaneChangeParameterization(LaneChangeInformation information, ArbiterLane initial, ArbiterLane target,
            bool targetOncoming, Coordinates goal, Coordinates departUpperBound, Coordinates defaultReturnLowerBound, Coordinates minimumReturnComplete,
            Coordinates defaultReturnUpperBound, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, VehicleState state, double speed)
        {
            // check if the target lane exists here
            bool validTarget = target.LanePath().GetClosestPoint(state.Front).Location.DistanceTo(state.Front) < 17 && target.IsInside(state.Front);

            // params
            bool toLeft = initial.LaneOnLeft != null ? initial.LaneOnLeft.Equals(target) || (targetOncoming && !initial.Way.WayId.Equals(target.Way.WayId)) : false;

            // get appropriate lateral reasoning
            ILateralReasoning lateralReasoning = toLeft ? this.leftLateralReasoning : this.rightLateralReasoning;

            #region Target Lane Valid Here

            // check if the target is currently valid
            if (validTarget)
            {
                // lane change parameterizations
                List<LaneChangeParameters> lcps = new List<LaneChangeParameters>();

                // distance to the current goal (means different things for all)
                double xGoal = initial.DistanceBetween(state.Front, goal);

                // get next stop
                List<WaypointType> types = new List<WaypointType>();
                types.Add(WaypointType.Stop);
                types.Add(WaypointType.End);
                ArbiterWaypoint nextMajor = initial.GetNext(state.Front, types, ignorable);
                double xLaneMajor = initial.DistanceBetween(state.Front, nextMajor.Position);
                xGoal = Math.Min(xGoal, xLaneMajor);

                #region Failed Vehicle Lane Change

                if (information.Reason == LaneChangeReason.FailedForwardVehicle)
                {
                    #region Target Opposing

                    // check if target lane backwards
                    if (targetOncoming)
                    {
                        // available and feasible
                        bool avail = false;
                        bool feas = false;

                        // check if min return distance < goal distance
                        double xReturnMin = initial.DistanceBetween(state.Front, minimumReturnComplete);
                        double xDepart = initial.DistanceBetween(state.Front, departUpperBound);

                        // dist to upper bound along lane and dist to end of adjacent lane
                        double adjLaneDist = initial.DistanceBetween(state.Front, minimumReturnComplete);

                        // this is feasible
                        feas = xGoal > xReturnMin ? true : false;

                        // check if not feasible that the goal is the current checkpoint
                        if (!feas && CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId].Position.Equals(goal))
                            feas = true;

                        // check adj and rear clear
                        bool adjRearClear = lateralReasoning.AdjacentAndRearClear(state);

                        // check if forwards clear
                        bool frontClear = lateralReasoning.ForwardClear(state, xReturnMin, 2.24, information, minimumReturnComplete);

                        Console.WriteLine("Adjacent, Rear: " + adjRearClear.ToString() + ", Forward: " + frontClear.ToString());

                        // if clear
                        if (frontClear && adjRearClear)
                        {
                            // notify
                            ArbiterOutput.Output("Lane Change Params: Target Oncoming Failed Vehicle: Adjacent, Rear, and Front Clear");

                            // available
                            avail = true;

                            // get lateral parameterization
                            TravelingParameters lateralParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal,
                                state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state));

                            // change into the opposing lane wih opposing forward parameterization
                            LaneChangeParameters lcp = new LaneChangeParameters(avail, feas, initial, false, target, targetOncoming, toLeft, null,
                                xDepart, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, lateralParams,
                                departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, information.Reason);

                            // we have been forced
                            lcp.ForcedOpposing = true;

                            // return created params
                            return lcp;
                        }

                        // fell through for some reason, return parameterization explaining why
                        LaneChangeParameters fallThroughParams = new LaneChangeParameters(avail, feas, initial, false, target, targetOncoming, toLeft, null,
                            xDepart, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, this.ForwardMonitor.LaneParameters,
                            departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, information.Reason);

                        // return fall through parameters
                        return fallThroughParams;
                    }

                    #endregion

                    #region Target Forwards

                    // otherwise target lane forwards
                    else
                    {
                        // check lateral clear and initial lane does not run out
                        if (lateralReasoning.AdjacentAndRearClear(state) && !initial.GetClosestPoint(defaultReturnUpperBound).Equals(initial.LanePath().EndPoint))
                        {
                            // notify
                            ArbiterOutput.Output("Lane Change Params: Failed Vehicle Target Forwards: Adjacent and Rear Clear");

                            // dist to upper bound along lane and dist to end of adjacent lane
                            double distToReturn = initial.DistanceBetween(state.Front, defaultReturnUpperBound);
                            double adjLaneDist = initial.DistanceBetween(state.Front, target.LanePath().EndPoint.Location);

                            // check enough lane room to pass
                            if (distToReturn < adjLaneDist && distToReturn <= initial.DistanceBetween(state.Front, goal))
                            {
                                // check enough room to change lanes
                                ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable);
                                double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position);

                                // check dist needed to complete
                                double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) +
                                (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));

                                // check return dist
                                if (distToReturn < xTargetLaneMajor && neededDistance <= xTargetLaneMajor)
                                {
                                    // parameters for traveling in current lane to hit other
                                    List<TravelingParameters> tps = new List<TravelingParameters>();

                                    // update lateral
                                    ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.Update(target, state);

                                    // check lateral reasoning for forward vehicle badness
                                    if (!((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker ||
                                        !((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped ||
                                        initial.DistanceBetween(state.Front, ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) >= distToReturn)
                                    {
                                        // get parameterization for lateral lane
                                        TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane,
                                            goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state));
                                        tps.Add(navParams);

                                        // get and add the vehicle parameterization for our lane
                                        if (this.ForwardMonitor.FollowingParameters.HasValue)
                                            tps.Add(this.ForwardMonitor.FollowingParameters.Value);

                                        // get final
                                        tps.Sort();
                                        TravelingParameters final = tps[0];

                                        // check final distance > needed
                                        if (navParams.DistanceToGo > neededDistance)
                                        {
                                            // set ignorable vhcs
                                            final.VehiclesToIgnore = this.ForwardMonitor.FollowingParameters.HasValue ? this.ForwardMonitor.FollowingParameters.Value.VehiclesToIgnore : new List<int>();
                                            if (((LateralReasoning)lateralReasoning).ForwardMonitor.FollowingParameters.HasValue)
                                                final.VehiclesToIgnore.AddRange(((LateralReasoning)lateralReasoning).ForwardMonitor.FollowingParameters.Value.VehiclesToIgnore);

                                            // parameterize
                                            LaneChangeParameters lcp = new LaneChangeParameters();
                                            lcp.Decorators = TurnDecorators.RightTurnDecorator;
                                            lcp.Available = true;
                                            lcp.Feasible = true;
                                            lcp.Parameters = final;
                                            lcp.Initial = initial;
                                            lcp.InitialOncoming = false;
                                            lcp.Target = target;
                                            lcp.TargetOncoming = false;
                                            lcp.Reason = LaneChangeReason.FailedForwardVehicle;
                                            lcp.DefaultReturnLowerBound = defaultReturnLowerBound;
                                            lcp.DefaultReturnUpperBound = defaultReturnUpperBound;
                                            lcp.DepartUpperBound = departUpperBound;
                                            lcp.MinimumReturnComplete = minimumReturnComplete;
                                            return lcp;
                                        }
                                    }
                                }
                            }
                        }

                        // otherwise infeasible
                        return null;
                    }

                    #endregion
                }

                #endregion

                #region Navigation Lane Change

                else if (information.Reason == LaneChangeReason.Navigation)
                {
                    // parameters for traveling in current lane to hit other
                    List<TravelingParameters> tps = new List<TravelingParameters>();

                    // get navigation parameterization
                    TravelingParameters lateralParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane,
                        goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state));
                    tps.Add(lateralParams);

                    // get and add the nav parameterization relative to our lane
                    tps.Add(this.ForwardMonitor.NavigationParameters);

                    // check avail
                    bool adjRearAvailable = lateralReasoning.AdjacentAndRearClear(state);

                    // if they are available we are in good shape, need to slow for nav, forward vehicles
                    if (adjRearAvailable)
                    {
                        // notify
                        ArbiterOutput.Output("Lane Change Params: Navigation: Adjacent and Rear Clear");

                        #region Check Forward and Lateral Vehicles

                        if (this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle && lateralParams.Type == TravellingType.Vehicle)
                        {
                            // check enough room to change lanes
                            ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable);
                            double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position);

                            // distnace to goal
                            double goalDist = initial.DistanceBetween(state.Front, goal);

                            // check dist needed to complete
                            double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) +
                            (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));

                            // check for proper distances
                            if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance)
                            {
                                // check distance to return (weeds out safety zone crap
                                Coordinates lateralVehicle = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition;
                                double distToReturn = initial.DistanceBetween(state.Front, initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(lateralVehicle), 30.0).Location);

                                // check passing params
                                LaneChangeInformation lci;
                                bool shouldPass = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.ShouldPass(out lci);

                                // check passing params
                                LaneChangeInformation lciInit;
                                bool shouldPassInit = this.ForwardMonitor.ForwardVehicle.ShouldPass(out lciInit);

                                // check forward lateral stopped and enough distance to go around and not vehicles between it and goal close enough to stop
                                if(shouldPass && lci.Reason == LaneChangeReason.FailedForwardVehicle && goalDist > distToReturn &&
                                    (!shouldPassInit || lciInit.Reason != LaneChangeReason.FailedForwardVehicle || this.ForwardMonitor.CurrentParameters.DistanceToGo > lateralParams.DistanceToGo + TahoeParams.VL * 5))
                                {
                                    // return that we should pass it as normal in the initial lane
                                    return null;
                                }

                                // check get distance to upper
                                double xUpper = this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped ? Math.Min(goalDist, neededDistance) : this.ForwardMonitor.ForwardVehicle.ForwardControl.xSeparation - 2;
                                Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), xUpper).Location;

                                // add current params if not stopped
                                if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
                                    tps.Add(this.ForwardMonitor.CurrentParameters);

                                // get final
                                tps.Sort();
                                TravelingParameters final = tps[0];

                                // parameterize
                                LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft,
                                    null, final.DistanceToGo - 3.0, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator,
                                    final, upper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation);

                                return lcp;
                            }
                        }

                        #endregion

                        #region Check Forward Vehicle

                        else if (this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle)
                        {
                            // check enough room to change lanes
                            ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable);
                            double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position);

                            // distnace to goal
                            double goalDist = initial.DistanceBetween(state.Front, goal);

                            // check dist needed to complete
                            double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) +
                            (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));

                            // check for proper distances
                            if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance)
                            {
                                // add current params if not stopped
                                if(!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
                                    tps.Add(this.ForwardMonitor.CurrentParameters);

                                // get final
                                tps.Sort();
                                TravelingParameters final = tps[0];

                                // check get distance to upper
                                double xUpper = this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped ? neededDistance : this.ForwardMonitor.ForwardVehicle.ForwardControl.xSeparation - 2;
                                Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), xUpper).Location;

                                // parameterize
                                LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft,
                                    null, final.DistanceToGo - 3.0, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator,
                                    final, upper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation);

                                return lcp;
                            }
                        }

                        #endregion

                        #region Lateral Vehicle

                        // check to see if should use the forward tracker, i.e. forward vehicle exists
                        else if (lateralParams.Type == TravellingType.Vehicle)
                        {
                            // add current params
                            tps.Add(this.ForwardMonitor.CurrentParameters);

                            // check enough room to change lanes
                            ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable);
                            double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position);

                            // distnace to goal
                            double goalDist = initial.DistanceBetween(state.Front, goal);

                            // check dist needed to complete
                            double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) +
                            (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));

                            // check for proper distances
                            if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance)
                            {
                                // check distance to return (weeds out safety zone crap
                                Coordinates lateralVehicle = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition;
                                double distToReturn = initial.DistanceBetween(state.Front, initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(lateralVehicle), 30.0).Location);

                                // check passing params
                                LaneChangeInformation lci;
                                bool shouldPass = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.ShouldPass(out lci);

                                // check forward lateral stopped and enough distance to go around and not vehicles between it and goal close enough to stop
                                if (shouldPass && lci.Reason == LaneChangeReason.FailedForwardVehicle && goalDist > distToReturn)
                                {
                                    // return that we should pass it as normal in the initial lane
                                    return null;
                                }

                                // check if we are already slowed for this vehicle and are at a good distance from it
                                if (speed < lateralParams.RecommendedSpeed + 1.0)
                                {
                                    // get final
                                    tps.Sort();
                                    TravelingParameters final = tps[0];

                                    // upper bound is nav bound
                                    Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), Math.Min(neededDistance, final.DistanceToGo)).Location;

                                    // parameterize
                                    LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft,
                                        null, final.DistanceToGo - 3.0, null, TurnDecorators.LeftTurnDecorator, final, upper, new Coordinates(),
                                        new Coordinates(), new Coordinates(), LaneChangeReason.Navigation);

                                    return lcp;
                                }
                                // otherwise need to slow for it or other
                                else
                                {
                                    // get final
                                    tps.Sort();
                                    TravelingParameters final = tps[0];

                                    // upper bound is nav bound
                                    Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), final.DistanceToGo).Location;

                                    // parameterize
                                    LaneChangeParameters lcp = new LaneChangeParameters(false, true, initial, false, target, false, toLeft,
                                        null, final.DistanceToGo - 3.0, null, TurnDecorators.LeftTurnDecorator, final, new Coordinates(), new Coordinates(),
                                        new Coordinates(), new Coordinates(), LaneChangeReason.Navigation);

                                    return lcp;
                                }
                            }
                        }

                        #endregion

                        #region No forward or lateral

                        // otherwise just go!
                        else
                        {
                            // add current params
                            tps.Add(this.ForwardMonitor.CurrentParameters);

                            // check enough room to change lanes
                            ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable);
                            double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position);

                            // distnace to goal
                            double goalDist = initial.DistanceBetween(state.Front, goal);

                            // check dist needed to complete
                            double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) +
                            (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));

                            // check for proper distances
                            if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance)
                            {
                                // get final
                                tps.Sort();
                                TravelingParameters final = tps[0];

                                // upper bound is nav bound
                                Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), Math.Min(neededDistance, final.DistanceToGo)).Location;

                                // parameterize
                                LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft,
                                    null, final.DistanceToGo, null, TurnDecorators.LeftTurnDecorator, final, upper, new Coordinates(),
                                    new Coordinates(), new Coordinates(), LaneChangeReason.Navigation);

                                // return the parameterization
                                return lcp;
                            }
                        }

                        #endregion
                    }
                    // otherwise we need to determine how to make this available
                    else
                    {
                        // notify
                        ArbiterOutput.Output("Lane Change Params: Navigation Adjacent and Rear NOT Clear");

                        // gets the adjacent vehicle
                        VehicleAgent adjacent = lateralReasoning.AdjacentVehicle;

                        // add current params
                        tps.Add(this.ForwardMonitor.CurrentParameters);

                        #region Pass Adjacent

                        // checks if it is failed for us to pass it
                        if (adjacent != null && (adjacent.IsStopped || adjacent.Speed < 1.5))
                        {
                            // get final
                            List<TravelingParameters> adjacentTravelingParams = new List<TravelingParameters>();
                            adjacentTravelingParams.Add(this.ForwardMonitor.CurrentParameters);
                            adjacentTravelingParams.Add(this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, null));

                            adjacentTravelingParams.Sort();
                            //tps.Sort();
                            TravelingParameters final = adjacentTravelingParams[0];// tps[0];

                            // parameterize
                            LaneChangeParameters lcp = new LaneChangeParameters();
                            lcp.Available = false;
                            lcp.Feasible = true;
                            lcp.Parameters = final;
                            return lcp;
                        }

                        #endregion

                        #region Follow Adjacent

                        // otherwise we need to follow it, as it is lateral, this means 0.0 speed
                        else if (adjacent != null)
                        {
                            // get and add the vehicle parameterization relative to our lane
                            TravelingParameters tmp = new TravelingParameters();
                            tmp.Behavior = new StayInLaneBehavior(initial.LaneId, new ScalarSpeedCommand(0.0), this.ForwardMonitor.CurrentParameters.VehiclesToIgnore,
                                initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(state.Front, true), initial.NumberOfLanesRight(state.Front, true));
                            tmp.NextState = CoreCommon.CorePlanningState;

                            // parameterize
                            LaneChangeParameters lcp = new LaneChangeParameters();
                            lcp.Available = false;
                            lcp.Feasible = true;
                            lcp.Parameters = tmp;
                            return lcp;
                        }

                        #endregion

                        #region Wait for the rear vehicle

                        else
                        {
                            TravelingParameters tp = new TravelingParameters();
                            tp.SpeedCommand = new ScalarSpeedCommand(0.0);
                            tp.UsingSpeed = true;
                            tp.DistanceToGo = 0.0;
                            tp.VehiclesToIgnore = new List<int>();
                            tp.RecommendedSpeed = 0.0;
                            tp.NextState = CoreCommon.CorePlanningState;
                            tp.Behavior = new StayInLaneBehavior(initial.LaneId, tp.SpeedCommand, new List<int>(), initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(state.Front, true), initial.NumberOfLanesRight(state.Front, true));

                            // parameterize
                            LaneChangeParameters lcp = new LaneChangeParameters();
                            lcp.Available = false;
                            lcp.Feasible = true;
                            lcp.Parameters = tp;
                            return lcp;
                        }

                        #endregion
                    }
                }

                #endregion

                #region Passing Lane Change

                else if (information.Reason == LaneChangeReason.SlowForwardVehicle)
                {
                    throw new Exception("passing slow vehicles not yet supported");
                }

                #endregion

                // fallout returns null
                return null;
            }

            #endregion

            #region Target Lane Not Valid, Plan Navigation

            // otherwise plan for when we approach target if this is a navigational change
            else if(information.Reason == LaneChangeReason.Navigation)
            {
                // parameters for traveling in current lane to hit other
                List<TravelingParameters> tps = new List<TravelingParameters>();

                // add current params
                tps.Add(this.ForwardMonitor.CurrentParameters);

                // distance between front of car and start of lane
                if (target.RelativelyInside(state.Front) ||
                    initial.DistanceBetween(state.Front, target.LanePath().StartPoint.Location) > 0)
                {
                    #region Vehicle	and Navigation

                    // check to see if we're not looped around wierd
                    if (lateralReasoning.LateralLane.LanePath().GetClosestPoint(state.Front).Equals(lateralReasoning.LateralLane.LanePath().StartPoint))
                    {
                        // initialize the forward tracker in the other lane
                        ForwardVehicleTracker fvt = new ForwardVehicleTracker();
                        fvt.Update(lateralReasoning.LateralLane, state);

                        // check to see if should use the forward tracker
                        if (fvt.ShouldUseForwardTracker)
                        {
                            // get navigation parameterization
                            TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane,
                                goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state));
                            tps.Add(navParams);
                        }
                        else
                        {
                            // get navigation parameterization
                            TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane,
                                goal, state.Front, CoreCommon.CorePlanningState, state, null);
                            tps.Add(navParams);
                        }
                    }

                    #endregion

                    #region Navigation

                    // check to see that nav point is downstream of us
                    else if (initial.DistanceBetween(state.Front, goal) > 0.0)
                    {
                        // get navigation parameterization
                        TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane,
                            goal, state.Front, CoreCommon.CorePlanningState, state, null);
                        tps.Add(navParams);
                    }

                    #endregion

                    else
                    {
                        return null;
                    }
                }
                else
                    return null;

                // get final
                tps.Sort();
                TravelingParameters final = tps[0];

                // parameterize
                LaneChangeParameters lcp = new LaneChangeParameters();
                lcp.Available = false;
                lcp.Feasible = true;
                lcp.Parameters = final;
                return lcp;
            }

            #endregion

            // fallout return null
            return null;
        }
 /// <summary>
 /// Plans a lane change from the initial lane to the adjacent lane
 /// </summary>
 /// <param name="initial"></param>
 /// <param name="adjacent"></param>
 /// <param name="vehicleState"></param>
 /// <param name="forwardVehicle"></param>
 /// <returns></returns>
 public Maneuver? PlanLaneChange(ArbiterLane initial, ArbiterWaypoint goal, LateralReasoning adjacent, VehicleState vehicleState, VehicleAgent forwardVehicle, LaneChangeInformation lci)
 {
     bool toLeft = initial.LaneOnLeft != null && adjacent.LateralLane.Equals(initial.LaneOnLeft);
     LaneChangeParameters updatedLCP;
     return this.LaneChangeManeuver(initial, toLeft, goal, vehicleState, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), lci, null, out updatedLCP);
 }
        /// <summary>
        /// Distinctly want to make lane change, parameters for doing so
        /// </summary>
        /// <param name="arbiterLane"></param>
        /// <param name="left"></param>
        /// <param name="vehicleState"></param>
        /// <param name="roadPlan"></param>
        /// <param name="blockages"></param>
        /// <param name="ignorable"></param>
        /// <returns></returns>
        public Maneuver? AdvancedDesiredLaneChangeManeuver(ArbiterLane lane, bool left, ArbiterWaypoint goal, RoadPlan rp, VehicleState vehicleState,
            List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, LaneChangeInformation laneChangeInformation, Maneuver? secondary,
            out LaneChangeParameters parameters)
        {
            // set aprams
            parameters = new LaneChangeParameters();

            // get final maneuver
            Maneuver? final = null;

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

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

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

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

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

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

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

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

                    return new Maneuver(tpCatch.Behavior, tpCatch.NextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                }
                else
                {
                    return secondary;
                }
            }
            else
            {
                return final;
            }
        }
        /// <summary>
        /// Checks if we should pass the forward vehicle
        /// </summary>
        /// <param name="lci"></param>
        /// <param name="lane"></param>
        /// <returns></returns>
        public bool ShouldPass(out LaneChangeInformation lci, ArbiterLane lane)
        {
            // passing reason set to none by default
            lci = new LaneChangeInformation(LaneChangeReason.NotApplicable, this.CurrentVehicle);

            // check the queuing state of the forward vehicle
            if (this.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed)
            {
                lci = new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.CurrentVehicle);
                return true;
            }

            // check inside any safety zone
            foreach (ArbiterSafetyZone asz in lane.SafetyZones)
            {
                if (asz.IsInSafety(this.CurrentVehicle.ClosestPosition))
                    return false;
            }
            foreach (ArbiterIntersection ai in CoreCommon.RoadNetwork.ArbiterIntersections.Values)
            {
                if (ai.IntersectionPolygon.IsInside(this.CurrentVehicle.ClosestPosition))
                    return false;
            }

            if ((this.CurrentVehicle.Speed < CoreCommon.Communications.GetVehicleSpeed().Value ||
                (this.CurrentVehicle.IsStopped && this.CurrentVehicle.StateMonitor.Observed.speedValid)) &&
                this.CurrentVehicle.Speed < 0.7 * lane.Way.Segment.SpeedLimits.MaximumSpeed)
            {
                lci = new LaneChangeInformation(LaneChangeReason.SlowForwardVehicle, this.CurrentVehicle);
                return true;
            }

            // fall out
            return false;
        }
        /// <summary>
        /// Returns if we should pass or nor
        /// </summary>
        /// <returns></returns>
        public bool ShouldPass(out LaneChangeInformation lci)
        {
            // passing reason set to none by default
            lci = new LaneChangeInformation(LaneChangeReason.NotApplicable, this.CurrentVehicle);

            // check the queuing state of the forward vehicle
            if (this.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed)
            {
                lci = new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.CurrentVehicle);
                return true;
            }
            else
            {
                // check if moving much too slow for the next stop for long period of time
                #warning need to implement slow moving vehicle pass after beta

                // for now return false
                return false;
            }
        }
        /// <summary>
        /// Behavior we would like to do other than going straight
        /// </summary>
        /// <param name="arbiterLane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="p"></param>
        /// <param name="blockages"></param>
        /// <returns></returns>
        /// <remarks>tries to go right, if not goest left if needs 
        /// to if forward vehicle ahead and we're stopped because of them</remarks>
        public Maneuver? SecondaryManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, List<ITacticalBlockage> blockages,
            LaneChangeParameters? entryParameters)
        {
            // check blockages
            if (blockages != null && blockages.Count > 0 && blockages[0] is OpposingLaneBlockage)
            {
                // 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);
            }

            // check dist needed to complete
            double neededDistance = (Math.Abs(arbiterLane.LaneId.Number - closestGood.LaneId.Number) * 1.5 * TahoeParams.VL) +
            (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));

            // get upper bound
            LinePath.PointOnPath xFront = arbiterLane.LanePath().GetClosestPoint(vehicleState.Front);
            Coordinates xUpper = arbiterLane.LanePath().AdvancePoint(xFront, -neededDistance).Location;

            if (entryParameters.HasValue)
            {
                // check if we should get back, keep speed nice n' lowi fpassing failed
                if (entryParameters.Value.Reason == LaneChangeReason.FailedForwardVehicle)
                {
                    double xToReturn = arbiterLane.DistanceBetween(entryParameters.Value.DefaultReturnLowerBound, vehicleState.Front);
                    if(xToReturn >= 0.0)
                        ArbiterOutput.Output("Distance until must return to lane: " + xToReturn);
                    else
                        ArbiterOutput.Output("Can return to lane from arbitrary upper bound: " + xToReturn);

                    // check can return
                    if (xToReturn < 0)
                    {
                        // check if right lateral exists exactly here
                        if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood))
                        {
                            ArbiterOutput.Output("Right lateral reasoning good and exists exactly here");
                            return this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true);
                        }
                        else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood))
                        {
                            ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here");

                            if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood))
                                this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger);

                            if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState))
                            {
                                ILateralReasoning tmpReasoning = this.rightLateralReasoning;
                                this.rightLateralReasoning = this.secondaryLateralReasoning;
                                Maneuver? tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true);
                                this.rightLateralReasoning = tmpReasoning;
                                return tmp;
                            }
                            else
                            {
                                ArbiterOutput.Output("Cosest good lane does not exist here??");
                                return null;
                            }
                        }
                        else
                        {
                            ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() +
                                ", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString());
                            return null;
                        }
                    }
                    else
                        return null;
                }
            }

            // lane change info
            LaneChangeInformation lci = new LaneChangeInformation(LaneChangeReason.Navigation, null);

            // notify
            ArbiterOutput.Output("In Opposing with no Previous state knowledge, attempting to return");

            // check if right lateral exists exactly here
            if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood))
            {
                ArbiterOutput.Output("Right lateral reasoning good and exists exactly here");
                return this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false);
            }
            else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood))
            {
                ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here");

                if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood))
                    this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger);

                if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState))
                {
                    ILateralReasoning tmpReasoning = this.rightLateralReasoning;
                    this.rightLateralReasoning = this.secondaryLateralReasoning;
                    Maneuver? tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false);
                    this.rightLateralReasoning = tmpReasoning;
                    return tmp;
                }
                else
                {
                    ArbiterOutput.Output("Cosest good lane does not exist here??");
                    return null;
                }
            }
            else
            {
                ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() +
                    ", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString());
                return null;
            }
        }