private ITrackingCommand BuildBackwardPass()
        {
            BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "UTurn Behavior: Initialize Backward Pass");

            // rotate the polygon into the current relative frame
            CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
            RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp);
            relTransform.TransformPointsInPlace(polygon);
            finalOrientation = finalOrientation.Transform(relTransform);
            polygonTimestamp = curTimestamp;

            // retrieve the vehicle state
            Coordinates headingVec = new Coordinates(1, 0);
            Coordinates headingVec180 = headingVec.Rotate180();
            Coordinates headingVec90 = headingVec.Rotate90();

            // figure out center point of turn
            Circle rearAxleCircle = Circle.FromPointSlopeRadius(new Coordinates(0,0), headingVec180, minRadius);
            Coordinates center = rearAxleCircle.center;

            // calculate the points of the wheels
            Coordinates rearLeftPoint = headingVec90*TahoeParams.T/2;
            Coordinates rearRightPoint = -headingVec90*TahoeParams.T/2;
            Coordinates frontLeftPoint = headingVec*TahoeParams.L + headingVec90*TahoeParams.T/2;
            Coordinates frontRightPoint = headingVec*TahoeParams.L - headingVec90*TahoeParams.T/2;

            double minHit = Math.PI/2.1;
            GetMinHitAngle(rearLeftPoint, center, ref minHit);
            GetMinHitAngle(rearRightPoint, center, ref minHit);
            //GetMinHitAngle(frontLeftPoint, center, false, ref minHit);
            //GetMinHitAngle(frontRightPoint, center, false, ref minHit);

            frontLeftPoint = headingVec*TahoeParams.FL + headingVec90*TahoeParams.T/2;
            frontRightPoint = headingVec*TahoeParams.FL - headingVec90*TahoeParams.T/2.0;
            rearRightPoint = -headingVec*TahoeParams.RL - headingVec90*TahoeParams.T/2.0;
            rearLeftPoint = -headingVec*TahoeParams.RL + headingVec90*TahoeParams.T/2.0;
            List<Polygon> obstacles = GetObstacles(curTimestamp);
            GetObstacleHitAngle(frontLeftPoint, center, obstacles, ref minHit);
            GetObstacleHitAngle(rearLeftPoint, center, obstacles, ref minHit);
            GetObstacleHitAngle(rearRightPoint, center, obstacles, ref minHit);

            // trim some off the hit for safety'
            minHit -= (0.3/minRadius);
            // move at least 0.6 meters
            minHit = Math.Max(minHit, 0.6 / minRadius);

            // calculate the exit stopping point
            // shift the line by the minimum turning radius
            Coordinates u = finalOrientation.P1 - finalOrientation.P0;
            u = u.Normalize().Rotate90();
            Line offsetLine = new Line();
            offsetLine.P0 = finalOrientation.P0 + u*(minRadius+2);
            offsetLine.P1 = finalOrientation.P1 + u*(minRadius+2);

            // final the intersection of the current turn circle with a radius of twice the min turn radius and the offset line
            Circle twoTurn = new Circle(2*minRadius + 2, center);
            Coordinates[] intersections;

            double startAngle = (-center).ArcTan;
            if (twoTurn.Intersect(offsetLine, out intersections)) {
                // figure out where there were hits
                for (int i = 0; i < intersections.Length; i++) {
                    // get the angle of the hit
                    double angle = (intersections[i] - center).ArcTan;

                    if (angle < startAngle)
                        angle += 2*Math.PI;

                    angle -= startAngle;

                    if (angle < minHit)
                        minHit = angle;
                }
            }

            minHit = Math.Max(minHit, 0.6 / minRadius);

            // set the stopping point at the min hit point
            Coordinates stopPoint = rearAxleCircle.GetPoint(startAngle+minHit);

            // calculate the stop distance
            stopDistance = rearAxleCircle.r*minHit;
            stopTimestamp = curTimestamp;
            curvature = 1/minRadius;
            // calculate the required steering angle
            double steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(-1/minRadius, uturnSpeed);

            ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(TransmissionGear.Reverse);
            ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);

            ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), uturnSpeed));
            ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);

            ChainedTrackingCommand cmd = new ChainedTrackingCommand(
                new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
                new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
            cmd.Label = backwardLabel;

            Services.UIService.PushCircle(new Circle(minRadius, center), curTimestamp, "uturn circle", true);
            Services.UIService.PushPoint(stopPoint, curTimestamp, "uturn stop point", true);
            Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true);

            return cmd;
        }
        private ITrackingCommand BuildForwardPass(out bool finalPass)
        {
            BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "UTurn Behavior: Determine Forward Pass");

            // rotate the polygon into the current relative frame
            CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
            RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp);
            relTransform.TransformPointsInPlace(polygon);
            finalOrientation = finalOrientation.Transform(relTransform);
            polygonTimestamp = curTimestamp;

            // retrieve the vehicle state
            Coordinates headingVec = new Coordinates(1, 0);
            Coordinates headingVec90 = headingVec.Rotate90();

            // check if we can make it out now
            Line curLine = new Line(new Coordinates(0,0), headingVec);
            Coordinates intersectionPoint;
            LineSegment finalLine = finalOrientation;
            Circle outCircle = Circle.FromLines(curLine, (Line)finalLine, out intersectionPoint);

            double steeringCommand;

            if (!outCircle.Equals(Circle.Infinite) && outCircle.r > minRadius) {
                // we found an out circle
                BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "found final pass output");

                // build the circle segment
                double hitAngle = (intersectionPoint - outCircle.center).ArcTan;
                double startAngle = (-outCircle.center).ArcTan;
                if (hitAngle < startAngle)
                    hitAngle += 2*Math.PI;

                hitAngle -= startAngle;

                if (stopOnLine) {
                    // get the angle of the end point of the line segment
                    double endPointAngle = (finalLine.P1 - outCircle.center).ArcTan;
                    if (endPointAngle < startAngle) endPointAngle += 2*Math.PI;
                    endPointAngle -= startAngle;

                    if (endPointAngle < hitAngle) {
                        hitAngle = endPointAngle;
                    }
                }

                // get the obstacles
                Coordinates frontLeftPoint = headingVec * TahoeParams.FL + headingVec90 * TahoeParams.T / 2;
                Coordinates frontRightPoint = headingVec * TahoeParams.FL - headingVec90 * TahoeParams.T / 2.0;
                Coordinates rearRightPoint = -headingVec * TahoeParams.RL - headingVec90 * TahoeParams.T / 2.0;
                List<Polygon> obstacles = GetObstacles(curTimestamp);
                GetObstacleHitAngle(frontLeftPoint, outCircle.center, obstacles, ref hitAngle);
                GetObstacleHitAngle(frontRightPoint, outCircle.center, obstacles, ref hitAngle);
                GetObstacleHitAngle(rearRightPoint, outCircle.center, obstacles, ref hitAngle);

                // calculate stopping distance
                stopDistance = outCircle.r*hitAngle;
                stopTimestamp = curTimestamp;
                curvature = 1/outCircle.r;

                intersectionPoint = outCircle.center + Coordinates.FromAngle(startAngle + hitAngle)*outCircle.r;

                // calculate steering angle
                steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1/outCircle.r, uturnSpeed);

                // mark that this will be the final pass
                finalPass = true;

                Services.UIService.PushCircle(outCircle, curTimestamp, "uturn circle", true);
                Services.UIService.PushPoint(intersectionPoint, curTimestamp, "uturn stop point", true);
                Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true);
            }
            else {
                finalPass = false;
                // draw out 3 circles
                //  - front right wheel
                //  - front left wheel
                //  - rear left wheel

                // figure out center point of turn
                Circle rearAxleCircle = Circle.FromPointSlopeRadius(Coordinates.Zero, headingVec, minRadius);
                Coordinates center = rearAxleCircle.center;

                // calculate the points of the wheels
                Coordinates rearLeftPoint = headingVec90*TahoeParams.T/2;
                Coordinates rearRightPoint = -headingVec90*TahoeParams.T/2;
                Coordinates frontLeftPoint = headingVec*TahoeParams.L + headingVec90*TahoeParams.T/2;
                Coordinates frontRightPoint = headingVec*TahoeParams.L - headingVec90*TahoeParams.T/2;

                // initialize min hit angle to slightly less than 90 degrees
                double minHit = Math.PI/2.1;
                //GetMinHitAngle(rearLeftPoint, center, true, ref minHit);
                //GetMinHitAngle(rearRightPoint, center, true, ref minHit);
                GetMinHitAngle(frontLeftPoint, center, ref minHit);
                GetMinHitAngle(frontRightPoint, center, ref minHit);

                // get the obstacles
                List<Polygon> obstacles = GetObstacles(curTimestamp);
                frontLeftPoint = headingVec*TahoeParams.FL + headingVec90*TahoeParams.T/2;
                frontRightPoint = headingVec*TahoeParams.FL - headingVec90*TahoeParams.T/2.0;
                rearRightPoint = -headingVec*TahoeParams.RL - headingVec90*TahoeParams.T/2.0;
                GetObstacleHitAngle(frontLeftPoint, center, obstacles, ref minHit);
                GetObstacleHitAngle(frontRightPoint, center, obstacles, ref minHit);
                GetObstacleHitAngle(rearRightPoint, center, obstacles, ref minHit);

                // trim some off the hit for safety
                //if (minHit > 0.5/minRadius)
                minHit -= (0.5/minRadius);
                minHit = Math.Max(minHit, 0.6 / minRadius);

                double startAngle = (Coordinates.Zero-center).ArcTan;
                double hitAngle = startAngle + minHit;

                // set the stopping point at the min hit point
                Coordinates stopPoint = rearAxleCircle.GetPoint(hitAngle);

                // calculate the stop distance
                stopDistance = minRadius*minHit;

                // calculate the required steering angle
                steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1/minRadius, uturnSpeed);

                Services.UIService.PushCircle(new Circle(minRadius, center), curTimestamp, "uturn circle", true);
                Services.UIService.PushPoint(stopPoint, curTimestamp, "uturn stop point", true);
                Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true);
            }

            // build the command
            ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(TransmissionGear.First);
            ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);

            ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), uturnSpeed));
            ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);

            ChainedTrackingCommand cmd = new ChainedTrackingCommand(
                new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
                new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
            cmd.Label = forwardLabel;

            return cmd;
        }
        private void ExecutePass()
        {
            // determine what to do

            // get the moving order
            AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
            curTimestamp = pose.timestamp;

            ParkerVehicleState state = new ParkerVehicleState(pose.xy, Coordinates.FromAngle(pose.heading));
            if (pulloutMode) {
                movingOrder = parker.GetNextPulloutOrder(state);
            }
            else {
                movingOrder = parker.GetNextParkingOrder(state);
            }

            // test for completion
            bool isPullinCompleted = false;
            if (parkingSpotLine.P0.DistanceTo(pose.xy) < 1.5 && Coordinates.FromAngle(pose.heading).Dot(parkingSpotLine.UnitVector) >= 0.5) {
                isPullinCompleted = true;
            }

            finalPass = false;
            if (movingOrder != null && movingOrder.DestState != null && parkingSpotLine.P0.DistanceTo(movingOrder.DestState.Loc) < 1.5 && parkingSpotLine.UnitVector.Dot(movingOrder.DestState.Heading) >= 0.5) {
                finalPass = true;
            }

            if (movingOrder.Completed || (isPullinCompleted && !pulloutMode)) {
                Type type;
                if (pulloutMode) {
                    type = typeof(ZoneParkingPullOutBehavior);
                }
                else {
                    type = typeof(ZoneParkingBehavior);
                }
                Services.BehaviorManager.ForwardCompletionReport(new SuccessCompletionReport(type));
                Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
            }
            else {
                // determine the stopping distance
                TransmissionGear targetGear;
                if (movingOrder.Forward) {
                    targetGear = TransmissionGear.First;
                }
                else {
                    targetGear = TransmissionGear.Reverse;
                }

                bool counterClockwise = false;
                if ((movingOrder.Forward && movingOrder.TurningRadius > 0) || (!movingOrder.Forward && movingOrder.TurningRadius < 0)) {
                    counterClockwise = true;
                }
                else {
                    counterClockwise = false;
                }

                double radius = pose.xy.DistanceTo(movingOrder.CenterPoint);
                double startAngle = (pose.xy - movingOrder.CenterPoint).ArcTan;
                double endAngle = (movingOrder.DestState.Loc - movingOrder.CenterPoint).ArcTan;

                if (counterClockwise) {
                    if (endAngle < startAngle) {
                        endAngle += 2*Math.PI;
                    }
                }
                else {
                    if (endAngle > startAngle) {
                        endAngle -= 2*Math.PI;
                    }
                }

                double stopDistance;
                if (Math.Abs(radius) < 100) {
                    stopDistance = Math.Abs(radius * (endAngle - startAngle));
                }
                else {
                    stopDistance = pose.xy.DistanceTo(movingOrder.DestState.Loc);
                }

                this.stopDist = stopDistance;
                this.stopTimestamp = curTimestamp;

                Services.UIService.PushPoint(movingOrder.DestState.Loc, curTimestamp, "uturn stop point", false);

                //double sign = movingOrder.Forward ? 1 : -1;
                double steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1/movingOrder.TurningRadius, parking_speed);
                ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(targetGear);
                ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);

                ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), parking_speed));
                ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);

                ChainedTrackingCommand cmd = new ChainedTrackingCommand(
                    new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
                    new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
                cmd.Label = command_label;

                Services.TrackingManager.QueueCommand(cmd);
            }

            passCompleted = false;
        }