calculate.go
1package waypoint
2
3import (
4 "crypto/sha256"
5 "encoding/hex"
6 "encoding/json"
7 "errors"
8 "fmt"
9 "io"
10 "log/slog"
11 "sync"
12 "time"
13
14 "github.com/paulmach/orb"
15 geolib "github.com/paulmach/orb/geo"
16 "git.kilimanjaro.io/rtw/routing"
17 "git.kilimanjaro.io/rtw/routing/graphhopper"
18)
19
20// NoRouteID is the SHA256 hash of an empty string, used as sentinel for unroutable paths
21var NoRouteID = func() string {
22 hash := sha256.Sum256([]byte(""))
23 return hex.EncodeToString(hash[:])
24}()
25
26// NoRouteSentinel represents a route that could not be calculated (no path exists)
27var NoRouteSentinel = Route{
28 ID: NoRouteID,
29 Distance: 0,
30 Time: 0,
31 Bbox: []float64{0, 0, 0, 0},
32}
33
34// CalculatedRoute holds the result of a single route calculation
35type CalculatedRoute struct {
36 WaypointID string
37 RouteKey string
38 Route Route
39 Points [][]float64 // Route points in [lon, lat] format
40 Elevation []float64 // Elevation data in meters
41 Error error
42 ErrorCodes []string // Error codes to populate on the waypoint (e.g., ["RouteTooFar"])
43 SourceID string // ID of source waypoint
44 SourceOrig []float64 // Original source point [lon, lat]
45 SourceSnap []float64 // Snapped source point [lon, lat]
46 DestOrig []float64 // Original destination point [lon, lat] (the waypoint being updated)
47 DestSnap []float64 // Snapped destination point [lon, lat]
48}
49
50// CalculateMissingRoutes finds all missing routes in the waypoints and fetches them
51// from the Stadia API in parallel. It saves route data to disk and returns the
52// calculated routes for updating waypoints.
53//
54// The storage parameter can be nil if you don't want to persist routes (useful for testing).
55func CalculateMissingRoutes(waypoints []Waypoint, storage *routing.RouteStorage) ([]CalculatedRoute, error) {
56 // Get routing requests for missing routes
57 requests, err := ToRoutingRequest(waypoints...)
58 if err != nil {
59 return nil, fmt.Errorf("failed to create routing requests: %w", err)
60 }
61
62 if len(requests) == 0 {
63 // No routes to calculate
64 return nil, nil
65 }
66
67 slog.Info("calculating missing routes", "count", len(requests))
68
69 // Process requests in parallel
70 results := make([]CalculatedRoute, len(requests))
71 var wg sync.WaitGroup
72
73 for i, req := range requests {
74 wg.Add(1)
75 go func(idx int, r RoutingRequest) {
76 defer wg.Done()
77 results[idx] = executeRoutingRequest(r, storage)
78 }(i, req)
79 }
80
81 wg.Wait()
82
83 // Check for errors and log them
84 var hasErrors bool
85 for _, result := range results {
86 if result.Error != nil {
87 hasErrors = true
88 slog.Error("route calculation failed",
89 "waypoint", result.WaypointID,
90 "routeKey", result.RouteKey,
91 "error", result.Error)
92 }
93 }
94
95 if hasErrors {
96 // Return results even if some failed - caller can decide what to do
97 slog.Warn("some route calculations failed, continuing with successful ones")
98 }
99
100 return results, nil
101}
102
103// executeRoutingRequest sends a single routing request to Graphhopper API with backoff retry
104func executeRoutingRequest(req RoutingRequest, storage *routing.RouteStorage) CalculatedRoute {
105 result := CalculatedRoute{
106 WaypointID: req.WaypointID,
107 RouteKey: req.RouteKey,
108 }
109
110 slog.Debug("executeRoutingRequest: starting",
111 "waypointID", req.WaypointID,
112 "routeKey", req.RouteKey)
113
114 // Read the request body to extract locations
115 bodyBytes, err := io.ReadAll(req.Request.Body)
116 if err != nil {
117 result.Error = fmt.Errorf("failed to read request body: %w", err)
118 slog.Error("executeRoutingRequest: failed to read request body",
119 "waypointID", req.WaypointID,
120 "error", err)
121 return result
122 }
123 req.Request.Body.Close()
124
125 slog.Debug("executeRoutingRequest: request body read",
126 "waypointID", req.WaypointID,
127 "bodySize", len(bodyBytes))
128
129 // Parse the request to get points
130 var requestPayload struct {
131 Points [][]float64 `json:"points"`
132 }
133 if err := json.Unmarshal(bodyBytes, &requestPayload); err != nil {
134 result.Error = fmt.Errorf("failed to parse request body: %w", err)
135 slog.Error("executeRoutingRequest: failed to parse request body",
136 "waypointID", req.WaypointID,
137 "error", err)
138 return result
139 }
140
141 slog.Debug("executeRoutingRequest: request parsed",
142 "waypointID", req.WaypointID,
143 "numPoints", len(requestPayload.Points))
144
145 if len(requestPayload.Points) < 2 {
146 result.Error = fmt.Errorf("request must have at least 2 points, got %d", len(requestPayload.Points))
147 slog.Error("executeRoutingRequest: insufficient points",
148 "waypointID", req.WaypointID,
149 "numPoints", len(requestPayload.Points))
150 return result
151 }
152
153 // Convert points to graphhopper format
154 points := make([]graphhopper.PointLngLat, len(requestPayload.Points))
155 for i, pt := range requestPayload.Points {
156 if len(pt) < 2 {
157 result.Error = fmt.Errorf("point %d has invalid coordinates", i)
158 slog.Error("executeRoutingRequest: invalid point coordinates",
159 "waypointID", req.WaypointID,
160 "pointIndex", i,
161 "pointLen", len(pt))
162 return result
163 }
164 points[i] = graphhopper.PointLngLat{pt[0], pt[1]}
165 }
166
167 slog.Debug("executeRoutingRequest: calling graphhopper API",
168 "waypointID", req.WaypointID,
169 "numPoints", len(points),
170 "firstPoint", points[0],
171 "lastPoint", points[len(points)-1])
172
173 // Execute the routing request with backoff
174 resp, err := graphhopperRouteWithBackoff(points)
175 if err != nil {
176 result.Error = fmt.Errorf("routing API error: %w", err)
177
178 // Check for non-retryable errors (4XX client errors)
179 // All 4XX errors mean the request is invalid and won't succeed on retry
180 if graphhopper.IsNonRetryable(err) {
181 // Map specific error types to error codes
182 switch {
183 case errors.Is(err, &graphhopper.RouteTooFarError{}):
184 result.ErrorCodes = []string{string(RouteTooFarError)}
185 case errors.Is(err, &graphhopper.PointNotFoundError{}):
186 result.ErrorCodes = []string{string(PointNotFoundError)}
187 default:
188 // Generic client error
189 result.ErrorCodes = []string{string(RouteFailedError)}
190 }
191 // Store NoRouteSentinel to prevent infinite retry loops
192 // The route ID will be NoRouteID which marks it as "calculated but no path"
193 result.Route = NoRouteSentinel
194 slog.Info("stored NoRouteSentinel for non-retryable error",
195 "waypointID", req.WaypointID,
196 "routeKey", req.RouteKey,
197 "errorCodes", result.ErrorCodes)
198 }
199
200 slog.Error("executeRoutingRequest: routing API error",
201 "waypointID", req.WaypointID,
202 "error", err)
203 return result
204 }
205
206 slog.Debug("executeRoutingRequest: received response",
207 "waypointID", req.WaypointID,
208 "numPaths", len(resp.Paths))
209
210 // Parse the response
211 route, decodedPoints, elevation, err := parseGraphhopperResponse(resp)
212 if err != nil {
213 result.Error = fmt.Errorf("failed to parse route response: %w", err)
214 slog.Error("executeRoutingRequest: failed to parse response",
215 "waypointID", req.WaypointID,
216 "error", err)
217 return result
218 }
219
220 slog.Debug("executeRoutingRequest: response parsed",
221 "waypointID", req.WaypointID,
222 "routeID", route.ID,
223 "distance", route.Distance,
224 "time", route.Time,
225 "bbox", route.Bbox,
226 "numDecodedPoints", len(decodedPoints),
227 "numElevationPoints", len(elevation))
228
229 result.Route = route
230 result.Points = decodedPoints
231 result.Elevation = elevation
232
233 // Extract source ID from route key
234 sourceID, _, _ := FromRouteKey(req.RouteKey)
235 result.SourceID = sourceID
236
237 // Store original points from request
238 if len(requestPayload.Points) >= 2 {
239 result.SourceOrig = []float64{requestPayload.Points[0][0], requestPayload.Points[0][1]}
240 result.DestOrig = []float64{requestPayload.Points[1][0], requestPayload.Points[1][1]}
241 }
242
243 // Set snapped points from decoded points
244 if result.Route.ID != NoRouteID && len(decodedPoints) >= 2 {
245 result.SourceSnap = decodedPoints[0]
246 result.DestSnap = decodedPoints[len(decodedPoints)-1]
247 }
248
249 // Save to storage if provided (skip for NoRoute sentinel)
250 if storage != nil && route.ID != NoRouteID {
251 slog.Debug("executeRoutingRequest: saving to storage",
252 "waypointID", req.WaypointID,
253 "routeID", route.ID,
254 "numPoints", len(decodedPoints),
255 "bbox", route.Bbox)
256
257 if err := storage.SaveRoute(route.ID, decodedPoints, route.Distance, route.Time, route.Bbox); err != nil {
258 slog.Error("executeRoutingRequest: failed to save route to storage",
259 "waypointID", req.WaypointID,
260 "routeID", route.ID,
261 "error", err)
262 } else {
263 slog.Debug("executeRoutingRequest: route saved successfully",
264 "waypointID", req.WaypointID,
265 "routeID", route.ID)
266 }
267
268 // Also save elevation data
269 if len(elevation) > 0 {
270 if err := storage.SaveElevation(route.ID, elevation); err != nil {
271 slog.Error("executeRoutingRequest: failed to save elevation to storage",
272 "waypointID", req.WaypointID,
273 "routeID", route.ID,
274 "error", err)
275 } else {
276 slog.Debug("executeRoutingRequest: elevation saved successfully",
277 "waypointID", req.WaypointID,
278 "routeID", route.ID,
279 "numElevationPoints", len(elevation))
280 }
281 }
282 } else {
283 slog.Debug("executeRoutingRequest: skipping storage save",
284 "waypointID", req.WaypointID,
285 "hasStorage", storage != nil,
286 "routeID", route.ID,
287 "isNoRoute", route.ID == NoRouteID)
288 }
289
290 slog.Debug("executeRoutingRequest: completed",
291 "waypointID", req.WaypointID,
292 "routeID", route.ID,
293 "hasError", result.Error != nil)
294
295 return result
296}
297
298// graphhopperRouteWithBackoff retries the routing request up to four times.
299// It does NOT retry on non-retryable errors (all 4XX client errors).
300func graphhopperRouteWithBackoff(pts []graphhopper.PointLngLat) (*graphhopper.Response, error) {
301 sleepTimes := []time.Duration{0, 5 * time.Second, 10 * time.Second, 30 * time.Second}
302 var lastErr error
303 for i, sleepTime := range sleepTimes {
304 if i > 0 {
305 time.Sleep(sleepTime)
306 }
307 resp, err := graphhopper.Route(pts)
308 if err == nil {
309 return resp, nil
310 }
311 lastErr = err
312 // Don't retry on non-retryable errors (all 4XX client errors)
313 // 4XX errors indicate a bad request that won't succeed on retry
314 if graphhopper.IsNonRetryable(err) {
315 return nil, fmt.Errorf("routing error: %w", err)
316 }
317 }
318 return nil, fmt.Errorf("routing error: no response in %d tries: %w", len(sleepTimes), lastErr)
319}
320
321// parseGraphhopperResponse extracts route data from Graphhopper API response
322func parseGraphhopperResponse(resp *graphhopper.Response) (Route, [][]float64, []float64, error) {
323 slog.Debug("parseGraphhopperResponse: starting",
324 "numPaths", len(resp.Paths))
325
326 if len(resp.Paths) == 0 {
327 slog.Debug("parseGraphhopperResponse: no paths in response, returning NoRouteSentinel")
328 return NoRouteSentinel, nil, nil, nil
329 }
330
331 // Log first path details
332 if len(resp.Paths) > 0 {
333 firstPath := resp.Paths[0]
334 slog.Debug("parseGraphhopperResponse: first path details",
335 "distance", firstPath.Distance,
336 "time", firstPath.Time,
337 "ascend", firstPath.Ascend,
338 "descend", firstPath.Descend,
339 "pointsEncoded", firstPath.PointsEncoded,
340 "pointsLen", len(firstPath.Points),
341 "bbox", firstPath.Bbox)
342 }
343
344 // Get points and elevation from response
345 points, err := resp.Points()
346 if err != nil {
347 slog.Error("parseGraphhopperResponse: failed to get points from response",
348 "error", err)
349 return Route{}, nil, nil, fmt.Errorf("failed to get points from response: %w", err)
350 }
351
352 slog.Debug("parseGraphhopperResponse: decoded points",
353 "numPoints", len(points),
354 "firstPoint", func() []float64 {
355 if len(points) > 0 {
356 return points[0]
357 }
358 return nil
359 }(),
360 "lastPoint", func() []float64 {
361 if len(points) > 0 {
362 return points[len(points)-1]
363 }
364 return nil
365 }())
366
367 elevation, err := resp.Elevation()
368 if err != nil {
369 slog.Error("parseGraphhopperResponse: failed to get elevation from response",
370 "error", err)
371 return Route{}, nil, nil, fmt.Errorf("failed to get elevation from response: %w", err)
372 }
373
374 slog.Debug("parseGraphhopperResponse: decoded elevation",
375 "numElevationPoints", len(elevation))
376
377 if len(points) == 0 {
378 slog.Warn("parseGraphhopperResponse: no points decoded, returning NoRouteSentinel")
379 return NoRouteSentinel, nil, nil, nil
380 }
381
382 // Compute ID as SHA256 hash of JSON-encoded points
383 routeID := routing.HashPoints(points)
384 slog.Debug("parseGraphhopperResponse: computed route ID",
385 "routeID", routeID)
386
387 // Get stats from response
388 stats := resp.Stats()
389 slog.Debug("parseGraphhopperResponse: response stats",
390 "distanceMeters", stats.Distance,
391 "timeSeconds", stats.Time,
392 "ascend", stats.Ascend,
393 "descend", stats.Descend)
394
395 // Build bounding box from points
396 bbox := calculateBbox(points)
397 slog.Debug("parseGraphhopperResponse: calculated bbox",
398 "bbox", bbox)
399
400 route := Route{
401 ID: routeID,
402 Distance: stats.Distance / 1000.0, // Convert meters to kilometers
403 Time: stats.Time, // Time in seconds (frontend expects seconds)
404 Bbox: bbox,
405 }
406
407 slog.Debug("parseGraphhopperResponse: completed",
408 "routeID", route.ID,
409 "distanceKm", route.Distance,
410 "timeMinutes", route.Time,
411 "bbox", route.Bbox)
412
413 return route, points, elevation, nil
414}
415
416// calculateBbox computes bounding box from points: [min_lon, min_lat, max_lon, max_lat]
417func calculateBbox(points [][]float64) []float64 {
418 slog.Debug("calculateBbox: starting",
419 "numPoints", len(points))
420
421 if len(points) == 0 {
422 slog.Warn("calculateBbox: no points provided, returning zero bbox")
423 return []float64{0, 0, 0, 0}
424 }
425
426 minLon, minLat := points[0][0], points[0][1]
427 maxLon, maxLat := points[0][0], points[0][1]
428
429 slog.Debug("calculateBbox: initialized with first point",
430 "firstPoint", points[0],
431 "initialMinLon", minLon,
432 "initialMinLat", minLat,
433 "initialMaxLon", maxLon,
434 "initialMaxLat", maxLat)
435
436 for i, pt := range points[1:] {
437 if len(pt) < 2 {
438 slog.Warn("calculateBbox: skipping point with insufficient coordinates",
439 "pointIndex", i+1,
440 "pointLen", len(pt))
441 continue
442 }
443 if pt[0] < minLon {
444 minLon = pt[0]
445 }
446 if pt[0] > maxLon {
447 maxLon = pt[0]
448 }
449 if pt[1] < minLat {
450 minLat = pt[1]
451 }
452 if pt[1] > maxLat {
453 maxLat = pt[1]
454 }
455 }
456
457 bbox := []float64{minLon, minLat, maxLon, maxLat}
458 slog.Debug("calculateBbox: completed",
459 "numPointsProcessed", len(points),
460 "bbox", bbox)
461
462 return bbox
463}
464
465// MaxSnapDistance is the maximum distance in meters for waypoint snapping
466const MaxSnapDistance = 1000.0
467
468// shouldSnapPoint determines if a waypoint should be snapped to a route point.
469// Returns true if the distance between original and snapped points is <= 1000m.
470func shouldSnapPoint(original, snapped []float64) bool {
471 if len(original) < 2 || len(snapped) < 2 {
472 return false
473 }
474
475 // geo.Distance expects orb.Point which is [lon, lat]
476 origPoint := orb.Point{original[0], original[1]}
477 snapPoint := orb.Point{snapped[0], snapped[1]}
478
479 distance := geolib.Distance(origPoint, snapPoint)
480 return distance <= MaxSnapDistance
481}
482
483// ToRouteUpdates converts calculated routes to route updates with point snapping applied.
484// It includes error codes for routes with errors and applies the 1000m distance limit for snapping.
485func ToRouteUpdates(routes []CalculatedRoute) []RouteUpdate {
486 updates := make([]RouteUpdate, 0, len(routes))
487
488 for _, r := range routes {
489 update := RouteUpdate{
490 WaypointID: r.WaypointID,
491 RouteKey: r.RouteKey,
492 Route: r.Route,
493 ErrorCodes: r.ErrorCodes,
494 }
495
496 // Only apply point updates for successful routes
497 if r.Error == nil && r.Route.ID != NoRouteID {
498 if len(r.DestSnap) >= 2 && len(r.DestOrig) >= 2 {
499 if shouldSnapPoint(r.DestOrig, r.DestSnap) {
500 update.NewPoint = r.DestSnap
501 }
502 }
503 }
504
505 updates = append(updates, update)
506 }
507
508 return updates
509}
510
511// ToSourcePointUpdates creates point updates for source waypoints.
512// This should be called separately to update source waypoint locations.
513func ToSourcePointUpdates(routes []CalculatedRoute) map[string][]float64 {
514 updates := make(map[string][]float64)
515
516 for _, r := range routes {
517 if r.Error != nil || r.Route.ID == NoRouteID {
518 continue
519 }
520
521 // Check if source point should be snapped
522 if len(r.SourceSnap) >= 2 && len(r.SourceOrig) >= 2 {
523 if shouldSnapPoint(r.SourceOrig, r.SourceSnap) {
524 updates[r.SourceID] = r.SourceSnap
525 }
526 }
527 }
528
529 return updates
530}
