diff --git a/SceneManager/AITasking.cs b/SceneManager/AITasking.cs
index 41ce4eb..40dd4e2 100644
--- a/SceneManager/AITasking.cs
+++ b/SceneManager/AITasking.cs
@@ -13,9 +13,21 @@ namespace SceneManager
                 return;
             }
 
-            if (currentWaypoint != null && currentWaypoint.DrivingFlag == VehicleDrivingFlags.StopAtDestination)
+            if (currentWaypoint != null)
             {
-                StopVehicleAtWaypoint(currentWaypoint, collectedVehicle);
+                float acceptedDistance = GetAcceptedStoppingDistance(waypoints, waypoints.IndexOf(currentWaypoint));
+                Game.LogTrivial($"{collectedVehicle.Vehicle.Model.Name} distance to collection waypoint: {collectedVehicle.Vehicle.DistanceTo2D(currentWaypoint.Position)}");
+                if(collectedVehicle.Vehicle.DistanceTo2D(currentWaypoint.Position) > (currentWaypoint.CollectorRadius))
+                {
+                    Game.LogTrivial($"{collectedVehicle.Vehicle.Model.Name} is driving to waypoint {currentWaypoint.Number}");
+                    collectedVehicle.Driver.Tasks.DriveToPosition(currentWaypoint.Position, currentWaypoint.Speed, (VehicleDrivingFlags)263075, acceptedDistance);
+                    LoopWhileDrivingToWaypoint(collectedVehicle, waypoints, currentWaypoint, acceptedDistance);
+                }
+                if(currentWaypoint.DrivingFlag == VehicleDrivingFlags.StopAtDestination)
+                {
+                    StopVehicleAtWaypoint(currentWaypoint, collectedVehicle);
+                }
+
             }
 
             DriveVehicleToNextWaypoint(collectedVehicle, waypoints, currentWaypoint);
@@ -25,7 +37,11 @@ namespace SceneManager
                 return;
             }
             Game.LogTrivial($"{collectedVehicle.Vehicle.Model.Name} all tasks complete.");
-            DismissDriver(collectedVehicle);
+            if (!collectedVehicle.Dismissed)
+            {
+                collectedVehicle.Dismiss();
+            }
+
         }
 
         private static void DriveVehicleToNextWaypoint(CollectedVehicle collectedVehicle, List<Waypoint> waypoints, Waypoint currentWaypoint)
@@ -38,37 +54,84 @@ namespace SceneManager
             var vehicle = collectedVehicle.Vehicle;
             var driver = vehicle.Driver;
 
