REBOL [
  Title: "Garmin GPS protocol handler"
  Purpose: "Garmin eTrex Legend protocol handler"
  Date: 2004-12-12
  Version: 0.4.2
  Author: "Piotr Gapinski"
  Email: news@rowery.olsztyn.pl
  Copyright: "Olsztynska Strona Rowerowa http://www.rowery.olsztyn.pl"
  File: %garmin-protocol.r
  Url: "http://www.rowery.olsztyn.pl/wspolpraca/rebol/gps/"
  License: "GNU Lesser General Public License (Version 2.1)"
  Library: [
    level: 'advanced
    platform: 'all
    type: [protocol tool]
    domain: [protocol]
    tested-under: [
      view 1.2.8 on [Linux WinXP]
    ]
    support: none
    license: 'LGPL
  ]
]

; system/ports/serial: [ttyS0 ttyS1]

garmin: context [
  ; link L1 protocol
  Pid_Etx_Byte: 03
  Pid_Ack_Byte: 06
  Pid_Command_Data: 10
  Pid_Xfer_Cmplt: 12
  Pid_Date_Time_Data: 14
  Pid_Dle_Byte: 16
  Pid_Position_Data: 17
  Pid_Prx_Wpt_Data: 19
  Pid_Nack_Byte: 21
  Pid_Records: 27
  Pid_Rte_Hdr: 29
  Pid_Rte_Wpt_Data: 30
  Pid_Almanac_Data: 31
  Pid_Trk_Data: 34
  Pid_Wpt_Data: 35
  Pid_Pvt_Data: 51
  Pid_Rte_Link_Data: 98
  Pid_Trk_Hdr: 99
  Pid_Protocol_Array: 253
  Pid_Product_Rqst: 254
  Pid_Product_Data: 255

  ; application A10 protocol
  Cmnd_Abort_Transfer: 0   ; abort current transfer
  Cmnd_Transfer_Alm: 1     ; transfer almanac
  Cmnd_Transfer_Posn: 2    ; transfer position
  Cmnd_Transfer_Prx: 3     ; transfer proximity waypoints
  Cmnd_Transfer_Rte: 4     ; transfer routes
  Cmnd_Transfer_Time: 5    ; transfer time
  Cmnd_Transfer_Trk: 6     ; transfer track log
  Cmnd_Transfer_Wpt: 7     ; transfer waypoints
  Cmnd_Turn_Off_Pwr: 8     ; turn off power
  Cmnd_Transfer_Display: 32; transfer gps display
  Cmnd_Start_Pvt_Data: 49  ; start transmitting PVT data
  Cmnd_Stop_Pvt_Data: 50   ; stop transmitting PVT data


  packet-size: func [bin [binary!]] [bin/3]
  packet-cmd: func [bin [binary!]] [bin/2]
  packet-data: func [bin [binary!]] [copy/part (skip bin 3) (packet-size bin)]
  get-long: func [bin [binary!]] [to-integer head reverse copy/part bin 4]

  encode-packet: func [
   "Zamienia block! danych na pakiet garmin gps"
    cmd [block!] "blok danych (PID CMD DATA)"
   /local simplify buffer csum][

    ; splaszcza zagniezdzone bloki
    simplify: func [dat [block!] /local buffer][buffer: copy [] foreach x dat [repend buffer x]]

    buffer: simplify reduce [cmd/1 ((length? cmd) - 1) (next cmd)]
    csum: 0   ; oblicz sume kontrolna (zanegowana dwojkowo) oryginalnych danych
    foreach byte buffer [csum: csum + (to-integer byte)]
    csum: to-binary reduce [256 - csum]

    ; bajty PID_DLE_BYTE musza zostac zdublowane! dotyczy to pol SIZE, DATA, CHKSUM (trzeba pominac pole CMD)
    replace/all next buffer Pid_Dle_Byte reduce [Pid_Dle_Byte Pid_Dle_Byte]
    to-binary simplify reduce [Pid_Dle_Byte buffer (to-integer csum) Pid_Dle_Byte Pid_Etx_Byte]
  ]

  sequence: func [[catch] buf [block!] /locl cmd dat][
    ; rozkazy obslugiwane przez sterownik
    dat: [
        'get 'waypoints (cmd: encode-packet reduce [Pid_Command_Data Cmnd_Transfer_Wpt 0])
      | 'get 'datetime (cmd: encode-packet reduce [Pid_Command_Data Cmnd_Transfer_Time 0])
      | 'get ['tracklogs | 'tracklog] (cmd: encode-packet reduce [Pid_Command_Data Cmnd_Transfer_Trk 0])
      | 'get ['routes | 'route] (cmd: encode-packet reduce [Pid_Command_Data Cmnd_Transfer_Rte 0])
      | 'get 'display (cmd: encode-packet reduce [Pid_Command_Data Cmnd_Transfer_Display 0])
      | 'get 'product 'info (cmd: encode-packet reduce [Pid_Product_Rqst Pid_Product_Rqst 0])
      | 'abort (cmd: encode-packet reduce [Pid_Command_Data Cmnd_Abort_Transfer 0])
      | 'power-off (cmd: encode-packet reduce [Pid_Command_Data Cmnd_Turn_Off_Pwr 0])
      | 'check (cmd: encode-packet reduce [28 0 0])
    ]
    either parse/all buf dat [cmd] [none]
  ]

  from-ieee: func [[catch]
   "Konwertuje binary! float ieee-32 na decimal!"
    dat [binary!] "liczba w formacie ieee-32"
   /local ieee] [

   ieee: make struct! [f [float]] none
   change third ieee dat
   return ieee/f
  ]

  from-semicircle: func [
   "Konwertuje liczbe czalkowita w formacie semicircle na decimal!"
    bin [binary!] "4 bajty"][

    (to-integer head reverse bin) * (180 / (power 2 31))
  ]

  from-timestamp: func [
   "Konwertuje liczbe sekund od 1989-12-31/00:00:00 do rebol date!"
    bin [binary!] "4 bajty"
   /local reference days dt][

    if (to-integer bin) <= 0 [return now]
    reference: 31-12-1989/00:00
    days: (to-integer bin) / 86400 ; 86400 = 24 * 3600 = sekund w ciagu dnia

    dt: reference + (to-integer days) ; czesc calkowita to liczba dni
    dt: dt + to-time to-integer ((days - (to-integer days)) * 86400) ; czesc ulamkowa to czas
  ]

  make-wpt: func [
   "Tworzy block! dla waypointa [nazwa lat lon alt]"
    bin [binary!] "pakiet D108"
   /local dat][

    ; dane zgodnie z protokołoem D108
    ; wspolrzedne geograficzne sš zakodowane jako semicircle (po 4 bajty na wspolrzedna)
    dat: packet-data bin
    reduce [
      ;; dodac ID typu waypointa
      (to-string trim copy/part (skip dat 48) 51) ; name 26
      (to-decimal from-semicircle copy/part (skip dat 24) 4) ; latitude
      (to-decimal from-semicircle copy/part (skip dat 28) 4) ; longitude
      (to-decimal from-ieee copy/part (skip dat 32) 4) ; altitude
    ]
  ]

  make-rte: func [
   "Tworzy block! dla trasy [rte [wpt lat lon alt] [wpr lat lon alt] ...]"
    bin [binary!] "pakiet D202 (hdr), D108 (data), D210 (link)"
   /local dat cmd][

    ; dane zgodnie z protokolem D202 (header), D108 (waypoint) oraz D210 (rte-link)
    cmd: packet-cmd bin
    dat: packet-data bin
    return switch cmd reduce [
      Pid_Rte_Hdr [ reduce [trim to-string copy/part (skip dat 0) 51] ] ;  (name)
      Pid_Rte_Wpt_Data [ make-wpt bin ] ; (D108 waypoint)
      ; Pid_Rte_Link_Data [ "??" ] ; ??
    ]
  ]

  make-trk: func [
   "Tworzy block! dla tracklogu [trk [wpt lat lon alt d-time] [wpt lat lon alt d-time] ...]"
    bin [binary!] "pakiet D301 (hdr), D310 (data)"
   /local dat][

    ; dane zgodnie z protokolem D310 (track) oraz D301 (header)
    cmd: packet-cmd bin
    dat: packet-data bin
    either (cmd = Pid_Trk_Hdr) [
      reduce [ (trim to-string copy/part (skip dat 2) 51) ] ; trk_ident
    ]
    [
      reduce [
        (to-decimal from-semicircle copy/part dat 4) ; latitude
        (to-decimal from-semicircle copy/part skip dat 4 4) ; longitude
        (to-decimal from-ieee copy/part (skip dat 12) 4) ; altitude
        (to-date from-timestamp head reverse copy/part (skip dat 8) 4) ; datetime GMT
      ]
    ]
  ]
  
  make-display: func [
   "Tworzy image! z binarnych danych zakodowanych"
    dat [binary!] "dane binarne"
    img-size [pair!] "wielkosc obrazka po zdekodowaniu"
    bpp [integer!]  "ile bitow na piksel"
   /local x rc chunks part img to-pixel cnt ind][
    
    bpp: any [bpp 2] ; ile bitow na piksel (dla wyswietlaczy b/w bedzie od 2 do 4)
    rc: copy []

    ; podziel odebrane dane na piksele (kazdy bajt na 8 / bpp pikseli)
    foreach x dat [
      chunks: split-byte x bpp
      foreach part chunks [append rc (255.255.255 / (part + 1))] ; zamien piksele na informacje o kolorze
    ]
    
    img: make image! img-size
    to-pixel: func [p [pair!]] [(p/y * img/size/x) + p/x + 1]
    cnt: 1
    ind: 0x0
  
    ; nanies kazdy piksel na przygotowany obrazek; uwzglednij szerokosc obrazka
    foreach x rc [
      either zero? (cnt // img/size/y) [
        ind: as-pair (ind/x + 1) 0
      ][
        poke img (to-pixel ind) x
        ind: ind + 0x1
      ]
      cnt: cnt + 1
    ]
    to-image layout [origin 0x0 image img effect [rotate 180 flip]]
  ]

  split-byte: func [
   "Dzieli bajt na bpp bitowe czesci"
    byte [binary! integer!] "bajt danych"
    bpp [integer!] "ile bitow w czesci"
   /local x tmp rc bit-test bit-set bit-clear][
  
    bit-test: func [byte [integer!] bit][either (byte and (to-integer power 2 bit)) <> 0 [true][false]]
    bit-set: func [byte [integer!] bit][(byte or (to-integer power 2 bit))]
    bit-clear: func [byte [integer!] bit][(byte and complement (to-integer power 2 bit))]
      
    if (binary? byte) [byte: to-integer byte]
    tmp: 0
    rc: copy []

    repeat x 8 [
      if (bit-test byte (x - 1)) [tmp: bit-set tmp ((x - 1) // bpp)]
      if zero? (x // bpp) [append rc tmp tmp: 0]
    ]    
    rc
  ]
]


make Root-Protocol [
  scheme: 'garmin
  port-id: 8882
  port-flags: system/standard/port-flags/pass-thru
  awake: none
  device: 'port1
  port: none
  lcmd: none


  init: func [[catch] port spec /local tmp] [
    if not parse/all spec [
      "garmin://" [
         copy tmp ["port" ["1" | "2" | "3" | "4"]] (port/device: to-word tmp)
         | end   (port/device: 'port1)
       ]
    ][
       throw make error! [user message "Bad Garmin-URL"]
    ]
    port/locals: make object! [item: 0 packets: 0 lcmd: none ldat: none]
  ]

  open: func [[catch] port] [
    throw-on-error [
      port/sub-port: system/words/open/binary/direct compose [
        scheme: 'serial
        device: (to-lit-word port/device)
        speed: 9600
        data-bits: 8
        parity: 'none
        stop-bits: 1
        rts-cts: no
        timeout: 2 ; 2 sekundy
      ]
      port/state/flags: port/state/flags or port-flags
      self/port: port
    ]
  ]

  insert: func [[catch] port buf [block! binary!] /local e cmd] [
    either block? buf [
      if error? set/any 'e try [cmd: garmin/sequence buf][throw e]
      if none? cmd [throw make error! [user message "Bad Garmin Protocol Command"]]
    ] [ cmd: buf ]
    send-packet self/lcmd: cmd
    none
  ]

  copy: func [[catch] port [port!] /part count [integer!]
   /local bin trk wpt packets rc dat time a b x gps protocol tmp][

    rc: system/words/copy []
    bin: recv-packet ; ack
    gps: self/port
    if not equal? garmin/Pid_Ack_Byte (garmin/packet-cmd bin) [throw none]

    net-utils/net-log ["LAST CMD" self/lcmd]
    net-utils/net-log ["LAST PACKET" first garmin/packet-data self/lcmd]

    switch/default (first garmin/packet-data self/lcmd) reduce [

      garmin/Cmnd_Transfer_Wpt [
        dat: garmin/packet-data bin: recv-packet
        port/locals/packets: packets:  to-integer head reverse system/words/copy/part dat 2 ; dwa bajty
        repeat i packets [
          port/locals/item: i
          send-packet reduce [garmin/Pid_Ack_Byte (garmin/packet-cmd bin) 0]
          bin: recv-packet
          if (gps/awake gps) [repend/only rc garmin/make-wpt bin]
        ]
        send-packet reduce [garmin/Pid_Ack_Byte (garmin/packet-cmd bin) 0]
        bin: recv-packet
        send-packet reduce [garmin/Pid_Ack_Byte (garmin/packet-cmd bin) 0]
        rc
      ]

      garmin/Cmnd_Transfer_Time [
        dat: garmin/packet-data bin: recv-packet
        send-packet reduce [garmin/Pid_Ack_Byte (garmin/packet-cmd bin) 0]
        time: now/zone + to-time reduce [
          to-integer to-binary reduce [dat/6 dat/5]
          to-integer dat/7
          to-integer dat/8
        ]

        to-date reduce [
          to-integer to-binary reduce [dat/4 dat/3] ; year
          to-integer dat/1 ; month
          to-integer dat/2 ; day
          time
        ]
      ]

      garmin/Cmnd_Transfer_Trk [
        dat: garmin/packet-data bin: recv-packet
        port/locals/packets: packets: to-integer head reverse system/words/copy/part dat 2 ; dwa bajty
        repeat i packets [
          port/locals/item: i
          send-packet reduce [garmin/Pid_Ack_Byte (garmin/packet-cmd bin) 0]
          trk: garmin/make-trk bin: recv-packet
          if (gps/awake gps) [repend/only either (length? trk) > 1 [last rc] [rc] trk]
        ]
        send-packet reduce [garmin/Pid_Ack_Byte (garmin/packet-cmd bin) 0]
        bin: recv-packet
        send-packet reduce [garmin/Pid_Ack_Byte (garmin/packet-cmd bin) 0]
        rc
      ]

      garmin/Pid_Product_Rqst [
        dat: garmin/packet-data bin: recv-packet
        send-packet reduce [garmin/Pid_Ack_Byte (garmin/packet-cmd bin) 0]
        
        repend rc [
          (to-integer head reverse system/words/copy/part dat 2) ; id
          (to-integer head reverse system/words/copy/part skip dat 2 2) / 100 ; version
          (to-string skip dat 4) ; product
        ]
        if error? try [dat: garmin/packet-data bin: recv-packet][return rc] ; protocol array
        protocol: dat
        
        send-packet reduce [garmin/Pid_Ack_Byte (garmin/packet-cmd bin) 0]
        comment { zamiana Pid_Protocol_Array na blok danych [ tag id ] }
        tmp: system/words/copy []
        while [not tail? protocol][
          a: to-binary reduce [protocol/3]
          b: to-binary reduce [protocol/2]
          repend tmp [(to-char protocol/1) (to-integer (rejoin [a b]))]
          protocol: skip protocol 3
        ]
        repend/only rc tmp
      ]

      garmin/Cmnd_Transfer_Display [
        dat: garmin/packet-data bin: recv-packet ; display info
        bits-per-pixel: garmin/get-long skip dat 12 ; ile pikseli w jednym bajcie danych
        img-lines: garmin/get-long skip dat 16 ; y
        img-pixels: to-integer garmin/get-long skip dat 20 ; x
        img-line-data: garmin/get-long skip dat 8; ile bajtow danch w pakiecie z GPS
        ; print ["bpp" bits-per-pixel "lines" img-lines "pixels" img-pixels "data" img-line-data]
        
        port/locals/item: 0
        port/locals/packets: packets: (img-pixels * img-lines) / (img-line-data * (8 / bits-per-pixel))
        gps/awake gps

        repeat i packets [
          port/locals/item: i
          send-packet reduce [garmin/Pid_Ack_Byte (garmin/packet-cmd bin) 0]
          if error? try [bin: recv-packet] [throw make error! [user message "Bad Garmin Display Data"]]
          if (gps/awake gps) [repend (either (empty? rc) [rc] [last rc]) skip (garmin/packet-data bin) 8]
        ]
        net-utils/net-log ["DISPLAY PACKETS" port/locals/packets]
        garmin/make-display rc/1 (to-pair reduce [img-pixels img-lines]) bits-per-pixel
      ]
      
      garmin/Cmnd_Transfer_Rte [
        dat: garmin/packet-data bin: recv-packet
        port/locals/packets: packets: to-integer head reverse system/words/copy/part dat 2 ; dwa bajty
        repeat i packets [
          port/locals/item: i
          send-packet reduce [garmin/Pid_Ack_Byte (garmin/packet-cmd bin) 0]
          rte: garmin/make-rte bin: recv-packet
          if not none? rte [if (gps/awake gps) [repend/only either (length? rte) > 1 [last rc] [rc] rte]]
        ]
        send-packet reduce [garmin/Pid_Ack_Byte (garmin/packet-cmd bin) 0]
        bin: recv-packet
        send-packet reduce [garmin/Pid_Ack_Byte (garmin/packet-cmd bin) 0]
        rc
      ]

      0 [true] ; check

    ][rc]
  ]

  send-packet: func [[catch]
   "wysyla pakiet danych do gps (bez obslugi potwierdzeń ACK/NACK)"
    cmd [block! binary!] "blok danych (PID CMD DATA) lub binarny pakiet"
   /local packet][

    either (block? cmd) [packet: garmin/encode-packet cmd] [packet: cmd]
    throw-on-error [system/words/insert port/sub-port packet]
    net-utils/net-log ["send-packet" packet]
    self/port/locals/lcmd: packet
  ]

  recv-packet: func [[catch]
   "zwraca pakiet DANYCH pobranych z GPS"
   /local buffer byte prev][

    buffer: system/words/copy []
    byte: prev: 0
    while [true] [
      byte: to-integer (system/words/copy/part self/port/sub-port 1)
      append buffer byte
      if all [(byte = garmin/Pid_Etx_Byte) (prev = garmin/Pid_Dle_Byte)] [break]
      prev: byte
    ]
    ; usun zdublowane bajty Pid_Dle_Byte
    replace/all buffer reduce [garmin/Pid_Dle_Byte garmin/Pid_Dle_Byte] garmin/Pid_Dle_Byte
    net-utils/net-log ["recv-packet" (to-binary buffer)]
    self/port/locals/ldat: to-binary buffer
  ]

  read: write: none

  net-utils/net-install :scheme self :port-id
  system/schemes/garmin/awake: does [true] ; default awake func
]


comment {  
  Example1: {
    ; GPS product informations (model, software)
    gps: open garmin://port1
    insert gps [check]
    if error? try [copy gps] [print "no GPS - no fun" halt]
    insert gps [get product info]
    print copy gps
    close gps
  }
  Example2: {
    ; GPS waypoints
    gps: open garmin://port1
    insert gps [check]
    if error? try [copy gps] [print "no GPS - no fun" halt]
    gps/awake: func [gps [port!]] [print ["waypoint" 1 + gps/locals/packets - gps/locals/item] return true]
    insert gps [get waypoints]
    wpts: copy gps
    either not empty? wpts [foreach location wpts [print first location]] [print "no waypoints"]
    close gps
  }
  Example3: {
    ; GPS screen shot
    gps: open garmin://port1
    insert gps [check]
    if error? try [copy gps] [print "no GPS - no fun" halt]
    gps/awake: func [gps [port!]] [print ["L" 1 + gps/locals/packets - gps/locals/item] return true]
    insert gps [get display]
    img: copy gps
    view layout [origin 0x0 image img]
    close gps
  }
  Example4: {
    ; GPS current date and time
    gps: open garmin://port1
    insert gps [check]
    if error? try [copy gps] [print "no GPS - no fun" halt]
    insert gps [get datetime]
    print copy gps
    close gps
  }
  Example5: {
    ; GPS routes
    gps: open garmin://port1
    insert gps [check]
    if error? try [copy gps] [print "no GPS - no fun" halt]
    gps/awake: func [gps [port!]] [print ["rte/packet" 1 + gps/locals/packets - gps/locals/item] return true]
    insert gps [get routes]
    rts: copy gps
    either not empty? rts [foreach route rts [print first route]] [print "no routes"]
    close gps
  }

  Changelog: [
    0.4.2 2004-12-12
      nowe
      - mozliwosc pobrania trasy (route, protokol A201/D202/D108/D210); komenda [get route|get routes];
        format zwracanych danych analogiczny jak przy tracklogu
      zmiany
      - tworzenie zrzutu ekranu (make-display) w oparciu o informacje o GPS (liczba linii danych do odczytu,
        bits-per-pixel i wymiary obrazka); mala optymalizacja funkcji przez rezygnacje z head reverse na
        danych (obrocenie obrazka jest teraz robione jako effect przy tworzeniu layout)
    0.4.1 2004-06-13
      nowe
      - mozliwosc pobrania zrzutu ekranu GPS (screen shot); nie obsluguje kolorowych GPS (tylko do 8bpp);
        komendy do pobierania danych [get display]
      zmiany
      - metoda insert wykorzystuje teraz funkcje send-packet
      - metoda insert moze operowac na binarnym pakiecie (nie tylko na bloku danych)
      usuniete usterki
      - przy pobieraniu tracklogu wysokosc byla blednie dekodowana; poprawione
    0.3.0 2004-06-10
      nowe
      - mozliwosc pobrania product-info (protokol A001); komenda [get product info]
      - obsluga awake callback wywolywanego dla kazdego odebranego pakietu z GPS; callback musi zwrocic true
        gdy dany pakie ma byc obsluzony (false gdy pominiety)
      zmiany
      - przy pobieraniu tracklogu (protokol A301/D301/D310); pobierane sa wspolrzedne, czas i wysokosc
    0.2.0 2004-02-02
      nowe
      - mozliwosc pobierania waypointow (protokol A100/D108); komenda [get waypoints]
      - mozliwosc pobierania tracklogu (protokol A301/D301/D310); komenda [get trakclog]
      - mozliwosc pobierania biezacej daty z GPS (protokol A600/D600); komenda [get datetime]
  ]
}