<*+M2EXTENSIONS *>
<*-CHECKDIV *>
<*-CHECKRANGE *>
<*-COVERFLOW *>
<*-IOVERFLOW*>
<*-PROCINLINE*>
<*+NOPTRALIAS*>
<*-CHECKNIL *>
<*-CHECKINDEX*>
<*-CHECKDINDEX *>
<*-CHECKSET*>
<*CPU="PENTIUM"*>
<* IF __GEN_C__ THEN *>
  <*+COMMENT*>
  <*-GENCTYPES*>
  <*-PROCINLINE*>
  <*-GENDEBUG*>
  <*-LINENO*>
  <*-GENHISTORY*>
  <*-GENDATE*>
  <*+NOHEADER*>
  <*+GENCDIV*>
  <*-GENKRC*>
  <*+NOOPTIMIZE*>
  <*-GENSIZE*>
  <*-ASSERT*>
<* ELSE *>
  <*-PROCINLINE*>
  <*-GENDEBUG*>
  <*-LINENO*>
  <*-GENHISTORY*>
<* END *>

MODULE gps2digipos; (* read serial gps and make "3704.04N/00805.14W" string to file *)

FROM SYSTEM IMPORT ADR, CARD8, CARD16, SHIFT, CAST;
FROM osi IMPORT File, WrLn, WrStr, WrStrLn, WrFixed, OpenRW, OpenWrite, WrBin, RdBin,
                Close, WrInt, WrHex, NextArg, usleep;
FROM mlib IMPORT termios, tcgetattr, tcsetattr, ioctl,
                 CS8, CLOCAL, CREAD, TCSAFLUSH,
                 B1200, B2400, B4800, B9600, B19200, B38400,
                 B57600, B115200, B230400, B460800,
                 TIOCMBIC, TIOCMBIS, TIOCM_DTR, TIOCM_RTS;
FROM aprsstr IMPORT IPNUM, Append, Length, IntToStr, Assign;

TYPE SET16=SET OF [0..15];
     SET8 =SET OF [0..7];

CONST CR=15C;
      LF=12C;
      FEET=1.0/0.3048;
      KNOTS=1.851984;
      PI=3.14159265358979323844;
VAR 
    tbuf, ttynamee, basefilename:ARRAY[0..1023] OF CHAR;
--    symt, symb:CHAR;
    baud:CARDINAL;
    tty:INTEGER;
    verb, usbrobust, junk, sumoff, withalti:BOOLEAN;
    comptyp, micessid, btime0, comintval, comcnt, msgtyp, mediantime, medians:CARDINAL;
    saved: termios;
    speed, course, lat, long, alt:LONGREAL;
    altok, posok:BOOLEAN;
    median:ARRAY[0..499] OF RECORD
      mlat, mlong, malt: LONGREAL;
      ok:BOOLEAN;
    END;
 
PROCEDURE Error(text:ARRAY OF CHAR);
BEGIN
  WrStr("agps2digipos: "); WrStr(text); WrStrLn(" error abort");
  HALT
END Error;

PROCEDURE truncc(r:LONGREAL):CARDINAL;
BEGIN
  IF r<=0.0 THEN RETURN 0 ELSIF r>=MAX(INTEGER) THEN RETURN MAX(INTEGER) ELSE RETURN TRUNC(r) END;
END truncc;

PROCEDURE SetStatLine(fd:INTEGER; rts, dtr:BOOLEAN):BOOLEAN;
VAR arg : ARRAY[0..255] OF CARDINAL;
    r:INTEGER;
BEGIN
  arg[0]:=TIOCM_RTS;
  IF rts THEN r:=ioctl(fd, TIOCMBIS, ADR(arg)) ELSE r:=ioctl(fd, TIOCMBIC, ADR(arg)) END;
  IF r>=0 THEN
    arg[0]:=TIOCM_DTR;
    IF dtr THEN r:=ioctl(fd, TIOCMBIS, ADR(arg)) ELSE r:=ioctl(fd, TIOCMBIC, ADR(arg)) END;
  END;
  RETURN r>=0
END SetStatLine;