-            for (int nextWaypoint = currentWaypoint.Number; nextWaypoint < waypoints.Count; nextWaypoint++)
+            for (int nextWaypoint = currentWaypoint.Number; nextWaypoint < waypoints.Count; nextWaypoint++) // Do we need to make this <= count?  Or nextWaypoint = currentWaypoint.Number - 1?  Is this dependent on direct vs. normal collection?
             {
                 if (!VehicleAndDriverNullChecks(waypoints, nextWaypoint, collectedVehicle))
                 {
                     return;
                 }
 
-                Game.LogTrivial($"{vehicle.Model.Name} is driving to waypoint {waypoints[nextWaypoint].Number}");
                 if (waypoints.ElementAtOrDefault(nextWaypoint) != null && !collectedVehicle.StoppedAtWaypoint)
                 {
-                    if(waypoints[nextWaypoint].DrivingFlag == VehicleDrivingFlags.Normal)
+                    collectedVehicle.CurrentWaypoint = waypoints[nextWaypoint];
+                    float acceptedDistance = GetAcceptedStoppingDistance(waypoints, nextWaypoint);
+
+                    Game.LogTrivial($"{vehicle.Model.Name} is driving to waypoint {waypoints[nextWaypoint].Number}");
+                    if (waypoints[nextWaypoint].DrivingFlag == VehicleDrivingFlags.IgnorePathFinding)
                     {
-                        collectedVehicle.Vehicle.Driver.Tasks.DriveToPosition(waypoints[nextWaypoint].Position, waypoints[nextWaypoint].Speed, (VehicleDrivingFlags)263083, 2f).WaitForCompletion();
+                        collectedVehicle.Driver.Tasks.DriveToPosition(waypoints[nextWaypoint].Position, waypoints[nextWaypoint].Speed, (VehicleDrivingFlags)17040299, acceptedDistance);
                     }
-                    else if (waypoints[nextWaypoint].DrivingFlag == VehicleDrivingFlags.IgnorePathFinding)
+                    else
                     {
-                        collectedVehicle.Vehicle.Driver.Tasks.DriveToPosition(waypoints[nextWaypoint].Position, waypoints[nextWaypoint].Speed, (VehicleDrivingFlags)17040299, 2f).WaitForCompletion();
+                        collectedVehicle.Driver.Tasks.DriveToPosition(waypoints[nextWaypoint].Position, waypoints[nextWaypoint].Speed, (VehicleDrivingFlags)263075, acceptedDistance);
+                    }
+                    LoopWhileDrivingToWaypoint(collectedVehicle, waypoints, nextWaypoint, acceptedDistance);
+
+                    if (collectedVehicle.SkipWaypoint)
+                    {
+                        collectedVehicle.SkipWaypoint = false;
+                        continue;
                     }
 
-                    collectedVehicle.Vehicle.Driver.Tasks.PerformDrivingManeuver(collectedVehicle.Vehicle, VehicleManeuver.GoForwardWithCustomSteeringAngle, 3);
-                }
+                    if (!collectedVehicle.Dismissed && waypoints.ElementAtOrDefault(nextWaypoint) != null && waypoints[nextWaypoint].DrivingFlag == VehicleDrivingFlags.StopAtDestination)
+                    {
+                        StopVehicleAtWaypoint(waypoints[nextWaypoint], collectedVehicle);
+                    }
 
-                if (waypoints.ElementAtOrDefault(nextWaypoint) != null && waypoints[nextWaypoint].DrivingFlag == VehicleDrivingFlags.StopAtDestination)
-                {
-                    Game.LogTrivial($"{collectedVehicle.Vehicle.Model.Name} stopping at waypoint.");
-                    collectedVehicle.Vehicle.Driver.Tasks.PerformDrivingManeuver(VehicleManeuver.GoForwardStraightBraking);
-                    collectedVehicle.StoppedAtWaypoint = true;
+                    if (!VehicleAndDriverNullChecks(collectedVehicle) || collectedVehicle.Dismissed)
+                    {
+                        return;
+                    }
+                    collectedVehicle.Driver.Tasks.PerformDrivingManeuver(collectedVehicle.Vehicle, VehicleManeuver.GoForwardWithCustomSteeringAngle, 3);
                 }
             }
         }
 
