;
; GPS Speedometer/Odometer program
;

; Parse GPRMC sentences into a list

(defun echo (fun)
  (with-serial (str 1 96)
    (loop
     (let ((line (read-line str)))
       (when (and (> (length line) 7) (string= (subseq line 0 7) "$GPRMC,"))
         (parse line fun))))))

(defun parse (line fun)
  (let ((start 7)
        (end (length line))
        i result)
    (loop
     (setq i start)
     ; Find comma
     (loop
      (when (or (= i end) (eq (char line i) #\,)) (return))
      (incf i))
     ; Extract parameter
     (push (if (= start i) nil (subseq line start i)) result)
     (setq start (1+ i))
     (when (= i end) (return)))
    ; Call function on result
    (funcall fun (reverse result))))

; Time display on MAX7219 eight digit 7-segment display

(defvar en 2)

(defun cmd (a d)
  (with-spi (str en)
    (write-byte a str)
    (write-byte d str)))

; Initialise display; brightness = 0 to 15
(defun on (bri)
  (cmd #xF 0)    ; Test mode off
  (cmd #x9 #xFF) ; Decode segments
  (cmd #xB 7)    ; 8 digits
  (cmd #xC 1)    ; Enable display
  (cmd #xA bri))

(defun clr ()
  (dotimes (d 8) (cmd (1+ d) #xF)))

(defun show (text)
  (let ((len (length text))
        (d 1) (b 0))
    (dotimes (i len)
      (let ((c (char-code (char text (- len i 1)))))
        (cond
         ((= (char-code #\.) c) 
          (incf b #x80))
         (t
          (cond
           ((<= (char-code #\0) c (char-code #\9))
            (incf b (- c (char-code #\0))))
           ((= (char-code #\-) c)
            (incf b #xA))
           (t (incf b #xF)))
          (cmd d b) (incf d) (setq b 0)))))))

(defvar *dist* 0)

(defun speed-dist (lst)
  (let ((knots (nth 6 lst))
        (dp 3))
    (when knots
      (let* ((len (length knots))
             (k (read-from-string (subseq knots 0 (- len dp 1))))
             (mk (read-from-string (subseq knots (- len dp))))
             (k1000 (+ (* k 1000) mk))
             (mph (truncate (+ (* k1000 38) 19) 33000))
             (m10 (truncate (* *dist* 19) 5943800))
             (miles (truncate m10 10))
             (tenths (mod m10 10))
             (smph (princ-to-string mph))
             (smiles (princ-to-string miles)))
        (when (>= k1000 1000) (incf *dist* k1000))
        (show
         (concatenate
          'string
          (align 3 smph)
          smph
          (align 4 smiles)
          smiles
          "."
          (princ-to-string tenths)))))))

(defun align (width str)
  (subseq "   " 0 (- width (length str))))
          
(defun go2 () (on 15) (show "  -   --") (echo speed-dist))