PROCEDURE SetComMode(fd:INTEGER; baud:CARDINAL);
VAR term : termios;
    res  : INTEGER;
    bd   : CARDINAL;
BEGIN
  IF baud=1200 THEN bd:=B1200;
  ELSIF baud=2400 THEN bd:=B2400;
  ELSIF baud=4800 THEN bd:=B4800;
  ELSIF baud=9600 THEN bd:=B9600;
  ELSIF baud=19200 THEN bd:=B19200;
  ELSIF baud=38400 THEN bd:=B38400;
  ELSIF baud=57600 THEN bd:=B57600;
  ELSIF baud=115200 THEN bd:=B115200;
  ELSIF baud=230400 THEN bd:=B230400;
  ELSIF baud=460800 THEN bd:=B460800;
  ELSE Error("unknown baudrate") END;

  res:=tcgetattr(fd, saved);
  res:=tcgetattr (fd, term);
  WITH term DO
    c_lflag :=0;
    c_oflag :=0;
    c_iflag :=0;
(*  cfmakeraw(&termios);*)
    c_cflag :=VAL(CARDINAL, CS8+CLOCAL+CREAD(*+CRTSCTS*))+bd;  (*0800018B2H*)
  END;
  res:=tcsetattr (fd, TCSAFLUSH, term);
END SetComMode;


PROCEDURE opentty;
BEGIN
  LOOP
    tty:=OpenRW(ttynamee);
    IF tty>=0 THEN
      SetComMode(tty, baud);
      EXIT
    ELSIF NOT usbrobust THEN Error("tty open") END;
    usleep(1000000);
  END;
  IF SetStatLine(tty, FALSE, FALSE) THEN END;
END opentty;

PROCEDURE GetNum(h-:ARRAY OF CHAR; eot:CHAR; VAR p,n:CARDINAL):BOOLEAN;
BEGIN
  n:=0;
  WHILE (h[p]>="0") & (h[p]<="9") DO
    n:=n*10+ORD(h[p])-ORD("0");
    INC(p);
  END;
  RETURN h[p]=eot
END GetNum;


PROCEDURE Parms;
VAR err:BOOLEAN;
    h:ARRAY[0..1023] OF CHAR;
    VAR i:CARDINAL;
BEGIN
  err:=FALSE;
  LOOP
    NextArg(h);
    IF h[0]=0C THEN EXIT END;
    IF (h[0]="-") & (h[1]<>0C) & (h[2]=0C) THEN

      IF h[1]="u" THEN usbrobust:=FALSE

      ELSIF h[1]="s" THEN sumoff:=TRUE;

      ELSIF h[1]="a" THEN withalti:=TRUE;
(*
      ELSIF h[1]="i" THEN 
        NextArg(h);
        IF h[0]>" " THEN
          symt:=h[0];
          IF h[1]>" " THEN symb:=h[1] ELSE symb:=0C END;
        ELSE Error("-i <icon> (house /-)") END;
*)
      ELSIF h[1]="f" THEN
        NextArg(basefilename);
        IF basefilename[0]=0C THEN Error("-f filename") END;

      ELSIF h[1]="t" THEN
        NextArg(h);
        i:=0;
        WHILE (h[i]<>0C) & (h[i]<>":") & (i<HIGH(ttynamee)) DO ttynamee[i]:=h[i]; INC(i) END;
        ttynamee[i]:=0C;
        IF h[i]<>0C THEN 
          INC(i);
          IF NOT GetNum(h, 0C, i, baud) THEN Error("need ttydevice:baud") END;
        END; 

     ELSIF h[1]="m" THEN
        NextArg(h);
        i:=0;
        IF NOT GetNum(h, 0C, i, mediantime)
        THEN Error("-m <seconds>") END;
        IF mediantime>HIGH(median) THEN
        IntToStr(HIGH(median), 1, h); Append(h," maximum -m"); Error(h) END;

     ELSIF h[1]="f" THEN
        NextArg(h);
        i:=0;
        IF NOT GetNum(h, 0C, i, comptyp) OR (comptyp>2)
        THEN Error("-f <format> 0=uncomp, 1=comp, 2=mic-e") END;

      ELSIF h[1]="v" THEN verb:=TRUE;

      ELSIF h[1]="h" THEN
        WrLn;
        WrStrLn("Read serial GPS and make position string to insert into APRS-beacon");
        WrStrLn(" -a                                altitude on");
        WrStrLn(" -f <file>                         writes <file.lat> <file.long> and <file.alt>");
        WrStrLn(" -h                                this");
