Snake, manual U-turn, line entry: same to-do pass test

README.md

@@ -25,7 +25,7 @@ Most of these have been offered to the official project too. When one is merged
 - **Headland gaps.** With "Drive across a headland gap up to" set on the U-turn tab, a pass that crosses a narrow tip of the field is driven on with the sections off, and the turn is made at the far end.
 - **U-turns with no room.** When no proper turn fits, the simple turn is moved back into the field instead of crossing the boundary. Where the headland clips a corner off the pass, AutoSteer drives on across it and says so (a thin spike of the headland line doesn't count).
 - **U-turn beside a fence that comes in.** With a hard boundary, a turn that had to be moved back to keep the implement off the fence at the end of the pass could be made far too early instead, where the fence comes in beside the pass: it landed on the next pass inside its crop and left a strip of it unsprayed. Now it is moved back from the end of the pass only.
-- **Skip worked passes.** A pass more than half done but left short of the end the tractor is at (its 20 m next to that end mostly not covered) counts as still to do there, so the turn can go onto it. When the order's next pass is more than four passes over (the ones in between were done before), the turn goes onto the nearest pass still to do instead of along the headland to it, and the order starts again from there. A pass counts as done by what is painted on the map, not by a section having been on somewhere on it: where a bay cuts the passes, doing the short piece no longer marks the whole pass done, and the long piece gets its turn from the other end. (The turn onto the nearest pass with room is official since #807.)
+- **Skip worked passes.** A pass more than half done but left short of the end the tractor is at (its 20 m next to that end mostly not covered) counts as still to do there, so the turn can go onto it. When the order's next pass is more than four passes over (the ones in between were done before), the turn goes onto the nearest pass still to do instead of along the headland to it, and the order starts again from there. A pass counts as done by what is painted on the map, not by a section having been on somewhere on it: where a bay cuts the passes, doing the short piece no longer marks the whole pass done, and the long piece gets its turn from the other end. With the headland on and its sections off, a pass lying only in the headland band is left to the headland laps: it no longer hides the passes still to do (that gave "End of field reached" with crop left). A manual U-turn and AutoSteer joining a pass from the headland count done passes the same way. (The turn onto the nearest pass with room is official since #807.)
 - **AutoSteer and a hard boundary.** At the end of a pass with no U-turn there (no pass with room at this end, no turn that fits, U-turn off), AutoSteer goes off with the alarm a metre into the headland, so there is the headland to turn by hand; before, it steered on through the fence. This also works on a pass bent round trees in the headland (Go around), which leaves the crop at an angle. And with no U-turn planned, when an end of the implement is heading out across a hard boundary (AutoSteer joining a line from a corner, for example), AutoSteer goes off with the alarm. It looks along the curve the tractor is steering, so on the first lap of a boundary curve it no longer goes off at a bend with the tractor on the line. A U-turn planned for one end is dropped when the tractor turns round on the pass, so these still work at the other end.
 - **AutoSteer flickering on and off.** With the steer switch / button set to Button, when the app took AutoSteer off just as it went on (a hard boundary warning, for example), the steer module's answer to the engage turned it on again, and it went on and off for seconds with the alarm. The app now ignores the module's "steering" for 1 s after it takes AutoSteer off itself.
 - **No line outside a hard boundary.** A pass that lies wholly outside a hard boundary (a lap of a boundary curve outside the fence, say) is no longer shown when the tractor is outside the field with AutoSteer off: the nearest pass inside is shown. Moving the line onto such a pass is refused with a message.

Shared/AgOpenWeb.Services/Pipeline/GpsPipelineService.cs

@@ -1111,7 +1111,7 @@ private void ProcessCycle(GpsData data)
         // the pass is driven, or offered with a sound (the nearest point is steered meanwhile).
         bool joinedNewLine = UpdateLineEntry(in intents, config, autoSteerEngaged, track, passNumber, nudgeOffset,
             headlandLine, boundary, contourOn || recordedPath != null, isSkipWorkedMode,
-            driftedEasting, driftedNorthing, headingRad);
+            driftedEasting, driftedNorthing, headingRad, turnLine, workHeadland);
         // AutoSteer may have chosen the pass to join (the nearest one still to do).
         passNumber = _guidanceWorking.HowManyPathsAway;
 
