fix: address review feedback for sweatbox taxi planner

internal/sweatbox/airport.go

@@ -126,10 +126,14 @@ func (g *Graph) Intersect(nameA, nameB string) bool {
 	return ok
 }
 
-// FindIntersection finds the first pair of waypoints (scan order of A then B)
-// within tol and returns the point on surface A (TWRTrainer FindNextWaypoint
-// semantics), plus the point indices on A and B. ok is false if either surface
-// is missing or no pair is within tolerance.
+// FindIntersection finds waypoints of A and B within tol and returns the
+// closest pair (ties broken by scan order of A then B). The returned point is
+// on surface A. ok is false if either surface is missing or no pair is within
+// tolerance.
+//
+// Closest-within-tol (rather than first-within-tol) keeps junctions correct on
+// dense polylines where several vertices of A lie within snap of a shared
+// vertex on B.
 //
 // When nameA and nameB refer to the same surface, returns the first waypoint.
 func (g *Graph) FindIntersection(nameA, nameB string) (pt Point, idxA, idxB int, ok bool) {
@@ -149,18 +153,24 @@ func (g *Graph) FindIntersection(nameA, nameB string) (pt Point, idxA, idxB int,
 	if ia == ib {
 		return sa.Points[0], 0, 0, true
 	}
-	tol := g.tolM
-	tolSq := tol * tol
+	tolSq := g.tolM * g.tolM
+	bestD := tolSq + 1
+	bestA, bestB := -1, -1
 	for a := range sa.Points {
 		pa := sa.Points[a]
 		for b := range sb.Points {
 			pb := sb.Points[b]
-			if geo.DistanceSq(pa.Lat, pa.Lon, pb.Lat, pb.Lon) <= tolSq {
-				return pa, a, b, true
+			d := geo.DistanceSq(pa.Lat, pa.Lon, pb.Lat, pb.Lon)
+			if d <= tolSq && d < bestD {
+				bestD = d
+				bestA, bestB = a, b
 			}
 		}
 	}
-	return Point{}, -1, -1, false
+	if bestA < 0 {
+		return Point{}, -1, -1, false
+	}
+	return sa.Points[bestA], bestA, bestB, true
 }
 
 // Neighbors returns names of surfaces that intersect name (excluding itself),

internal/sweatbox/taxi.go

@@ -7,6 +7,10 @@ import (
 	"github.com/renorris/openfsd/internal/geo"
 )
 
+// pathStitchEpsM is the only distance used when collapsing adjacent path samples
+// at leg joints. Intersection snap (~100 ft) must not drop real polyline vertices.
+const pathStitchEpsM = 1.0
+
 // TaxiPlan is a validated taxi route: ordered surface steps, hold-shorts, an
 // optional final parking, and an expanded waypoint polyline for the engine.
 //
@@ -23,6 +27,7 @@ type TaxiPlan struct {
 	// Waypoints is the ordered ground path (lat/lon) along surface polylines.
 	Waypoints []Point
 	// HoldAt lists hold-short positions resolved along the route.
+	// Every entry has WaypointIndex >= 0 (off-route holds are rejected).
 	HoldAt []TaxiHold
 }
 
@@ -32,8 +37,7 @@ type TaxiHold struct {
 	Name string
 	// Point is where the aircraft should stop.
 	Point Point
-	// WaypointIndex is the index into TaxiPlan.Waypoints of Point (−1 if the
-	// hold could not be placed on the expanded path but validation passed).
+	// WaypointIndex is the index into TaxiPlan.Waypoints of Point.
 	WaypointIndex int
 }
 
@@ -120,7 +124,7 @@ func (g *Graph) PlanTaxi(current string, steps, holds []string) (TaxiPlan, strin
 		}
 	}
 
-	// Normalize / validate holds.
+	// Normalize / validate holds (existence + kind only; on-route check after path).
 	normHolds := make([]string, 0, len(holds))
 	for _, raw := range holds {
 		tok := strings.TrimSpace(raw)
@@ -137,7 +141,6 @@ func (g *Graph) PlanTaxi(current string, steps, holds []string) (TaxiPlan, strin
 		}
 		if s.Kind == SurfaceRunway {
 			// Keep the designator the instructor typed when it is an end name.
-			// Prefer the token if it matches an end; else combined name.
 			up := strings.ToUpper(tok)
 			if up == s.RwyA || up == s.RwyB {
 				name = up
@@ -166,64 +169,44 @@ func (g *Graph) PlanTaxi(current string, steps, holds []string) (TaxiPlan, strin
 
 	// First step must intersect current taxiway/runway (when current is set).
 	if curName != "" {
-		first := firstRouteSurface(normSteps, parking, g)
-		if first == "" {
-			return zero, "First step of taxi route must intersect with current taxiway/runway."
-		}
-		if !g.Intersect(curName, first) && !sameSurface(g, curName, first) {
+		first := firstRouteSurface(normSteps, parking)
+		if first == "" || (!g.Intersect(curName, first) && !sameSurface(g, curName, first)) {
 			return zero, "First step of taxi route must intersect with current taxiway/runway."
 		}
 	}
 
 	// Consecutive surface steps must intersect.
-	routeSurfaces := make([]string, 0, len(normSteps)+1)
-	if curName != "" && (len(normSteps) == 0 || !sameSurface(g, curName, normSteps[0])) {
-		// Include current only for intersection chaining into first step;
-		// not as a plan step.
-	}
-	routeSurfaces = append(routeSurfaces, normSteps...)
-	for i := 1; i < len(routeSurfaces); i++ {
-		a, b := routeSurfaces[i-1], routeSurfaces[i]
+	for i := 1; i < len(normSteps); i++ {
+		a, b := normSteps[i-1], normSteps[i]
 		if !g.Intersect(a, b) {
 			return zero, fmt.Sprintf("%s and %s do not intersect.", displayName(a), displayName(b))
 		}
 	}
 
-	// When current is set and differs from first step, current must intersect first
-	// (already checked). Parking-only from an intersecting surface is ok.
-	if len(normSteps) >= 1 && curName != "" && !sameSurface(g, curName, normSteps[0]) {
-		if !g.Intersect(curName, normSteps[0]) {
-			return zero, "First step of taxi route must intersect with current taxiway/runway."
-		}
-	}
-
-	// Parking exit: last surface (or current) must be able to leave toward parking.
-	// We do not require a 100 ft snap between last surface and parking; TWRTrainer
-	// taxis to the closest waypoint on the last surface then direct to parking.
+	// Parking-only: need a current surface to leave from.
 	if parking != "" && len(normSteps) == 0 {
-		// Taxi direct from current surface to parking — current required.
-		if curName == "" {
-			return zero, "First step of taxi route must intersect with current taxiway/runway."
-		}
-		if g.Surface(curName) == nil {
+		if curName == "" || g.Surface(curName) == nil {
 			return zero, "First step of taxi route must intersect with current taxiway/runway."
 		}
 	}
 
-	plan := TaxiPlan{
-		Steps:   append([]string(nil), normSteps...),
-		Holds:   append([]string(nil), normHolds...),
-		Parking: parking,
+	wps := g.buildTaxiPath(curName, normSteps, parking)
+	wps, holdAt, errMsg := g.resolveHolds(normSteps, curName, normHolds, wps)
+	if errMsg != "" {
+		return zero, errMsg
 	}
 
-	wps, holdAt := g.buildTaxiPath(curName, normSteps, parking, normHolds)
-	plan.Waypoints = wps
-	plan.HoldAt = holdAt
-	return plan, ""
+	return TaxiPlan{
+		Steps:     append([]string(nil), normSteps...),
+		Holds:     append([]string(nil), normHolds...),
+		Parking:   parking,
+		Waypoints: wps,
+		HoldAt:    holdAt,
+	}, ""
 }
 
 // firstRouteSurface is the first taxiway/runway on the route, or parking name.
-func firstRouteSurface(normSteps []string, parking string, g *Graph) string {
+func firstRouteSurface(normSteps []string, parking string) string {
 	if len(normSteps) > 0 {
 		return normSteps[0]
 	}
@@ -243,15 +226,13 @@ func displayName(name string) string {
 	return strings.ToUpper(name)
 }
 
-// buildTaxiPath expands surface polylines into a waypoint list and places holds.
-func (g *Graph) buildTaxiPath(current string, steps []string, parking string, holds []string) ([]Point, []TaxiHold) {
+// buildTaxiPath expands surface polylines into a waypoint list.
+func (g *Graph) buildTaxiPath(current string, steps []string, parking string) []Point {
 	var wps []Point
 	if len(steps) == 0 && parking == "" {
-		return nil, nil
+		return nil
 	}
 
-	// Chain of surfaces we walk (excluding parking).
-	// When current differs from first step, entry is their intersection.
 	type leg struct {
 		surface string
 		from    Point
@@ -262,21 +243,21 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho
 
 	var legs []leg
 
-	// Determine starting surface and entry point.
-	startSurface := ""
+	// Entry onto first step from a different current surface (point on first step).
 	var entry Point
 	entrySet := false
 	if len(steps) > 0 {
-		startSurface = steps[0]
-		if current != "" && !sameSurface(g, current, startSurface) {
-			if p, _, _, ok := g.FindIntersection(current, startSurface); ok {
-				entry = p
+		if current != "" && !sameSurface(g, current, steps[0]) {
+			if p, _, idxB, ok := g.FindIntersection(current, steps[0]); ok {
+				sb := g.Surface(steps[0])
+				if sb != nil && idxB >= 0 && idxB < len(sb.Points) {
+					entry = sb.Points[idxB]
+				} else {
+					entry = p
+				}
 				entrySet = true
 			}
 		}
-	} else {
-		// Parking-only: walk on current toward parking.
-		startSurface = current
 	}
 
 	// Build legs between consecutive steps.
@@ -291,7 +272,6 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho
 		} else {
 			// From previous intersection (point on this surface).
 			if p, _, idxB, ok := g.FindIntersection(steps[i-1], sName); ok {
-				// Prefer the point on this surface (B).
 				sb := g.Surface(sName)
 				if sb != nil && idxB >= 0 && idxB < len(sb.Points) {
 					lg.from = sb.Points[idxB]
@@ -321,11 +301,8 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho
 				}
 			}
 		} else {
-			// Destination is this surface (typically a runway): to = entry onto it
-			// from previous, already as from; keep to unset so we still include from.
-			// For a multi-step route ending on a runway, the end point is the
-			// intersection of the previous surface and this runway — already "from".
-			// Single-step runway from another current: "from" is entry; path is [entry].
+			// Destination is this surface (typically a runway). Path ends at entry
+			// from the previous surface (or from current when single-step).
 			if !lg.fromSet && current != "" && !sameSurface(g, current, sName) {
 				if p, _, idxB, ok := g.FindIntersection(current, sName); ok {
 					sb := g.Surface(sName)
@@ -341,7 +318,7 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho
 		legs = append(legs, lg)
 	}
 
-	// Parking-only leg: from closest on current to parking point.
+	// Parking-only leg: leave current at closest waypoint toward parking.
 	if len(steps) == 0 && parking != "" && current != "" {
 		ps := g.Surface(parking)
 		lg := leg{surface: current}
@@ -356,7 +333,7 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho
 		legs = append(legs, lg)
 	}
 
-	// Expand legs into waypoints.
+	// Expand legs into waypoints (stitch with tiny epsilon only).
 	for _, lg := range legs {
 		s := g.Surface(lg.surface)
 		if s == nil || len(s.Points) == 0 {
@@ -364,16 +341,16 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho
 		}
 		switch {
 		case lg.fromSet && lg.toSet:
-			seg := walkPolyline(s, lg.from, lg.to, g.tolM)
-			wps = appendUniquePoints(wps, seg, g.tolM)
+			seg := walkPolyline(s, lg.from, lg.to)
+			wps = appendUniquePoints(wps, seg, pathStitchEpsM)
 		case lg.fromSet:
-			wps = appendUniquePoints(wps, []Point{lg.from}, g.tolM)
+			wps = appendUniquePoints(wps, []Point{lg.from}, pathStitchEpsM)
 		case lg.toSet:
-			wps = appendUniquePoints(wps, []Point{lg.to}, g.tolM)
+			wps = appendUniquePoints(wps, []Point{lg.to}, pathStitchEpsM)
 		}
 	}
 
-	// Append parking coordinate (always, even if within snap of last taxiway point).
+	// Append parking coordinate (always, even if near last taxiway point).
 	if parking != "" {
 		if ps := g.Surface(parking); ps != nil && len(ps.Points) > 0 {
 			p := ps.Points[0]
@@ -383,75 +360,208 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho
 		}
 	}
 
-	// Resolve hold-short positions against the route surfaces.
-	holdAt := g.resolveHolds(steps, current, holds, wps)
-	return wps, holdAt
+	return wps
 }
 
-// resolveHolds places each hold at the intersection of the hold target with a
-// route surface (first match in route order). Destination runway is not auto-
-// added here — the engine holds short of the final runway by status.
-func (g *Graph) resolveHolds(steps []string, current string, holds []string, wps []Point) []TaxiHold {
+// resolveHolds places each hold on the route path. Holds must map onto Waypoints
+// (within intersection snap). Off-route holds are rejected with an instructor string.
+// May insert HOLD vertices into wps when they lie on a route surface but were
+// outside the expanded turn-to-turn segment (e.g. already on first surface).
+//
+// Placement prefers the latest path position that intersects the hold target:
+// for each route step (reverse order), the step∩hold point is a candidate; when
+// the hold is the step itself (e.g. destination runway), the entry from the
+// previous surface (or current) is used. HOLD surfaces use their single point.
+func (g *Graph) resolveHolds(steps []string, current string, holds []string, wps []Point) ([]Point, []TaxiHold, string) {
 	if len(holds) == 0 {
-		return nil
+		return wps, nil, ""
 	}
-	// Surfaces along the taxi (current + steps) for intersection search.
-	chain := make([]string, 0, len(steps)+1)
-	if current != "" {
-		chain = append(chain, current)
-	}
-	chain = append(chain, steps...)
 
 	out := make([]TaxiHold, 0, len(holds))
 	for _, h := range holds {
 		hs := g.Surface(h)
-		var hp Point
-		found := false
+		var bestPoint Point
+		bestIdx := -1
+
 		if hs != nil && hs.Kind == SurfaceHold && len(hs.Points) == 1 {
-			hp = hs.Points[0]
-			found = true
-		} else {
-			// Intersection of hold target with any route surface.
-			for _, sName := range chain {
-				if sameSurface(g, sName, h) {
-					// Holding short of a surface we're taxiing on: use first
-					// intersection of that surface with a neighboring chain member.
-					continue
-				}
-				if p, _, _, ok := g.FindIntersection(sName, h); ok {
-					hp = p
-					found = true
-					break
-				}
+			hp := hs.Points[0]
+			// HOLD is on-route if it snaps to any route-step (or current) surface vertex.
+			if !g.holdNearRouteSurface(hp, steps, current) {
+				return wps, nil, fmt.Sprintf("%s is not on the taxi route.", displayName(h))
 			}
-			// If hold is the destination runway itself, use entry onto it.
-			if !found && len(steps) > 0 && sameSurface(g, steps[len(steps)-1], h) {
-				if len(steps) >= 2 {
-					if p, _, _, ok := g.FindIntersection(steps[len(steps)-2], steps[len(steps)-1]); ok {
-						hp = p
-						found = true
-					}
-				} else if current != "" {
-					if p, _, _, ok := g.FindIntersection(current, steps[0]); ok {
-						hp = p
-						found = true
-					}
+			idx := findWaypointIndex(wps, hp, g.tolM)
+			if idx < 0 {
+				// Inject so the engine sees the hold on the path (common when
+				// already on the first surface: expansion starts at the next turn).
+				wps, idx = insertWaypoint(wps, hp, pathStitchEpsM)
+			}
+			out = append(out, TaxiHold{Name: h, Point: wps[idx], WaypointIndex: idx})
+			continue
+		}
+
+		// Collect candidate intersections with route steps (reverse: prefer last crossing).
+		for i := len(steps) - 1; i >= 0; i-- {
+			sName := steps[i]
+			var cand Point
+			var ok bool
+			if sameSurface(g, sName, h) {
+				// Holding short of a surface that is itself a step (e.g. dest runway):
+				// stop at the entry onto that surface.
+				if i > 0 {
+					cand, ok = intersectionOnSurface(g, steps[i-1], sName)
+				} else if current != "" && !sameSurface(g, current, sName) {
+					cand, ok = intersectionOnSurface(g, current, sName)
+				} else {
+					// Already on the hold surface as first step — use first path point
+					// that lies on this surface.
+					cand, ok = firstPathPointOnSurface(g, wps, h)
+				}
+			} else {
+				cand, ok = intersectionOnSurface(g, sName, h)
+			}
+			if !ok {
+				continue
+			}
+			idx := findWaypointIndex(wps, cand, g.tolM)
+			if idx < 0 {
+				continue
+			}
+			// Keep the candidate furthest along the path.
+			if idx > bestIdx {
+				bestIdx = idx
+				bestPoint = wps[idx]
+			}
+		}
+
+		// Fallback: hold ∩ current only when no step candidate mapped onto the path
+		// (avoids Issue 1: A∩19 beating B∩19 when A is current but route leaves via B).
+		if bestIdx < 0 && current != "" && !sameSurface(g, current, h) {
+			if cand, ok := intersectionOnSurface(g, current, h); ok {
+				if idx := findWaypointIndex(wps, cand, g.tolM); idx >= 0 {
+					bestIdx = idx
+					bestPoint = wps[idx]
 				}
 			}
 		}
-		th := TaxiHold{Name: h, WaypointIndex: -1}
-		if found {
-			th.Point = hp
-			th.WaypointIndex = findWaypointIndex(wps, hp, g.tolM)
+
+		if bestIdx < 0 {
+			return wps, nil, fmt.Sprintf("%s is not on the taxi route.", displayName(h))
 		}
-		out = append(out, th)
+		out = append(out, TaxiHold{Name: h, Point: bestPoint, WaypointIndex: bestIdx})
 	}
-	return out
+	// Re-snap indices after possible HOLD insertions shifted the path.
+	for i := range out {
+		idx := findWaypointIndex(wps, out[i].Point, g.tolM)
+		if idx < 0 {
+			return wps, nil, fmt.Sprintf("%s is not on the taxi route.", displayName(out[i].Name))
+		}
+		out[i].WaypointIndex = idx
+		out[i].Point = wps[idx]
+	}
+	return wps, out, ""
 }
 
-// walkPolyline returns points along s from near "from" to near "to" (inclusive),
-// walking the shorter direction along the polyline when ambiguous.
-func walkPolyline(s *Surface, from, to Point, tolM float64) []Point {
+// holdNearRouteSurface reports whether p is within intersection snap of any
+// vertex on a route step (or current) surface.
+func (g *Graph) holdNearRouteSurface(p Point, steps []string, current string) bool {
+	if current != "" && g.PointIndexWithin(current, p) >= 0 {
+		return true
+	}
+	for _, sName := range steps {
+		if g.PointIndexWithin(sName, p) >= 0 {
+			return true
+		}
+	}
+	// Also allow closest-within-tol when the hold is not exactly a stored vertex.
+	tolSq := g.tolM * g.tolM
+	check := func(name string) bool {
+		s := g.Surface(name)
+		if s == nil {
+			return false
+		}
+		for _, q := range s.Points {
+			if geo.DistanceSq(p.Lat, p.Lon, q.Lat, q.Lon) <= tolSq {
+				return true
+			}
+		}
+		return false
+	}
+	if current != "" && check(current) {
+		return true
+	}
+	for _, sName := range steps {
+		if check(sName) {
+			return true
+		}
+	}
+	return false
+}
+
+// insertWaypoint inserts p into wps if not already present within epsM.
+// Insertion is before the closest existing waypoint (or append if empty).
+// Returns the updated slice and the index of p.
+func insertWaypoint(wps []Point, p Point, epsM float64) ([]Point, int) {
+	if idx := findWaypointIndex(wps, p, epsM); idx >= 0 {
+		return wps, idx
+	}
+	if len(wps) == 0 {
+		return []Point{p}, 0
+	}
+	// Insert before the closest path vertex so holds ahead of the first turn
+	// appear earlier on the path.
+	best := 0
+	bestD := geo.DistanceSq(p.Lat, p.Lon, wps[0].Lat, wps[0].Lon)
+	for i := 1; i < len(wps); i++ {
+		d := geo.DistanceSq(p.Lat, p.Lon, wps[i].Lat, wps[i].Lon)
+		if d < bestD {
+			bestD = d
+			best = i
+		}
+	}
+	out := make([]Point, 0, len(wps)+1)
+	out = append(out, wps[:best]...)
+	out = append(out, p)
+	out = append(out, wps[best:]...)
+	return out, best
+}
+
+// intersectionOnSurface returns the intersection point preferred on surface B
+// (the second name), falling back to the point on A.
+func intersectionOnSurface(g *Graph, nameA, nameB string) (Point, bool) {
+	p, idxA, idxB, ok := g.FindIntersection(nameA, nameB)
+	if !ok {
+		return Point{}, false
+	}
+	if sb := g.Surface(nameB); sb != nil && idxB >= 0 && idxB < len(sb.Points) {
+		return sb.Points[idxB], true
+	}
+	if sa := g.Surface(nameA); sa != nil && idxA >= 0 && idxA < len(sa.Points) {
+		return sa.Points[idxA], true
+	}
+	return p, true
+}
+
+// firstPathPointOnSurface finds the first waypoint within snap of any point on surface h.
+func firstPathPointOnSurface(g *Graph, wps []Point, h string) (Point, bool) {
+	s := g.Surface(h)
+	if s == nil || len(s.Points) == 0 || len(wps) == 0 {
+		return Point{}, false
+	}
+	tolSq := g.tolM * g.tolM
+	for _, wp := range wps {
+		for _, sp := range s.Points {
+			if geo.DistanceSq(wp.Lat, wp.Lon, sp.Lat, sp.Lon) <= tolSq {
+				return wp, true
+			}
+		}
+	}
+	return Point{}, false
+}
+
+// walkPolyline returns points along s from the vertex nearest "from" to the
+// vertex nearest "to" (inclusive), walking index-forward or index-reverse along
+// the open polyline (no closed-loop shorter-arc logic).
+func walkPolyline(s *Surface, from, to Point) []Point {
 	if s == nil || len(s.Points) == 0 {
 		return nil
 	}
@@ -463,11 +573,9 @@ func walkPolyline(s *Surface, from, to Point, tolM float64) []Point {
 	if iFrom == iTo {
 		return []Point{s.Points[iFrom]}
 	}
-	// Forward path length vs reverse.
 	if iFrom < iTo {
 		return append([]Point(nil), s.Points[iFrom:iTo+1]...)
 	}
-	// Reverse.
 	out := make([]Point, 0, iFrom-iTo+1)
 	for i := iFrom; i >= iTo; i-- {
 		out = append(out, s.Points[i])
@@ -491,12 +599,14 @@ func closestIndex(pts []Point, p Point) int {
 	return best
 }
 
-func appendUniquePoints(dst []Point, src []Point, tolM float64) []Point {
-	tolSq := tolM * tolM
+// appendUniquePoints appends src onto dst, skipping a point only when it is
+// within epsM of the current last point (stitch joint dedup — not intersection snap).
+func appendUniquePoints(dst []Point, src []Point, epsM float64) []Point {
+	epsSq := epsM * epsM
 	for _, p := range src {
 		if len(dst) > 0 {
 			last := dst[len(dst)-1]
-			if geo.DistanceSq(last.Lat, last.Lon, p.Lat, p.Lon) <= tolSq {
+			if geo.DistanceSq(last.Lat, last.Lon, p.Lat, p.Lon) <= epsSq {
 				continue
 			}
 		}
@@ -507,10 +617,15 @@ func appendUniquePoints(dst []Point, src []Point, tolM float64) []Point {
 
 func findWaypointIndex(wps []Point, p Point, tolM float64) int {
 	tolSq := tolM * tolM
+	// Prefer closest within tol so a hold snaps to the best path vertex.
+	best := -1
+	bestD := tolSq + 1
 	for i, q := range wps {
-		if geo.DistanceSq(q.Lat, q.Lon, p.Lat, p.Lon) <= tolSq {
-			return i
+		d := geo.DistanceSq(q.Lat, q.Lon, p.Lat, p.Lon)
+		if d <= tolSq && d < bestD {
+			bestD = d
+			best = i
 		}
 	}
-	return -1
+	return best
 }

internal/sweatbox/taxi_test.go

@@ -1,8 +1,11 @@
 package sweatbox
 
 import (
+	"math"
 	"strings"
 	"testing"
+
+	"github.com/renorris/openfsd/internal/geo"
 )
 
 func TestPlanTaxi_KBTVHappyPaths(t *testing.T) {
@@ -26,24 +29,6 @@ func TestPlanTaxi_KBTVHappyPaths(t *testing.T) {
 		t.Fatalf("waypoints too short: %v", plan.Waypoints)
 	}
 
-	// Current on A, taxi B 19 — first step B intersects A.
-	plan, errMsg = g.PlanTaxi("A", []string{"B", "19"}, []string{"19"})
-	if errMsg != "" {
-		t.Fatalf("A→B→19: %s", errMsg)
-	}
-	if len(plan.Holds) != 1 || plan.Holds[0] != "19" {
-		t.Errorf("holds = %v", plan.Holds)
-	}
-	if len(plan.HoldAt) != 1 {
-		t.Fatalf("HoldAt len = %d", len(plan.HoldAt))
-	}
-	if plan.HoldAt[0].Name != "19" {
-		t.Errorf("HoldAt name = %q", plan.HoldAt[0].Name)
-	}
-	if plan.HoldAt[0].Point.Lat == 0 {
-		t.Error("HoldAt point unset")
-	}
-
 	// Via C K J to 33
 	plan, errMsg = g.PlanTaxi("C", []string{"C", "K", "J", "33"}, nil)
 	if errMsg != "" {
@@ -53,8 +38,7 @@ func TestPlanTaxi_KBTVHappyPaths(t *testing.T) {
 		t.Errorf("expected multi-point path, got %d", len(plan.Waypoints))
 	}
 
-	// Parking destination @G1 via A (A is near gates; G1 may be far — still allowed)
-	// Use GA9 which snaps to J.
+	// Parking destination @GA9 (snaps near J).
 	plan, errMsg = g.PlanTaxi("J", []string{"J", "@GA9"}, nil)
 	if errMsg != "" {
 		t.Fatalf("J @GA9: %s", errMsg)
@@ -65,11 +49,14 @@ func TestPlanTaxi_KBTVHappyPaths(t *testing.T) {
 	if len(plan.Steps) != 1 || plan.Steps[0] != "J" {
 		t.Errorf("Steps = %v", plan.Steps)
 	}
-	// Last waypoint should be parking.
+	// Last waypoint should be parking (even if within snap of last taxiway point).
 	last := plan.Waypoints[len(plan.Waypoints)-1]
 	ps := g.Surface("GA9")
-	if ps == nil || last.Lat != ps.Points[0].Lat {
-		t.Errorf("last wp = %v, parking = %v", last, ps)
+	if ps == nil || last != ps.Points[0] {
+		t.Errorf("last wp = %v, parking = %v", last, ps.Points[0])
+	}
+	if len(plan.Waypoints) < 2 {
+		t.Fatal("parking within snap of taxiway must still append parking as final point")
 	}
 
 	// Bare parking name as last step.
@@ -82,6 +69,52 @@ func TestPlanTaxi_KBTVHappyPaths(t *testing.T) {
 	}
 }
 
+// Issue 1 regression: hold short of 19 on A→B→19 must be B∩19, not A∩19.
+func TestPlanTaxi_HoldAtCorrectCrossing_KBTV(t *testing.T) {
+	g := NewGraph(loadKBTV(t))
+
+	bX19, _, _, ok := g.FindIntersection("B", "19")
+	if !ok {
+		t.Fatal("B and 19 must intersect on KBTV")
+	}
+	aX19, _, _, ok := g.FindIntersection("A", "19")
+	if !ok {
+		t.Fatal("A and 19 must intersect on KBTV")
+	}
+	// Sanity: the two crossings are far apart (~560 m).
+	if geo.Distance(bX19.Lat, bX19.Lon, aX19.Lat, aX19.Lon) < 200 {
+		t.Fatalf("test setup: A∩19 and B∩19 too close")
+	}
+
+	plan, errMsg := g.PlanTaxi("A", []string{"B", "19"}, []string{"19"})
+	if errMsg != "" {
+		t.Fatalf("A→B→19 hs 19: %s", errMsg)
+	}
+	if len(plan.HoldAt) != 1 {
+		t.Fatalf("HoldAt len = %d", len(plan.HoldAt))
+	}
+	h := plan.HoldAt[0]
+	if h.Name != "19" {
+		t.Errorf("HoldAt name = %q", h.Name)
+	}
+	if h.WaypointIndex < 0 || h.WaypointIndex >= len(plan.Waypoints) {
+		t.Fatalf("WaypointIndex = %d, path len %d", h.WaypointIndex, len(plan.Waypoints))
+	}
+	if plan.Waypoints[h.WaypointIndex] != h.Point {
+		t.Errorf("Waypoints[i]=%v HoldAt.Point=%v", plan.Waypoints[h.WaypointIndex], h.Point)
+	}
+	// Hold must be near B∩19, not A∩19.
+	dB := geo.Distance(h.Point.Lat, h.Point.Lon, bX19.Lat, bX19.Lon)
+	dA := geo.Distance(h.Point.Lat, h.Point.Lon, aX19.Lat, aX19.Lon)
+	if dB > DefaultIntersectionTolM {
+		t.Errorf("hold is %.1fm from B∩19 (want ≤%.1f); A∩19 dist=%.1fm point=%v",
+			dB, DefaultIntersectionTolM, dA, h.Point)
+	}
+	if dA < dB {
+		t.Errorf("hold closer to A∩19 (%.1fm) than B∩19 (%.1fm)", dA, dB)
+	}
+}
+
 func TestPlanTaxi_ValidationErrors(t *testing.T) {
 	g := NewGraph(loadKBTV(t))
 
@@ -107,6 +140,8 @@ func TestPlanTaxi_ValidationErrors(t *testing.T) {
 		{"pair no intersect", "A", []string{"A", "D"}, nil, "do not intersect"},
 		{"duplicate consecutive", "A", []string{"A", "A", "B"}, nil, "occurs twice in a row"},
 		{"off surface empty current ok if steps ok", "", []string{"A", "B"}, nil, ""},
+		// Issue 5: hold of 19 is not on C–K–J–33 path (even though C∩19 exists).
+		{"off-route hold", "C", []string{"C", "K", "J", "33"}, []string{"19"}, "not on the taxi route"},
 	}
 	for _, tc := range tests {
 		t.Run(tc.name, func(t *testing.T) {
@@ -125,11 +160,6 @@ func TestPlanTaxi_ValidationErrors(t *testing.T) {
 }
 
 func manySteps(n int) []string {
-	// Alternate A/B would fail consecutive check; use unique invalid for count test —
-	// count is checked before surface lookup... actually count is first, then normalize.
-	// For max steps we only need len > 100; unknown steps come after.
-	// Build 101 "A" would fail consecutive after count check order:
-	// count is checked first — good.
 	out := make([]string, n)
 	for i := range out {
 		out[i] = "A"
@@ -180,16 +210,30 @@ func TestPlanTaxi_HoldShortNamedAndRunway(t *testing.T) {
 	if len(plan.HoldAt) != 2 {
 		t.Fatalf("HoldAt = %+v", plan.HoldAt)
 	}
-	// BHP uses its single point.
+	// BHP must land on path.
 	if plan.HoldAt[0].Name != "BHP" {
 		t.Errorf("hold0 = %q", plan.HoldAt[0].Name)
 	}
-	if plan.HoldAt[0].Point.Lat != 10.001 {
+	if plan.HoldAt[0].WaypointIndex < 0 {
+		t.Errorf("BHP WaypointIndex = %d", plan.HoldAt[0].WaypointIndex)
+	}
+	if math.Abs(plan.HoldAt[0].Point.Lat-10.001) > 1e-9 {
 		t.Errorf("BHP point = %v", plan.HoldAt[0].Point)
 	}
+	// Runway 9 hold at B∩9 (entry onto runway), on path.
 	if plan.HoldAt[1].Name != "9" {
 		t.Errorf("hold1 = %q", plan.HoldAt[1].Name)
 	}
+	if plan.HoldAt[1].WaypointIndex < 0 || plan.HoldAt[1].WaypointIndex >= len(plan.Waypoints) {
+		t.Fatalf("rwy WaypointIndex = %d", plan.HoldAt[1].WaypointIndex)
+	}
+	bX9, ok := intersectionOnSurface(g, "B", "9")
+	if !ok {
+		t.Fatal("B∩9 missing")
+	}
+	if geo.Distance(plan.HoldAt[1].Point.Lat, plan.HoldAt[1].Point.Lon, bX9.Lat, bX9.Lon) > DefaultIntersectionTolM {
+		t.Errorf("rwy hold %v far from B∩9 %v", plan.HoldAt[1].Point, bX9)
+	}
 
 	// HOLD cannot be a taxi step.
 	_, errMsg = g.PlanTaxi("A", []string{"BHP"}, nil)
@@ -208,6 +252,9 @@ func TestPlanTaxi_HoldShortNamedAndRunway(t *testing.T) {
 	if len(plan.Waypoints) == 0 {
 		t.Error("expected waypoints to parking")
 	}
+	if plan.Waypoints[len(plan.Waypoints)-1] != g.Surface("G1").Points[0] {
+		t.Error("final waypoint must be parking")
+	}
 }
 
 func TestPlanTaxi_NilGraph(t *testing.T) {
@@ -228,24 +275,114 @@ func TestPlanTaxi_EmptyHoldToken(t *testing.T) {
 
 func TestPlanTaxi_PathWalksIntermediate(t *testing.T) {
 	g := NewGraph(loadKBTV(t))
-	// A has many points; A→E should walk along A between shared points.
-	plan, errMsg := g.PlanTaxi("A", []string{"A", "E"}, nil)
+	aSurf := g.Surface("A")
+	if aSurf == nil {
+		t.Fatal("taxiway A missing")
+	}
+
+	// FindIntersection(nameA,nameB) → (pt on A, idxA, idxB, ok).
+	// C∩A: index on A is idxB; A∩B: index on A is idxA.
+	_, _, iEntry, ok := g.FindIntersection("C", "A")
+	if !ok {
+		t.Fatal("C∩A")
+	}
+	_, iExit, _, ok := g.FindIntersection("A", "B")
+	if !ok {
+		t.Fatal("A∩B")
+	}
+	lo, hi := iEntry, iExit
+	if lo > hi {
+		lo, hi = hi, lo
+	}
+	wantSpan := hi - lo + 1
+	if wantSpan < 2 {
+		t.Fatalf("unexpected A index span %d..%d", lo, hi)
+	}
+
+	// Current on C: taxi A B — walk A from C∩A to A∩B inclusive of intermediate A vertices.
+	plan, errMsg := g.PlanTaxi("C", []string{"A", "B"}, nil)
 	if errMsg != "" {
 		t.Fatal(errMsg)
 	}
-	// Intersection A∩E near north end; from start of A (if current=A, from unset)
-	// path should at least include the intersection point.
-	if len(plan.Waypoints) < 1 {
-		t.Fatal("no waypoints")
+	for i := lo; i <= hi; i++ {
+		found := false
+		for _, wp := range plan.Waypoints {
+			if wp == aSurf.Points[i] {
+				found = true
+				break
+			}
+		}
+		if !found {
+			t.Errorf("missing A[%d]=%v in path %v", i, aSurf.Points[i], plan.Waypoints)
+		}
 	}
-	// Current off A: from C onto A then to B
-	plan, errMsg = g.PlanTaxi("C", []string{"A", "B"}, nil)
+	if len(plan.Waypoints) < wantSpan {
+		t.Errorf("path len %d < A span %d", len(plan.Waypoints), wantSpan)
+	}
+}
+
+// Issue 2: dense polyline vertices (~10 m spacing) must not collapse under 100 ft snap.
+func TestPlanTaxi_DensePolylineRetainsVertices(t *testing.T) {
+	// Build a N–S taxiway with 20 points ~10 m apart, then turn onto short east taxiway.
+	const n = 20
+	pts := make([]Point, n)
+	// ~10 m in latitude ≈ 10/111320 degrees.
+	dLat := 10.0 / 111320.0
+	for i := 0; i < n; i++ {
+		pts[i] = Point{Lat: 40.0 + float64(i)*dLat, Lon: -75.0}
+	}
+	// End of A shares last point with start of B.
+	bPts := []Point{
+		pts[n-1],
+		{Lat: pts[n-1].Lat, Lon: -75.0 + dLat}, // ~10 m east at mid-lat (approx)
+	}
+	apt := Airport{
+		Surfaces: []Surface{
+			{Kind: SurfaceTaxiway, Name: "A", Points: pts},
+			{Kind: SurfaceTaxiway, Name: "B", Points: bPts},
+		},
+	}
+	g := NewGraph(&apt)
+
+	// Verify consecutive spacing is well under 100 ft.
+	d := geo.Distance(pts[0].Lat, pts[0].Lon, pts[1].Lat, pts[1].Lon)
+	if d > 15 || d < 5 {
+		t.Fatalf("setup spacing = %.1fm, want ~10m", d)
+	}
+
+	plan, errMsg := g.PlanTaxi("A", []string{"A", "B"}, nil)
 	if errMsg != "" {
 		t.Fatal(errMsg)
 	}
-	// Should include C∩A and A∩B and intermediates on A between them.
-	if len(plan.Waypoints) < 2 {
-		t.Fatalf("want path along A, got %d pts: %v", len(plan.Waypoints), plan.Waypoints)
+	// When already on A, path starts at A∩B (last point) only for the A leg
+	// (from unset, to = last). That yields a single point on A then B.
+	// Use current off A so we walk full A from start: place a parking as current
+	// that intersects first point of A.
+	apt2 := apt
+	apt2.Surfaces = append(apt2.Surfaces, Surface{
+		Kind: SurfaceParking, Name: "P1",
+		Points: []Point{pts[0]},
+	})
+	g2 := NewGraph(&apt2)
+	// Taxi from P1 (intersects A[0]) along A to B — first step A must ∩ current P1.
+	plan, errMsg = g2.PlanTaxi("P1", []string{"A", "B"}, nil)
+	if errMsg != "" {
+		t.Fatal(errMsg)
+	}
+	// Full walk of A from index 0 to n-1 inclusive = n vertices, then B may add one more.
+	// With 100 ft stitch dedup this would collapse to ~5; with 1 m stitch we keep all n.
+	countOnA := 0
+	for _, wp := range plan.Waypoints {
+		for _, ap := range pts {
+			if wp == ap {
+				countOnA++
+				break
+			}
+		}
+	}
+	if countOnA < n {
+		t.Fatalf("retained %d/%d A vertices (100ft collapse bug if ~5); path=%v",
+			countOnA, n, plan.Waypoints)
 	}
 }
 
@@ -261,28 +398,39 @@ func TestWalkPolyline_Directions(t *testing.T) {
 	s := &Surface{Points: []Point{
 		{0, 0}, {1, 0}, {2, 0}, {3, 0},
 	}}
-	fwd := walkPolyline(s, Point{0, 0}, Point{2, 0}, 30)
+	fwd := walkPolyline(s, Point{0, 0}, Point{2, 0})
 	if len(fwd) != 3 {
 		t.Fatalf("fwd len = %d", len(fwd))
 	}
-	rev := walkPolyline(s, Point{3, 0}, Point{1, 0}, 30)
+	rev := walkPolyline(s, Point{3, 0}, Point{1, 0})
 	if len(rev) != 3 || rev[0].Lat != 3 || rev[2].Lat != 1 {
 		t.Fatalf("rev = %v", rev)
 	}
-	same := walkPolyline(s, Point{1, 0}, Point{1, 0}, 30)
+	same := walkPolyline(s, Point{1, 0}, Point{1, 0})
 	if len(same) != 1 {
 		t.Fatalf("same = %v", same)
 	}
-	if walkPolyline(nil, Point{}, Point{}, 30) != nil {
+	if walkPolyline(nil, Point{}, Point{}) != nil {
 		t.Error("nil surface")
 	}
 }
 
-func TestAppendUniquePoints(t *testing.T) {
-	var dst []Point
-	dst = appendUniquePoints(dst, []Point{{1, 2}, {1, 2}, {3, 4}}, 30.48)
+func TestAppendUniquePoints_StitchEpsNotSnap(t *testing.T) {
+	// Points ~10 m apart must all be kept with pathStitchEpsM.
+	dLat := 10.0 / 111320.0
+	src := []Point{
+		{40.0, -75.0},
+		{40.0 + dLat, -75.0},
+		{40.0 + 2*dLat, -75.0},
+	}
+	dst := appendUniquePoints(nil, src, pathStitchEpsM)
+	if len(dst) != 3 {
+		t.Fatalf("stitch eps kept %d, want 3 (would be 1 under 100ft snap)", len(dst))
+	}
+	// Exact duplicate still collapses.
+	dst = appendUniquePoints(nil, []Point{{1, 2}, {1, 2}, {3, 4}}, pathStitchEpsM)
 	if len(dst) != 2 {
-		t.Fatalf("dst = %v", dst)
+		t.Fatalf("exact dup: %v", dst)
 	}
 }
 
@@ -295,11 +443,14 @@ func TestPlanTaxi_CaseInsensitive(t *testing.T) {
 	if plan.Steps[0] != "B" {
 		t.Errorf("steps[0]=%q", plan.Steps[0])
 	}
+	if plan.HoldAt[0].WaypointIndex < 0 {
+		t.Error("hold index")
+	}
 }
 
 func TestPlanTaxi_MaxHoldsBoundary(t *testing.T) {
 	g := NewGraph(loadKBTV(t))
-	// 10 holds of A is ok (count only); 11 fails.
+	// 10 holds of B is ok (B is on A→B→19); 11 fails count before path.
 	holds := make([]string, 10)
 	for i := range holds {
 		holds[i] = "B"
@@ -314,3 +465,91 @@ func TestPlanTaxi_MaxHoldsBoundary(t *testing.T) {
 		t.Errorf("11 holds: %q", errMsg)
 	}
 }
+
+func TestPlanTaxi_HOLDOffRouteRejected(t *testing.T) {
+	apt := Airport{
+		Surfaces: []Surface{
+			{Kind: SurfaceTaxiway, Name: "A", Points: []Point{{10, 20}, {10.001, 20}}},
+			{Kind: SurfaceTaxiway, Name: "B", Points: []Point{{10.001, 20}, {10.002, 20}}},
+			// HOLD far from path.
+			{Kind: SurfaceHold, Name: "FAR", Points: []Point{{11, 21}}},
+		},
+	}
+	g := NewGraph(&apt)
+	_, errMsg := g.PlanTaxi("A", []string{"A", "B"}, []string{"FAR"})
+	if !strings.Contains(errMsg, "not on the taxi route") {
+		t.Errorf("got %q", errMsg)
+	}
+}
+
+func TestPlanTaxi_HoldOnCurrentDestinationSurface(t *testing.T) {
+	// Already on runway 19, taxi via B (off then... 19→B): hold short of 19 uses
+	// firstPathPointOnSurface when hold is the first step and current.
+	g := NewGraph(loadKBTV(t))
+	// Route must leave 19 onto a connected surface then... actually hold of 19
+	// while steps start with 19: PlanTaxi("19", []string{"19", "B"}, []string{"19"})
+	// sameSurface first step and current — not Already there (multi-step).
+	plan, errMsg := g.PlanTaxi("19", []string{"19", "B"}, []string{"19"})
+	if errMsg != "" {
+		t.Fatalf("%s", errMsg)
+	}
+	if len(plan.HoldAt) != 1 || plan.HoldAt[0].WaypointIndex < 0 {
+		t.Fatalf("HoldAt=%+v", plan.HoldAt)
+	}
+}
+
+func TestInsertWaypoint(t *testing.T) {
+	wps, idx := insertWaypoint(nil, Point{1, 2}, pathStitchEpsM)
+	if len(wps) != 1 || idx != 0 {
+		t.Fatalf("empty: %v idx=%d", wps, idx)
+	}
+	// Already present within eps
+	wps2, idx2 := insertWaypoint(wps, Point{1, 2}, pathStitchEpsM)
+	if len(wps2) != 1 || idx2 != 0 {
+		t.Fatalf("dup: %v idx=%d", wps2, idx2)
+	}
+	// Insert before closer of two distant points (~5 m from first).
+	dLat := 5.0 / 111320.0
+	base := []Point{{40.0, -75.0}, {40.1, -75.0}}
+	wps3, idx3 := insertWaypoint(base, Point{40.0 + dLat, -75.0}, pathStitchEpsM)
+	if idx3 != 0 || len(wps3) != 3 {
+		t.Fatalf("insert: %v idx=%d", wps3, idx3)
+	}
+}
+
+func TestHoldNearRouteSurface_CurrentOnly(t *testing.T) {
+	apt := Airport{Surfaces: []Surface{
+		{Kind: SurfaceTaxiway, Name: "A", Points: []Point{{10, 20}, {10.001, 20}}},
+		{Kind: SurfaceHold, Name: "H1", Points: []Point{{10, 20}}},
+	}}
+	g := NewGraph(&apt)
+	if !g.holdNearRouteSurface(Point{10, 20}, nil, "A") {
+		t.Error("should be near current A")
+	}
+	if g.holdNearRouteSurface(Point{0, 0}, []string{"A"}, "") {
+		t.Error("far point")
+	}
+}
+
+func TestIntersectionOnSurface_PrefersB(t *testing.T) {
+	g := NewGraph(loadKBTV(t))
+	p, ok := intersectionOnSurface(g, "A", "B")
+	if !ok {
+		t.Fatal("A∩B")
+	}
+	// Point should be on B (exact shared vertex on both).
+	b := g.Surface("B")
+	found := false
+	for _, q := range b.Points {
+		if q == p {
+			found = true
+			break
+		}
+	}
+	if !found {
+		// Within snap of a B point is also fine.
+		if g.PointIndexWithin("B", p) < 0 {
+			t.Errorf("point %v not on B", p)
+		}
+	}
+}