Racket code to load GPX files

gpx.rkt

#lang racket
(require xml
         racket/date
         (only-in srfi/19 string->date)
         map-widget)

(provide gpx-load
         gpx-lookup-position
         gpx-lookup-elevation
         gpx-total-distance)

;; Read the GPX XML document from the input port IN.  While reading the XML
;; contents, white space is collapsed and comments are skipped.
(define (read-gpx in)
  (parameterize ((collapse-whitespace #t)
                 (read-comments #f))
    (read-xml/document in)))

;; Convenience function to check if E is an XML document by NAME
(define (e-name? e name)
  (and (element? e) (eq? (element-name e) name)))

(define (get-track gpx)
  (for/first ([e (element-content gpx)] #:when (e-name? e 'trk))
    e))

(define (get-first-track-seg track)
  (for/first ([e (element-content track)] #:when (e-name? e 'trkseg))
    e))

(define (count-track-points track)
  (for/sum ([e (element-content track)] #:when (e-name? e 'trkpt))
    1))

(define (parse-track-point trkpt)
  (let ((lat #f)
        (lon #f)
        (timestamp #f)
        (elevation #f))
    (for ([a (element-attributes trkpt)])
      (case (attribute-name a)
        ((lat) (set! lat (string->number (attribute-value a))))
        ((lon) (set! lon (string->number (attribute-value a))))))
    (for ([e (element-content trkpt)] #:when (element? e))
      (let ((data (pcdata-string
                   (for/first ([e (element-content e)] #:when (pcdata? e)) e))))
        (case (element-name e)
          ((time) (set! timestamp (date->seconds (string->date data "~Y-~m-~dT~H:~M:~SZ"))))
          ((ele) (set! elevation (string->number data))))))
    (list timestamp lat lon elevation)))

;; Read track points from the GPX document specified as an input port.
;; Returns a vector of track points, each track point is a (vector lat lon
;; distance elevation timestamp)
;;
;; LIMITATIONS:
;;
;;  * only the first track segment is read from the GPX file
;;  * way-points are not read
;;
;; HINT: to read the GPX from a file name use:
;;
;;     (call-with-input-file FILE-NAME gpx-read-trackpoints)
;;
(define (gpx-read-trackpoints in)
  (define gpx
    (let* ((xml (read-gpx in))
           (e (document-element xml)))
      (if (eq? (element-name e) 'gpx)
          e
          (error "not a gpx file"))))
  (define track (get-track gpx))
  (unless track (error "could not find track"))
  (define track-segment (get-first-track-seg track))
  (unless track-segment (error "could not find track segment"))
  (define track-points (make-vector (count-track-points track-segment)))
  (define dst 0)
  (define last-lat #f)
  (define last-lon #f)
  (define index 0)
  (for ([e (element-content track-segment)] #:when (e-name? e 'trkpt))
    (match-define (list pt-timestamp pt-lat pt-lon pt-ele) (parse-track-point e))
    (when (and last-lat last-lon pt-lat pt-lon)
      (set! dst (+ dst (map-distance/degrees last-lat last-lon pt-lat pt-lon))))
    (vector-set! track-points index (vector pt-lat pt-lon dst pt-ele pt-timestamp))
    (set! last-lat pt-lat)
    (set! last-lon pt-lon)
    (set! index (add1 index)))
  
  track-points)

(define (gpx-load input)
  (cond ((path-string? input)
         (call-with-input-file input gpx-read-trackpoints))
        ((port? input)
         (gpx-read-trackpoints))
        (#t
         (error "gpx-load: unknown input type"))))


(define (binary-search vec val
                       #:cmp (cmp-fn <=)
                       #:key (key-fn #f)
                       #:start (start 0)
                       #:end (end (vector-length vec)))

  (define (do-search start end)
    (if (= start end)
        start
        ;; Other
        (let* ((mid (exact-truncate (/ (+ start end) 2)))
               (mid-item (vector-ref vec mid))
               (mid-val (if key-fn (key-fn mid-item) mid-item)))
          (if (cmp-fn val mid-val)
              (do-search start mid)
              (if (cmp-fn mid-val val)
                  (do-search (+ mid 1) end)
                  mid)))))

  (do-search start end))

(define (gpx-lookup-position track-points dst)

  (define index (binary-search
                 track-points dst
                 #:key (lambda (p) (vector-ref p 2))))
  (cond ((<= index 0)
         (match-define (vector lat lon dst ele ts) (vector-ref track-points 0))
         (vector lat lon))
        ((>= index (vector-length track-points))
         (match-define (vector lat lon dst ele ts)
           (vector-ref track-points (sub1 (vector-length track-points))))
         (vector lat lon))
        (#t
         (match-define (vector lat1 lon1 dst1 ele1 ts1)
           (vector-ref track-points (sub1 index)))
         (match-define (vector lat2 lon2 dst2 ele2 ts2)
           (vector-ref track-points index))
         (define f (/ (- dst dst1) (- dst2 dst1)))
         (vector
          (+ (* f lat1) (* (- 1 f) lat2))
          (+ (* f lon1) (* (- 1 f) lon2))))))

(define (gpx-lookup-elevation track-points dst)
  (define index (binary-search
                 track-points dst
                 #:key (lambda (p) (vector-ref p 2))))
  (cond ((<= index 0)
         (match-define (vector lat lon dst ele ts) (vector-ref track-points 0))
         ele)
        ((>= index (vector-length track-points))
         (match-define (vector lat lon dst ele ts)
           (vector-ref track-points (sub1 (vector-length track-points))))
         ele)
        (#t
         (match-define (vector lat1 lon1 dst1 ele1 ts1)
           (vector-ref track-points (sub1 index)))
         (match-define (vector lat2 lon2 dst2 ele2 ts2)
           (vector-ref track-points index))
         (define f (/ (- dst dst1) (- dst2 dst1)))
         (+ (* f ele1) (* (- 1 f) ele2)))))

(define (gpx-total-distance track-points)
  (let ((p (vector-ref track-points (sub1 (vector-length track-points)))))
    (vector-ref p 2)))