@@ -2949,7 +2949,8 @@ private bool PassOutsideHardFence(Models.Track.Track track, int pass, double nud
     /// </summary>
     private bool UpdateLineEntry(in PipelineIntentBatch intents, ConfigurationStore config, bool autoSteerEngaged,
         Models.Track.Track? track, int passNumber, double nudgeOffset, List<Vec3>? headlandLine, Boundary? boundary,
-        bool otherGuidance, bool skipWorked, double e, double n, double headingRad)
+        bool otherGuidance, bool skipWorked, double e, double n, double headingRad,
+        IReadOnlyList<Vec3>? turnLine = null, IReadOnlyList<Vec3>? workHeadland = null)
     {
         var mode = config.Guidance.LineEntryMode;
         bool trackSwitched = !ReferenceEquals(track, _lineEntryTrack);
@@ -2988,8 +2989,13 @@ private bool UpdateLineEntry(in PipelineIntentBatch intents, ConfigurationStore
         // choice of pass and is left alone. Fork only.
         if (engagedNow && !snapped && skipWorked && canPlan && InHeadland(boundary!, headlandLine!, e, n))
         {
+            // The snake's view of the field (its tests, on the turn line and the crop as a turn has them).
+            var snakeView = new YouTurnStateMachine.TickContext(
+                new Position { Easting = e, Northing = n, Heading = headingRad * 180.0 / Math.PI },
+                track, boundary, turnLine ?? headlandLine, 0, IsSkipWorkedMode: true, 0, 0,
+                IsPointCovered: _isPointCovered, WorkHeadlandLine: workHeadland, PassEndLine: headlandLine);
             var chosen = NearestOpenPassEntry(config, track!, passNumber, nudgeOffset, headlandLine!, boundary!,
-                e, n, headingRad, out bool onWorkedPass);
+                e, n, headingRad, in snakeView, out bool onWorkedPass);
             if (chosen is { } c)
             {
                 _lineEntryOffer = null;
@@ -3069,7 +3075,8 @@ private bool UpdateLineEntry(in PipelineIntentBatch intents, ConfigurationStore
     /// The pass AutoSteer joins when engaged in the headland with "Ignore worked tracks" (fork only):
     /// the nearest pass to <paramref name="passNumber"/> (that one first, then one either side, two
     /// either side, up to <see cref="NearestOpenPassReach"/>) that is still to do (not worked: the
-    /// snake's test, <see cref="YouTurnStateMachine.IsPassWorked"/>), not wholly outside a hard fence,
+    /// snake's tests, <see cref="YouTurnStateMachine.IsPassToJoin"/>: worked on the whole, or its piece
+    /// from the end it is entered at done, a bay cutting it), not wholly outside a hard fence,
     /// and that the way to the start of the pass reaches straight (<see cref="PlanLineEntry"/>: on the
     /// pass before the crop, at the U-turn radius, in the headland, the implement off a hard fence),
     /// or that the tractor is on already. Of two at the same distance the shorter way. A pass the way
@@ -3078,7 +3085,7 @@ private bool UpdateLineEntry(in PipelineIntentBatch intents, ConfigurationStore
     /// </summary>
     private (int Pass, LineEntryPlan Plan)? NearestOpenPassEntry(ConfigurationStore config, Models.Track.Track track,
         int passNumber, double nudgeOffset, List<Vec3> headlandLine, Boundary boundary, double e, double n,
-        double headingRad, out bool onWorkedPass)
+        double headingRad, in YouTurnStateMachine.TickContext snakeView, out bool onWorkedPass)
     {
         onWorkedPass = false;
         int planned = 0;
@@ -3088,7 +3095,7 @@ private bool UpdateLineEntry(in PipelineIntentBatch intents, ConfigurationStore
             foreach (int pass in k == 0 ? new[] { passNumber } : new[] { passNumber - k, passNumber + k })
             {
                 if (PassOutsideHardFence(track, pass, nudgeOffset, boundary)) continue;
-                if (_youTurnStateMachine.IsPassWorked(track, pass, nudgeOffset, boundary, headlandLine, _isPointCovered))
+                if (!_youTurnStateMachine.IsPassToJoin(in snakeView, track, pass, nudgeOffset))
                 {
                     if (k == 0) onWorkedPass = true;
                     continue;
@@ -3104,13 +3111,33 @@ private bool UpdateLineEntry(in PipelineIntentBatch intents, ConfigurationStore
                 _logger.LogDebug("[LineEntry] Pass {Pass} is still to do: {Outcome}{Length}", PassName(pass), plan.Outcome,
                     double.IsNaN(length) ? "" : string.Create(CultureInfo.InvariantCulture, $", way {length:F0} m"));
                 if (double.IsNaN(length)) continue;
+                // Still to do on the whole, but its piece from the end the way enters it at is done (a
+                // bay cuts the pass): the snake's turns call it worked there too.
+                var entry = plan.Outcome == LineEntryOutcome.Planned ? EntryPose(plan.Path!) : new Vec3(e, n, headingRad);
+                if (!_youTurnStateMachine.IsPassToJoin(in snakeView, track, pass, nudgeOffset, entry))
+                {
+                    _logger.LogDebug("[LineEntry] Pass {Pass}: its piece from this end is done", PassName(pass));
+                    if (k == 0) onWorkedPass = true;
+                    continue;
+                }
                 if (best == null || length < best.Value.Length) best = (pass, plan, length);
             }
             if (best is { } b) return (b.Pass, b.Plan);
         }
         return null;
     }
 
+    /// <summary>The last point of a way and the heading (radians) it arrives at.</summary>
+    private static Vec3 EntryPose(IReadOnlyList<Vec3> path)
+    {
+        var last = path[^1];
+        int k = path.Count - 2;
+        while (k > 0 && GeometryMath.Distance(path[k], last) < 0.5) k--;
+        var before = path[Math.Max(k, 0)];
+        return new Vec3(last.Easting, last.Northing,
+            Math.Atan2(last.Easting - before.Easting, last.Northing - before.Northing));
+    }
+
     private static double PolylineLength(IReadOnlyList<Vec3> path)
     {
         double len = 0;

Shared/AgOpenWeb.Services/YouTurn/YouTurnPathingService.cs

@@ -53,7 +53,8 @@ public void ComputeNextTrack(
         YouTurnWorkingState turn,
         int uTurnSkipRows,
         bool isSkipWorkedMode,
-        Models.Track.Track? selectedTrack)
+        Models.Track.Track? selectedTrack,
+        Func<int, bool>? isWorked = null)
     {
         if (referenceTrack.Points.Count < 2) return;
 
@@ -72,7 +73,7 @@ public void ComputeNextTrack(
         // Skip-worked mode: find next unworked path instead of fixed skip.
         if (isSkipWorkedMode && selectedTrack != null)
         {
-            pathsToMove = GetNextUnworkedPathSkip(selectedTrack, guidance.HowManyPathsAway, positiveOffset, pathsToMove);
+            pathsToMove = GetNextUnworkedPathSkip(selectedTrack, guidance.HowManyPathsAway, positiveOffset, pathsToMove, isWorked);
         }
 
         int offsetChange = positiveOffset ? pathsToMove : -pathsToMove;