--        WrStrLn(' -i <icon>                         2 Icon chars "/-" (House), "/>" (Car)...');
        WrStrLn(' -m <seconds>                      time to read gps to make median position');
        WrStrLn(" -s                                GPS Checksum check OFF");
        WrStrLn(" -t <tty>:<baud>                   default /dev/ttyS0:9600"); 
        WrStrLn(" -u                                not retry until open removable USB tty");
        WrStrLn(" -v                                verbous");
        WrStrLn(' example:  -t /dev/ttyS0:9600 -u -f test -a -m 30 -v');
        WrLn;
        HALT

      ELSE err:=TRUE END;
(*
      h[0]:=0C;
*)
    ELSE err:=TRUE END;
    IF err THEN EXIT END;
  END;
  IF err THEN WrStr(">"); WrStr(h); WrStrLn("< use -h"); HALT END;
END Parms;


PROCEDURE skip(b-:ARRAY OF CHAR; VAR p:CARDINAL; len:CARDINAL);
BEGIN
  WHILE (p<len) & (b[p]<>",") DO INC(p) END;
  IF p<len THEN INC(p) END;
END skip;

PROCEDURE getnum(b-:ARRAY OF CHAR; VAR p:CARDINAL; len:CARDINAL; VAR n:CARDINAL):BOOLEAN;
BEGIN
  IF (p<len) & (b[p]>="0") & (b[p]<="9") THEN 
    n:=ORD(b[p]) - ORD("0");
    INC(p);
    RETURN TRUE
  END;
  n:=0;
  RETURN FALSE;
END getnum;


PROCEDURE decodeline(b-:ARRAY OF CHAR; len:CARDINAL);
VAR i, n :CARDINAL;
    div  :LONGREAL;
    sign :BOOLEAN;    
BEGIN 
  IF (b[0]="$") & (b[1]="G") (* & (b[2]="P")*) THEN
    IF (b[3]="R") & (b[4]="M") & (b[5]="C") THEN