+        public static void LoopWhileDrivingToWaypoint(CollectedVehicle collectedVehicle, List<Waypoint> waypoints, int nextWaypoint, float acceptedDistance)
+        {
+            while (VehicleAndDriverNullChecks(collectedVehicle) && !collectedVehicle.Dismissed && !collectedVehicle.SkipWaypoint && waypoints.ElementAtOrDefault(nextWaypoint) != null && collectedVehicle.Vehicle.FrontPosition.DistanceTo2D(waypoints[nextWaypoint].Position) > acceptedDistance)
+            {
+                //Game.LogTrivial($"Looping while {collectedVehicle.Vehicle.Model.Name} drives to waypoint {waypoints[nextWaypoint].Number} ({collectedVehicle.Vehicle.DistanceTo2D(waypoints[nextWaypoint].Position)}m away from collector radius {waypoints[nextWaypoint].CollectorRadius})");
+                //Game.LogTrivial($"Distance of front of vehicle to waypoint: {collectedVehicle.Vehicle.FrontPosition.DistanceTo2D(waypoints[nextWaypoint].Position)}");
+                GameFiber.Yield();
+            }
+        }
+
+        public static void LoopWhileDrivingToWaypoint(CollectedVehicle collectedVehicle, List<Waypoint> waypoints, Waypoint currentWaypoint, float acceptedDistance)
+        {
+            while (VehicleAndDriverNullChecks(collectedVehicle) && !collectedVehicle.Dismissed && !collectedVehicle.SkipWaypoint && collectedVehicle.Vehicle.FrontPosition.DistanceTo2D(currentWaypoint.Position) > acceptedDistance)
+            {
+                //Game.LogTrivial($"Looping while {collectedVehicle.Vehicle.Model.Name} drives to waypoint {currentWaypoint.Number} ({collectedVehicle.Vehicle.DistanceTo2D(currentWaypoint.Position)}m away)");
+                GameFiber.Yield();
+            }
+        }
+
+        private static float GetAcceptedStoppingDistance(List<Waypoint> waypoints, int nextWaypoint)
+        {
+            float dist;
+            if(Settings.SpeedUnit == SpeedUnits.MPH)
+            {
+                dist = (MathHelper.ConvertMilesPerHourToKilometersPerHour(waypoints[nextWaypoint].Speed) * MathHelper.ConvertMilesPerHourToKilometersPerHour(waypoints[nextWaypoint].Speed)) / (250 * 0.8f);
+            }
+            else
+            {
+                dist = (waypoints[nextWaypoint].Speed * waypoints[nextWaypoint].Speed) / (250 * 0.8f);
+            }
+            var acceptedDistance = MathHelper.Clamp(dist, 2, 10);
+            Game.LogTrivial($"Accepted distance: {acceptedDistance}");
+            return acceptedDistance;
+        }
+
         private static bool VehicleAndDriverNullChecks(CollectedVehicle collectedVehicle)
         {
             if (collectedVehicle == null)
@@ -120,51 +183,18 @@ namespace SceneManager
             {
                 return;
             }
-
+            var stoppingDistance = GetAcceptedStoppingDistance(currentWaypoint.Path.Waypoints, currentWaypoint.Path.Waypoints.IndexOf(currentWaypoint));
             Game.LogTrivial($"{collectedVehicle.Vehicle.Model.Name} stopping at waypoint.");
-            //collectedVehicle.Driver.Tasks.DriveToPosition(collectedVehicle.Vehicle.FrontPosition, 10f, (VehicleDrivingFlags)2147483648); // This causes FPS loss
-            Rage.Native.NativeFunction.Natives.x260BE8F09E326A20(collectedVehicle.Vehicle, 3f, -1, true);
+            Rage.Native.NativeFunction.Natives.x260BE8F09E326A20(collectedVehicle.Vehicle, stoppingDistance, -1, true);
             collectedVehicle.StoppedAtWaypoint = true;
+            collectedVehicle.Driver.Tasks.Clear();
 
-            while (currentWaypoint != null && collectedVehicle.Vehicle && collectedVehicle.StoppedAtWaypoint)
+
+            while (currentWaypoint != null && VehicleAndDriverNullChecks(collectedVehicle) && collectedVehicle.StoppedAtWaypoint)
             {
                 GameFiber.Yield();
             }
-            Rage.Native.NativeFunction.Natives.x260BE8F09E326A20(collectedVehicle.Vehicle, 3f, 1, true);
-        }
-
-        public static void DismissDriver(CollectedVehicle cv)
-        {
-            if (!cv.Vehicle)
-            {
-                Game.LogTrivial($"Vehicle is not valid after tasks completed.");
-                return;
-            }
-            if (!cv.Vehicle.Driver)
-            {
-                Game.LogTrivial($"Driver is not valid after tasks completed.");
-                return;
-            }
-
-            cv.StoppedAtWaypoint = false;
-            if (cv.Vehicle && cv.Vehicle.Driver)
-            {
-                cv.Vehicle.Driver.Dismiss();
-                cv.Vehicle.Driver.Tasks.Clear();
-                cv.Vehicle.Driver.BlockPermanentEvents = false;
-                if (cv.Vehicle.Driver.GetAttachedBlip())
-                {
-                    cv.Vehicle.Driver.GetAttachedBlip().Delete();
-                }
-                cv.Vehicle.Driver.IsPersistent = false;
-
-                cv.Vehicle.Dismiss();
-                cv.Vehicle.IsSirenOn = false;
-                cv.Vehicle.IsSirenSilent = true;
-                cv.Vehicle.IsPersistent = false;
-
-                Game.LogTrivial($"{cv.Vehicle.Model.Name} dismissed successfully.");
-            }
+            Rage.Native.NativeFunction.Natives.x260BE8F09E326A20(collectedVehicle.Vehicle, 1f, 1, true);
         }
     }
 }