/// <summary>
        /// Write partition informaton
        /// </summary>
        /// <param name="sw"></param>
        private void WritePartitionInformation(StreamWriter sw)
        {
            // get list of all partitions that connect waypoints
            List <IConnectAreaWaypoints> icaws = new List <IConnectAreaWaypoints>();

            #region Populate partitions

            // get lane partitions
            foreach (ArbiterSegment asg in roadNetwork.ArbiterSegments.Values)
            {
                foreach (ArbiterLane al in asg.Lanes.Values)
                {
                    foreach (ArbiterLanePartition alp in al.Partitions)
                    {
                        icaws.Add(alp);
                    }
                }
            }

            // get interconnects
            foreach (ArbiterInterconnect ai in roadNetwork.ArbiterInterconnects.Values)
            {
                icaws.Add(ai);
            }

            // zones (holy stuff what a hack)
            foreach (ArbiterZone az in roadNetwork.ArbiterZones.Values)
            {
                icaws.Add(new SceneZonePartition(az));
            }

            #endregion

            // notify
            sw.WriteLine("NumberOfPartitions" + "\t" + icaws.Count.ToString());
            string completionPercent = "";

            #region Create Partitions in Road Graph

            // write each
            for (int i = 0; i < icaws.Count; i++)
            {
                IConnectAreaWaypoints icaw = icaws[i];
                sw.WriteLine("Partition");

                string id = "";
                if (icaw is SceneZonePartition)
                {
                    id = ("PartitionId" + "\t" + ((SceneZonePartition)icaw).Zone.ToString());
                }
                else
                {
                    id = ("PartitionId" + "\t" + icaw.ConnectionId.ToString());
                }
                sw.WriteLine(id);

                // notify
                double percent = ((double)i) / ((double)icaws.Count) * 100.0;
                string tmpP    = percent.ToString("F0") + "% Complete";
                if (tmpP != completionPercent)
                {
                    completionPercent = tmpP;
                    EditorOutput.WriteLine(completionPercent);
                }

                #region Interconnect

                if (icaw is ArbiterInterconnect)
                {
                    ArbiterInterconnect ai = (ArbiterInterconnect)icaw;
                    sw.WriteLine("PartitionType" + "\t" + "Interconnect");
                    sw.WriteLine("Sparse" + "\t" + "False");
                    sw.WriteLine("FitType" + "\t" + "Line");

                    Coordinates c = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
                    sw.WriteLine("FitParameters" + "\t" + c.ArcTan.ToString("F6"));
                    sw.WriteLine("LeftBoundary" + "\t" + "None");
                    sw.WriteLine("RightBoundary" + "\t" + "None");
                    sw.WriteLine("NumberOfPoints" + "\t" + "2");
                    sw.WriteLine("Points");
                    sw.WriteLine(ai.InitialGeneric.ToString());
                    sw.WriteLine(ai.FinalGeneric.ToString());
                    sw.WriteLine("End_Points");

                    List <ArbiterWaypoint> aws = this.GetNearbyStops(ai);
                    sw.WriteLine("NumberOfNearbyStoplines" + "\t" + aws.Count);
                    if (aws.Count != 0)
                    {
                        sw.WriteLine("NearbyStoplines");
                        foreach (ArbiterWaypoint aw in aws)
                        {
                            sw.WriteLine(aw.ToString());
                        }
                        sw.WriteLine("End_NearbyStoplines");
                    }

                    #region Adjacent

                    List <string> adjacentPartitions = new List <string>();

                    // add current
                    adjacentPartitions.Add(ai.ToString());

                    #region Initial

                    if (icaw.InitialGeneric is ArbiterWaypoint)
                    {
                        // wp
                        ArbiterWaypoint aw = (ArbiterWaypoint)icaw.InitialGeneric;

                        // prev
                        if (aw.PreviousPartition != null)
                        {
                            adjacentPartitions.Add(aw.PreviousPartition.ToString());
                        }

                        // next
                        if (aw.NextPartition != null)
                        {
                            adjacentPartitions.Add(aw.NextPartition.ToString());
                        }

                        // exits
                        if (aw.IsExit)
                        {
                            foreach (ArbiterInterconnect ais in aw.Exits)
                            {
                                if (!ais.Equals(ai))
                                {
                                    adjacentPartitions.Add(ais.ToString());
                                }
                            }
                        }

                        if (aw.IsEntry)
                        {
                            foreach (ArbiterInterconnect ais in aw.Entries)
                            {
                                if (!ais.Equals(ai))
                                {
                                    adjacentPartitions.Add(ais.ToString());
                                }
                            }
                        }
                    }
                    else if (icaw.InitialGeneric is ArbiterPerimeterWaypoint)
                    {
                        adjacentPartitions.Add((new SceneZonePartition(((ArbiterPerimeterWaypoint)icaw.InitialGeneric).Perimeter.Zone)).ToString());
                    }

                    #endregion

                    #region Final

                    if (icaw.FinalGeneric is ArbiterWaypoint)
                    {
                        // wp
                        ArbiterWaypoint aw = (ArbiterWaypoint)icaw.FinalGeneric;

                        // prev
                        if (aw.PreviousPartition != null)
                        {
                            adjacentPartitions.Add(aw.PreviousPartition.ToString());
                        }

                        // next
                        if (aw.NextPartition != null)
                        {
                            adjacentPartitions.Add(aw.NextPartition.ToString());
                        }

                        // exits
                        if (aw.IsExit)
                        {
                            foreach (ArbiterInterconnect ais in aw.Exits)
                            {
                                adjacentPartitions.Add(ais.ToString());
                            }
                        }

                        if (aw.IsEntry)
                        {
                            foreach (ArbiterInterconnect ais in aw.Entries)
                            {
                                if (!ais.Equals(ai))
                                {
                                    adjacentPartitions.Add(ais.ToString());
                                }
                            }
                        }
                    }
                    else if (icaw.FinalGeneric is ArbiterPerimeterWaypoint)
                    {
                        adjacentPartitions.Add((new SceneZonePartition(((ArbiterPerimeterWaypoint)icaw.FinalGeneric).Perimeter.Zone)).ToString());
                    }

                    #endregion

                    sw.WriteLine("NumberOfLaneAdjacentPartitions" + "\t" + adjacentPartitions.Count.ToString());
                    if (adjacentPartitions.Count != 0)
                    {
                        sw.WriteLine("LaneAdjacentPartitions");
                        foreach (string s in adjacentPartitions)
                        {
                            sw.WriteLine(s);
                        }
                        sw.WriteLine("End_LaneAdjacentPartitions");
                    }

                    #endregion

                    sw.WriteLine("NumberOfLeftLaneAdjacentPartitions" + "\t" + "0");
                    sw.WriteLine("NumberOfRightLaneAdjacentPartitions" + "\t" + "0");

                    List <IConnectAreaWaypoints> nearby = this.GetNearbyPartitions(ai, icaws);
                    sw.WriteLine("NumberOfNearbyPartitions" + "\t" + nearby.Count.ToString());
                    if (nearby.Count != 0)
                    {
                        sw.WriteLine("NearbyPartitions");
                        foreach (IConnectAreaWaypoints tmp in nearby)
                        {
                            sw.WriteLine(tmp.ToString());
                        }
                        sw.WriteLine("End_NearbyPartitions");
                    }

                    sw.WriteLine("End_Partition");
                }

                #endregion

                #region Zone

                else if (icaw is SceneZonePartition)
                {
                    SceneZonePartition szp = (SceneZonePartition)icaw;
                    sw.WriteLine("PartitionType" + "\t" + "Zone");
                    sw.WriteLine("Sparse" + "\t" + "False");
                    sw.WriteLine("FitType" + "\t" + "Polygon");

                    string count = szp.Zone.Perimeter.PerimeterPoints.Count.ToString();
                    string wps   = "";
                    foreach (ArbiterPerimeterWaypoint apw in szp.Zone.Perimeter.PerimeterPoints.Values)
                    {
                        wps = wps + "\t" + apw.Position.X.ToString("f6") + "\t" + apw.Position.Y.ToString("f6");
                    }
                    sw.WriteLine("FitParameters" + "\t" + count + wps);

                    sw.WriteLine("LeftBoundary" + "\t" + "None");
                    sw.WriteLine("RightBoundary" + "\t" + "None");
                    sw.WriteLine("NumberOfPoints" + "\t" + szp.Zone.Perimeter.PerimeterPoints.Count.ToString());
                    sw.WriteLine("Points");
                    foreach (ArbiterPerimeterWaypoint apw in szp.Zone.Perimeter.PerimeterPoints.Values)
                    {
                        sw.WriteLine(apw.WaypointId.ToString());
                    }
                    sw.WriteLine("End_Points");

                    List <ArbiterWaypoint> aws = this.GetNearbyStops(szp);
                    sw.WriteLine("NumberOfNearbyStoplines" + "\t" + aws.Count);
                    if (aws.Count != 0)
                    {
                        sw.WriteLine("NearbyStoplines");
                        foreach (ArbiterWaypoint aw in aws)
                        {
                            sw.WriteLine(aw.ToString());
                        }
                        sw.WriteLine("End_NearbyStoplines");
                    }

                    #region Adjacent

                    List <string> adjacentStrings = new List <string>();

                    // add current
                    adjacentStrings.Add(szp.ToString());

                    foreach (ArbiterPerimeterWaypoint apw in szp.Zone.Perimeter.PerimeterPoints.Values)
                    {
                        if (apw.IsExit)
                        {
                            foreach (ArbiterInterconnect ai in apw.Exits)
                            {
                                adjacentStrings.Add(ai.ToString());
                            }
                        }

                        if (apw.IsEntry)
                        {
                            foreach (ArbiterInterconnect ais in apw.Entries)
                            {
                                adjacentStrings.Add(ais.ToString());
                            }
                        }
                    }

                    sw.WriteLine("NumberOfLaneAdjacentPartitions" + "\t" + adjacentStrings.Count.ToString());
                    if (adjacentStrings.Count != 0)
                    {
                        sw.WriteLine("LaneAdjacentPartitions");
                        foreach (string s in adjacentStrings)
                        {
                            sw.WriteLine(s);
                        }
                        sw.WriteLine("End_LaneAdjacentPartitions");
                    }


                    #endregion

                    sw.WriteLine("NumberOfLeftLaneAdjacentPartitions" + "\t" + "0");
                    sw.WriteLine("NumberOfRightLaneAdjacentPartitions" + "\t" + "0");

                    List <IConnectAreaWaypoints> nearby = this.GetNearbyPartitions(szp, icaws);
                    sw.WriteLine("NumberOfNearbyPartitions" + "\t" + nearby.Count.ToString());
                    if (nearby.Count != 0)
                    {
                        sw.WriteLine("NearbyPartitions");
                        foreach (IConnectAreaWaypoints tmp in nearby)
                        {
                            sw.WriteLine(tmp.ToString());
                        }
                        sw.WriteLine("End_NearbyPartitions");
                    }

                    sw.WriteLine("End_Partition");
                }

                #endregion

                #region Lane

                else if (icaw is ArbiterLanePartition)
                {
                    ArbiterLanePartition alp = (ArbiterLanePartition)icaw;
                    sw.WriteLine("PartitionType" + "\t" + "Lane");
                    string sparseString = alp.Type == PartitionType.Sparse ? "True" : "False";
                    sw.WriteLine("Sparse" + "\t" + sparseString);

                    if (alp.Type != PartitionType.Sparse)                    //alp.UserPartitions.Count <= 1)
                    {
                        sw.WriteLine("FitType" + "\t" + "Line");
                        sw.WriteLine("FitParameters" + "\t" + alp.Vector().ArcTan.ToString("F6"));
                    }
                    else
                    {
                        sw.WriteLine("FitType" + "\t" + "Polygon");

                        /*List<Coordinates> polyCoords = new List<Coordinates>();
                         * polyCoords.Add(alp.Initial.Position);
                         * polyCoords.AddRange(alp.NotInitialPathCoords());
                         * LinePath lpr = (new LinePath(polyCoords)).ShiftLateral(-TahoeParams.VL * 3.0);
                         * LinePath lpl = (new LinePath(polyCoords)).ShiftLateral(TahoeParams.VL * 3.0);
                         * List<Coordinates> finalCoords = new List<Coordinates>(polyCoords.ToArray());
                         * finalCoords.AddRange(lpr);
                         * finalCoords.AddRange(lpl);
                         * Polygon p = Polygon.GrahamScan(finalCoords);*/

                        if (alp.SparsePolygon == null)
                        {
                            alp.SetDefaultSparsePolygon();
                        }

                        string coordinateString = "";
                        foreach (Coordinates c in alp.SparsePolygon)
                        {
                            coordinateString = coordinateString + "\t" + c.X.ToString("F6") + "\t" + c.Y.ToString("F6");
                        }

                        sw.WriteLine("FitParameters" + "\t" + alp.SparsePolygon.Count.ToString() + coordinateString);
                    }

                    sw.WriteLine("LaneWidth" + "\t" + alp.Lane.Width.ToString("F6"));
                    sw.WriteLine("LeftBoundary" + "\t" + alp.Lane.BoundaryLeft.ToString());
                    sw.WriteLine("RightBoundary" + "\t" + alp.Lane.BoundaryRight.ToString());
                    sw.WriteLine("NumberOfPoints" + "\t" + "2");
                    sw.WriteLine("Points");
                    sw.WriteLine(alp.InitialGeneric.ToString());
                    sw.WriteLine(alp.FinalGeneric.ToString());
                    sw.WriteLine("End_Points");

                    List <ArbiterWaypoint> aws = this.GetNearbyStops(alp);
                    sw.WriteLine("NumberOfNearbyStoplines" + "\t" + aws.Count);
                    if (aws.Count != 0)
                    {
                        sw.WriteLine("NearbyStoplines");
                        foreach (ArbiterWaypoint aw in aws)
                        {
                            sw.WriteLine(aw.ToString());
                        }
                        sw.WriteLine("End_NearbyStoplines");
                    }

                    #region Adjacent

                    List <string> adjacentPartitions = new List <string>();

                    // add current
                    adjacentPartitions.Add(alp.ToString());

                    #region Initial

                    if (icaw.InitialGeneric is ArbiterWaypoint)
                    {
                        // wp
                        ArbiterWaypoint aw = (ArbiterWaypoint)icaw.InitialGeneric;

                        // prev
                        if (aw.PreviousPartition != null)
                        {
                            adjacentPartitions.Add(aw.PreviousPartition.ToString());
                        }

                        // next
                        if (aw.NextPartition != null && !aw.NextPartition.Equals(alp))
                        {
                            adjacentPartitions.Add(aw.NextPartition.ToString());
                        }

                        // exits
                        if (aw.IsExit)
                        {
                            foreach (ArbiterInterconnect ais in aw.Exits)
                            {
                                adjacentPartitions.Add(ais.ToString());
                            }
                        }

                        if (aw.IsEntry)
                        {
                            foreach (ArbiterInterconnect ais in aw.Entries)
                            {
                                adjacentPartitions.Add(ais.ToString());
                            }
                        }
                    }

                    #endregion

                    #region Final

                    if (icaw.FinalGeneric is ArbiterWaypoint)
                    {
                        // wp
                        ArbiterWaypoint aw = (ArbiterWaypoint)icaw.FinalGeneric;

                        // prev
                        if (aw.PreviousPartition != null && !aw.PreviousPartition.Equals(alp))
                        {
                            adjacentPartitions.Add(aw.PreviousPartition.ToString());
                        }

                        // next
                        if (aw.NextPartition != null)
                        {
                            adjacentPartitions.Add(aw.NextPartition.ToString());
                        }

                        // exits
                        if (aw.IsExit)
                        {
                            foreach (ArbiterInterconnect ais in aw.Exits)
                            {
                                adjacentPartitions.Add(ais.ToString());
                            }
                        }

                        if (aw.IsEntry)
                        {
                            foreach (ArbiterInterconnect ais in aw.Entries)
                            {
                                adjacentPartitions.Add(ais.ToString());
                            }
                        }
                    }

                    #endregion

                    sw.WriteLine("NumberOfLaneAdjacentPartitions" + "\t" + adjacentPartitions.Count.ToString());
                    if (adjacentPartitions.Count != 0)
                    {
                        sw.WriteLine("LaneAdjacentPartitions");
                        foreach (string s in adjacentPartitions)
                        {
                            sw.WriteLine(s);
                        }
                        sw.WriteLine("End_LaneAdjacentPartitions");
                    }

                    #endregion

                    List <string> leftAlps  = new List <string>();
                    List <string> rightAlps = new List <string>();

                    foreach (ArbiterLanePartition tmpAlp in alp.NonLaneAdjacentPartitions)
                    {
                        if (tmpAlp.Lane.Equals(alp.Lane.LaneOnLeft))
                        {
                            leftAlps.Add(tmpAlp.ToString());
                        }
                        else
                        {
                            rightAlps.Add(tmpAlp.ToString());
                        }
                    }

                    sw.WriteLine("NumberOfLeftLaneAdjacentPartitions" + "\t" + leftAlps.Count.ToString());
                    if (leftAlps.Count != 0)
                    {
                        sw.WriteLine("LeftLaneAdjacentPartitions");
                        foreach (string s in leftAlps)
                        {
                            sw.WriteLine(s);
                        }
                        sw.WriteLine("End_LeftLaneAdjacentPartitions");
                    }

                    sw.WriteLine("NumberOfRightLaneAdjacentPartitions" + "\t" + rightAlps.Count.ToString());
                    if (rightAlps.Count != 0)
                    {
                        sw.WriteLine("RightLaneAdjacentPartitions");
                        foreach (string s in rightAlps)
                        {
                            sw.WriteLine(s);
                        }
                        sw.WriteLine("End_RightLaneAdjacentPartitions");
                    }

                    List <IConnectAreaWaypoints> nearby = this.GetNearbyPartitions(alp, icaws);
                    sw.WriteLine("NumberOfNearbyPartitions" + "\t" + nearby.Count.ToString());
                    if (nearby.Count != 0)
                    {
                        sw.WriteLine("NearbyPartitions");
                        foreach (IConnectAreaWaypoints tmp in nearby)
                        {
                            sw.WriteLine(tmp.ToString());
                        }
                        sw.WriteLine("End_NearbyPartitions");
                    }

                    sw.WriteLine("End_Partition");
                }

                #endregion
            }

            #endregion
        }
        /// <summary>
        /// Startup the vehicle from a certain position, pass the next state back,
        /// and initialize the lane agent if possible
        /// </summary>
        /// <param name="vehicleState"></param>
        /// <returns></returns>
        public IState Startup(VehicleState vehicleState, CarMode carMode)
        {
            // check car mode
            if (carMode == CarMode.Run)
            {
                // check areas
                ArbiterLane         al  = CoreCommon.RoadNetwork.ClosestLane(vehicleState.Front);
                ArbiterZone         az  = CoreCommon.RoadNetwork.InZone(vehicleState.Front);
                ArbiterIntersection ain = CoreCommon.RoadNetwork.InIntersection(vehicleState.Front);
                ArbiterInterconnect ai  = CoreCommon.RoadNetwork.ClosestInterconnect(vehicleState.Front, vehicleState.Heading);

                if (az != null)
                {
                    // special zone startup
                    return(new ZoneStartupState(az));
                }

                if (ain != null)
                {
                    if (al != null)
                    {
                        // check lane stuff
                        PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);

                        // get distance from front of car
                        double dist = lanePoint.pt.DistanceTo(vehicleState.Front);

                        // check dist to lane
                        if (dist < al.Width + 1.0)
                        {
                            // check orientation relative to lane
                            Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();

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

                            // forwards or backwards
                            bool forwards = true;

                            // check dist to each other
                            if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
                            {
                                // not going forwards
                                forwards = false;
                            }

                            // stay in lane if forwards
                            if (forwards)
                            {
                                ArbiterOutput.Output("Starting up in lane: " + al.ToString());
                                return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
                            }
                            else
                            {
                                // Opposing lane
                                return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
                            }
                        }
                    }

                    // startup intersection state
                    return(new IntersectionStartupState(ain));
                }

                if (al != null)
                {
                    // get a startup chute
                    ArbiterLanePartition startupChute = this.GetStartupChute(vehicleState);

                    // check if in a startup chute
                    if (startupChute != null && !startupChute.IsInside(vehicleState.Front))
                    {
                        ArbiterOutput.Output("Starting up in chute: " + startupChute.ToString());
                        return(new StartupOffChuteState(startupChute));
                    }
                    // not in a startup chute
                    else
                    {
                        PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);

                        // get distance from front of car
                        double dist = lanePoint.pt.DistanceTo(vehicleState.Front);

                        // check orientation relative to lane
                        Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();

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

                        // forwards or backwards
                        bool forwards = true;

                        // check dist to each other
                        if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
                        {
                            // not going forwards
                            forwards = false;
                        }

                        // stay in lane if forwards
                        if (forwards)
                        {
                            ArbiterOutput.Output("Starting up in lane: " + al.ToString());
                            return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
                        }
                        else
                        {
                            // opposing
                            return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
                        }
                    }
                }

                // fell out
                ArbiterOutput.Output("Cannot find area to startup in");
                return(CoreCommon.CorePlanningState);
            }
            else
            {
                return(CoreCommon.CorePlanningState);
            }
        }