#include sys.fth

\ ==========================================================================

: SendEmit                      \ char -- ; Send char to console and RS232.
  dup emit
  semit
;

: SendType                      \ $string count -- ; Send to console and RS232.
  bounds ?do
    i c@ SendEmit
  loop
;

: SendStr                       \ $string -- ; Send string to map system.
  pad copy count SendType
;

\ -------------------------------------------------------------------------

509869 constant StartLat        \ Romsey: 50.98695 degrees North.
 14884 constant StartLon        \ Romsey:  1.4884  degrees West.

\ TSE 539959 constant StartLat TSE \
\ TSE  29935 constant StartLon TSE \

  2000 constant Interval
    25 constant IncRate
    10 constant StepsToTake

\ -------------------------------------------------------------------------

: SendDate
    " 090600" SendStr
;

: SendSpeed
  " 000.0" SendStr              \ Ground speed.
;

variable CurBear

: SendHead
  CurBear @ <# # # # #> SendType
  " .0" SendStr
;

: SendMag
  " 000.0,W" SendStr            \ Magnetic variation.

;

\ -------------------------------------------------------------------------

: SendCRLF
  13 SendEmit
  10 SendEmit
;

\ -------------------------------------------------------------------------

86400 constant s/day

: :00 # 6 base ! # decimal ;
: time# <# :00 :00 #S #> ;

: SendTime
  time s/day mod time# SendType
;

\ -------------------------------------------------------------------------

: SendDegMin                    \ IsLatitude? ScaledDegrees -- ;
  dup
  10000 /
  rot if                        \ Is this a latitude figure?
    <# # # #> SendType
  else
    <# # # # #> SendType
  endif
  10000 mod
  6 * 10 /
  dup
  100 / <# # # #> SendType
  " ." SendStr
  100 mod <# # # #> SendType
;

\ -------------------------------------------------------------------------

variable CurLat
variable CurLon

: SendMC

    " $GPRMC," SendStr
    SendTime " ," SendStr
    " A," SendStr
    True  CurLat @ SendDegMin " ,N," SendStr
    False CurLon @ SendDegMin " ,W," SendStr
    SendSpeed " ," SendStr
    SendHead " ," SendStr
    SendDate " ," SendStr
    SendMag

    SendCRLF
;

\ -------------------------------------------------------------------------

: SendSA
    " $GPRSA," SendStr          \ Magic.
    " A," SendStr               \ Fix type.
    " 3" SendStr                \ Number of satellites.
    SendCRLF
;

\ ==========================================================================

variable LastIncLat
variable LastIncLon
variable LastBearing

: SetInc                        \ Counter -- NewIncLat NewIncLon Bearing ;

  StepsToTake mod 0=
  if
    3 RandBits >r

\ TSE     r> drop   0 >r              \ Force Northwards travel. TSE \

    r@ 0 = if IncRate        0                0 endif
    r@ 1 = if IncRate        IncRate negate  45 endif
    r@ 2 = if 0              IncRate negate  90 endif
    r@ 3 = if IncRate negate IncRate negate 135 endif
    r@ 4 = if IncRate negate 0              180 endif
    r@ 5 = if IncRate negate IncRate        225 endif
    r@ 6 = if 0              IncRate        270 endif
    r@ 7 = if IncRate        IncRate        315 endif

    rot dup LastIncLat !
    rot dup LastIncLon !
    rot dup LastBearing !

    r> drop

  else

    LastIncLat @
    LastIncLon @
    LastBearing @

  endif
;

\ ==========================================================================

variable Fixed

variable IncCount

: run

  time ShiftRegister !

  0 Fixed !

  StartLat CurLat !
  StartLon CurLon !
  0        IncCount !

  begin
    Fixed @ 0=
    if
      5 Fixed !
      SendSA
    endif

    Fixed @ 1- Fixed !
    SendMC

    Interval tpause

    IncCount @
    SetInc

    CurBear !
    CurLon @ + CurLon !
    CurLat @ + CurLat !

    IncCount @ 1+ IncCount !

    key? not
  while
    pause
  repeat
;
run