(* $GPRMC,141333.593,A,8915.1000,N,01300.2000,E,0.00,00.00,140410,0,,A*7C *)
(* $GPRMC,112430.00,A,8912.41130,N,01300.61995,E,0.039,,200513,,,A*77 *)
      i:=7;
      skip(b, i, len);
      IF b[i]<>"A" THEN RETURN END;
      skip(b, i, len);
      IF getnum(b, i, len, n) THEN lat:=FLOAT(n*10) ELSE RETURN END;
      IF getnum(b, i, len, n) THEN lat:=lat + FLOAT(n) ELSE RETURN END;
      IF getnum(b, i, len, n) THEN lat:=lat + FLOAT(n)/6.0 ELSE RETURN END;
      IF getnum(b, i, len, n) THEN lat:=lat + FLOAT(n)/60.0 ELSE RETURN END;
      IF b[i]="." THEN INC(i) ELSE RETURN END;
      IF getnum(b, i, len, n) THEN lat:=lat + FLOAT(n)/600.0 ELSE RETURN END;
      IF getnum(b, i, len, n) THEN lat:=lat + FLOAT(n)/6000.0 ELSE RETURN END;
      IF getnum(b, i, len, n) THEN lat:=lat + FLOAT(n)/60000.0 END;
      IF getnum(b, i, len, n) THEN lat:=lat + FLOAT(n)/600000.0 END;
      skip(b, i, len);
      IF b[i]="S" THEN lat:=-lat ELSIF b[i]<>"N" THEN RETURN END;
      skip(b, i, len); 

      IF getnum(b, i, len, n) THEN long:=FLOAT(n*100) ELSE RETURN END;
      IF getnum(b, i, len, n) THEN long:=long + FLOAT(n*10) ELSE RETURN END;
      IF getnum(b, i, len, n) THEN long:=long + FLOAT(n) ELSE RETURN END;
      IF getnum(b, i, len, n) THEN long:=long + FLOAT(n)/6.0 ELSE RETURN END;
      IF getnum(b, i, len, n) THEN long:=long + FLOAT(n)/60.0 ELSE RETURN END;
      IF b[i]="." THEN INC(i) ELSE RETURN END;
      IF getnum(b, i, len, n) THEN long:=long + FLOAT(n)/600.0 ELSE RETURN END;
      IF getnum(b, i, len, n) THEN long:=long + FLOAT(n)/6000.0 ELSE RETURN END;
      IF getnum(b, i, len, n) THEN long:=long + FLOAT(n)/60000.0 END;
      IF getnum(b, i, len, n) THEN long:=long + FLOAT(n)/600000.0 END;
      skip(b, i, len);
      IF b[i]="W" THEN long:=-long ELSIF b[i]<>"E" THEN RETURN END;
      skip(b, i, len);

      speed:=0.0;
      WHILE getnum(b, i, len, n) DO speed:=speed*10.0 + FLOAT(n) END;
      IF b[i]="." THEN
        INC(i);
        div:=0.1;
        WHILE getnum(b, i, len, n) DO speed:=speed + FLOAT(n)*div; div:=div*0.1 END;
      END;
      speed:=speed*KNOTS;                                      (* knots to km/h *) 
      skip(b, i, len);

      course:=0.0;
      WHILE getnum(b, i, len, n) DO course:=course*10.0 + FLOAT(n) END;
      IF b[i]="." THEN
        INC(i);
        div:=0.1;
        WHILE getnum(b, i, len, n) DO course:=course + FLOAT(n)*div; div:=div*0.1 END;
      END;
      posok:=TRUE;

    ELSIF (b[3]="G") & (b[4]="G") & (b[5]="A") THEN
(* $GPGGA,152554,3938.5665,N,10346.2039,W,1,08,1.7,12382.7,M,-22.3,M,,*7B *)
(* $GPGGA,112435.00,4812.41112,N,01305.61998,E,1,08,1.04,398.3,M,44.9,M,,*59 *)
      i:=7; 
      skip(b, i, len);
      skip(b, i, len);
      skip(b, i, len);
      skip(b, i, len);
      skip(b, i, len);
      skip(b, i, len);
      skip(b, i, len);
      skip(b, i, len);

      alt:=0.0;
      IF b[i]="-" THEN sign:=TRUE; INC(i) ELSE sign:=FALSE END; 
      WHILE getnum(b, i, len, n) DO altok:=TRUE; alt:=alt*10.0 + FLOAT(n) END;
      IF b[i]="." THEN
        INC(i);
        div:=0.1;
        WHILE getnum(b, i, len, n) DO alt:=alt + FLOAT(n)*div; div:=div*0.1 END;
      END;
      IF sign THEN alt:=-alt END;
    END; 
  END;
END decodeline;


PROCEDURE checksum(b-:ARRAY OF CHAR; len:CARDINAL):BOOLEAN;

  PROCEDURE Hex(d:CARDINAL):CHAR;
  BEGIN
    d:=d MOD 16;
    IF d>9 THEN INC(d, ORD("A")-10-ORD("0")) END;
    RETURN CHR(d+ORD("0"))
  END Hex;

TYPE SET8=SET OF [0..7];

VAR i:CARDINAL;
    cs:SET8;
    ok:BOOLEAN;
BEGIN
  ok:=TRUE;
  i:=1;
  cs:=SET8{};
  WHILE (i<len) & (b[i]<>"*") DO
    cs:=cs/CAST(SET8, b[i]);
    INC(i);
  END;
  IF i+2>=len THEN ok:=FALSE END;

  IF ok THEN
    IF (b[i+1]<>Hex(CAST(CARDINAL, cs) DIV 16))
    OR (b[i+2]<>Hex(CAST(CARDINAL, cs) MOD 16)) THEN ok:=FALSE END;
  END;
  IF verb & NOT ok THEN WrStrLn("GPS Checksum Error") END;
  RETURN ok
