public static void SmoothAndTrack(LinePath basePath, bool useTargetPath, 
            IList<LinePath> leftBounds, IList<LinePath> rightBounds,
            double maxSpeed, double? endingHeading, bool endingOffsetBound,
            CarTimestamp curTimestamp, bool doAvoidance,
            SpeedCommand speedCommand, CarTimestamp behaviorTimestamp,
            string commandLabel, ref bool cancelled,
            ref LinePath previousSmoothedPath, ref CarTimestamp previousSmoothedPathTimestamp, ref double? approachSpeed)
        {
            // if we're in listen mode, just return for now
            if (OperationalBuilder.BuildMode == BuildMode.Listen) {
                return;
            }

            PathPlanner.PlanningResult result;

            double curSpeed = Services.StateProvider.GetVehicleState().speed;
            LinePath targetPath = new LinePath();
            double initialHeading = 0;
            // get the part we just used to make a prediction
            if (useTargetPath && previousSmoothedPath != null) {
                //targetPath = previousSmoothedPath.Transform(Services.RelativePose.GetTransform(previousSmoothedPathTimestamp, curTimestamp));
                // interpolate the path with a smoothing spline
                //targetPath = targetPath.SplineInterpolate(0.05);
                //Services.UIService.PushRelativePath(targetPath, curTimestamp, "prediction path2");
                // calculate the point speed*dt ahead
                /*double lookaheadDist = curSpeed*0.20;
                if (lookaheadDist > 0.1) {
                    LinePath.PointOnPath pt = targetPath.AdvancePoint(targetPath.ZeroPoint, lookaheadDist);
                    // get the heading
                    initialHeading = targetPath.GetSegment(pt.Index).UnitVector.ArcTan;
                    // adjust the base path start point to be the predicted location
                    basePath[0] = pt.Location;

                    // get the part we just used to make a prediction
                    predictionPath = targetPath.SubPath(targetPath.ZeroPoint, pt);
                    // push to the UI
                    Services.UIService.PushRelativePath(predictionPath, curTimestamp, "prediction path");
                    //basePath[0] = new Coordinates(lookaheadDist, 0);
                    Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");
                    Services.Dataset.ItemAs<double>("initial heading").Add(initialHeading, curTimestamp);
                    // calculate a piece of the sub path
                    //targetPath = targetPath.SubPath(targetPath.ZeroPoint, 7);
                }*/

                // get the tracking manager to predict stuff like whoa
                AbsolutePose absPose;
                OperationalVehicleState vehicleState;
                Services.TrackingManager.ForwardPredict(out absPose, out vehicleState);
                // insert the stuff stuff
                basePath[0] = absPose.xy;
                initialHeading = absPose.heading;

                // start walking down the path until the angle is cool
                double angle_threshold = 30*Math.PI/180.0;
                double dist;
                LinePath.PointOnPath newPoint = new LinePath.PointOnPath();
                for (dist = 0; dist < 10; dist += 1) {
                    // get the point advanced from the 2nd point on the base path by dist
                    double distTemp = dist;
                    newPoint = basePath.AdvancePoint(basePath.GetPointOnPath(1), ref distTemp);

                    // check if we're past the end
                    if (distTemp > 0) {
                        break;
                    }

                    // check if the angle is coolness or not
                    double angle = Math.Acos((newPoint.Location-basePath[0]).Normalize().Dot(basePath.GetSegment(newPoint.Index).UnitVector));

                    if (Math.Acos(angle) < angle_threshold) {
                        break;
                    }
                }

                // create a new version of the base path with the stuff section removed
                basePath = basePath.RemoveBetween(basePath.StartPoint, newPoint);

                Services.UIService.PushRelativePath(basePath, curTimestamp, "subpath2");

                // zero that stuff out
                targetPath = new LinePath();
            }

            StaticObstacles obstacles = null;
            // only do the planning is we're in a lane scenario
            // otherwise, the obstacle grid will be WAY too large
            if (doAvoidance && leftBounds.Count == 1 && rightBounds.Count == 1) {
                // get the obstacles predicted to the current timestamp
                obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp);
            }

            // start the planning timer
            Stopwatch planningTimer = Stopwatch.StartNew();

            // check if there are any obstacles
            if (obstacles != null && obstacles.polygons != null && obstacles.polygons.Count > 0) {
                if (cancelled) return;

                // we need to do the full obstacle avoidance
                // execute the obstacle manager
                LinePath avoidancePath;
                List<ObstacleManager.ObstacleType> obstacleSideFlags;
                bool success;
                Services.ObstacleManager.ProcessObstacles(basePath, leftBounds, rightBounds, obstacles.polygons,
                    out avoidancePath, out obstacleSideFlags, out success);

                // check if we have success
                if (success) {
                    // build the boundary lists
                    // start with the lanes
                    List<Boundary> leftSmootherBounds  = new List<Boundary>();
                    List<Boundary> rightSmootherBounds = new List<Boundary>();

                    double laneMinSpacing = 0.1;
                    double laneDesiredSpacing = 0.1;
                    double laneAlphaS = 0.1;
                    leftSmootherBounds.Add(new Boundary(leftBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));
                    rightSmootherBounds.Add(new Boundary(rightBounds[0], laneMinSpacing, laneDesiredSpacing, laneAlphaS));

                    // sort out obstacles as left and right
                    double obstacleMinSpacing = 0.8;
                    double obstacleDesiredSpacing = 0.8;
                    double obstacleAlphaS = 100;
                    int totalObstacleClusters = obstacles.polygons.Count;
                    for (int i = 0; i < totalObstacleClusters; i++) {
                        if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Left) {
                            Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
                            bound.CheckFrontBumper = true;
                            leftSmootherBounds.Add(bound);
                        }
                        else if (obstacleSideFlags[i] == ObstacleManager.ObstacleType.Right) {
                            Boundary bound = new Boundary(obstacles.polygons[i], obstacleMinSpacing, obstacleDesiredSpacing, obstacleAlphaS);
                            bound.CheckFrontBumper = true;
                            rightSmootherBounds.Add(bound);
                        }
                    }

                    if (cancelled) return;

                    // execute the smoothing
                    PathPlanner planner = new PathPlanner();
                    planner.Options.alpha_w = 0;
                    planner.Options.alpha_d = 10;
                    planner.Options.alpha_c = 10;
                    result = planner.PlanPath(avoidancePath, targetPath, leftSmootherBounds, rightSmootherBounds, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
                }
                else {
                    // mark that we did not succeed
                    result = new PathPlanner.PlanningResult(SmoothResult.Infeasible, null);
                }
            }
            else {

                if (cancelled) return;

                // do the path smoothing
                PathPlanner planner = new PathPlanner();

                List<LineList> leftList = new List<LineList>();
                foreach (LinePath ll in leftBounds) leftList.Add(ll);
                List<LineList> rightList = new List<LineList>();
                foreach (LinePath rl in rightBounds) rightList.Add(rl);

                planner.Options.alpha_w = 0;
                planner.Options.alpha_s = 0.1;
                planner.Options.alpha_d = 10;
                planner.Options.alpha_c = 10;

                result = planner.PlanPath(basePath, targetPath, leftList, rightList, initialHeading, maxSpeed, Services.StateProvider.GetVehicleState().speed, endingHeading, curTimestamp, endingOffsetBound);
            }

            planningTimer.Stop();

            BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning took {0} ms", planningTimer.ElapsedMilliseconds);

            Services.Dataset.ItemAs<bool>("route feasible").Add(result.result == SmoothResult.Sucess, LocalCarTimeProvider.LocalNow);

            if (result.result == SmoothResult.Sucess) {
                // insert the point (-1,0) so we make sure that the zero point during tracking is at the vehicle
                //Coordinates startingVec = result.path[1].Point-result.path[0].Point;
                //Coordinates insertPoint = result.path[0].Point-startingVec.Normalize();
                //result.path.Insert(0, new OperationalLayer.PathPlanning.PathPoint(insertPoint, maxSpeed));

                previousSmoothedPath = new LinePath(result.path);
                previousSmoothedPathTimestamp = curTimestamp;
                Services.UIService.PushLineList(previousSmoothedPath, curTimestamp, "smoothed path", true);

                if (cancelled) return;

                // we've planned out the path, now build up the command
                ISpeedGenerator speedGenerator;
                if (speedCommand is ScalarSpeedCommand) {
                    /*if (result.path.HasSpeeds) {
                        speedGenerator = result.path;
                    }
                    else {*/
                    speedGenerator = new ConstantSpeedGenerator(maxSpeed, null);
                    //}
                }
                else if (speedCommand is StopAtDistSpeedCommand) {
                    StopAtDistSpeedCommand stopCommand = (StopAtDistSpeedCommand)speedCommand;
                    IDistanceProvider distProvider = new TravelledDistanceProvider(behaviorTimestamp, stopCommand.Distance);
                    speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);

                    BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());

                }
                else if (speedCommand is StopAtLineSpeedCommand) {
                    IDistanceProvider distProvider = new StoplineDistanceProvider();
                    speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);

                    BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
                }
                else if (speedCommand == null) {
                    throw new InvalidOperationException("Speed command is null");
                }
                else {
                    throw new InvalidOperationException("Speed command " + speedCommand.GetType().FullName + " is not supported");
                }

                if (cancelled) return;

                // build up the command
                TrackingCommand trackingCommand = new TrackingCommand(new FeedbackSpeedCommandGenerator(speedGenerator), new PathSteeringCommandGenerator(result.path), false);
                trackingCommand.Label = commandLabel;

                // queue it to execute
                Services.TrackingManager.QueueCommand(trackingCommand);
            }
        }
        public void Process(object param)
        {
            if (cancelled) return;

            DateTime start = HighResDateTime.Now;

            OperationalVehicleState vs = Services.StateProvider.GetVehicleState();

            LinePath curBasePath = basePath;
            LinePath curLeftBound = leftBound;
            LinePath curRightBound = rightBound;

            // transform the base path to the current iteration
            CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
            if (pathTime != curTimestamp) {
                RelativeTransform relTransform = Services.RelativePose.GetTransform(pathTime, curTimestamp);
                curBasePath = curBasePath.Transform(relTransform);
                curLeftBound = curLeftBound.Transform(relTransform);
                curRightBound = curRightBound.Transform(relTransform);
            }

            // get the distance between the zero point and the start point
            double distToStart = Coordinates.Zero.DistanceTo(curBasePath.GetPoint(curBasePath.StartPoint));

            // get the sub-path between 5 and 25 meters ahead
            double startDist = TahoeParams.FL;
            LinePath.PointOnPath startPoint = curBasePath.AdvancePoint(curBasePath.ZeroPoint, ref startDist);
            double endDist = 30;
            LinePath.PointOnPath endPoint = curBasePath.AdvancePoint(startPoint, ref endDist);

            if (startDist > 0) {
                // we've reached the end
                Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
                return;
            }

            // get the sub-path
            LinePath subPath = curBasePath.SubPath(startPoint, endPoint);
            // add (0,0) as the starting point
            subPath.Insert(0, new Coordinates(0, 0));

            // do the same for the left, right bound
            startDist = TahoeParams.FL;
            endDist = 40;
            startPoint = curLeftBound.AdvancePoint(curLeftBound.ZeroPoint, startDist);
            endPoint = curLeftBound.AdvancePoint(startPoint, endDist);
            curLeftBound = curLeftBound.SubPath(startPoint, endPoint);

            startPoint = curRightBound.AdvancePoint(curRightBound.ZeroPoint, startDist);
            endPoint = curRightBound.AdvancePoint(startPoint, endDist);
            curRightBound = curRightBound.SubPath(startPoint, endPoint);

            if (cancelled) return;

            Services.UIService.PushRelativePath(subPath, curTimestamp, "subpath");
            Services.UIService.PushRelativePath(curLeftBound, curTimestamp, "left bound");
            Services.UIService.PushRelativePath(curRightBound, curTimestamp, "right bound");

            // run a path smoothing iteration
            lock (this) {
                planner = new PathPlanner();
            }

            ////////////////////////////////////////////////////////////////////////////////////////////////////
            // start of obstacle manager - hik
            bool obstacleManagerEnable = true;

            PathPlanner.SmoothingResult result;

            if (obstacleManagerEnable == true) {

                // generate fake obstacles (for simulation testing only)
                double obsSize = 10.5 / 2;
                List<Coordinates> obstaclePoints = new List<Coordinates>();
                List<Obstacle> obstacleClusters = new List<Obstacle>();
                // fake left obstacles (for simulation only)
                int totalLeftObstacles = Math.Min(0, curLeftBound.Count - 1);
                for (int i = 0; i < totalLeftObstacles; i++) {
                    obstaclePoints.Clear();
                    obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, obsSize));
                    obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, -obsSize));
                    obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, -obsSize));
                    obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, obsSize));
                    obstacleClusters.Add(new Obstacle());
                    obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
                }
                // fake right obstacles (for simulation only)
                int totalRightObstacles = Math.Min(0, curRightBound.Count - 1);
                for (int i = 0; i < totalRightObstacles; i++) {
                    obstaclePoints.Clear();
                    obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, obsSize));
                    obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, -obsSize));
                    obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, -obsSize));
                    obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, obsSize));
                    obstacleClusters.Add(new Obstacle());
                    obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
                }
                // fake center obstacles (for simulation only)
                int totalCenterObstacles = Math.Min(0, subPath.Count - 1);
                for (int i = 2; i < totalCenterObstacles; i++) {
                    obstaclePoints.Clear();
                    obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, obsSize));
                    obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, -obsSize));
                    obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, -obsSize));
                    obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, obsSize));
                    obstacleClusters.Add(new Obstacle());
                    obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
                }

                obstaclePoints.Clear();
                obstaclePoints.Add(new Coordinates(10000, 10000));
                obstaclePoints.Add(new Coordinates(10000, 10001));
                obstaclePoints.Add(new Coordinates(10001, 10000));
                obstacleClusters.Add(new Obstacle());
                obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);

                obstaclePoints.Clear();
                obstaclePoints.Add(new Coordinates(1000, 1000));
                obstaclePoints.Add(new Coordinates(1000, 1001));
                obstaclePoints.Add(new Coordinates(1001, 1000));
                obstacleClusters.Add(new Obstacle());
                obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);

                obstaclePoints.Clear();
                obstaclePoints.Add(new Coordinates(-1000, -1000));
                obstaclePoints.Add(new Coordinates(-1000, -1001));
                obstaclePoints.Add(new Coordinates(-1001, -1000));
                obstacleClusters.Add(new Obstacle());
                obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);

                foreach (Obstacle obs in obstacleClusters) {
                    obs.cspacePolygon = new Polygon(obs.obstaclePolygon.points);
                }

                // find obstacle path and left/right classification
                LinePath obstaclePath = new LinePath();
                //Boolean successFlag;
                //double laneWidthAtPathEnd = 10.0;
            //#warning this currently doesn't work
                /*obstacleManager.ProcessObstacles(subPath, new LinePath[] { curLeftBound }, new LinePath[] { curRightBound },
                                                   obstacleClusters, laneWidthAtPathEnd,
                                                                                 out obstaclePath, out successFlag);
                 */

                // prepare left and right lane bounds
                double laneMinSpacing = 0.1;
                double laneDesiredSpacing = 0.5;
                double laneAlphaS = 10000;
                List<Boundary> leftBounds  = new List<Boundary>();
                List<Boundary> rightBounds = new List<Boundary>();
                leftBounds.Add(new Boundary(curLeftBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));
                rightBounds.Add(new Boundary(curRightBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));

                // sort out obstacles as left and right
                double obstacleMinSpacing = 0.1;
                double obstacleDesiredSpacing = 1.0;
                double obstacleAlphaS = 10000;
                Boundary bound;
                int totalObstacleClusters = obstacleClusters.Count;
                for (int i = 0; i < totalObstacleClusters; i++) {
                    if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Left) {
                        // obstacle cluster is to the left of obstacle path
                        bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
                                               obstacleDesiredSpacing, obstacleAlphaS, true);
                        bound.CheckFrontBumper = true;
                        leftBounds.Add(bound);
                    }
                    else if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Right) {
                        // obstacle cluster is to the right of obstacle path
                        bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
                                               obstacleDesiredSpacing, obstacleAlphaS, true);
                        bound.CheckFrontBumper = true;
                        rightBounds.Add(bound);
                    }
                    else {
                        // obstacle cluster is outside grid, hence ignore obstacle cluster
                    }
                }

                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Start();

                // PlanPath function call with obstacle path and obstacles
                result = planner.PlanPath(obstaclePath, obstaclePath, leftBounds, rightBounds,
                                                                    0, maxSpeed, vs.speed, null, curTimestamp, false);

                stopwatch.Stop();
                Console.WriteLine("============================================================");
                Console.WriteLine("With ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
                Console.WriteLine("============================================================");
            }
            else {
                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Start();

                // original PlanPath function call
                result = planner.PlanPath(subPath, curLeftBound, curRightBound,
                                                                    0, maxSpeed, vs.speed, null, curTimestamp);

                stopwatch.Stop();
                Console.WriteLine("============================================================");
                Console.WriteLine("Without ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
                Console.WriteLine("============================================================");
            }

            // end of obstacle manager - hik
            ////////////////////////////////////////////////////////////////////////////////////////////////////

            //PathPlanner.PlanningResult result = planner.PlanPath(subPath, curLeftBound, curRightBound, 0, maxSpeed, vs.speed, null, curTimestamp);

            //SmoothedPath path = new SmoothedPath(pathTime);

            lock (this) {
                planner = null;
            }

            if (cancelled) return;

            if (result.result == UrbanChallenge.PathSmoothing.SmoothResult.Sucess) {
                // start tracking the path
                Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetSmoothedPathVelocityCommand(result.path));
                //Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetConstantSteeringConstantSpeedCommand(-.5, 2));

                /*TrackingCommand cmd = new TrackingCommand(
                    new FeedbackSpeedCommandGenerator(new ConstantSpeedGenerator(2, null)),
                    new SinSteeringCommandGenerator(),
                    true);
                Services.TrackingManager.QueueCommand(cmd);*/

                // send the path's we're tracking to the UI
                Services.UIService.PushRelativePath(result.path, curTimestamp, "smoothed path");

                cancelled = true;
            }
        }
        public virtual void Initialize(Behavior b)
        {
            cancelled = false;

            planner = new PathPlanner();

            curTimestamp = Services.RelativePose.CurrentTimestamp;
        }