/// <summary>
 /// Update sensor states
 /// </summary>
 /// <param name="vehicleState"></param>
 /// <param name="vehicles"></param>
 /// <param name="obstacles"></param>
 public void Update(VehicleState vehicleState, SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, double speed, LocalRoadEstimate lre, PathRoadModel prm)
 {
     this.vehicleStateChannel.PublishUnreliably(vehicleState);
     this.observedVehicleChannel.PublishUnreliably(vehicles);
     this.observedObstacleChannel.PublishUnreliably(obstacles);
     this.vehicleSpeedChannel.PublishUnreliably(speed);
     this.localRoadChannel.PublishUnreliably(lre);
     if (prm != null) {
         this.localRoadChannel.PublishUnreliably(prm);
     }
 }
        public LocalRoadEstimate GetLocalRoadEstimate(SimVehicleState state)
        {
            LocalRoadEstimate lre = new LocalRoadEstimate();

            /*ArbiterLanePartition closestPartition = this.RoadNetwork.ClosestLane(state.Position).GetClosestPartition(state.Position);

            LinePath path;

            if (IsPartitionSameDirection(closestPartition, state.Heading)) {
                path = BuildPath(closestPartition, true, 25, 25);
            }
            else {
                path = BuildPath(closestPartition, false, 25, 25);
            }

            Matrix3 absTransform = Matrix3.Rotation(-state.Heading.ArcTan)*Matrix3.Translation(-state.Position.X, -state.Position.Y);
            path.TransformInPlace(absTransform);

            // get the closest point
            LinePath.PointOnPath p0 = path.ZeroPoint;

            List<Coordinates> points = new List<Coordinates>(5);
            List<double> dists = new List<double>();
            dists.Add(0);
            points.Add(path.GetPoint(p0));
            // iterate through and add points every five meteres
            LinePath.PointOnPath pt = p0;
            double sumDist = 0;
            for (int i = 0; i < 3; i++) {
                double dist = 5;
                pt = path.AdvancePoint(pt, ref dist);

                if (dist != 5) {
                    sumDist += (5-dist);

                    dists.Add(sumDist);
                    points.Add(path.GetPoint(pt));
                }

                if (dist > 0)
                    break;
            }

            pt = p0;
            sumDist = 0;
            for (int i = 0; i < 2; i++) {
                double dist = -5;
                pt = path.AdvancePoint(pt, ref dist);

                if (dist != -5) {
                    sumDist += (5+dist);

                    dists.Insert(0, sumDist);
                    points.Insert(0, path.GetPoint(pt));
                }

                if (dist < 0)
                    break;
            }

            // do a least squares fit over the points
            Matrix X = new Matrix(points.Count, 3);
            Matrix W = new Matrix(points.Count, points.Count);
            Matrix Y = new Matrix(points.Count, 1);
            for (int i = 0; i < points.Count; i++) {
                X[i, 0] = 1;
                X[i, 1] = points[i].X;
                X[i, 2] = points[i].X*points[i].X;
                W[i, i] = 1/(dists[i] + 1);
                Y[i, 0] = points[i].Y;
            }

            Matrix beta = null;
            try {
                Matrix X_T = X.Transpose();
                Matrix inv = (X_T*W*X).Inverse;
                beta = inv*X_T*W*Y;
            }
            catch (Exception) {
            }

            if (beta != null) {
                lre.isModelValid = true;
                lre.roadCurvature = beta[2, 0];
                lre.roadCurvatureVar = double.MaxValue;
                lre.roadHeading = beta[1, 0];
                lre.roadHeadingVar = 0;

                lre.centerLaneEstimate = new UrbanChallenge.Common.Sensors.LocalRoadEstimate.LaneEstimate();
                lre.centerLaneEstimate.center = beta[0, 0];
                lre.centerLaneEstimate.centerVar = 0;
                lre.centerLaneEstimate.exists = true;
                lre.centerLaneEstimate.id = closestPartition.PartitionId.ToString();
                lre.centerLaneEstimate.width = closestPartition.Lane.Width;
                lre.centerLaneEstimate.widthVar = 0;

                lre.leftLaneEstimate = new UrbanChallenge.Common.Sensors.LocalRoadEstimate.LaneEstimate();
                lre.leftLaneEstimate.exists = false;
                if (closestPartition.Lane.LaneOnLeft != null) {
                    ArbiterLanePartition leftParition = closestPartition.Lane.LaneOnLeft.GetClosestPartition(state.Position);
                    if (closestPartition.NonLaneAdjacentPartitions.Contains(leftParition)) {
                        double dist;

                        if (IsPartitionSameDirection(leftParition, state.Heading)) {
                            dist = (state.Position-leftParition.Initial.Position).Cross(leftParition.Vector().Normalize());
                        }
                        else {
                            dist = (state.Position-leftParition.Final.Position).Cross(leftParition.Vector().Rotate180().Normalize());
                        }

                        lre.leftLaneEstimate.center = dist;
                        lre.leftLaneEstimate.centerVar = 0;
                        lre.leftLaneEstimate.exists = true;
                        lre.leftLaneEstimate.id = leftParition.PartitionId.ToString();
                        lre.leftLaneEstimate.width = leftParition.Lane.Width;
                        lre.leftLaneEstimate.widthVar = 0;
                    }
                }

                if (closestPartition.Lane.LaneOnRight != null) {
                    ArbiterLanePartition rightPartition = closestPartition.Lane.LaneOnRight.GetClosestPartition(state.Position);
                    if (closestPartition.NonLaneAdjacentPartitions.Contains(rightPartition)) {
                        double dist;
                        if (IsPartitionSameDirection(rightPartition, state.Heading)) {
                            dist = (state.Position-rightPartition.Initial.Position).Cross(rightPartition.Vector().Normalize());
                        }
                        else {
                            dist = (state.Position-rightPartition.Final.Position).Cross(rightPartition.Vector().Rotate180().Normalize());
                        }

                        lre.rightLaneEstimate.center = dist;
                        lre.rightLaneEstimate.centerVar = 0;
                        lre.rightLaneEstimate.exists = true;
                        lre.rightLaneEstimate.id = rightPartition.PartitionId.ToString();
                        lre.rightLaneEstimate.width = rightPartition.Lane.Width;
                        lre.rightLaneEstimate.widthVar = 0;
                    }
                }
            }
            else {
                lre.isModelValid = false;
            }*/
            lre.centerLaneEstimate.exists = false;
            lre.leftLaneEstimate.exists = false;
            lre.rightLaneEstimate.exists = false;

            lre.stopLineEstimate = new StopLineEstimate();
            double? distToStopline = DistanceToNextStop(state);
            lre.stopLineEstimate.stopLineExists = distToStopline.HasValue;
            lre.stopLineEstimate.distToStopline = distToStopline.GetValueOrDefault(double.MaxValue);
            lre.stopLineEstimate.distToStoplineVar = 0;

            lre.timestamp = (double)SimulatorClient.GetCurrentTimestamp;

            return lre;
        }