END checksum;


PROCEDURE showline(b-:ARRAY OF CHAR; len:CARDINAL);
VAR i:CARDINAL;
BEGIN
  i:=0;
  WHILE i<len DO
    IF b[i]<" " THEN
      IF b[i]=LF THEN WrLn ELSE WrStr(".") END;
    ELSE WrStr(b[i]) END;
    INC(i);
  END;
  WrLn;
END showline;


PROCEDURE wrfile(b:ARRAY OF CHAR; len:CARDINAL; ext:ARRAY OF CHAR);
VAR s:ARRAY[0..2000] OF CHAR;
    f:File;
BEGIN
  Assign(s, basefilename); Append(s, ext);
  f:=OpenWrite(s);
  IF f>=0 THEN
    IF len>0 THEN WrBin(f, b, len) END;
    Close(f);
  END;
END wrfile;

PROCEDURE wrpos(lat, long, alt:LONGREAL; withalt:BOOLEAN);

  PROCEDURE num(n:CARDINAL):CHAR;
  BEGIN RETURN CHR(n MOD 10 + ORD("0")) END num;

VAR b:ARRAY[0..200] OF CHAR;
    rp:INTEGER; 
    i,n, nl:CARDINAL;
    a:LONGREAL;
BEGIN
(* "4805.44N" "01333.64E" "/A=000000"*)
  i:=0;
  a:=ABS(lat);
  n:=truncc(a);
  b[i]:=num(n DIV 10); INC(i);
  b[i]:=num(n); INC(i);
  n:=truncc((a-FLOAT(n))*6000.0);
  b[i]:=num(n DIV 1000); INC(i);
  b[i]:=num(n DIV 100); INC(i);
  b[i]:="."; INC(i);
  b[i]:=num(n DIV 10); INC(i);
  b[i]:=num(n); INC(i);
  IF lat>=0.0 THEN b[i]:="N" ELSE b[i]:="S" END; INC(i);
--  b[i]:=symt; INC(i);
  wrfile(b, i, ".lat");

  i:=0;
  a:=ABS(long); 
  n:=truncc(a);
  b[i]:=num(n DIV 100); INC(i);
  b[i]:=num(n DIV 10); INC(i);
  b[i]:=num(n); INC(i);
  n:=truncc((a-FLOAT(n))*6000.0);
  b[i]:=num(n DIV 1000); INC(i);
  b[i]:=num(n DIV 100); INC(i);
  b[i]:="."; INC(i);
  b[i]:=num(n DIV 10); INC(i);
  b[i]:=num(n); INC(i);
  IF lat>=0.0 THEN b[i]:="E" ELSE b[i]:="W" END; INC(i);
--  IF symb>" " THEN b[i]:=symb; INC(i) END;

  wrfile(b, i, ".long");

  IF withalt THEN
    i:=0;
    IF alt>0.0 THEN
      b[i]:="/"; INC(i);
      b[i]:="A"; INC(i);
      b[i]:="="; INC(i);
      n:=truncc(ABS(alt*FEET+0.5));
      IF alt>=0.0 THEN b[i]:=num(n DIV 100000) ELSE b[i]:="-" END; INC(i);
      b[i]:=num(n DIV 10000); INC(i);
      b[i]:=num(n DIV 1000); INC(i);
      b[i]:=num(n DIV 100); INC(i);
      b[i]:=num(n DIV 10); INC(i);
      b[i]:=num(n); INC(i);
    END;
    wrfile(b, i, ".alt");
  END;
END wrpos;


PROCEDURE medianpos(lat, long, alt:LONGREAL);
BEGIN
  IF medians<=HIGH(median) THEN
    WITH median[medians] DO
      mlat:=lat;
      mlong:=long;
      malt:=alt;
    END;
    INC(medians);
  END;
END medianpos;

PROCEDURE getmedian(VAR lat, long, alt:LONGREAL);
VAR i, j, ax, bx, ay, by:CARDINAL;
    minx, maxx, miny, maxy:LONGREAL;
BEGIN
  IF medians=0 THEN Error("no positions got") END;

  j:=medians DIV 10;
  FOR i:=0 TO medians-1 DO median[i].ok:=TRUE END;
  LOOP                                                   (* kill extrem values *)
    minx:=MAX(REAL);
    maxx:=MIN(REAL);
    ax:=0;
    bx:=0;
    miny:=MAX(REAL);
    maxy:=MIN(REAL);
    ay:=0;
    by:=0;
    j:=0;
    FOR i:=0 TO medians-1 DO
      IF median[i].ok THEN
        WITH median[i] DO
          IF mlat<minx THEN minx:=mlat; ax:=i END;
          IF mlat>maxx THEN maxx:=mlat; bx:=i END;
          IF mlong<miny THEN miny:=mlong; ay:=i END;
          IF mlong>maxy THEN maxy:=mlong; by:=i END;
        END;
        INC(j);
      END;
    END;         
    IF j<=medians DIV 2 +5 THEN EXIT END;               (* let values to make mean *)

    median[ax].ok:=FALSE;
    median[bx].ok:=FALSE;
    median[ay].ok:=FALSE;
    median[by].ok:=FALSE;
  END;

  j:=0;
  lat:=0.0;
  long:=0.0;
  alt:=0.0;
  IF verb THEN WrStrLn("   lat           long                alt  ok") END;
  FOR i:=0 TO medians-1 DO                              (* make mean valus *)
    IF median[i].ok THEN
      WITH median[i] DO
        lat:=lat + mlat;
        long:=long + mlong;
        alt:=alt + malt;
        INC(j); 
      END;
    END;

    IF verb THEN 
      WITH median[i] DO
        WrFixed(mlat, 8, 14);
        WrFixed(mlong, 8, 14);
        WrFixed(malt, 1, 14);
        WrInt(ORD(ok), 2); WrStrLn(""); 
      END;
    END;

  END; 
  IF j=0 THEN Error("no positions got") END;

  lat :=lat /LFLOAT(j);
  long:=long/LFLOAT(j);
  alt :=alt /LFLOAT(j);
END getmedian;


VAR c:CHAR;
    len, rp:INTEGER;
    gpsp:CARDINAL;
    gpsb:ARRAY[0..99] OF CHAR;

BEGIN
  sumoff:=FALSE;
  junk:=TRUE;
  posok:=FALSE;
  altok:=FALSE;
  verb:=FALSE;
  usbrobust:=TRUE;
  medians:=0;
  mediantime:=30;
  baud:=9600;
  ttynamee:="/dev/ttyS0";
  basefilename:="gpspos";
  gpsp:=0;
--  symt:="/";
--  symb:="U";
  comptyp:=0;  
  withalti:=FALSE;
  comintval:=5;
  comcnt:=0;
  Parms;
  opentty;
  LOOP
    len:=RdBin(tty, tbuf, SIZE(tbuf));
    IF len<=0 THEN
      Close(tty);
      usleep(1000000);
      opentty;
      junk:=TRUE;
    END;
        
    IF NOT junk THEN 
      FOR rp:=0 TO len-1 DO 
        c:=tbuf[rp]; 
        IF (c=CR) OR (c=LF) THEN 
          IF gpsp>0 THEN          
            IF sumoff OR checksum(gpsb, gpsp) THEN decodeline(gpsb, gpsp);
            ELSE altok:=FALSE; posok:=FALSE; END;
            IF verb THEN showline(gpsb, gpsp) END;
            IF posok & (altok OR NOT withalti) THEN 
              medianpos(lat, long, alt);
              IF mediantime>0 THEN
                DEC(mediantime);
                IF verb THEN WrInt(mediantime, 4); WrStrLn(" mediantime") END;
              ELSE
                getmedian(lat, long, alt);
                wrpos(lat, long, alt, withalti & altok);
                EXIT 

              END;
              altok:=FALSE; posok:=FALSE;
            END;
          END;
          gpsp:=0; 
        ELSIF (c>" ") & (gpsp<HIGH(gpsb)) THEN 
          gpsb[gpsp]:=c;
          INC(gpsp);     
        END;
      END;
    END;
    junk:=FALSE;

  END;


END gps2digipos.
(*
tty
filename
symbol tab
*)