<*+M2EXTENSIONS *>
<*-CHECKDIV *>
<*-CHECKRANGE *>
<*-GENFRAME*>
<*-COVERFLOW *>
<*-IOVERFLOW*>
<*-NOPTRALIAS*>
<*-DOREORDER*>
<*-PROCINLINE*>
<*-GENPTRINIT*>
<*+STORAGE *>
<* IF __GEN_C__ THEN *>
  <*GENWIDTH="120"*>
  <*+STORAGE *>
  <*-GENCTYPES*>
  <*+COMMENT*>
  <*-GENHISTORY*>
  <*-GENDEBUG*>
  <*-GENDATE*>
  <*-LINENO*>
  <*-CHECKINDEX*>
  <*-CHECKDINDEX*>
  <*+GENCDIV*>
  <*-GENKRC*>
  <*+NOOPTIMIZE*>
  <*-GENSIZE*>
  <*-CHECKNIL*>
  <*-COVERFLOW*>
  <*-IOVERFLOW*>
  <*-CHECKRANGE*>
  <*-CHECKSET*>
  <*-CHECKDIV*>
  <*-GENCONSTENUM*>
  <*-GENTYPEDEF*>
  <*-XCOMMENTS*>
  <*-ASSERT*>
<* ELSE *>
  <*HEAPLIMIT="100000000"*>
  <*+GENHISTORY*>
  <*+GENDEBUG*>
  <*+LINENO*>
  <*+CHECKINDEX*>
  <*-CHECKDINDEX*>
  <*+NOOPTIMIZE*>
  <*-ASSERT*>
  <*+CHECKNIL*>
  <*-COVERFLOW*>
  <*-IOVERFLOW*>
  <*-CHECKRANGE*>
  <*+CHECKSET*>
  <*+CHECKDIV*>
<* END *>

<*NEW WITHALM*>
<*-WITHALM*>
<*NEW WITHSOUND*>
<*+WITHSOUND*>

MODULE gpssdr;

(*
\brief   GNSS core 'c' function library: geodesy related functions.
\author  Glenn D. MacGougan (GDM)
\date    2007-11-28
\since   2005-08-14

\b "LICENSE INFORMATION" \n
Copyright (c) 2007, refer to 'author' doxygen tags \n
All rights reserved. \n

Redistribution and use in source and binary forms, with or without
modification, are permitted provided the following conditions are met: \n

- Redistributions of source code must retain the above copyright
 notice, this list of conditions and the following disclaimer. \n
- Redistributions in binary form must reproduce the above copyright
 notice, this list of conditions and the following disclaimer in the
 documentation and/or other materials provided with the distribution. \n
- The name(s) of the contributor(s) may not be used to endorse or promote
 products derived from this software without specific prior written
 permission. \n

THIS SOFTWARE IS PROVIDED BY THE CONTRIBUTORS `AS IS'' AND ANY EXPRESS
OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.
*)

FROM SYSTEM IMPORT FILL, MOVE, ADR, CAST, CARD8, CARD16, INT16, INT8, SHIFT, BYTE;

FROM osi IMPORT Werr, WrStr, WrStrLn, WrInt, File, OpenRead, Close,
     WrFixed, ALLOCATE, NextArg, RdBin, IsFifo, ln, sin, cos, sqrt,
     OpenWrite, WrBin, OpenNONBLOCK, OpenAppend, WrCard, Seek;

IMPORT math;

FROM aprsstr IMPORT StrToCard, StrToInt, StrToFix, Append, IntToStr, 
                    FixToStr, DateToStr, Length;
CONST
      PI=3.1415926535897932384626433832795;
      LIGHT=299792458.0;
      KNOTS=1.852;                             (* nautic miles *)
      FREQ=1575420000.0;
      EARTH=6378137.0;
      RAD=PI/180;
      PI2=PI*2.0;
      WEEK=86400*7;
      LF=12C;
      CR=15C;
      NL=CR+LF; 
      SAMPRATE=2046000;
      SEARCHTIME=2000*2100;
      GOLDSIZE=1023;
      LONGLOST=SAMPRATE*60*5;
      MAXINBUF=4096;
      FREQMUL=2.0**33/(SAMPRATE);
      MAXSAT=32;
      IFIRLEN=5;
      IFIRFINEXP=6;
      IFIRFINE=1<<IFIRFINEXP;     (* 1<<IFIRFINEXP *)
      FFTSIZE=2048; 
      FRAMES30=10;
      ZEROTIME=315964800;
      EPOCH=86400*7*1024;
      SYNGOODMS=1000; 
      MINALT=-700;
      PRNFINETUNE=0.9999966;
      SECONDS_IN_WEEK=604800;
      UNIVERSAL_GRAVITY_CONSTANT=3.986005E14; 
      E2=0.00669437999014132;
      ERRALT=-10000.0;
      MINREM=6;                   (* for first fix *)
      MAXREM=15;                  (* say no fix *)  


      SOUNDOVERSAMP=32;
      SOUNDSAMP=SOUNDOVERSAMP*1000;
      MEDIANLEVEL=20000.0;

      TWOPOWERMINUS60=1.0/1152921504606846976.0;
      TWOPOWERMINUS55=1.0/36028797018963968.0;
      TWOPOWERMINUS43=1.0/8796093022208.0;
      TWOPOWERMINUS38=1.0/274877906944.0;
      TWOPOWERMINUS33=1.0/8589934592.0;
      TWOPOWERMINUS31=1.0/2147483648.0;
      TWOPOWERMINUS30=1.0/1073741824.0;
      TWOPOWERMINUS29=1.0/536870912.0;
      TWOPOWERMINUS23=1.0/8388608.0;
      TWOPOWERMINUS21=1.0/2097152.0;
      TWOPOWERMINUS20=1.0/1048576.0;
      TWOPOWERMINUS19=1.0/524288.0;
      TWOPOWERMINUS11=1.0/2048.0;
      TWOPOWERMINUS5 =1.0/32.0;
      SPECIALMESSAGE="????????????????????????????????"
                    +" ?"+'"'+"????'???+?-./0123456789:?????"
                    +"?ABCDEFGHIJKLMNOPQRSTUVWXYZ?????"
                    +"????????????????????????????????"
                    +"????????????????????????????????"
                    +"????????????????????????????????"
                    +"????????????????????????????????"
                    +"????????????????????????o???????";

--page4: 1=57 2=25 3=26 4=27 5=28 6=57 7=29 8=30 9=31 10=32 11=57 12=62
--       13=52 14=53 15=54 16=57 17=55 18=56 19=58 20=59 21=57 22=60 23=61 24=62 25=63
--page5: 1..24   25=51
--start 6. jan 1980
TYPE
     STATE=(UNUSED, AQUISIT, TRACE);
     Complex=RECORD Re, Im:REAL END;
     SET8= SET OF [0..7];
     SET16=SET OF [0..15];
     SET32=SET OF [0..31];
     FRP300=ARRAY[0..FRAMES30-1] OF SET32;
     SATPOS=RECORD
              x,y,z,
              vx,vy,vz,
              azimuth,elevation,
              doppler, clkcorr, clkdrift:LONGREAL;
              azel:BOOLEAN;
            END;


     LPCONTEXT=RECORD
       uca1, ila, uca2:REAL;
     END;

     pSATDEMOD=POINTER TO SATDEMOD;
     SATDEMOD=RECORD
       fb:ARRAY[0..FFTSIZE-1] OF Complex;
       ber,
       ifasei,
       prn,
       lastsearch,
       lastsync,
       msok,
       histc,
       dds, 
       goldp,
       freqsumcnt,
       wp          :CARDINAL;
       prnnoise,
       peakthres,
       level,
       ifase,
       prndoppler,
       bitsum,
       squelch,
       lastph,
       leadlag,
       freqsum,
       freq,
       sic0,
       sqc0,
       sio,
       sqo,
       sic,
       sqc,
       sumi,
       sumq,
       sumqco,
       sumqc       :REAL;
       freqi       :INTEGER;
       scan,
       fixprn, 
       lastb       :BOOLEAN;

       insync      :STATE;
       chhist      :ARRAY[0..19] OF REAL;
       next        :pSATDEMOD;
<* IF WITHSOUND THEN *>
       sound:      RECORD dds:CARDINAL; lpci, lpcq:LPCONTEXT END;
<* END *>
     END;

     SAT=RECORD
plusc,
minusc,
       pmcnt,
       bert,
       framep,
       bitc        :CARDINAL;
       datasyn, 
       invers      :BOOLEAN;
       frame       :FRP300;
       freqmedc,
       fsyn,
       timesamp    :CARDINAL; 
       satpos      :SATPOS; 
       insyn,
       satok       :BOOLEAN;
       range,
       rangec,
       corrone,
       mydist,
       prange,
       prangem,
       mfreq,
       prndoppler  :LONGREAL;
       timesampfrac,
       sdil,
       snois,
       sig, 
       snr,
       prnnois,
       freqmed     :REAL;
       next        :CARDINAL;
     END;


     EPHEMERIS=RECORD
       done:SET8;
       health,            (* 6 bit *)
       ura,               (* 4 bit *)
       wn,
       wncorr,
       tow,
       AODO,              (* u5  900 *) 
       IODC,
       toc,               (* 2^4 *)
       toe,               (* u16 2^4 0 .. 604784 *)
       IODE2,             (* u8 *)
       IODE3:CARDINAL;    (* u8 *)
       af0a,              (* i22 2^-31 *)
       af1a,              (* i16 2^-43 *)
       af2a,              (* i8  2^-55 *)
       tgd,               (* 2^-31*)
       m0,                (* i32 2^-31 *)
       deltan,            (* i16 2^-43 *)
       ecc,               (* u32 2^-33 0.0  .. 0.03 *)
       roota,             (* u32 2^-19 2530 .. 8192 *)
       omega0,            (* i32 2^-31 *)
       i0,                (* i32 2^-31 *)
       w,                 (* i32 2^-31 *)
       omegadot,          (* i24 2^-43 0..-6.33E-7*)
       idot,              (* i14 2^-43 *)
       cuc,               (* i16 2^-29 *)
       cus,               (* i16 2^-29 *)
       crc,               (* i16 2^-5 *)
       crs,               (* i16 2^-5 *)
       cic,               (* i16 2^-29 *)
       cis:LONGREAL;      (* i16 2^-29 *)
     END;

<* IF WITHALM THEN *>

     ALMANACHS=RECORD
       done:BOOLEAN;
       fromsat,
       fromtoa,
       fromwna,
       toa,
       wna, 
       lasttoa,
       helth:CARDINAL;    (* alm *)
       af0,               (* alm i22 2^-31 *)
       af1,               (* alm i16 2^-43 *)
       m0,                (* alm i24 2^-23 *)
       i0,                (* alm i16 2^-19 *)
       ecc,               (* alm u32 2^-33 0.0  .. 0.03 *)
       roota,             (* alm u32 2^-19 2530 .. 8192 *)
       omega0,            (* alm i32 2^-31 *)
       w,                 (* alm i32 2^-31 *)
       omegadot:LONGREAL; (* alm i24 2^-43 0..-6.33E-7*)
     END;

<* END *>


     ALMANACH=RECORD
--       lastalt    :LONGREAL;
       tlsf,
       tls        :INTEGER;
       page18ok   :BOOLEAN; 
       a0,                      (* i32 2^-30 *)
       a1         :LONGREAL;    (* i24 2^-50 *)   
       wnt,
       wnlsf,
       tot        :CARDINAL;
       ephtmp,
       ephem      :ARRAY[0..31] OF EPHEMERIS;
<* IF WITHALM THEN *>
       alms       :ARRAY[0..31] OF ALMANACHS;
<* END *>

       printed    :ARRAY[0..31] OF CARDINAL;
     END; 

     POSL=RECORD
       lat,long,alt:LONGREAL;
     END;

     POS=RECORD
       lat,long,alt    :LONGREAL;
       speed, dir, clb :REAL;
     END;

     BESTDIL=RECORD
       d:REAL;
       s0,s1 :CARD8;
     END;

VAR
    debugfn,
    warmbootfn,
    egmfn,
    almfn, 
    rinexfn,
    iqfn        :ARRAY[0..1023] OF CHAR;
    nmeafd,
    iqfd        :INTEGER;
    falselocks,
    zerotime,
    searchkhz,
    vtgcnt,
    nmeaintervall,
    maxsat,
    insamp,
    REVSIZE,
    scansat,
    sampnavi,
    sampc,
    isize       :CARDINAL;
    last3dfix,
    noauto23d,
    healthignore,
    d3wasok,
    posok,
    fixall,
    fixalt,
    upsample,
    u8signed,
    verb,
    verb3,
    verb2       :BOOLEAN;
    nmeavdil,
    nmeahdil,
    egmmeter,
    maxalt      :REAL;
    iqbuf       :ARRAY[0..MAXINBUF*2-1] OF REAL;
    sat         :ARRAY[0..MAXSAT] OF SAT; 
    psat        :pSATDEMOD;   
    GOLD        :ARRAY[0..31] OF ARRAY[0..GOLDSIZE] OF BOOLEAN;  (* 1 more for clock recovery *)
    DDS         :ARRAY[0..16383] OF REAL;
    IFIRTAB     :ARRAY[0..IFIRFINE*IFIRLEN-1] OF REAL;
    ifir        :ARRAY[0..IFIRLEN*2+1] OF REAL;

    REV         :ARRAY[0..4095] OF RECORD x,y:CARDINAL; END;
    FFTSIN      :ARRAY[0..16] OF REAL;
    almanach    :ALMANACH;
    userpos, 
    fixpos      :POS;
    medcnt      :CARDINAL;
    fixsat      :SET32;

    kaldat      :RECORD
      pos       :POS;  
      time      :LONGREAL;
      strength  :REAL;
    END;
    bestdils:ARRAY[0..49] OF BESTDIL;
    dopdone:SET32;

posfixc, nofixc:CARDINAL;
errsum:LONGREAL;


<* IF WITHSOUND THEN *>
    soundsum   :ARRAY[0..SOUNDOVERSAMP*2-1] OF REAL; 
    afbuf      :ARRAY[0..4095] OF INT16;
    soundfd    :INTEGER;
    afbufw     :CARDINAL;
    sounddone  :SET32;
    soundoffset,
    lpAR, lpAL, olpAR, soundgain:REAL;
<* END *>


PROCEDURE Error(text-:ARRAY OF CHAR);
BEGIN
  Werr("gpssdr:"); Werr(text); Werr(" error abort"+LF);
  HALT
END Error;

PROCEDURE hex(n:CARDINAL; cap:BOOLEAN):CHAR;
BEGIN
  n:=n MOD 16;
  IF n<=9 THEN RETURN CHR(n+ORD("0")) END;
  IF cap THEN  RETURN CHR(n+(ORD("A")-10)) END;
  RETURN CHR(n+(ORD("a")-10))
END hex;


PROCEDURE sqr(x:REAL):REAL;BEGIN RETURN x*x END sqr;
PROCEDURE sqrl(x:LONGREAL):LONGREAL; BEGIN RETURN x*x END sqrl;
PROCEDURE maxr(a, b:REAL):REAL; BEGIN IF b>a THEN RETURN b END; RETURN a END maxr;
--PROCEDURE minr(a,b:REAL):REAL; BEGIN IF a<b THEN RETURN a END; RETURN b END minr;

PROCEDURE rinexfloat(x:LONGREAL; VAR s:ARRAY OF CHAR);
VAR e:INTEGER;
    i,j,n:CARDINAL;
BEGIN
  s[0]:=" ";
  IF x<0.0 THEN x:=-x; s[0]:="-" END;
  e:=0;
  IF x>0.0 THEN
    WHILE x<  1.0 DO x:=x*10.0; DEC(e) END;
    WHILE x>=10.0 DO x:=x*0.1; INC(e) END;
  END;
  s[2]:=".";
  i:=2;
  REPEAT
    n:=TRUNC(x);
    x:=(x-LFLOAT(n))*10;
    j:=i;
    IF i=2 THEN j:=1 END;
    s[j]:=CHR(n+ORD("0"));
    INC(i);
  UNTIL i>14;
  s[i]:="E";
  INC(i);
  s[i]:="+";
  IF e<0 THEN e:=-e; s[i]:="-" END; 
  INC(i);
  s[i]:=CHR(e DIV 10+ORD("0"));
  INC(i);
  s[i]:=CHR(e MOD 10+ORD("0"));
  INC(i);
  IF HIGH(s)>=i THEN s[i]:=0C END;
END rinexfloat;

PROCEDURE rinexint(m:INTEGER; w:CARDINAL; VAR s:ARRAY OF CHAR);
VAR i,j,n:CARDINAL;
BEGIN
  IF m<0 THEN n:=-m ELSE n:=m END;
  IF w>0 THEN
    IF w>HIGH(s) THEN w:=HIGH(s) END;
    i:=w+1;
    REPEAT
      DEC(i);
      IF m<0 THEN
        s[i]:="-";
        m:=0;
      ELSE
        s[i]:=CHR(n MOD 10+ORD("0"));
        n:=n DIV 10;
      END;
    UNTIL (n=0) OR (i=0);
    j:=0;
    WHILE j<w DO
      IF j<=w-i THEN s[j]:=s[j+i] ELSE s[j]:=" " END;
      INC(j);
    END;
    IF j<HIGH(s) THEN s[j]:=0C END;
  END;
END rinexint;


PROCEDURE fastatang2(x,y:REAL):REAL;
VAR w, ax, ay:REAL;
BEGIN
  ax:=ABS(x);
  ay:=ABS(y);
  IF ay>ax THEN
    IF ay>0.0 THEN w:=ax/ay ELSE w:=0.0 END;
    w:=PI/2.0 - (w*1.055 - w*w*0.267);                    (* arctan *)
  ELSE
    IF ax>0.0 THEN w:=ay/ax ELSE w:=0.0 END;
    w:=w*1.055 - w*w*0.267;
  END;
  IF x<0.0 THEN w:=PI-w END;
  IF y<0.0 THEN w:=-w END;
  RETURN w
END fastatang2;

PROCEDURE CABSQ(a,b:REAL): REAL;
BEGIN RETURN a*a+b*b END CABSQ;


--- fft
PROCEDURE genrev(size:CARDINAL);
VAR i, j, s, n2, ri : CARDINAL;
    w:REAL;
BEGIN
  s:=0;
  ri:=1;
  n2:=size DIV 2;
  FOR i:=1 TO size-1 DO
    IF i<ri THEN
      REV[s].x:=i-1;
      REV[s].y:=ri-1;
      INC(s);
    END;
    j:=n2;
    WHILE j<ri DO
      ri:=ri-j;
      j:=j DIV 2;
    END;
    ri:=ri+j;
  END;
  REVSIZE:=s;

  w:=PI;
  FOR i:=0 TO 16 DO 
    FFTSIN[i]:=sin(w);
    w:=w*0.5;
  END;
END genrev;


PROCEDURE buttf(VAR R, A, B:Complex);
VAR h:Complex;
BEGIN
  h.Re:=A.Re*B.Re - A.Im*B.Im;
  h.Im:=A.Re*B.Im + A.Im*B.Re;
  B.Re:=R.Re - h.Re;
  B.Im:=R.Im - h.Im;
  R.Re:=R.Re + h.Re;
  R.Im:=R.Im + h.Im;
END buttf;


PROCEDURE Transform(VAR feld:ARRAY OF Complex);
VAR i, j, z, idd2, id, M,  N : CARDINAL;
    h, wcpx           : Complex;
    ck, r, sk, cs, sn, sko : REAL;
BEGIN
  N:=HIGH(feld)+1;

(*bitrev*)
  FOR i:=0 TO REVSIZE-1 DO
    WITH REV[i] DO
      h:=feld[y];
      feld[y]:=feld[x];
      feld[x]:=h;
    END;
  END;
(*bitrev*)

  M:=TRUNC(0.5+ln(FLOAT(N))*1.44269504);
  idd2:=1;
  sko:=FFTSIN[0];
  FOR z:=1 TO M DO
    id:=2*idd2;
    sk:=sko;
    ck:=FFTSIN[z];
    sko:=ck;
    ck:=2.0*ck*ck;
    r:=-2.0*ck;
    cs:=1.0;
    sn:=0.0;
    i:=0;
    WHILE i<N DO
      h:=feld[i+idd2];

      WITH feld[i] DO
        feld[i+idd2].Re:=Re - h.Re;
        feld[i+idd2].Im:=Im - h.Im;
        Re:=Re + h.Re;      
        Im:=Im + h.Im;
      END;
      INC(i,id);
    END;
    IF z<>1 THEN
      FOR j:=1 TO idd2-1 DO
        ck:=r*cs+ck;
        cs:=cs+ck;
        sk:=r*sn+sk;
        sn:=sn+sk;
        wcpx.Re:=cs;
        wcpx.Im:=sn;
        i:=j;
        WHILE i<N DO
          buttf(feld[i], wcpx, feld[i+idd2]);
          INC(i,id);
        END;
      END;
    END;
    idd2:=id;
  END;
END Transform;
--- fft


PROCEDURE MakeDDS(VAR dds:ARRAY OF REAL);
VAR i:CARDINAL;
    r:REAL; 
BEGIN
  r:=PI*2.0/FLOAT(HIGH(dds)+1);
  FOR i:=0 TO HIGH(dds) DO dds[i]:=sin(FLOAT(i)*r) END;
END MakeDDS;

PROCEDURE mkfir();
VAR i, m:CARDINAL;
    u, w:REAL;
    f:ARRAY[0..IFIRFINE*IFIRLEN-1] OF REAL;
BEGIN
  m:=(HIGH(IFIRTAB)+1) DIV 2;
  FOR i:=0 TO HIGH(IFIRTAB) DO
    IF i=m THEN u:=1.0
    ELSE
      w:=0.5*2.0*PI*VAL(REAL, IFIRLEN)*((VAL(REAL,i)-VAL(REAL,m))/VAL(REAL, HIGH(IFIRTAB)+1));
      u:=sin(w)/w;
    END;
    f[i]:=u*(1.0/IFIRLEN);
  END;

--WrStrLn("");
  u:=2.0/FLOAT(HIGH(IFIRTAB)+1);
  FOR i:=0 TO HIGH(IFIRTAB) DO                                      (* hamming *)
--WrInt(VAL(INTEGER, f[i])/32, 4);WrStr(" ");
    f[i]:=f[i]*(0.54+0.46*cos(PI*(FLOAT(i)*u-1.0)));
--WrInt(VAL(INTEGER, f[i])/32, 4);WrStr(" ");
  END;
--WrStrLn("");

  FOR i:=0 TO HIGH(IFIRTAB) DO                                      (* reorder for high access speed *)
--WrInt(i*IFIRFINE MOD (IFIRLEN*IFIRFINE) + i DIV IFIRLEN ,4);
    IFIRTAB[i]:=f[i*IFIRFINE MOD (IFIRLEN*IFIRFINE) + IFIRFINE-1-i DIV IFIRLEN];
--WrInt(IFIRTAB[i]/32,4);
--IF i MOD IFIRLEN=IFIRLEN-1 THEN WrStrLn("") END;
  END;
--WrStrLn("");
END mkfir;

PROCEDURE createnonblockfile(fn:ARRAY OF CHAR):INTEGER;
VAR fd:INTEGER;
BEGIN
  fd:=OpenNONBLOCK(fn);
  IF (fd<0) OR NOT IsFifo(fd) THEN   (* no pipe *)
    IF fd>=0 THEN Close(fd) END;
    fd:=OpenWrite(fn)
  END;
  RETURN fd
END createnonblockfile;


PROCEDURE Parms;
VAR err            :BOOLEAN;
    u              :REAL;     
    h              :ARRAY[0..1023] OF CHAR;
    n              :CARDINAL;
BEGIN
  almanach.tls:=18;
  iqfn:="";
  egmfn:="";
  isize:=1;
  verb:=FALSE;
  verb2:=FALSE;
  verb3:=FALSE;
  err:=FALSE;
  u8signed:=FALSE;
  maxsat:=4;
  insamp:=2000000;
  fixsat:=SET32{};
  nmeaintervall:=50;
  searchkhz:=20;
  zerotime:=ZEROTIME+EPOCH*2;
  nmeafd:=-1;
  kaldat.strength:=40.0;
<* IF WITHSOUND THEN *>
  soundfd:=-1;
<* END *>
  LOOP
    NextArg(h);
    IF h[0]=0C THEN EXIT END;
    IF (h[0]="-") & (h[1]<>0C) & (h[2]=0C) THEN

      IF h[1]="i" THEN
        NextArg(iqfn);
        IF (iqfn[0]=0C) OR (iqfn[0]="-") THEN Error("-i <iqfilename> | <egm-meter>") END;

      ELSIF h[1]="W" THEN
        NextArg(warmbootfn);
        IF (warmbootfn[0]=0C) OR (warmbootfn[0]="-") THEN Error("-W <iwarmbootfile>") END;

      ELSIF h[1]="E" THEN
        NextArg(egmfn);
        IF StrToFix(u, egmfn) THEN egmmeter:=u; egmfn:="";
        ELSIF (egmfn[0]=0C) OR (egmfn[0]="-") THEN Error("-E <egm96-filename>") END;

      ELSIF h[1]="D" THEN
        NextArg(debugfn);
        IF (debugfn[0]=0C) OR (debugfn[0]="-") THEN Error("-D <debugfilename>") END;

      ELSIF h[1]="R" THEN
        NextArg(rinexfn);
        IF (rinexfn[0]=0C) OR (rinexfn[0]="-") THEN Error("-R <rinexfilename>") END;

      ELSIF h[1]="N" THEN
        NextArg(h);
        IF (h[0]=0C) OR (h[0]="-") THEN Error("-N <nmea-file-or-pipe>") END;
        nmeafd:=createnonblockfile(h);
        IF nmeafd<0 THEN Error("write nmea file or pipe") END;

<* IF WITHSOUND THEN *>
      ELSIF h[1]="S" THEN
        NextArg(h);
        IF StrToFix(u, h) THEN
          soundoffset:=u; 
          NextArg(h);
        END;
        IF (h[0]=0C) OR (h[0]="-") THEN Error("-S [<bfoHz>] <sound-file-or-pipe>") END;
        soundfd:=createnonblockfile(h);
        IF soundfd<0 THEN Error("sound output file or pipe") END;
<* END *>

<* IF WITHALM THEN *>
      ELSIF h[1]="a" THEN
        NextArg(almfn);
        IF (almfn[0]=0C) OR (almfn[0]="-") THEN Error("-a <almanachfile>") END;
<* END *>

      ELSIF h[1]="f" THEN
        NextArg(h);
        IF (h[0]="i") & (h[1]="1") & (h[2]="6") THEN isize:=2
        ELSIF (h[0]="u") & (h[1]="8") THEN isize:=1
        ELSIF (h[0]="i") & (h[1]="8") THEN isize:=1; u8signed:=TRUE
        ELSIF (h[0]="f") & (h[1]="3") & (h[2]="2") THEN isize:=4
        ELSE Error("-f u8|i8|i16|f32") END;

      ELSIF h[1]="r" THEN
        NextArg(h);
        IF NOT StrToCard(h, insamp) OR (insamp<1000000) THEN Error("-r <samplerate(hz) >1800000") END;

      ELSIF h[1]="b" THEN
        NextArg(h);
        IF NOT StrToCard(h, n) OR (n=0) THEN Error("-b <nmea beacon intervall") END;
        nmeaintervall:=50/n;

      ELSIF h[1]="A" THEN
        NextArg(h);
        IF NOT StrToFix(maxalt,h) THEN Error("-A <maxalt>") END;

      ELSIF h[1]="L" THEN
        NextArg(h);
        IF NOT StrToFix(u,h) THEN Error("-L <lat> <long> <alt>") END;
        fixpos.lat:=u*RAD;
        NextArg(h);
        IF NOT StrToFix(u,h) THEN Error("-L <lat> <long> <alt>") END;
        fixpos.long:=u*RAD;
        NextArg(h);
        IF NOT StrToFix(u,h) THEN Error("-L <lat> <long> <alt>") END;
        fixpos.alt:=u;
        IF (fixpos.lat=0.0) & (fixpos.long=0.0) THEN fixalt:=TRUE ELSE fixall:=TRUE END;

      ELSIF h[1]="m" THEN
        NextArg(h);
        IF NOT StrToCard(h, maxsat) OR (maxsat<1) OR (maxsat>HIGH(sat)) THEN Error("-m <1..32>") END;

      ELSIF h[1]="e" THEN
        NextArg(h);
        IF NOT StrToCard(h, n) THEN Error("-e <epoche>") END;
        zerotime:=ZEROTIME + EPOCH*n;
     
      ELSIF h[1]="l" THEN
        NextArg(h);
        IF NOT StrToInt(h, almanach.tls) THEN Error("-l <[-]seconds>") END;

      ELSIF h[1]="p" THEN
        NextArg(h);
        IF NOT StrToCard(h, n) OR (n<1) OR (n>32) THEN Error("-p <prn 1..32>") END;
        INCL(fixsat, n-1);
 
      ELSIF h[1]="w" THEN
        NextArg(h);
        IF NOT StrToCard(h, searchkhz) THEN Error("-w <khz>") END;

      ELSIF h[1]="k" THEN
        NextArg(h);
        IF NOT StrToFix(kaldat.strength, h) THEN Error("-k <strength>") END;

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

      ELSIF h[1]="3" THEN noauto23d:=TRUE;

      ELSIF h[1]="H" THEN healthignore:=TRUE;

      ELSIF h[1]="V" THEN
        verb:=TRUE;
        NextArg(h);
        IF h[0]="2" THEN verb2:=TRUE END;
        IF h[0]="3" THEN verb2:=TRUE; verb3:=TRUE END;


      ELSIF h[1]="h" THEN
        WrStrLn("GPS receiver from iq file or pipe from sdr");
        WrStrLn("eg. rtl-stick with <3ppm and working tuner on 1.575GHz and preamp antenna");
        WrStrLn("with >=1 sats write almanach and ephemeris file, view navigation message, clock");
        WrStrLn("with fix entered position and >=1 sats view freq drift and sat positions");
        WrStrLn('all sat could be found within 2ms with a "1000GHz CPU" so it is done in random peaces');
        WrStrLn("");
        WrStrLn(" -3                    switch off auto 2d/3d-fix (3 sat if altitude ok or given -L 0 0 <alt>)");
        WrStrLn(" -A <m>                max altitude, lower for more reliable fix on ground (50000)");
<* IF WITHALM THEN *>
        WrStrLn(" -a <files>            write (append) almanach filenames appended with sat number");
<* END *>
        WrStrLn(" -D <files>            write raw 50 baud navigations msg bits to files");
        WrStrLn(" -E <file> | <meter>   meter or filename of egm96 file (WW15MGH.DAC) for altitude correction (else wgs84 altitude)");
        WrStrLn(" -e <epoche>           gps time wraps every about 19year since 1980, enter epoche (2)");
        WrStrLn(" -f u8|i8|i16|f32      IQ data format (u8)");
        WrStrLn(" -H                    ignore health status of sat (may lead to wrong position/speed)");
        WrStrLn(" -h                    this");
        WrStrLn(" -i <file>             IQ-filename or pipe from sdr receiver");
        WrStrLn(" -k <strength>         position noise filter 0=off (40)");
        WrStrLn(' -L <lat> <long> <alt> rx position and altitude degrees in WGS84, 1 sat enough for frequency (see with -v) or "0 0 <altitude>" for 3 sat position');
        WrStrLn("                        to get frequency with 1 sat or initial 2d fix with 3 sat, wrong altitude leads to wrong position");
        WrStrLn(" -l <s>                give actual leap seconds to get utc before found in sat data (18)");
        WrStrLn(" -m <n>                channels which will search for sats (1..32) more need more cpu (4)");
        WrStrLn(" -N <file>             write nmea output to file or pipe");
        WrStrLn(" -p <n>                sat number which will be continously traced (1..32) may be repeated with -m 1 do nothing else");
        WrStrLn(" -R <file>             write rinex LNAV ephemerides");
        WrStrLn(" -r <Hz>               iq-samplerate in Hz (2000000)");
<* IF WITHSOUND THEN *>
        WrStrLn(" -S [<offset>] <file>  output .wav file or pipe, only locked sat have sound");
<* END *>
        WrStrLn(" -v                    verbous");
        WrStrLn(" -V [n]                more verbous -V 2 -V 3");
        WrStrLn(" -w <kHz>              sat frequency search width in kHz, doppler(4khz)+rx offset, wider for fast moving or unexact rx lo (20)");
        WrStrLn("");
        WrStrLn("rtl_sdr -s 2000000 -f 1575420000 - | ./gpssdr -i /dev/stdin -f u8 -r 2000000 -m 6 -v");

        HALT
 
      ELSE err:=TRUE END;
    ELSE err:=TRUE END;
    IF err THEN EXIT END;
  END;
  IF err THEN Werr(">"); Werr(h); Werr("< use -h"+LF); HALT END;


END Parms;


<* IF WITHSOUND THEN *>


---- wav file header

PROCEDURE wwav(fd:INTEGER; hz, ch:CARDINAL);
CONST bytes=2;
VAR b:ARRAY[0..43] OF CHAR;
BEGIN
  b:="RIFF    WAVEfmt ";
  b[4]:=377C;                                     (* len *)
  b[5]:=377C;
  b[6]:=377C;
  b[7]:=377C;

  b[16]:=20C;
  b[17]:=0C;
  b[18]:=0C;
  b[19]:=0C;

  b[20]:=1C;                                      (* PCM/ALAW *)
  b[21]:=0C;

  b[22]:=CHR(ch);                                  (* channels *)
  b[23]:=0C;

  b[24]:=CHR(hz MOD 100H);                        (* samp *)
  b[25]:=CHR(hz DIV 100H MOD 100H);
  b[26]:=CHR(hz DIV 10000H MOD 100H);
  b[27]:=CHR(hz DIV 1000000H);

  b[28]:=CHR(hz*bytes MOD 256);                   (* byte/s *)
  b[29]:=CHR(hz*bytes DIV 256 MOD 256);
  b[30]:=CHR(hz*bytes DIV 65536);
  b[31]:=0C;

  b[32]:=CHR(bytes);                              (* block byte *)
  b[33]:=0C;
  b[34]:=20C;                                     (* bit/samp *)
  b[35]:=0C;

  b[36]:="d";
  b[37]:="a";
  b[38]:="t";
  b[39]:="a";

  b[40]:=377C;       (* len *)
  b[41]:=377C;
  b[42]:=377C;
  b[43]:=377C;

  IF fd>=0 THEN WrBin(fd, b, 44) END;
END wwav;


PROCEDURE initlp(fg:REAL);
BEGIN
  lpAR:=fg/(254.0/22050.0*38.0);
  lpAL:=lpAR*lpAR*2.888;
  olpAR:=1.0-lpAR;
END initlp;


PROCEDURE lp(VAR lpc:LPCONTEXT; u:REAL):REAL;
BEGIN
  WITH lpc DO
    uca1:=uca1 + (u-uca1)*lpAR - ila;
    uca2:=uca2*olpAR + ila;
    ila:=ila + (uca1-uca2)*lpAL;
    RETURN uca2;
  END;
END lp;


---- make sat audible after despreding

PROCEDURE soundout(s:pSATDEMOD);
VAR d, i:CARDINAL;
    pan, panl, panr, si, fr:REAL;
BEGIN
  IF lpAR=0.0 THEN
    initlp(500.0/(SOUNDSAMP));
    wwav(soundfd, SOUNDSAMP,2);
  END;
  IF s^.prn IN sounddone THEN 
--WrStr("b");
    FOR i:=0 TO SOUNDOVERSAMP*2-1 DO
      si:=soundsum[i]*soundgain;
      soundsum[i]:=0.0;
      IF ABS(si)>MEDIANLEVEL THEN soundgain:=soundgain*0.95;
      ELSIF soundgain<MEDIANLEVEL THEN soundgain:=soundgain*1.0002 END;
      IF soundgain<0.00001 THEN soundgain:=0.00001 END;
      IF si>30000.0 THEN si:=30000.0 ELSIF si<-30000.0 THEN si:=-30000.0 END;
      afbuf[afbufw]:=VAL(INTEGER, si);
      INC(afbufw);
      IF afbufw>HIGH(afbuf) THEN WrBin(soundfd, afbuf, SIZE(afbuf)); afbufw:=0 END;
    END;
--WrStr(" g:"); WrFixed(soundgain,8,1);
    sounddone:=SET32{};
  END;
  INCL(sounddone, s^.prn);

  pan:=sat[s^.prn].satpos.azimuth*(1.0/PI2);
  IF pan=0.0 THEN pan:=FLOAT(s^.prn)*(1.0/MAXSAT) END;   (* no sat pos so use prn num for stereo pan *)
  pan:=pan*0.8;
  panr:=0.9-pan;
  panl:=0.1+pan;

  fr:=s^.freq*(1.0/FREQMUL)+soundoffset;
  IF fr<-SOUNDSAMP*1000*0.5 THEN fr:=-SOUNDSAMP*0.5 ELSIF fr>SOUNDSAMP*0.5 THEN fr:=SOUNDSAMP*0.5 END;
--WrStr(" fr=");WrFixed(fr,1,1);WrStrLn("");
  FOR i:=0 TO SOUNDOVERSAMP-1 DO
    INC(s^.sound.dds, VAL(INTEGER, fr*(4294967296.0/SOUNDSAMP)));
    d:=CAST(CARDINAL, SHIFT(CAST(SET32, s^.sound.dds),-18));
    si:=lp(s^.sound.lpci, s^.sumi)*DDS[d] - lp(s^.sound.lpcq, s^.sumq)*DDS[(d+(HIGH(DDS)+1) DIV 4) MOD (HIGH(DDS)+1)];
    soundsum[i*2  ]:=soundsum[i*2  ] + si*panl;
    soundsum[i*2+1]:=soundsum[i*2+1] + si*panr;
  END;
END soundout;

<* END *>


---- generate gold codes with shift registers

PROCEDURE goldcode;
CONST TAPS=ARRAY OF CARD8 {2,6, 3,7, 4,8, 5,9, 1,9, 2,10, 1,8, 2,9, 3,10,  2,3,  3,4,  5,6,  6,7,  7,8,  8,9,  9,10, 1,4,  2,5,  3,6,  4,7,  5,8,  6,9,  1,3,  4,6,  5,7,  6,8, 7,9,  8,10,  1,6,  2,7,  3,8,  4,9};

TYPE SET10=SET OF [0..9];

VAR i, sat:CARDINAL;
    a,b:SET10;
BEGIN
  FOR sat:=0 TO HIGH(GOLD) DO
    a:=SET10{0..9};
    b:=SET10{0..9};
    FOR i:=1 TO HIGH(GOLD[0]) DO  (* index+1 for fast demodulation *)
      GOLD[sat][i]:=ODD(ORD(9 IN a)+ORD((TAPS[sat*2]-1) IN b)+ORD((TAPS[sat*2+1]-1) IN b));
      IF ODD(ORD(9 IN a)+ORD(2 IN a)) THEN a:=SHIFT(a,1)+SET10{0} ELSE a:=SHIFT(a,1) END;
      IF ODD(ORD(1 IN b)+ORD(2 IN b)+ORD(5 IN b)+ORD(7 IN b)+ORD(8 IN b)+ORD(9 IN b))
      THEN b:=SHIFT(b,1)+SET10{0} ELSE b:=SHIFT(b,1) END;
    END;
    GOLD[sat][0]:=GOLD[sat][1];
  END;
END goldcode;


PROCEDURE wbit(sat, frame:CARDINAL; t-:ARRAY OF CHAR; s:SET32);
VAR i:CARDINAL;
BEGIN
  WrInt(sat+1,2);
  WrStr(",");
  WrInt(frame,1);
  WrStr(t);
  WrStr("[");
  FOR i:=29 TO 0 BY -1 DO WrInt(ORD(i IN s), 1) END;
  WrStrLn("]");
END wbit;


PROCEDURE GpsDate(wn,tow:CARDINAL; VAR s:ARRAY OF CHAR);
BEGIN DateToStr(zerotime - CAST(CARDINAL, almanach.tls) + wn*SECONDS_IN_WEEK+tow, s);
END GpsDate;

PROCEDURE WrDate(wn, tow:CARDINAL);
VAR  h:ARRAY[0..99] OF CHAR;
BEGIN
  GpsDate(wn,tow,h);
  WrStr(h);
END WrDate;


PROCEDURE twnwrap(wn, tw6, to:CARDINAL):CARDINAL;
BEGIN
  tw6:=tw6*6;
  IF VAL(INTEGER, to-tw6)<0 THEN INC(to, SECONDS_IN_WEEK) END;
  RETURN wn*SECONDS_IN_WEEK + to
END twnwrap;

PROCEDURE satgood(s:CARDINAL):BOOLEAN;
BEGIN
  WITH sat[s] DO
    RETURN insyn & datasyn & satok & (sat[s].bert=0) & (sat[s].satpos.elevation>0.0)
  END;
END satgood;

---------- navigation
---- egm altitude correction 

PROCEDURE egm96(lat, long:REAL; VAR ok:BOOLEAN):REAL;   (* read and interpolate geoid correction from datafile in RAD *)

CONST 
--GEOIDFN="WW15MGH.DAC";
      RESOL=4;                      (* 1/deg *)
      LONGS=360*RESOL;              (* values in file around a latitude *) 

  PROCEDURE rdgeoid(fd:File; lat, long:INTEGER; VAR ok:BOOLEAN):REAL;
  VAR b:ARRAY[0..1] OF CHAR;
      n:INTEGER;
  BEGIN
    Seek(fd, (lat*LONGS + long)*2);
    IF RdBin(fd, b, 2)<>2 THEN                      (* no data in file *)      
      n:=0;
      ok:=FALSE;
    ELSE
      n:=ORD(b[1])+ORD(b[0])*256;
      IF n>=32768 THEN DEC(n,65536) END;
    END;
    RETURN VAL(REAL, n)
  END rdgeoid;

VAR ilat, ilong    :INTEGER;
    fd             :INTEGER;
    flat, flong, g :REAL;

BEGIN
  ok:=FALSE;
  IF egmfn<>"" THEN
    fd:=OpenRead(egmfn);
    ok:=fd>=0;
  END;
  g:=0.0;
  IF ok THEN
    lat:=90.0-lat*(1.0/RAD);
    long:=long*(1.0/RAD);
    IF long<0.0 THEN long:=360.0 + long END;
    lat:=lat*RESOL;
    long:=long*RESOL;
    IF lat >=0.0 THEN ilat :=TRUNC(lat)  ELSE ilat :=0 END;
    IF long>=0.0 THEN ilong:=TRUNC(long) ELSE ilong:=0 END;
    flat :=lat -VAL(REAL, ilat);
    flong:=long-VAL(REAL, ilong);
    g:=(( rdgeoid(fd, ilat,   ilong  , ok)*(1.0-flat)
        + rdgeoid(fd, ilat+1, ilong  , ok)*flat)*(1.0-flong)
        +(rdgeoid(fd, ilat,   ilong+1, ok)*(1.0-flat)
        + rdgeoid(fd, ilat+1, ilong+1, ok)*flat)*flong)*0.01;
    Close(fd);
  END;
  RETURN g
END egm96;

---- egm

---- nmea output

PROCEDURE nmea(utc:CARDINAL; pos-:POS; fix, noauto:BOOLEAN; satc:CARDINAL; pdop, hdop, vdop,egm:REAL);   (* RAD *)
CONST Z=ORD("0");
VAR s,h,hn:ARRAY[0..200] OF CHAR;
    sn, satcnt, sc:CARDINAL;

  PROCEDURE cstr(n,f:CARDINAL; VAR s:ARRAY OF CHAR);
  BEGIN
    s[f]:=0C;
    WHILE f>0 DO
      DEC(f);
      s[f]:=CHR(ORD("0")+n MOD 10);
      n:=n DIV 10;
    END
  END cstr;      

  PROCEDURE dB(u:REAL):CARDINAL;
  VAR d:REAL;
  BEGIN
    d:=0.0;
    IF u>0.0001 THEN d:=math.log(u)*8.68588963 END;
    IF d<=0.0 THEN d:=0.0 ELSIF d>99.0 THEN d:=99.0 END;
    RETURN VAL(CARDINAL, d)
  END dB;


  PROCEDURE deg(d:LONGREAL; lon:BOOLEAN);
  VAR n,p :CARDINAL;
  BEGIN
    p:=Length(s);
    n:=TRUNC(ABS(d)*(600000*180/PI));
    IF lon THEN s[p]:=CHR(n DIV 60000000 MOD 10+Z); INC(p) END;
    s[p]:=CHR(n DIV 6000000 MOD 10+Z); INC(p);
    s[p]:=CHR(n DIV 600000 MOD 10+Z); INC(p);
    s[p]:=CHR(n DIV 100000 MOD 6+Z); INC(p);
    s[p]:=CHR(n DIV 10000 MOD 10+Z); INC(p);
    s[p]:="."; INC(p);
    s[p]:=CHR(n DIV 1000 MOD 10+Z); INC(p);
    s[p]:=CHR(n DIV 100 MOD 10+Z); INC(p);
    s[p]:=CHR(n DIV 10 MOD 10+Z); INC(p);
    s[p]:=CHR(n MOD 10+Z); INC(p);
    s[p]:=0C;
  END deg;

  PROCEDURE sum;
  VAR i,p:CARDINAL;
      cs:SET8;  
  BEGIN
    p:=Length(s);
    s[p]:="*"; INC(p);
    cs:=SET8{};
    FOR i:=1 TO p-2 DO cs:=cs/CAST(SET8,s[i]) END;
    s[p]:=hex(CAST(CARDINAL, cs) DIV 16, TRUE); INC(p);
    s[p]:=hex(CAST(CARDINAL, cs), TRUE); INC(p);
    s[p]:=CR; INC(p);
    s[p]:=LF; INC(p);
    WrBin(nmeafd, s, p); 
  END sum;

  PROCEDURE date;
  VAR p:CARDINAL;
  BEGIN
    p:=Length(s);
    s[p]:=h[8]; INC(p);
    s[p]:=h[9]; INC(p);
    s[p]:=h[5]; INC(p);
    s[p]:=h[6]; INC(p);
    s[p]:=h[2]; INC(p);
    s[p]:=h[3]; INC(p);
    s[p]:=0C;
  END date;

  PROCEDURE time;
  VAR p:CARDINAL;
  BEGIN
    p:=Length(s);
    s[p]:=h[11]; INC(p);
    s[p]:=h[12]; INC(p);
    s[p]:=h[14]; INC(p);
    s[p]:=h[15]; INC(p);
    s[p]:=h[17]; INC(p);
    s[p]:=h[18]; INC(p);
    s[p]:=0C;
    Append(s,".000,");
  END time;

  PROCEDURE apos;
  BEGIN
    deg(pos.lat, FALSE);
    IF pos.lat<0.0 THEN Append(s, ",S,") ELSE Append(s, ",N,") END; 
    deg(pos.long, TRUE);
    IF pos.long<0.0 THEN Append(s, ",W,") ELSE Append(s, ",E,") END; 
  END apos;


BEGIN
  satcnt:=0;
  FOR sn:=0 TO HIGH(sat) DO IF satgood(sn) THEN INC(satcnt) END END;

  s:="$GPGGA,";
  DateToStr(utc,h);
  time;
  apos;
  IntToStr(ORD(fix),1, hn); Append(s,hn);
  Append(s, ",");
  cstr(satcnt, 2, hn); Append(s,hn);
  Append(s, ",");
  FixToStr(hdop,2,hn); Append(s,hn);
  Append(s, ",");
  FixToStr(pos.alt-egm,2,hn); Append(s,hn);
  Append(s, ",M,");
  FixToStr(egm,2,hn); Append(s,hn);
  Append(s, ",M,,0000");
  sum; 

  s:="$GPRMC,";
  DateToStr(utc,h);
  time;
  IF fix THEN Append(s,"A,") ELSE Append(s,"V,") END;
  apos;
  FixToStr(pos.speed*EARTH/(EARTH+pos.alt)*(3.6/KNOTS),3,hn); Append(s,hn); (* speed over ground wherever ground is *)
  Append(s, ",");
  FixToStr(pos.dir*(1.0/RAD),3,hn); Append(s,hn);
  Append(s, ",");
  date;
  Append(s, ",,");
  sum; 

  s:="$GPGSA,";
  IF noauto THEN Append(s, "M,") ELSE Append(s, "A,") END;
  Append(s, CHR(ORD("1")+ORD(fix)+ORD(fix&(satc>=4))));
  sc:=0;
  FOR sn:=0 TO HIGH(sat) DO
    IF (sc<12) & sat[sn].satok THEN 
      Append(s, ",");
      cstr(sn+1,2,hn); Append(s,hn);
      INC(sc);
    END;
  END;
  FOR sn:=sc TO 12 DO Append(s, ",") END;
  FixToStr(pdop,2,hn); Append(s,hn);
  Append(s, ",");
  FixToStr(hdop,2,hn); Append(s,hn);
  Append(s, ",");
  FixToStr(vdop,2,hn); Append(s,hn);
  sum; 

  INC(vtgcnt);
  IF vtgcnt>=6 THEN
    vtgcnt:=0;
    IF satcnt>0 THEN
      sc:=0;
      FOR sn:=0 TO HIGH(sat) DO
--        WITH sat[sn] DO
          IF satgood(sn) THEN
            IF sc MOD 4=0 THEN
              s:="$GPGSV,";
              cstr((satcnt+3) DIV 4,1,hn); Append(s,hn); Append(s, ",");
              cstr(1+sc DIV 4,1,hn); Append(s,hn); Append(s, ",");
              cstr(satcnt,2,hn); Append(s,hn); Append(s, ",");
            END;
            cstr(sn+1,2,hn); Append(s,hn); Append(s, ",");
            cstr(VAL(INTEGER, sat[sn].satpos.elevation/RAD),2,hn); Append(s,hn); Append(s, ",");
            cstr(VAL(INTEGER, sat[sn].satpos.azimuth/RAD),3,hn); Append(s,hn); Append(s, ",");
            cstr(dB(sat[sn].sig),2,hn); Append(s,hn);
            IF (sc MOD 4=3) OR (sc+1=satcnt) THEN sum ELSE Append(s, ",") END;
            INC(sc);
          END;
--        END;
      END;
    END;
  END;
(*
                                            
2019.04.07 23:21:00                          knots dir
$GPRMC,032855.000,A,4815.0819,N,01302.2757,E,0.29,21.09,160225,,,A*5B
$GPVTG,21.09,T,,M,0.29,N,0.5,K,A*39
                                           fix
                                             sat
                                                hdop
                                                    alt
                                                            egm
$GPGGA,032856.000,4815.0819,N,01302.2758,E,1,06,1.4,344.8,M,46.5,M,,0000*59

       lines
         line
           sats
              prn
                 el az  snr
$GPGSV,3,1,12,16,68,258,30,18,55,057,18,26,53,197,34,23,48,123,38*73
$GPGSV,3,2,12,27,44,291,20,10,24,167,39,29,12,097,29,08,10,287,*72
$GPGSV,3,3,12,15,08,074,20,07,07,330,,05,03,028,,31,02,211,*79

                         PDOP HDOP VDOP
           12 channels uses sat nr
         1 no 2 2d 3 3d fix
       A/M auto 2d
$GPGSA,A,3,07,,,,,,,,,,,,1.7,1.0,0.5*33
*)

END nmea;

---- nmea output


PROCEDURE wgs84(lat, long, heig:LONGREAL; VAR x,y,z:LONGREAL);       (* wgs84 ecef *)
VAR n,sl,h:LONGREAL;
BEGIN
  sl:=math.sin(lat);
  n:=EARTH/math.sqrt(1.0 - E2*sl*sl);
  h:=(heig+n)*math.cos(lat);
  z:=(n*(1.0-E2)+heig)*sl;
  y:=h*math.sin(long);
  x:=h*math.cos(long);
END wgs84;

PROCEDURE wgs84r(x,y,z:LONGREAL; VAR lat, long, heig:LONGREAL);
CONST A=EARTH;
      B=6356752.31424518;

VAR d,s,p,n:LONGREAL;

BEGIN
  IF (x=0.0) & (y=0.0) THEN
    long:=0.0;
    IF z<0.0 THEN
      heig:=-z-B;
      lat:= -PI/2.0;
    ELSE
      heig:=z-B;
      lat:=PI/2.0;
    END;
  ELSE 
    p:=math.sqrt(x*x + y*y); (* best formula for any longitude and applies well near the poles *)
    long:=2.0*math.atan2(y, x+p);
    lat:=math.atan(z/(p*(1.0-E2)));   (* set approximate initial latitude assuming a height of 0.0*)
    heig:=0.0;
    REPEAT
      d:=heig;
      s:=math.sin(lat);
      n:=A/math.sqrt(1.0-E2*s*s);
      heig:=p/math.cos(lat)-n;
      lat:=math.atan(z/(p*(1.0-E2*n/(n+heig))));
    UNTIL ABS(heig-d)<=0.01;  (* 10mm *)
  END;
END wgs84r;


PROCEDURE dist(x0,y0,z0,x1,y1,z1:LONGREAL):LONGREAL;
BEGIN
  x0:=x0-x1;
  y0:=y0-y1;
  z0:=z0-z1;
  RETURN math.sqrt(x0*x0 + y0*y0 + z0*z0);
END dist;

PROCEDURE angdiff(a,b:LONGREAL):LONGREAL;
BEGIN
  a:=a-b;
  IF a<-PI THEN a:=a+PI2 ELSIF a>PI THEN a:=a-PI2 END;
  RETURN a
END angdiff;

---- azimuth RAD +-PI of 2 positions fast but only exact for small distance 

PROCEDURE shortdir(a, b:POS):REAL;
VAR x, y:LONGREAL;
BEGIN
  y:=a.lat-b.lat;
  x:=a.long-b.long;
  IF x>PI THEN x:=x-PI2 ELSIF x<-PI THEN x:=x+PI2 END; 
  x:=x*cos((b.lat+a.lat)*0.5);
  RETURN math.atan2(x,y)
END shortdir;


PROCEDURE shortdist(a-,b-:POS):LONGREAL;  (* only exact on short distance *)
BEGIN RETURN math.sqrt(sqrl(angdiff(a.lat,b.lat)) + sqrl(angdiff(a.long,b.long)*math.cos((a.lat+b.lat)*0.5)))*EARTH
END shortdist;


PROCEDURE rotatexyz(lat,long,x,y,z:LONGREAL; VAR xr,yr,zr:LONGREAL); 
VAR sinlat, coslat, sinlon, coslon:LONGREAL;
BEGIN
  sinlat:=math.sin(lat);
  coslat:=math.cos(lat);
  sinlon:=math.sin(long);
  coslon:=math.cos(long);
  xr:=coslat*z - sinlat*coslon*x - sinlat*sinlon*y;
  yr:=coslon*y - sinlon*x;
  zr:=coslat*coslon*x + coslat*sinlon*y + sinlat*z;
END rotatexyz;


----- azimuth and elevation from 2 positions

PROCEDURE azelxyzxyz(fromX, fromY, fromZ, toX, toY, toZ:LONGREAL; VAR azimuth, elevation:LONGREAL);
VAR lat, lon, dX, dY, dZ, dN, dE, dUp, tmp:LONGREAL;
BEGIN
  elevation:=0.0;
  azimuth  :=0.0; 
  wgs84r(fromX, fromY, fromZ, lat, lon, tmp);

  dX:=toX-fromX;
  dY:=toY-fromY;
  dZ:=toZ-fromZ;

  rotatexyz(lat, lon, dX, dY, dZ, dN, dE, dUp);
  tmp:=math.sqrt(dN*dN + dE*dE);
  IF tmp=0.0 THEN 
    IF dUp<0.0 THEN elevation:=-PI/2.0 ELSE elevation:=PI/2.0 END;
  ELSE elevation:=math.atan(dUp/tmp) END;
  azimuth:=math.atan2(dE, dN);
  IF azimuth<0.0 THEN azimuth:=azimuth+PI2 END;
END azelxyzxyz;


PROCEDURE SatPosVelocity(transmission_gpsweek:CARDINAL; transmission_gpstow:LONGREAL;
                         ephem_week, toe:CARDINAL;
                         m0, delta_n, ecc, sqrta, omega0, i0, w, omegadot, idot,
                         cuc, cus, crc, crs, cic, cis,
                         estimateOfTrueRange, estimteOfRangeRate:LONGREAL;
                         VAR x, y, z, vx, vy, vz:LONGREAL);
CONST WGS84_EARTH_ROTATION_RATE=7.2921151467E-5; 
VAR
  j:CARDINAL;
  tot,        (* time of transmission (including gps week) [s] *)
  tk,         (* time from ephemeris reference epoch       [s] *)
  a,          (* semi-major axis of orbit                  [m] *)
  n,          (* corrected mean motion                     [rad/s] *)
  M,          (* mean anomaly,                             [rad]   (Kepler's equation for eccentric anomaly, solved by iteration) *)
  E,          (* eccentric anomaly                         [rad] *)
  v,          (* true anomaly                              [rad] *)
  u,          (* argument of latitude, corrected           [rad] *)
  r,          (* radius in the orbital plane               [m] *)
  i,          (* orbital inclination                       [rad] *)
  cos2u,      (* cos(2*u)                                  [] *)
  sin2u,      (* sin(2*u)                                  [] *)
  d_u,        (* argument of latitude correction           [rad] *)
  d_r,        (* radius correction                         [m] *)
  d_i,        (* inclination correction                    [rad] *)
  x_op,       (* x position in the orbital plane           [m] *)
  y_op,       (* y position in the orbital plane           [m] *)
  omegak,     (* corrected longitude of the ascending node [rad] *)
  cos_omegak, (* cos(omegak) *)
  sin_omegak, (* sin(omegak) *)
  cosu,       (* cos(u) *)
  sinu,       (* sin(u) *)
  cosi,       (* cos(i) *)
  sini,       (* sin(i) *)
  cosE,       (* cos(E) *)
  sinE,       (* sin(E) *)
  omegadotk,  (* corrected rate of right ascension         [rad/s] *)
  edot,       (* edot = n/(1.0 - ecc*cos(E)),              [rad/s]  *)
  vdot,       (* d/dt of true anomaly                      [rad/s] *)
  udot,       (* d/dt of argument of latitude              [rad/s] *)
  idotdot,    (* d/dt of the rate of the inclination angle [rad/s^2] *)
  rdot,       (* d/dt of the radius in the orbital plane   [m/s] *)
  tmpa,       (* temp *)
  tmpb,       (* temp *)
  vx_op,      (* x velocity in the orbital plane           [m/s] *)
  vy_op:LONGREAL;      (* y velocity in the orbital plane  [m/s] *)

BEGIN
  (* compute the times from the reference epochs 
   By including the week in the calculation, week rollover and older ephemeris bugs are mitigated
   The result should be between -302400 and 302400 if the ephemeris is within one week of transmission *)

  tot:=LFLOAT(transmission_gpsweek*SECONDS_IN_WEEK) + transmission_gpstow;
  tk := tot - LFLOAT(ephem_week*SECONDS_IN_WEEK + toe);
                                                    (* compute the corrected mean motion term *)
  a:=sqrta*sqrta;
  n:=math.sqrt(UNIVERSAL_GRAVITY_CONSTANT/(a*a*a)); (* computed mean motion *)
  n:=n+delta_n;                                     (* corrected mean motion *)

                                                    (* Kepler's equation for eccentric anomaly *)
  M:=m0 + n*tk;                                     (* mean anomaly *)
  E:=M;
  FOR j:=0 TO 6 DO E:=M + ecc * math.sin(E) END;

  cosE:=math.cos(E);
  sinE:=math.sin(E);

                                                    (* true anomaly *)
  v:=math.atan2((math.sqrt(1.0 - ecc*ecc)*sinE), (cosE - ecc));

                                                    (* argument of latitude *)
  u:=v + w;
                                                    (* radius in orbital plane *)
  r:=a*(1.0 - ecc*math.cos(E)); 
                                                    (* orbital inclination *)
  i:=i0;

                                                    (* second harmonic perturbations *)
  cos2u:=math.cos(2.0*u);
  sin2u:=math.sin(2.0*u);
                                                    (* argument of latitude correction *)
  d_u:=cuc*cos2u + cus*sin2u;
                                                    (* radius correction *)
  d_r:=crc*cos2u + crs*sin2u;
                                                    (* correction to inclination *)
  d_i:=cic*cos2u + cis*sin2u;
                                                    (* corrected argument of latitude *)
  u:=u+d_u;
                                                    (* corrected radius *)
  r:=r+d_r;
                                                    (* corrected inclination *)
  i:=i + d_i + idot * tk;

                                                    (* positions in orbital plane *)
  cosu:=math.cos(u);
  sinu:=math.sin(u);
  x_op:=r*cosu;
  y_op:=r*sinu;


  (* compute the corrected longitude of the ascending node
  This equation deviates from that in Table 20-IV p. 100 GPSICD200C with the inclusion of the 
  signal propagation time (estimateOfTrueRange/LIGHTSPEED) term. This compensates for the Sagnac effect.
  The omegak term is thus sensitive to the estimateOfTrueRange term which is usually unknown without
  prior information. The average signal propagation time/range (70ms * c) can be used on first use
  and this function must be called again to iterate this term. The sensitivity of the omegak term
  typically requires N iterations - GDM_DEBUG{find out how many iterations are needed, how sensitive to the position? *)

  omegak:=omega0 + (omegadot - WGS84_EARTH_ROTATION_RATE)*tk - WGS84_EARTH_ROTATION_RATE*(LFLOAT(toe) + estimateOfTrueRange/LIGHT);

  (* compute the WGS84 ECEF coordinates, 
     vector r with components x & y is now rotated using, R3(-omegak)*R1(-i) *)

  cos_omegak:=math.cos(omegak);
  sin_omegak:=math.sin(omegak);
  cosi:=math.cos(i);
  sini:=math.sin(i);
  x:=x_op*cos_omegak - y_op*sin_omegak*cosi;
  y:=x_op*sin_omegak + y_op*cos_omegak*cosi;
  z:=y_op*sini;

  (* Satellite Velocity Computations are below
     see Reference Remodi, B. M (2004). GPS Tool Box: Computing satellite velocities using the broadcast ephemeris. 
     GPS Solutions. Volume 8(3), 2004. pp. 181-183 
     example source code was available at [http:www.ngs.noaa.gov/gps-toolbox/bc_velo/bc_velo.c]  
     recomputed the cos and sin of the corrected argument of latitude *)

  cos2u:=math.cos(2.0*u);
  sin2u:=math.sin(2.0*u);

  edot:=n/(1.0 - ecc*cosE);
  vdot:=sinE*edot*(1.0 + ecc*math.cos(v))/(math.sin(v)*(1.0-ecc*cosE) );  
  udot:=vdot + 2.0*(cus*cos2u - cuc*sin2u)*vdot;
  rdot:=a*ecc*sinE*n/(1.0-ecc*cosE) + 2.0*(crs*cos2u - crc*sin2u)*vdot;
  idotdot:=idot + (cis*cos2u - cic*sin2u)*2.0*vdot;

  vx_op:=rdot*cosu - y_op*udot;
  vy_op:=rdot*sinu + x_op*udot;

  (* corrected rate of right ascension including similarily as above, for omegak, 
     compensation for the Sagnac effect *)

  omegadotk:=omegadot - WGS84_EARTH_ROTATION_RATE*(1.0 + estimteOfRangeRate/LIGHT);

  tmpa:=vx_op - y_op*cosi*omegadotk;
  tmpb:=x_op*omegadotk + vy_op*cosi - y_op*sini*idotdot;

  vx:=tmpa * cos_omegak - tmpb*sin_omegak;
  vy:=tmpa * sin_omegak + tmpb*cos_omegak;
  vz:=vy_op*sini + y_op*cosi*idotdot;
END SatPosVelocity;


PROCEDURE ClockCorrDrift(transmission_gpsweek:CARDINAL; transmission_gpstow:LONGREAL;
                         ephem_week, toe, toc:CARDINAL;
                         af0, af1, af2, ecc, sqrta, delta_n, m0, tgd:LONGREAL;
                         mode:CARDINAL;
                         VAR clock_correction, clock_drift:LONGREAL);

CONST CLOCK_CORRECTION_RELATIVISTIC_CONSTANT_F=-4.442807633E-10;
      RATIO_OF_SQUARED_FREQUENCIES_L1_OVER_L2=1.64694444444444444444444444;

VAR i:CARDINAL; 
    tot, tk,tc, d_tr, d_tsv, a, n, M, E:LONGREAL;
BEGIN
  (* compute the times from the reference epochs 
   By including the week in the calculation, week rollover and old ephmeris bugs are mitigated
   The result should be between -302400 and 302400 if the ephemeris is within one week of transmission *)

  tot:=LFLOAT(transmission_gpsweek*SECONDS_IN_WEEK) + transmission_gpstow;
  tk:=tot - LFLOAT(ephem_week*SECONDS_IN_WEEK + toe);
  tc:=tot - LFLOAT(ephem_week*SECONDS_IN_WEEK + toc);

  (* compute the corrected mean motion term *)
  a:=sqrta*sqrta;
  n:=math.sqrt(UNIVERSAL_GRAVITY_CONSTANT/(a*a*a));
  n:=n+delta_n;

  (* Kepler's equation for eccentric anomaly *)
  M:=m0 + n*tk;
  E:=M;
  FOR i:=0 TO 6 DO E:=M + ecc*math.sin(E) END;
 
  (* relativistic correction*)
  d_tr:=CLOCK_CORRECTION_RELATIVISTIC_CONSTANT_F*ecc*sqrta*math.sin(E);

  (* clock correcton *)
  d_tsv:=af0 + af1*tc + af2*tc*tc;

  IF mode=0 THEN (* L1 only *) d_tsv:=d_tsv - tgd;
  ELSIF mode=1 THEN (* L2 only *) d_tsv:=d_tsv - tgd*RATIO_OF_SQUARED_FREQUENCIES_L1_OVER_L2 END;

  clock_correction:=(d_tsv + d_tr)*LIGHT;
  clock_drift:=(af1 + 2.0*af2*tc)*LIGHT;
END ClockCorrDrift;


----- calculate position of sat

PROCEDURE SatPos(userX, userY, userZ:LONGREAL;
                 gpsweek:CARDINAL;
                 gpstow:LONGREAL;            (* !< time of week of signal transmission  (gpstow-psr/c) [s] *)
                 ephem_week,                 (* !< ephemeris: GPS week (0-1024+) [weeks] *)
                 toe,                        (* !< ephemeris: time of week  [s] *)
                 toc:CARDINAL;               (* !< ephemeris: clock reference time of week [s] *)
                 af0,                        (* !< ephemeris: polynomial clock correction coefficient       [s],   Note: parameters from ephemeris preferred vs almanac (22 vs 11 bits) *)
                 af1,          (* !< ephemeris: polynomial clock correction coefficient                     [s/s], Note: parameters from ephemeris preferred vs almanac (16 vs 11 bits) *)
                 af2,          (* !< ephemeris: polynomial clock correction coefficient                     [s/s^2]   *)
                 tgd,          (* !< ephemeris: group delay differential between L1 and L2                  [s] *)
                 m0,           (* !< ephemeris: mean anomaly at reference time                              [rad] *)
                 delta_n,      (* !< ephemeris: mean motion difference from computed value                  [rad/s] *)
                 ecc,          (* !< ephemeris: eccentricity                                                [] *)
                 sqrta,        (* !< ephemeris: square root of the semi-major axis                          [m^(1/2)] *)
                 omega0,       (* !< ephemeris: longitude of ascending node of orbit plane at weekly epoch  [rad] *)
                 i0,           (* !< ephemeris: inclination angle at reference time                         [rad] *)
                 w,            (* !< ephemeris: argument of perigee                                         [rad] *)
                 omegadot,     (* !< ephemeris: rate of right ascension                                     [rad/s] *)
                 idot,         (* !< ephemeris: rate of inclination angle                                   [rad/s] *)
                 cuc,          (* !< ephemeris: amplitude of the cosine harmonic correction term to the argument of latitude  [rad] *)
                 cus,          (* !< ephemeris: amplitude of the sine   harmonic correction term to the argument of latitude  [rad] *)
                 crc,          (* !< ephemeris: amplitude of the cosine harmonic correction term to the orbit radius          [m] *)
                 crs,          (* !< ephemeris: amplitude of the sine   harmonic correction term to the orbit radius          [m] *)
                 cic,          (* !< ephemeris: amplitude of the cosine harmonic correction term to the angle of inclination  [rad] *)
                 cis:LONGREAL; (* !< ephemeris: amplitude of the sine   harmonic correction term to the angle of inclination  [rad] *)
             VAR 
                 clock_correction, (* !< clock correction for this satellite for this epoch           [m] *)
                 clock_drift,      (* !< clock drift correction for this satellite for this epoch     [m/s] *)
                 satX,             (* !< satellite X position WGS84 ECEF                              [m] *)
                 satY,             (* !< satellite Y position WGS84 ECEF                              [m] *)
                 satZ,             (* !< satellite Z position WGS84 ECEF                              [m] *)
                 satVx,            (* !< satellite X velocity WGS84 ECEF                              [m/s] *)
                 satVy,            (* !< satellite Y velocity WGS84 ECEF                              [m/s] *)
                 satVz,            (* !< satellite Z velocity WGS84 ECEF                              [m/s] *)
                 azimuth,          (* !< satelilte azimuth                                            [rad] *)
                 elevation,        (* !< satelilte elevation                                          [rad] *)
                 doppler:LONGREAL  (* !< satellite doppler with respect to the user position          [m/s], Note: User must convert to Hz *)
                );
VAR
   tow, range, range_rate, vx, vy, vz, dx,dy,dz:LONGREAL;
   week,i :CARDINAL;
BEGIN
  IF toe=0 THEN INC(ephem_week) END;     (* repair week wrap bug *)
--WrStr(" GPS toe="); WrCard(toe,1); WrStr(" week=");  WrCard(ephem_week,1); WrStr(" "); 
  satX:=0.0;
  satY:=0.0;
  satZ:=0.0;
  vx:=0.0;
  vy:=0.0;
  vz:=0.0;
  ClockCorrDrift(gpsweek, gpstow, ephem_week, toe, toc, af0, af1, af2,
                 ecc, sqrta, delta_n, m0, tgd, 0, clock_correction, clock_drift);
  (* adjust for week rollover *)  
  week:=gpsweek;
  tow:=gpstow + clock_correction*(1.0/LIGHT);
  IF tow<0.0 THEN
    tow:=tow+SECONDS_IN_WEEK;
    DEC(week);
  END;
  IF tow>SECONDS_IN_WEEK THEN
    tow:=tow-SECONDS_IN_WEEK;
    INC(week);
  END;

  (*iterate to include the Sagnac correction
   since the range is unknown, an approximate of 70 ms is good enough to start 
   the iterations so that 2 iterations are enough for sub mm accuracy *)
  range:=0.070*LIGHT; 
  range_rate:=0.0;
  FOR i:=0 TO 1 DO
    SatPosVelocity(week, tow, ephem_week, toe, m0, delta_n, ecc, sqrta, omega0,
                   i0, w, omegadot, idot, cuc, cus, crc, crs, cic, cis, range, range_rate,
                   satX, satY, satZ, vx, vy, vz);
    dx:=userX-satX;
    dy:=userY-satY;
    dz:=userZ-satZ;
    range:=math.sqrt(dx*dx + dy*dy + dz*dz);
    range_rate:=(vx*dx + vy*dy + vz*dz)/range; (* this method uses the NovAtel style sign convention! *)
  END;
  azelxyzxyz(userX, userY, userZ, satX, satY, satZ, azimuth, elevation);
  satVx:=vx;
  satVy:=vy;
  satVz:=vz;
  doppler:=range_rate;
END SatPos;


---- make median (over desired beacon intervall) of pseudo distance from all sat to all other

PROCEDURE medranges;
CONST H=4294967296.0; (* 2^32 *)
      HH=H*0.5;
VAR i   :CARDINAL;
    b, r:LONGREAL;
BEGIN
  IF medcnt=0 THEN
    FOR i:=0 TO HIGH(sat) DO sat[i].prangem:=0.0; sat[i].pmcnt:=0 END; 
  END;
  b:=-1.0;
  FOR i:=0 TO HIGH(sat) DO
    WITH sat[i] DO
      IF insyn & datasyn THEN
        IF b<0.0 THEN b:=prange;
        ELSE
          r:=prange-b;
          IF r>HH THEN r:=r-H ELSIF r<-HH THEN r:=r+H END;    (* sample counter wrap *)
          prangem:=prangem+r;
        END; 
        INC(pmcnt);
      END;
    END;
  END;  
END medranges;

----in ephemeris announced sat error index to meters

PROCEDURE urai(idx:CARDINAL):REAL;
BEGIN
  IF idx<=6 THEN RETURN math.pow(2.0, 1.0+FLOAT(idx)*0.5) END;
  RETURN math.pow(2.0, FLOAT(idx-2))
END urai;


-------2 or 3d fix

PROCEDURE angleofsats(x0,y0,z0,x1,y1,z1,x2,y2,z2:LONGREAL):LONGREAL;
VAR d1, d2:LONGREAL;
BEGIN
  x1:=x1-x0; 
  y1:=y1-y0;
  z1:=z1-z0;
  d1:=math.sqrt(sqrl(x1) + sqrl(y1) + sqrl(z1));
  IF d1=0.0 THEN RETURN 0.0 END;

  d1:=1.0/d1;
  x2:=x2-x0;
  y2:=y2-y0;
  z2:=z2-z0;
  d2:=math.sqrt(sqrl(x2) + sqrl(y2) + sqrl(z2));
  IF d2=0.0 THEN RETURN 0.0 END;

  d2:=1.0/d2;
  d1:=0.5*math.sqrt(sqrl(x2*d2 - x1*d1) + sqrl(y2*d2 - y1*d1) + sqrl(z2*d2 - z1*d1));
  IF ABS(d1)>=1.0 THEN RETURN PI END;

  RETURN math.asin(d1)*2.0
END angleofsats;


PROCEDURE dil(s1-,s2-:SATPOS; upos-:POS):REAL;
VAR mx,my,mz:LONGREAL;
BEGIN
  IF (upos.lat<>0.0) OR (upos.long<>0.0) THEN
    wgs84(upos.lat, upos.long, upos.alt, mx, my, mz);
  ELSE mx:=0.0; my:=0.0; mz:=0.0 END;
  RETURN PI*0.5-ABS(ABS(angleofsats(mx,my,mz, s1.x,s1.y,s1.z, s2.x,s2.y,s2.z))-PI*0.5);
END dil;


PROCEDURE calcdop(us:CARDINAL; upos-:POS);
VAR i,j,k:CARDINAL;
--    x0,y0,z0,x1,y1,z1,x2,y2,z2:LONGREAL;
    h,h1:BESTDIL;
BEGIN
--  wgs84(userpos.lat, userpos.long, userpos.alt, x0, y0, z0);
  FILL(ADR(bestdils), 0C ,SIZE(bestdils));
IF verb THEN WrStr("dops:") END;
  i:=us; 
  WHILE i<=HIGH(sat) DO
    j:=sat[i].next;
    WHILE j<=HIGH(sat) DO
      h.d:=dil(sat[i].satpos, sat[j].satpos, upos);  
IF verb THEN WrFixed(h.d,2,1); WrStr(" "); END;

--      h.d:=dist(sat[i].satpos.x, sat[i].satpos.y, sat[i].satpos.z, sat[j].satpos.x, sat[j].satpos.y, sat[j].satpos.z);
--IF verb THEN (* WrInt(i+1,1);WrStr(",");WrInt(j+1,1);WrStr(",");*) WrFixed(h.d*0.000001,1,1); WrStr(" "); END;
      IF h.d>0.0 THEN
        k:=0;
        WHILE h.d<bestdils[k].d DO INC(k) END;            (* sort on hdil 0 terminated *)
        h.s0:=i;
        h.s1:=j;
        WHILE (k<HIGH(bestdils)) & ((k<4) OR (h.d>0.5)) DO  (* remove bad if min. 4 good and let one 0.0 at end *)
          h1:=bestdils[k];
          bestdils[k]:=h;
          INC(k);
          h:=h1;
        END;
      END;
      j:=sat[j].next;
    END;
    i:=sat[i].next;
  END;
IF verb THEN
 WrStr("/sort:");
 i:=0; WHILE bestdils[i].d>0.0 DO WrFixed(bestdils[i].d,2,1); WrStr(" "); INC(i) END;
 WrStrLn("");
END;
END calcdop;


----  use nearest sats pos as startup position (time sample arrived first of all sat) 

PROCEDURE guesspos(VAR p:POSL);
VAR i:CARDINAL;
    alt, minr:LONGREAL;
BEGIN
  FILL(ADR(p), 0C, SIZE(p));
  minr:=MAX(REAL);
  FOR i:=0 TO HIGH(sat) DO
    IF sat[i].insyn & sat[i].datasyn & sat[i].satok & (sat[i].rangec<minr) THEN
      minr:=sat[i].rangec;
      wgs84r(sat[i].satpos.x, sat[i].satpos.y,  sat[i].satpos.z, p.lat,p.long, alt);
    END;
  END;
IF verb & (minr<MAX(REAL)) THEN WrStr(" guess:[");WrFixed(p.lat*(1.0/RAD),5,1); WrStr(","); WrFixed(p.long*(1.0/RAD),5,1); WrStrLn("]") END;
END guesspos;


---- sum range errors between all sat pairs 

PROCEDURE check(p-:POSL):LONGREAL;
VAR sum,x1,y1,z1:LONGREAL;
    i:CARDINAL;
    sp0,sp1:SATPOS;
BEGIN
  wgs84(p.lat, p.long, p.alt, x1, y1, z1);
  sum:=0.0;
  i:=0;
  WHILE bestdils[i].d>0.0 DO
    sp0:=sat[bestdils[i].s0].satpos;
    sp1:=sat[bestdils[i].s1].satpos;
    sum:=sum + ABS((dist(sp0.x, sp0.y, sp0.z, x1, y1, z1) - sat[bestdils[i].s0].range)
                  -(dist(sp1.x, sp1.y, sp1.z, x1, y1, z1) - sat[bestdils[i].s1].range));
    INC(i);
  END;
  IF i=0 THEN RETURN MAX(REAL) END;

  RETURN sum/LFLOAT(i)   (* normalize to 1 pair *)
END check;

----- go along best direction to minimum range difference to all sats

PROCEDURE aquisith(VAR p:POSL; resol:LONGREAL):LONGREAL; 
CONST DIRS=8;
VAR o,w,best  :LONGREAL;
    dir, i:CARDINAL;
    p1        :POSL;
BEGIN
  p1:=p;
  best:=MAX(REAL);
  o:=0.0;
  i:=0;
  LOOP
    w:=LFLOAT(i)*(PI2/DIRS);
    o:=o*0.00000004; 
    p.lat :=p1.lat  + math.cos(w)*o;
    p.long:=p1.long + math.sin(w)*o;
    IF p.long>PI THEN p.long:=p.long-PI2 ELSIF p.long<-PI THEN p.long:=p.long+PI2 END; 
    IF ABS(p.lat)>PI*0.5 THEN EXIT END;

INC(posfixc);
    o:=check(p);
    IF o<best THEN
      best:=o;
      dir:=i;
      p1:=p;
      IF best<resol THEN EXIT END;    (* save cpu *)

    ELSE
      i:=(i+1) MOD DIRS;
      IF i=dir THEN EXIT END;         (* all directions make it worse *)

    END;
  END;
  RETURN best
END aquisith;


----- dilutations for nmea output

PROCEDURE calcdil(up-:POS; us, satc:CARDINAL);
CONST CHECKSTEP=100.0; 
      HCHECK=CHECKSTEP*PI2/40000000.0;
VAR i:CARDINAL;
    x,y,z:LONGREAL;
    p:POSL;
    e:REAL;
BEGIN
  nmeahdil:=0.0;
  nmeavdil:=0.0;
  IF satc>=4 THEN
    p.lat:=up.lat;
    p.long:=up.long;
    p.alt:=up.alt;
    wgs84(p.lat, p.long, p.alt, x,y,z);
    i:=us;
    WHILE i<=HIGH(sat) DO
      WITH sat[i] DO range:=dist(satpos.x, satpos.y, satpos.z, x, y, z) END;
      i:=sat[i].next;
    END;
    p.alt:=p.alt+CHECKSTEP;
    IF NOT fixalt THEN nmeavdil:=CHECKSTEP/maxr(aquisith(p, 0.01), 0.1) END;

    p.lat:=up.lat+HCHECK;
    p.long:=up.long;
    p.alt:=up.alt;
    e:=CHECKSTEP/maxr(check(p), 0.001);
--WrStr(" e="); WrFixed(e,2,1); WrStr(" ");
    p.lat:=up.lat;
    p.long:=up.long+HCHECK/maxr(math.cos(up.long), 0.1);
    IF p.long>PI THEN p.long:=p.long-PI2 END;
    nmeahdil:=maxr(CHECKSTEP/maxr(check(p),0.001), e);
  END;
END calcdil;


PROCEDURE finetuneranges(us:CARDINAL; p-:POS; moving:BOOLEAN);
VAR i,j,nearest:CARDINAL;
    x0,y0,z0:LONGREAL;
    m, n:LONGREAL;
BEGIN
  wgs84(p.lat, p.long, p.alt, x0, y0, z0);
  m:=0.0;
  j:=0;
  i:=us;
  n:=MAX(REAL);
  WHILE i<=HIGH(sat) DO
    WITH sat[i] DO
      mydist:=dist(satpos.x, satpos.y, satpos.z, x0, y0, z0) - rangec;
      m:=m+mydist;
      IF rangec<n THEN n:=rangec; nearest:=i END;  
      i:=next;
    END;
    INC(j);
  END;
  IF j>=4 THEN
    m:=m/LFLOAT(j);
    i:=us;
IF verb THEN WrStr("corr:") END;
    WHILE i<=HIGH(sat) DO
      WITH sat[i] DO
--        corrone:=corrone+(mydist-m-corrone)*0.1;
        corrone:=corrone*0.95;
        IF NOT moving THEN
           corrone:=corrone+(mydist-m)*0.045;
           IF corrone>30.0 THEN corrone:=30.0 ELSIF corrone<-30.0 THEN corrone:=-30.0 END;
        END;
--        IF i=nearest THEN corrone:=0.0 END;
IF verb THEN WrInt(i+1,1); WrStr(":"); WrFixed(corrone,1,1); WrStr(" "); END;
        i:=next;
      END;
    END;
IF verb THEN WrStrLn("") END;
  END;
END finetuneranges;


PROCEDURE checkfix(VAR po:POSL):REAL;
CONST CHECKSTEP=200.0;
VAR e0, e1, e2, dir:REAL;
    ph:POSL;
BEGIN
  dir:=CHECKSTEP*0.05;
  LOOP
    ph:=po;
    e0:=aquisith(ph, 0.01);
    ph:=po;
    ph.alt:=ph.alt+CHECKSTEP;
    e1:=aquisith(ph, 0.01);
    ph:=po;
    ph.alt:=ph.alt-CHECKSTEP;
    e2:=aquisith(ph, 0.01);
IF verb2 THEN
WrStr(" cf:[");WrFixed(po.lat*(1.0/RAD),5,1); WrStr(","); WrFixed(po.long*(1.0/RAD),5,1); WrStr(" "); WrFixed(po.alt,1,1);
WrStr(" ret="); WrFixed(maxr(e0, CHECKSTEP/maxr(0.01, e1+e2-e0*2.0)),1,1);
WrStr(" e0="); WrFixed(e0,2,1); WrStr(" e1="); WrFixed(e1,2,1); WrStr(" e2="); WrFixed(e2,2,1); WrStr(" dir="); WrFixed(dir,2,1);  WrStr("] ");
END;
    IF (ABS(dir)<0.25) OR (ABS(e1-e2)<0.25) THEN EXIT END;

    IF (dir<0.0) & (po.alt<MINALT) OR (dir>=0.0) & (po.alt>maxalt) OR ((e1>e2) = (dir>=0.0)) THEN dir:=dir*(-0.5) END;
    ph:=po;
    e0:=aquisith(po, 0.01);
    po.alt:=ph.alt+dir;
  END;
  RETURN maxr(e0, CHECKSTEP/maxr(0.01, e1+e2-e0*2.0))
END checkfix;


---- make position and speed by try and error, makes more stable result as equation resolv

PROCEDURE d23fix(medcnt:CARDINAL; VAR pstart:POS; VAR offs:REAL; VAR us, satc:CARDINAL; d3ok:BOOLEAN);
VAR med:LONGREAL;
    i:CARDINAL;
    p:POSL;
    pl:POS;
--    dd:SET32;
BEGIN
  med:=(LIGHT/SAMPRATE*PRNFINETUNE)/VAL(LONGREAL,medcnt);
--  dd:=SET32{};
  satc:=0;
  us:=MAX(CARDINAL);
  FOR i:=0 TO HIGH(sat) DO
    WITH sat[i] DO
      IF insyn & datasyn & satok & (bert=0) & (pmcnt=medcnt) THEN
        rangec:=prangem*med + satpos.clkcorr  (*  + corrone*);
        range:=rangec    + corrone;
        next:=us;
        us:=i;
--        INCL(dd, i);
        INC(satc);
      END;
    END;
  END;

  calcdop(us, pstart);
  p.lat :=pstart.lat;
  p.long:=pstart.long;
  p.alt :=pstart.alt;

  IF satc>=4-ORD(fixalt OR NOT noauto23d & d3ok) THEN   (* if we have altitude let it as it is and make pos with 3 sat *) 
    IF p.alt<=ERRALT THEN 
      IF (p.lat=0.0) & (p.long=0.0) THEN guesspos(p) END;
      p.alt:=0.0;                 (* start alt from zero *)
    END;
    IF satc>=4 THEN
      IF fixalt THEN offs:=aquisith(p, 0.01) ELSE offs:=checkfix(p) END;
      last3dfix:=(offs<=MINREM);
    ELSIF last3dfix OR fixalt THEN offs:=aquisith(p, 0.01);
    ELSE offs:=-1.0 END;
    pstart.lat :=p.lat;
    pstart.long:=p.long;
    pstart.alt :=p.alt;
  ELSE offs:=-1.0 END;
END d23fix;


PROCEDURE speeds(satc, us:CARDINAL; VAR pstart:POS);
VAR p, ps:POSL;
    i:CARDINAL;
    pl:POS;
    x0, y0, z0, e, best, dir, ms, msmax, msmin:LONGREAL;
BEGIN
  IF satc>=4-ORD(fixalt OR NOT noauto23d) THEN
    p.lat :=pstart.lat;
    p.long:=pstart.long;
    p.alt :=pstart.alt;
    wgs84(p.lat, p.long, p.alt, x0,y0,z0);
    msmin:=MAX(REAL);
    msmax:=MIN(REAL);
    i:=us;
--WrStr(" spd:");
    WHILE i<=HIGH(sat) DO
      WITH sat[i] DO 
        ms:=(LIGHT/FREQ)*mfreq-satpos.doppler;                             (* user speed doppler direction to sat in m/s *)
        range:=dist(satpos.x, satpos.y, satpos.z, x0, y0, z0) + ms;
        ms:=ABS(ms);
--WrFixed(ms,1,1); WrStr(",");
        IF ms>msmax THEN msmax:=ms END;
        IF ms<msmin THEN msmin:=ms END;
      END;
      i:=sat[i].next;
    END;
    IF satc>=4 THEN dir:=(msmax-msmin+5.0)*4.0 ELSE dir:=0.0 END;;
IF verb2 THEN WrStrLn("speeds") END;

    best:=MAX(REAL);
    LOOP
      ps:=p;
      ps.alt:=ps.alt+dir;
      e:=aquisith(ps, 0.01);
      IF e<best THEN
        IF e<best THEN best:=e END;
        p:=ps;
      ELSE dir:=dir*(-0.5) END;
--WrFixed(e,2,1); WrStr("/");WrFixed(best,2,1); WrStr("/"); WrFixed(dir,2,1); WrStr(" ");
      IF (best<0.01) OR (ABS(dir)<0.01) THEN EXIT END;

    END;
--WrStr(" spd:"); WrFixed(best,3,1); WrStr(" "); 
    pl.lat :=p.lat;
    pl.long:=p.long;
    pstart.speed:=shortdist(pstart, pl);        (* fixed position - shifted by doppler *)
    pstart.dir:=shortdir(pstart, pl);
    IF pstart.dir<0.0 THEN pstart.dir:=pstart.dir+PI2 END;
    IF NOT fixalt & (satc>=4) THEN pstart.clb:=pstart.alt-p.alt ELSE pstart.clb:=0.0 END;
    finetuneranges(us, pstart, pstart.speed>0.5);
    calcdil(pstart, us, satc);
  END;
END speeds;

---------2 or 3d fix

PROCEDURE noisefilter(VAR p:POS; timef:LONGREAL);
VAR dt,direrr,lowpass, altstrength, posstrength, spd:LONGREAL;
BEGIN
  posstrength:=1.0-1.0/kaldat.strength;
  altstrength:=1.0-2.0/kaldat.strength;
  dt:=timef-kaldat.time;
  spd:=p.speed;
  IF spd>10.0*kaldat.pos.speed THEN spd:=kaldat.pos.speed END;
  IF dt>5.0 THEN dt:=5.0 END;
  IF (kaldat.time>0.0) & (dt>0.1) THEN
    direrr:=angdiff(p.dir, shortdir(p, kaldat.pos));   (* pos move dir - doppler dir *)
    lowpass:=posstrength/(sqr(spd)*0.1/(sqr(direrr)*50.0+1.0)+1.0);
    p.lat :=p.lat *(1.0-lowpass) + kaldat.pos.lat *lowpass;
    p.long:=p.long*(1.0-lowpass) + kaldat.pos.long*lowpass;
IF verb THEN
WrStr("kal dt=");WrFixed(dt,1,1);
WrStr(" lp=");WrFixed(lowpass,3,1);
END;

    lowpass:=ABS((p.alt-kaldat.pos.alt)-p.clb*dt)/(ABS(p.clb*dt)+1.0);
    IF lowpass>altstrength THEN lowpass:=altstrength END;
    p.alt :=p.alt*(1.0-lowpass) + (kaldat.pos.alt+p.clb*dt)*lowpass;

IF verb THEN
WrStr(" alt=");WrFixed(p.alt,1,1);
WrStr(" alto=");WrFixed(kaldat.pos.alt,1,1);
WrStr(" clb=");WrFixed(p.clb,3,1);
WrStr(" lpa=");WrFixed(lowpass,3,1);
WrStrLn("");
END;
  ELSIF verb THEN WrStr("timejump="); WrFixed(dt,1,1); WrStrLn("");
  END;
  kaldat.pos:=p;
  p.speed:=spd;
  kaldat.time:=timef;
END noisefilter;

PROCEDURE initpos;
BEGIN
  FILL(ADR(userpos), 0C, SIZE(userpos));
  d3wasok:=FALSE;
  posok:=FALSE;
  falselocks:=0;
  userpos.alt:=ERRALT;
END initpos;


PROCEDURE satposspeeds(VAR timef:LONGREAL; upos-:POS);
VAR sn, wnn, timec, satc:CARDINAL;
    myx, myy, myz, tow1:LONGREAL;
    sp:SATPOS;
    satwgs:POS;
BEGIN
  IF (upos.lat<>0.0) OR (upos.long<>0.0) THEN
    wgs84(upos.lat, upos.long, upos.alt, myx, myy, myz);
  ELSE myx:=0.0; myy:=0.0; myz:=0.0 END;

  FOR sn:=0 TO HIGH(sat) DO
    IF sat[sn].insyn & sat[sn].datasyn THEN
      FILL(ADR(sp), 0C, SIZE(sp));

      IF sat[sn].freqmedc>0 THEN
        sat[sn].mfreq:=sat[sn].freqmed/VAL(REAL, sat[sn].freqmedc);
        sat[sn].freqmed:=0.0;
        sat[sn].freqmedc:=0;
      END; 
      WITH almanach.ephem[sn] DO

        timef:=VAL(LONGREAL, wncorr*SECONDS_IN_WEEK + tow*6)
             +(VAL(LONGREAL, sampc-sat[sn].timesamp)
               +VAL(LONGREAL, sat[sn].timesampfrac)*(1.0/IFIRFINE))*(1.0/SAMPRATE)
             -VAL(LONGREAL,medcnt)*0.01;                 (* shift time to middle of prn rounding intervall *)

        wnn:=VAL(CARDINAL, timef*(1.0/SECONDS_IN_WEEK));
        tow1:=timef-VAL(LONGREAL, wnn*SECONDS_IN_WEEK);
        wnn:=wnn MOD 1024;
        timec:=VAL(CARDINAL, timef);

        IF (done>=SET8{1,2,3}) & (healthignore OR (health=0)) & (ura<15)
        & (wncorr*SECONDS_IN_WEEK + tow*6<=twnwrap(wncorr,tow,toe)) THEN

          SatPos(
          myx, myy, myz, wnn MOD 1024, tow1, wn,
          toe, toc, af0a, af1a, af2a, tgd, m0*PI, deltan*PI, ecc, roota, omega0*PI, i0*PI, w*PI, omegadot*PI, idot*PI,
          cuc, cus, crc, crs, cic, cis,
          sp.clkcorr, sp.clkdrift, sp.x, sp.y, sp.z, sp.vx, sp.vy, sp.vz,
          sp.azimuth, sp.elevation, sp.doppler);
          sp.azel:=posok;
          sat[sn].satpos:=sp; 
          sat[sn].satok:=TRUE;

IF verb THEN 
  wgs84r(sp.x,sp.y,sp.z,satwgs.lat,satwgs.long,satwgs.alt);
  WrStr("prn:"); WrInt(sn+1,2);
  WrStr(" ");WrDate(timec DIV SECONDS_IN_WEEK, timec MOD SECONDS_IN_WEEK);
  WrStr(" po:"); WrFixed(satwgs.lat/RAD, 6,1); WrStr("/");WrFixed(satwgs.long/RAD, 6,1); WrStr("/");WrFixed(satwgs.alt*0.001, 1,1);       
  IF posok THEN
    WrStr(" az:"); WrFixed(sp.azimuth/RAD,4,1);
    WrStr(" el:"); WrFixed(sp.elevation/RAD,4,1);
    WrStr(" d:"); WrFixed(dist(myx,myy,myz,sp.x,sp.y,sp.z),0,1);
    WrStr(" dop:"); WrFixed(sp.doppler*(FREQ/LIGHT),1,1);WrStr("("); WrFixed(sat[sn].mfreq - sp.doppler*(FREQ/LIGHT),1,1); WrStr(") ");
  END;
  WrStr(" pd:"); WrFixed(sat[sn].prndoppler*(FREQ/2046.0/IFIRFINE),1,1);
  WrStr(" +- "); WrInt(sat[sn].plusc,1); WrStr(" "); WrInt(sat[sn].minusc,1);
  sat[sn].minusc:=0; sat[sn].plusc:=0;
  WrStr(" sig:"); WrFixed(sat[sn].sig,0,1);
  WrStr(" pn:"); WrFixed(sat[sn].prnnois,2,1);
--    WrStr(" mp:"); WrFixed(sat[sn].multipath,2,1);
  WrStr(" ber:"); WrInt(sat[sn].bert,1);
  WrStrLn("");
END;

        ELSE sat[sn].satok:=FALSE END;

      END;
    ELSE sat[sn].satok:=FALSE END;    
  END;
END satposspeeds;


---- set of pseudoranges ready to make position and speeds

PROCEDURE navigate;
VAR satc, utime, usedsats:CARDINAL;
    timef:LONGREAL;
    d2ok:REAL;
    eok:BOOLEAN;
BEGIN
  medranges;
  INC(medcnt);

  IF medcnt>=nmeaintervall THEN              (* we want a beacon *)

IF verb THEN 
  WrInt(medcnt,1); WrStr(" "); WrInt((sampc-sampnavi) DIV (SAMPRATE DIV 50),1); WrStrLn("--nav"); 
END;

    d2ok:=-1.0;
    sampnavi:=sampc;
    IF fixall THEN userpos:=fixpos; posok:=TRUE ELSIF fixalt THEN userpos.alt:=fixpos.alt END; 
    satposspeeds(timef, userpos);
    utime:=VAL(CARDINAL, timef);

    d23fix(medcnt, userpos, d2ok, usedsats, satc, d3wasok);
    IF d2ok>=0.0 THEN
      IF NOT fixall THEN
        IF NOT posok THEN satposspeeds(timef, userpos) END;   (* doppler to user known if latest user pos known *)
        speeds(satc, usedsats, userpos);
      END;
      IF d2ok<MINREM THEN d3wasok:=TRUE; posok:=TRUE; falselocks:=0;
      ELSIF d2ok<100000.0 THEN falselocks:=0;
      ELSE
        INC(falselocks);
        IF falselocks>10 THEN initpos END; (* get no fix, try new guesspos *)
      END;
      IF (kaldat.strength>0.0) & d3wasok & (d2ok<MAXREM) THEN noisefilter(userpos, timef) END;
      IF verb THEN
IF NOT d3wasok OR (d2ok>=MAXREM) THEN INC(nofixc); WrStr("no good fix now ") END;
        IF satc<4 THEN  WrStr("2dfix ") END;
        WrStr(" d23:[res:"); WrFixed(d2ok,1,1); WrStr(" ");WrFixed(userpos.lat*(1.0/RAD),5,1); WrStr(","); WrFixed(userpos.long*(1.0/RAD),5,1); WrStr(","); WrFixed(userpos.alt,1,1); WrStrLn("]");
errsum:=errsum+d2ok;
      END;
    ELSE
      posok:=FALSE;
      IF verb THEN INC(nofixc); WrStr("too less sync sat now ") END;
    END;

IF verb THEN 
  WrStr(" u:[");WrFixed(userpos.lat*(1.0/RAD),5,1); WrStr(","); WrFixed(userpos.long*(1.0/RAD),5,1); WrStr(",");
  WrFixed(userpos.alt,0,1); WrStr(","); WrFixed(userpos.speed*3.6,2,1); WrStr("km/h,");
  WrFixed(userpos.dir*(1.0/RAD),1,1); WrStr("deg,"); WrFixed(userpos.clb,2,1); WrStr("m/s");  WrStr("]");
  WrStr(" resum:"); WrFixed(errsum,1,1);
  WrStr(" posfixc:"); WrInt(posfixc,1);
  WrStr(" nofixc:"); WrInt(nofixc,1);
(*
  IF manpos THEN
    WrStr(" medd:"); WrFixed(meddist,1,1);
    WrStr(" meda:"); WrFixed(medalt,1,1);
  END;
*)
  WrStrLn("");
END;

    IF (d2ok>=0.0) & (egmfn<>"") THEN
      egmmeter:=egm96(userpos.lat, userpos.long, eok);
      IF NOT eok THEN WrStrLn(" egm file fail"); egmfn:="" END;
    END;
    nmea(utime+zerotime-VAL(CARDINAL,almanach.tls), userpos, posok & d3wasok & (d2ok<MAXREM), noauto23d, satc, sqrt(sqr(nmeahdil)+sqr(nmeavdil)),nmeahdil,nmeavdil, egmmeter);

    medcnt:=0;
  END;
END navigate;

---------- navigation


----------------- almanach


PROCEDURE ctolf(c:CARDINAL; bits:INTEGER; scale:LONGREAL):LONGREAL;
VAR r:LONGREAL;
BEGIN
  r:=VAL(LONGREAL, c);
  IF (bits<0) & (bits>=-32) & (ABS(bits)-1 IN CAST(SET32,c)) THEN
    r:=VAL(LONGREAL, VAL(INTEGER,CAST(SET32,c)+SHIFT(SET32{0..31},ABS(bits))));
  END;
  RETURN r*scale;
END ctolf;

(*
PROCEDURE ctolf(c:CARDINAL; bits:INTEGER; scale:INTEGER):LONGREAL;
BEGIN
  IF (bits<0) & (bits>-32) & (ABS(bits)-1 IN CAST(SET32,c)) THEN
    RETURN VAL(LONGREAL, VAL(INTEGER,CAST(SET32,c)+SHIFT(SET32{0..31},ABS(bits))))*(2.0**scale)
  END;
--WrStr(" hs=");WrFixed(VAL(LONGREAL, c)*(2.0**scale),20,1);
  RETURN VAL(LONGREAL, c)*(2.0**scale);
END ctolf;
*)


PROCEDURE btoc(b-:ARRAY OF SET32; start, bits:CARDINAL):CARDINAL;
--VAR s:INTEGER;
BEGIN RETURN CAST(CARDINAL, SHIFT(b[start DIV 30],VAL(INTEGER, bits+start MOD 30)-30)*SHIFT(SET32{0..31},VAL(INTEGER,bits)-32)) END btoc;

PROCEDURE btoc32(b-:ARRAY OF SET32; start:CARDINAL):CARDINAL;
BEGIN RETURN ASH(btoc(b,start,8),24) + btoc(b,start+14,24) END btoc32;

PROCEDURE btoc24(b-:ARRAY OF SET32; start:CARDINAL):CARDINAL;
BEGIN RETURN btoc(b,start,24) END btoc24;

PROCEDURE btoc16(b-:ARRAY OF SET32; start:CARDINAL):CARDINAL;
BEGIN RETURN btoc(b,start,16) END btoc16;

(*
PROCEDURE modwn(w10,w8:CARDINAL):CARDINAL;
VAR r:CARDINAL;
    d:INTEGER;
BEGIN
  r:=w8+w10 DIV 256*256;
  d:=VAL(INTEGER,r)-VAL(INTEGER,w10);
  IF d>127 THEN r:=(r+(1024-256)) MOD 1024;
  ELSIF d<-127 THEN r:=(r+256) MOD 1024;
  END;
  RETURN r
END modwn;
*)


----- write ephemerides in rinex format file

PROCEDURE rinexfile;
CONST LF=12C;
VAR i:CARDINAL;
    f:INTEGER;
    h,hh:ARRAY[0..1999] OF CHAR;
    new:BOOLEAN;


BEGIN
  IF rinexfn[0]=0C THEN RETURN END;

  FOR i:=0 TO HIGH(almanach.ephem) DO
    f:=-1;
    WITH almanach.ephem[i] DO
      IF (done>=SET8{1,2,3}) & (IODE2=IODE3) & (IODE2=IODC MOD 256) & almanach.page18ok
      & (almanach.printed[i]<>IODE3+32768) THEN
        almanach.printed[i]:=IODE3+32768;
        new:=FALSE;

        IF (f<0) & (rinexfn[0]<>0C) THEN
          f:=OpenAppend(rinexfn);
          IF f<0 THEN
            f:=OpenWrite(rinexfn);
            new:=TRUE;
          END;
        END;
        IF f<0 THEN WrStr(hh); WrStrLn(" almanach file wite error"); RETURN END;
        h:="";
--header
        IF new THEN
          Append(h, "     4.02           N: GNSS NAV DATA    M:                  RINEX VERSION / TYPE"+NL);
          Append(h, "GPSSDR V0.1         DXL                 ");
          GpsDate(wn, tow*6,hh);
          hh[4]:=hh[5];  hh[5]:=hh[6]; hh[6]:=hh[8]; hh[7]:=hh[9];
          hh[8]:=" ";  hh[9]:=hh[11]; hh[10]:=hh[12]; hh[11]:=hh[14]; hh[12]:=hh[15]; hh[13]:=hh[17]; hh[14]:=hh[18];
          hh[15]:=0C;  Append(h,hh);  
          Append(h, " UTC PGM / RUN BY / DATE"+NL);
--          Append(h, "GPS EPHEMERIDES TO RINEX"+NL);
          Append(h, "    ");
          rinexint(almanach.tls, 6, hh); Append(h,hh);
          rinexint(almanach.tlsf, 4, hh); Append(h,hh);
          rinexint(almanach.wnlsf, 9, hh); Append(h,hh);
          rinexint(tow/(86400/6), 1, hh); Append(h,hh);
          Append(h, "                                    LEAP SECONDS"+NL);
          Append(h, "                                                            END OF HEADER"+NL);
        END;
--line 0
        Append(h, "> EPH G");
        hh[0]:=CHR((i+1) DIV 10+ORD("0"));
        hh[1]:=CHR((i+1) MOD 10+ORD("0")); hh[2]:=0C;
        Append(h,hh); Append(h, " LNAV"+NL);
--line 1
        Append(h, "G");Append(h,hh); Append(h, " ");
        GpsDate(wn, tow*6, hh);
        hh[4]:=" "; hh[7]:=" "; hh[13]:=" "; hh[16]:=" ";  Append(h,hh);
        rinexfloat(af0a, hh); Append(h,hh);
        rinexfloat(af1a, hh); Append(h,hh);
        rinexfloat(af2a, hh); Append(h,hh); Append(h, NL);
---line 2
        Append(h,"    ");
        rinexfloat(LFLOAT(IODE2), hh); Append(h,hh);
        rinexfloat(crs, hh); Append(h,hh);
        rinexfloat(deltan*PI, hh); Append(h,hh);
        rinexfloat(m0*PI, hh); Append(h,hh); Append(h, NL);
---line 3
        Append(h,"    ");
        rinexfloat(cuc, hh); Append(h,hh);
        rinexfloat(ecc, hh); Append(h,hh);
        rinexfloat(cus, hh); Append(h,hh);
        rinexfloat(roota, hh); Append(h,hh); Append(h, NL);
---line 4
        Append(h,"    ");
        rinexfloat(LFLOAT(toe), hh); Append(h,hh);
        rinexfloat(cic, hh); Append(h,hh);
        rinexfloat(omega0*PI, hh); Append(h,hh);
        rinexfloat(cis, hh); Append(h,hh); Append(h, NL);
---line 5
        Append(h,"    ");
        rinexfloat(i0*PI, hh); Append(h,hh);
        rinexfloat(crc, hh); Append(h,hh);
        rinexfloat(w*PI, hh); Append(h,hh);
        rinexfloat(omegadot*PI, hh); Append(h,hh); Append(h, NL);
---line 6
        Append(h,"    ");
        rinexfloat(idot*PI, hh); Append(h,hh);
        rinexfloat(0.0, hh); Append(h,hh);
        rinexfloat(LFLOAT(almanach.wnt), hh); Append(h,hh);
        rinexfloat(0.0, hh); Append(h,hh); Append(h, NL);
---line 7
        Append(h,"    ");
        rinexfloat(urai(ura), hh); Append(h,hh);
        rinexfloat(LFLOAT(health), hh); Append(h,hh); 
        rinexfloat(tgd, hh); Append(h,hh);
        rinexfloat(LFLOAT(IODC), hh); Append(h,hh); Append(h, NL);
---line 8
        Append(h,"    ");
        rinexfloat(LFLOAT(tow*6), hh); Append(h,hh); Append(h, NL);

      END;
    END; 

    IF f>=0 THEN WrBin(f, h, Length(h)); Close(f) END;
  END;
END rinexfile;


<* IF WITHALM THEN *>


------ write ephemerides and almanach in a file

PROCEDURE fileephemeris(eph:BOOLEAN);
CONST LF=12C;
VAR i:CARDINAL;
    f:INTEGER;
    h,hh:ARRAY[0..1999] OF CHAR;

  PROCEDURE openf(n:CARDINAL):BOOLEAN;
  BEGIN
    IF (f<0) & (almfn[0]<>0C) THEN
      IntToStr(n+1,2,h);
      IF h[0]=" " THEN h[0]:="0" END;
      Assign(hh, almfn);
      Append(hh,h);
      f:=OpenAppend(hh);
      IF f<0 THEN f:=OpenWrite(hh) END;
    END;
    RETURN f>=0
  END openf;


BEGIN
  IF almfn[0]=0C THEN RETURN END;

  FOR i:=0 TO HIGH(almanach.alms) DO
    f:=-1;
  
    IF eph THEN
      WITH almanach.ephem[i] DO
        IF (done>=SET8{1,2,3}) & (IODE2=IODE3) & (IODE2=IODC MOD 256) & (almanach.printed[i]<>IODE3+32768) THEN
          IF NOT openf(i) THEN WrStr(hh); WrStrLn(" almanach file wite error"); RETURN END;

          almanach.printed[i]:=IODE3+32768;
          h:="Ephemeris sat:";IntToStr(i+1,1,hh); Append(h,hh); Append(h, LF);
          Append(h, "health:"); IntToStr(health, 1,hh); Append(h,hh); Append(h, LF); 
          Append(h, "ura   :"); IntToStr(ura, 1,hh); Append(h,hh);Append(h, LF);
          Append(h, "toc   :"); IntToStr(toc, 1,hh); Append(h,hh); 
          Append(h, " ("); GpsDatew(twnwrap(wn,tow,toc),hh); Append(h,hh); Append(h, ")"+LF);
          Append(h, "iodc  :"); IntToStr(IODC,1,hh); Append(h,hh); Append(h, LF);
          Append(h, "aodo  :"); IntToStr(AODO,1,hh); Append(h,hh); Append(h, LF);
          Append(h, "toe   :"); IntToStr(toe, 1,hh); Append(h,hh);
          Append(h, " ("); GpsDatew(twnwrap(wn,tow,toe),hh); Append(h,hh); Append(h, ")"+LF);
          Append(h, "iode2 :"); IntToStr(IODE2,1,hh); Append(h,hh); Append(h, LF);
          Append(h, "iode3 :"); IntToStr(IODE3,1,hh); Append(h,hh); Append(h, LF);
          Append(h, "tgd   :"); FixToStr(tgd,15,hh); Append(h,hh); Append(h, LF);
          Append(h, "af0a  :"); FixToStr(af0a,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "af1a  :"); FixToStr(af1a,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "af2a  :"); FixToStr(af2a,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "m0    :"); FixToStr(m0,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "deltan:"); FixToStr(deltan,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "ecc   :"); FixToStr(ecc,15,hh); Append(h,hh); IF (ecc<0.0) OR (ecc>0.03) THEN Append(h, " out of range") END; Append(h, LF);
          Append(h, "roota :"); FixToStr(roota,15,hh);Append(h,hh);  IF (roota<2530.0) OR (roota>8192.0) THEN Append(h, " out of range") END;Append(h, LF);
          Append(h, "omega0:"); FixToStr(omega0,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "i0    :"); FixToStr(i0,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "omega :"); FixToStr(w,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "omega.:"); FixToStr(omegadot,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "idot  :"); FixToStr(idot,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "cuc   :"); FixToStr(cuc,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "cus   :"); FixToStr(cus,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "crc   :"); FixToStr(crc,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "crs   :"); FixToStr(crs,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "cic   :"); FixToStr(cic,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "cis   :"); FixToStr(cis,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, LF);
        END;
      END; 

    ELSE
      WITH almanach.alms[i] DO
        IF (done & (lasttoa<>fromtoa+1)) THEN
          IF NOT openf(i) THEN WrStr(hh); WrStrLn(" almanach file wite error"); RETURN END;

          lasttoa:=fromtoa+1;          
          h:="Almanach sat:";
          IntToStr(i+1,1,hh); Append(h,hh);
          Append(h, " (from:"); IntToStr(fromsat+1, 1,hh); Append(h,hh);Append(h, ")"+LF);
          Append(h, "wna   :"); IntToStr(wna, 1,hh); Append(h,hh); Append(h, LF);
          Append(h, "toa   :"); IntToStr(toa, 1,hh); Append(h,hh); Append(h, LF);
          Append(h, "fwna  :"); IntToStr(fromwna, 1,hh); Append(h,hh); Append(h, LF);
          Append(h, "ftoa  :"); IntToStr(fromtoa, 1,hh); Append(h,hh);
          Append(h, " ("); GpsDatew(modwn(wna,fromwna)*SECONDS_IN_WEEK + fromtoa,hh); Append(h,hh); Append(h, ")"+LF);
          Append(h, "helth :"); IntToStr(helth,1,hh); Append(h,hh); Append(h, LF);
          Append(h, "af0   :"); FixToStr(af0,15,hh); Append(h,hh); Append(h, LF);
          Append(h, "af1   :"); FixToStr(af1,15,hh); Append(h,hh); Append(h, LF);
          Append(h, "m0    :"); FixToStr(m0/PI,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "ecc   :"); FixToStr(ecc,15,hh); Append(h,hh); IF (ecc<0.0) OR (ecc>0.03) THEN Append(h, " out of range") END; Append(h, LF);
          Append(h, "roota :"); FixToStr(roota,15,hh);Append(h,hh);  IF (roota<2530.0) OR (roota>8192.0) THEN Append(h, " out of range") END;Append(h, LF);
          Append(h, "omega0:"); FixToStr(omega0/PI,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "w     :"); FixToStr(w/PI,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "omega.:"); FixToStr(omegadot,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, "i0    :"); FixToStr(i0/PI,15,hh);Append(h,hh);  Append(h, LF);
          Append(h, LF);
        END;
      END;
    END;
    IF f>=0 THEN WrBin(f, h, Length(h)); Close(f) END;
  END;
END fileephemeris;


PROCEDURE stoalm(from, tosat:CARDINAL; frame-:FRP300);
VAR i:CARDINAL;
BEGIN

WrStr(" sto alm=");WrInt(tosat+1,1); WrStr(" ");
  IF btoc(frame,90,8)=almanach.alms[from].toa THEN
    WITH almanach.alms[tosat] DO
      fromsat :=from;
      helth   :=btoc(frame,136,8);
      fromtoa :=almanach.alms[from].toa;
      fromwna :=almanach.alms[from].wna;
--      toa     :=btoc(frame,90,8)*4096;
      af0     :=ctolf(btoc(frame,289,3)+btoc(frame,270,8)*8, -11, TWOPOWERMINUS20);   (* seconds *)
      af1     :=ctolf(btoc(frame,278,11), -11, TWOPOWERMINUS38);                       (* s/s *)
      m0      :=ctolf(btoc24(frame,240), -24, TWOPOWERMINUS23)*PI;    (* Mean Anomaly at Reference Time *)
      w       :=ctolf(btoc24(frame,210), -24, TWOPOWERMINUS23)*PI;    (* Argument of Perigee *) 
      omega0  :=ctolf(btoc24(frame,180), -24, TWOPOWERMINUS23)*PI;    (* Longitude of Ascending Node of Orbit Plane at Weekly Epoch *) 
      roota   :=ctolf(btoc24(frame,150), 24, TWOPOWERMINUS11);        (* Square Root of the Semi-Major Axis *) 
      omegadot:=ctolf(btoc16(frame,120), -16, TWOPOWERMINUS38)*PI;    (* Rate of Right Ascension *) 
      i0      :=(ctolf(btoc16(frame,98), -16, TWOPOWERMINUS19)+0.3)*PI;  (* delta i + 0.3 *) 
      ecc     :=ctolf(btoc16(frame,68), 16, TWOPOWERMINUS21);         (* Excentricity *)
      done:=TRUE;
 
    END;

    fileephemeris(FALSE);
  END;
END stoalm;

<* END *>

PROCEDURE warmbootfile(wr:BOOLEAN);
VAR res,fd:INTEGER;
BEGIN
  IF warmbootfn<>"" THEN
    IF wr THEN fd:=OpenWrite(warmbootfn) ELSE fd:=OpenRead(warmbootfn) END;
    IF fd>=0 THEN
      IF wr THEN WrBin(fd, almanach, SIZE(almanach))
      ELSE res:=RdBin(fd, almanach, SIZE(almanach)) END;
      Close(fd);
    END;
  END;
END warmbootfile;


----- 300 bit frame ready

PROCEDURE frame300(s:CARDINAL; frame-:FRP300; ifase:REAL):BOOLEAN;
VAR subframeid, page, i, j:CARDINAL;
    h:ARRAY[0..99] OF CHAR;
    ps:pSATDEMOD;
BEGIN
  almanach.ephtmp[s].tow:=btoc(frame, 30,17);
  almanach.ephem[s].tow:=almanach.ephtmp[s].tow;
  IF almanach.ephtmp[s].tow=0 THEN INC(almanach.ephtmp[s].wncorr) END;
  subframeid:=btoc(frame,30+19,3);
  sat[s].timesamp:=sampc;
  sat[s].timesampfrac:=ifase;

IF verb THEN 
  WrInt(s+1,2);WrStr(":");
  WrDate(almanach.ephtmp[s].wncorr, almanach.ephtmp[s].tow*6);
  WrStr(" ls="); WrInt(almanach.tls,1);
--WrStr(" df="); WrFixed(sat[s].freqmed,1,1);
--  WrStr(" do="); WrFixed(sat[s].prndoppler,4,1);
  WrStr(" sig="); WrFixed(sat[s].sig,1,1);
  WrStr(" subf=");WrInt(subframeid,1); WrStr(" tow=");WrInt(almanach.ephtmp[s].tow,1); WrStr(" ");
END;

  INCL(almanach.ephtmp[s].done, subframeid);
  IF subframeid=1 THEN
    WITH almanach.ephtmp[s] DO
      wn:=btoc(frame,60,10);
      wncorr:=wn;
      IODC:=btoc(frame,210,8)+btoc(frame,82,2)*256;
      af2a:=ctolf(btoc(frame,240,8),-8, TWOPOWERMINUS55);
      af1a:=ctolf(btoc16(frame,248),-16, TWOPOWERMINUS43);    (* 2^-43 *) 
      af0a:=ctolf(btoc(frame,270,22),-22, TWOPOWERMINUS31);
      toc:=btoc16(frame,218)*16;
      tgd:=ctolf(btoc(frame,196,8),-8, TWOPOWERMINUS31);
      health:=btoc(frame,60+16,6);
      ura:=btoc(frame,60+12,4);

IF verb THEN 
  WrStr(" wnc=");WrInt(wncorr,1); WrStr(" ");
  WrStr(" ura=");WrInt(ura,1); WrStr(" ");
  WrStr(" health=");WrInt(btoc(frame,60+16,6),1); WrStr(" ");
END;

    END;  
  ELSIF subframeid=2 THEN
    WITH almanach.ephtmp[s] DO
      AODO:=btoc(frame,286,5);
      toe:=btoc16(frame,270)*16;                              (* Reference Time Ephemeris *)
      roota:=ctolf(btoc32(frame,226), 32, TWOPOWERMINUS19);
      cus:=ctolf(btoc16(frame,210),-16, TWOPOWERMINUS29);     (* Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude *)
      ecc:=ctolf(btoc32(frame,166), 32, TWOPOWERMINUS33);
      cuc:=ctolf(btoc16(frame,150),-16, TWOPOWERMINUS29);     (* Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude *)
      m0:=ctolf(btoc32(frame,106), -32, TWOPOWERMINUS31);
      deltan:=ctolf(btoc16(frame,90), -16, TWOPOWERMINUS43);
      crs:=ctolf(btoc16(frame,68),-16, TWOPOWERMINUS5);       (* Amplitude of the Sine Harmonic Correction Term to the Orbit Radius *)
      IODE2:=btoc(frame,60,8);

--IF verb THEN WrStr(" toe=");WrInt(toe,1); WrStr(" "); WrStr(" "); END;

    END;

  ELSIF subframeid=3 THEN
    WITH almanach.ephtmp[s] DO
      idot:=ctolf(btoc(frame,278,14),-14, TWOPOWERMINUS43);
      IODE3:=btoc(frame,270,8);
      omegadot:=ctolf(btoc24(frame,240),-24, TWOPOWERMINUS43);
      w:=ctolf(btoc32(frame,196), -32, TWOPOWERMINUS31);
      crc:=ctolf(btoc16(frame,180),-16, TWOPOWERMINUS5);      (* Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius *)
      i0:=ctolf(btoc32(frame,136),-32, TWOPOWERMINUS31);      (* Inclination Angle at Reference Time *)
      cis:=ctolf(btoc16(frame,120),-16, TWOPOWERMINUS29);     (* Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination *)
      cic:=ctolf(btoc16(frame,60),-16, TWOPOWERMINUS29);      (* Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination *)
      omega0:=ctolf(btoc32(frame,76),-32, TWOPOWERMINUS31);

    END;
<* IF WITHALM THEN *>
    fileephemeris(TRUE);
<* END *>
    rinexfile;
    warmbootfile(TRUE);
    dopdone:=SET32{};

  ELSIF subframeid=4 THEN
    page:=btoc(frame,62,6);

IF verb THEN WrStr(" page=");WrInt(page,1); WrStr(" "); END;

    IF page=56 THEN               (* page=18 *)

      almanach.tls:=VAL(INTEGER,VAL(INT8, VAL(CARD8, btoc(frame,240,8))));
      almanach.a0:=ctolf(btoc32(frame,240),-32, TWOPOWERMINUS30);
      almanach.a1:=ctolf(btoc24(frame,150),-24, TWOPOWERMINUS60);
      almanach.tot:=btoc(frame,218,8)*4096;
      almanach.wnt:=btoc(frame,226,8);
      almanach.wnlsf:=btoc(frame,248,8);
      almanach.tlsf:=VAL(INTEGER,VAL(INT8, VAL(CARD8, btoc(frame,270,8))));  (* signed *)
      almanach.page18ok:=TRUE;

IF verb THEN 
  WrStr(" tls=");WrInt(almanach.tls,1); WrStr(" ");
  WrStr(" tot=");WrInt(almanach.tot,1); WrStr(" ");
  WrStr(" wnt=");WrInt(almanach.wnt,1); WrStr(" ");
  WrStr(" tlsf=");WrInt(almanach.tlsf,1); WrStr(" ");
END;

    ELSIF page=55 THEN  (* page 17(55) special messages *)
      j:=btoc16(frame,68); h[0]:=CHR(j MOD 256); h[1]:=CHR(j DIV 256);
      FOR i:=0 TO 5 DO
        j:=btoc24(frame,90+i*30);
        h[2+i*3]:=CHR(j MOD 256);
        h[3+i*3]:=CHR(j DIV 256 MOD 256);
        h[4+i*3]:=CHR(j DIV 65536);
      END;
      j:=btoc16(frame,270); h[20]:=CHR(j MOD 256); h[21]:=CHR(j DIV 256);

IF verb THEN 
  WrStr("Special Message:");  
  FOR i:=0 TO 21 DO WrStr(SPECIALMESSAGE[ORD(h[i])]) END;
END;

<* IF WITHALM THEN *>
    ELSIF (page>=25) & (page<=32) THEN stoalm(s, page-1, frame); (* almanach 25..32 *)
<* END *>
    END;

  ELSIF subframeid=5 THEN
    page:=btoc(frame,62,6);

IF verb THEN WrStr(" page=");WrInt(btoc(frame,60+2,6),1); WrStr(" "); END;

<* IF WITHALM THEN *>
    IF page=51 THEN 
      almanach.alms[s].toa:=btoc(frame,68,8);
      almanach.alms[s].wna:=btoc(frame,76,8);
    ELSIF (page>=1) & (page<=24) THEN stoalm(s, page-1, frame); (* almanach 1..24 *)
    END;
<* END *>

  ELSE   (* not existing subframe number *)
IF verb THEN WrStr("FALSE LOCK subf="); WrInt(subframeid,1); WrStrLn(""); END;
    RETURN FALSE
  END; 


  WITH almanach.ephtmp[s] DO
    IF (done>=SET8{1,2,3}) & (IODE2=IODE3) & (IODE2=IODC MOD 256) THEN
      almanach.ephem[s]:=almanach.ephtmp[s]
    ELSE

IF verb THEN
  WrStr("---IODE-missmatch ");
  WrInt(IODC MOD 256,1); WrStr(" "); WrInt(IODE3,1); WrStr(" "); WrInt(IODE2,1); WrStr(" ");
END;
 
    END;
  END;

IF verb THEN
  WrStr(" wnc=");WrInt(almanach.ephem[s].wncorr,1); WrStr(" ");

  WrStr(" sq");
  ps:=psat;
  REPEAT
    IF ps^.insync=TRACE THEN
      WrInt(ps^.prn+1,1); WrStr(":");
      WrInt(VAL(INTEGER,ps^.squelch),1); WrStr(" ");
    END;
    ps:=ps^.next;
  UNTIL ps=NIL;
  WrStrLn("");
END;

  RETURN TRUE;

END frame300;
---------------almanach


---- hamming code used for (fast) check of correct frame start and polarity

PROCEDURE hamm(VAR w:SET32; wo:SET32):BOOLEAN;

  PROCEDURE rot(s:SET32; n:INTEGER):SET32;
  BEGIN RETURN SHIFT(s,n)/SHIFT(s,n-32) END rot;

VAR h:SET32;
BEGIN
  w:=w*SET32{0..29}+SHIFT(wo,30);
  IF 30 IN w THEN w:=w/CAST(SET32, 03FFFFFC0H) END;

  h:=(w*CAST(SET32,0FBFFBF00H))
     /(rot(w,1)*CAST(SET32,007FFBF01H)) 
     /(rot(w,2)*CAST(SET32,0FC0F8100H)) 
     /(rot(w,3)*CAST(SET32,0F81FFE02H)) 
     /(rot(w,4)*CAST(SET32,0FC00000EH)) 
     /(rot(w,5)*CAST(SET32,007F00001H)) 
     /(rot(w,6)*CAST(SET32,000003000H));

  RETURN w/(h/rot(h,6)/rot(h,12)/rot(h,18)/rot(h,24))*SET32{0..5}=SET32{};
END hamm;


---- navigation data bits (polarisation unknown) 50bit/s and sample time of sat clock yet

PROCEDURE bit50(s:CARDINAL; b:BOOLEAN; ber:CARDINAL; samper, freq, noiselev, prnn, ifase, level:REAL);
CONST FRAMELEN=30;

VAR i:CARDINAL;
    w,wh:SET32; 
BEGIN
  WITH sat[s] DO
    insyn:=TRUE;
    freqmed:=freqmed+freq;
    INC(freqmedc);
    prndoppler:=samper;
    snr:=noiselev;
    sig:=level;   
    bert:=ber;
    prnnois:=prnn;
--    multipath:=echo;
    prange:=(VAL(LONGREAL, sampc)+VAL(LONGREAL, ifase)*(1.0/IFIRFINE));
    bitc:=fsyn;

<* IF __GEN_C__ THEN *>
<* ELSE *>

    IF debugfn[0]<>0C THEN
---debugfile
--      by:=CHR(48+ORD(b));
IF verb THEN IF b THEN WrStr(":") ELSE WrStr(".") END; END;
(*
      IF testf[s]<0 THEN
        CardToStr(s+1, 1, h);
        Assign(hh, debugfn); Append(hh,h); Append(hh,".txt");
        testf[s]:=OpenWrite(hh);
      END;
      IF testf[s]>=0 THEN WrBin(testf[s], by, 1) END;
*)
    END;
---debugfile
<* END *>

    w:=SHIFT(frame[framep],1)*SET32{0..29};
    IF b THEN INCL(w, 0) END;
    frame[framep]:=w;
    INC(fsyn);
    IF fsyn>=FRAMELEN THEN
-- wbit(s, 0, "?", w);
      IF framep=0 THEN
        wh:=w*SET32{22..29};                 (* preamble mask *)
        IF wh=SET32{22,23,25,29} THEN        (* preamble normal lock *)
          fsyn:=0;
          invers:=FALSE;
IF verb2 THEN wbit(s, 0,"^", frame[0]) END;
        ELSIF wh=SET32{24,26,27,28} THEN     (* preamble invers lock *)
          invers:=TRUE;
          frame[0]:=frame[0]/SET32{0..29};
          fsyn:=0;

IF verb2 THEN wbit(s, 0, "v", frame[0]) END;

        ELSE datasyn:=FALSE END;             (* go on hunting preamble *)
        IF (fsyn=0) & hamm(frame[framep], SET32{}) THEN framep:=1 END;  (* found 300 bit block start *)
      ELSE
        fsyn:=0;
        IF invers THEN frame[framep]:=frame[framep]/SET32{0..29} END;
        IF hamm(frame[framep], frame[framep-1]) THEN

IF verb2 THEN wbit(s, framep, "+", frame[framep]) END;

          INC(framep);
          IF framep>=FRAMES30 THEN
            datasyn:=frame300(s, frame, ifase);

            framep:=0;
          END;
        ELSE
          framep:=0;
          datasyn:=FALSE;
IF verb2 THEN wbit(s, framep, "-", w) END;
        END;
      END;
    END; 


--- find set of sat in same 50bit/s data sample

--WrStr(" p"); WrInt(s+1,1);WrStr(" ");WrInt(bitc,1);WrStr(":");
    IF datasyn (*& (bert=0)*) THEN
      i:=0;
      LOOP
        IF sat[i].datasyn (* & (sat[i].bert=0)*) & (sat[i].bitc<>bitc) THEN EXIT END;  (* not all sat in same 20ms periode *)

        INC(i);
        IF i>HIGH(sat) THEN                     (* todo: bad sat exclusion set no datasyn *)
--WrStr("(n)");
          navigate;
          EXIT

        END; 
      END;
    END;


  END;
END bit50;


PROCEDURE resetsat(prn:CARDINAL);
BEGIN
  sat[prn].insyn:=FALSE;
  sat[prn].datasyn:=FALSE;
  sat[prn].satok:=FALSE;
  sat[prn].fsyn:=0;
  sat[prn].framep:=0;
END resetsat;

----- demodulated 1000bit/s stream bits find center of change every 20 bits

PROCEDURE bit1000(ps:pSATDEMOD; b:REAL);
CONST MED=0.01;
VAR i,m:CARDINAL;
    r,max:REAL;
    chg:BOOLEAN;      
BEGIN
  WITH ps^ DO
    chg:=lastb<>(b>=0);
    lastb:=(b>=0);

    max:=-1.0;
--WrStr(" {");
    FOR i:=0 TO HIGH(chhist) DO
      r:=chhist[i];
--WrFixed(r,3,1); WrStr(" ");
      IF r>max THEN max:=r; m:=i END;
    END;
--WrStrLn("}");

    chhist[histc]:=chhist[histc]*(1.0-MED);      (* where is bit change in the 20 chirps for a databit*)
    IF chg THEN 
      chhist[histc]:=chhist[histc]+MED;
      IF (m<>histc) THEN ber:=20 END;            (* timestamp not sync to median 50baud bit change *)
    END;

    bitsum:=bitsum+b;                            (* sum 20 chip bits to a databit *)
    freqsum:=freqsum+freq;
    INC(freqsumcnt);
    IF m=histc THEN
      IF ber>0 THEN DEC(ber) END;
IF verb3 THEN WrStrLn("") END;
      bit50(ps^.prn, bitsum>=0.0, ber, prndoppler, freqsum/(FREQMUL*VAL(REAL, freqsumcnt)), squelch, prnnoise, ifase, level);
      freqsumcnt:=0;
      freqsum:=0.0; 
      bitsum:=0.0;
    END;
    histc:=(histc+1) MOD (HIGH(chhist)+1);    
  END;
IF verb3 THEN WrInt(ORD(b>=0.0),1) END;
END bit1000;


----- shedule search with unused channels and prefer just lost sats

PROCEDURE nextsat(ps:pSATDEMOD);
VAR o:CARDINAL;
    s:pSATDEMOD;
BEGIN
  o:=scansat;
  LOOP
    s:=psat;
    WHILE (s<>NIL) & (s^.insync<>UNUSED) & (s^.prn<>scansat) DO s:=s^.next END;
    IF (s=NIL) OR (s^.insync=UNUSED) THEN
      ps^.prn:=scansat;
IF verb2 THEN WrStr("start scan prn:"); WrCard(scansat+1,1); WrStrLn(""); END;
      EXIT

    END;
    scansat:=(scansat+1) MOD 32;
    IF o=scansat THEN EXIT END;

  END;
END nextsat;


----- find sats by random sliding spread code and then fft peak search in 500hz steps

PROCEDURE search(ps:pSATDEMOD; VAR b:ARRAY OF Complex; ii,iq:REAL):BOOLEAN;
VAR i:CARDINAL;
    d,max,e:REAL;
    m:INTEGER;
BEGIN
  WITH ps^ DO
    IF GOLD[prn][goldp] THEN
      b[wp].Re:=-ii;
      b[wp].Im:=-iq;
    ELSE
      b[wp].Re:=ii;
      b[wp].Im:=iq;
    END;
    INC(goldp);
    IF goldp>HIGH(GOLD[0]) THEN goldp:=1 END;
    INC(wp);
    IF wp<=HIGH(b) THEN RETURN FALSE END;  

    wp:=0;
    Transform(b);
    max:=-1.0;
    e:=0.0;

    FOR i:=0 TO HIGH(b) DO          (* find peak in spectrum search range *)
      IF (i<=searchkhz) OR (i>HIGH(b)-searchkhz) THEN 
        d:=CABSQ(b[i].Re, b[i].Re);
        e:=e+d;
        IF d>max THEN max:=d; m:=i END;
      END;
    END;
    IF max>e*peakthres THEN                    (* optimize to find all sats in fft *)
      peakthres:=max/e*1.4;
      IF m>VAL(INTEGER, HIGH(b) DIV 2) THEN m:=m-VAL(INTEGER, HIGH(b)+1) END;  (* make signed freq around zero *)
      freq:=(FREQMUL*500.0)*VAL(REAL,-m);
      leadlag:=freq;                           (* preset slow part of pll loop filter *)
      squelch:=0.0;
      msok:=0;

IF verb THEN
  WrInt(prn+1,2);
  WrStr(":fftpeak:");
  WrFixed(freq/FREQMUL,1,1);WrStr("Hz,lev:");
  WrFixed(max/e,2,1); WrStrLn("");
END;

      RETURN TRUE

    ELSE
      peakthres:=peakthres*0.9995;
      goldp:=1;
      IF NOT fixprn & (sampc-lastsearch>SEARCHTIME) THEN         (* search intervall over *)
        lastsearch:=sampc;
        IF sampc-lastsync>LONGLOST THEN scan:=TRUE END;          (* long time no sync *)
        IF scan THEN nextsat(ps) END;                            (* try other sat *)
      END;
    END;
  END;
  RETURN FALSE
END search;


PROCEDURE trace(ps:pSATDEMOD; si,sq:REAL);
VAR d:CARDINAL;
    hi, df, dp, di, dq:REAL; 
BEGIN
  WITH ps^ DO

---- split spread spectrum into baseband channels

    IF ODD(wp) THEN                  (* half bit for data *)
      INC(dds, freqi);
      d:=CAST(CARDINAL, SHIFT(CAST(SET32, dds),-18));
      di:=DDS[d];
      dq:=DDS[(d+(HIGH(DDS)+1) DIV 4) MOD (HIGH(DDS)+1)];

      IF GOLD[prn][ASH(wp, -1)] THEN
        sumi:=sumi+sio;
        sumq:=sumq+sqo;

        sumqco:=sumqco+sqc0;
        sqc0:=sic*dq + sqc*di;
        sumqc:=sumqc+sqc0;

      ELSE
        sumi:=sumi-sio;
        sumq:=sumq-sqo;

        sumqco:=sumqco-sqc0;
        sqc0:=sic*dq + sqc*di;
        sumqc:=sumqc-sqc0;

      END;
      sio:=si*di - sq*dq;
      sqo:=si*dq + sq*di;

    ELSE                                (* half bit for clock sync *)
      sic:=si;
      sqc:=sq;
    END;

    INC(wp);    
    IF wp>=2047 THEN   

----chip ready, 1 iq sample 1000hz per sat

      IF msok>SYNGOODMS THEN bit1000(ps, sumq) END; 
<* IF WITHSOUND THEN *>
      IF soundfd>=0 THEN soundout(ps) END;
<* END *>

----interpolate 1mhz/300m resolution with constant groop delay pll lowpass  

      IF sumq<>0.0 THEN
        hi:=(sumqc-sumqco)/sumq;

IF verb THEN 
  IF hi<0.0 THEN INC(sat[prn].minusc) ELSE INC(sat[prn].plusc) END;
END;
        prnnoise:=prnnoise+(ABS(hi)-prnnoise)*0.01;
--        multipath:=multipath+((sumqc+sumqco)/sumq-multipath)*0.005;

        IF hi>1.0 THEN hi:=1.0 ELSIF hi<-1.0 THEN hi:=-1.0 END; (* trimm to low prn noise and fast lock *)      
      ELSE hi:=0.0 END;


      IF msok>SYNGOODMS THEN     (* locked, so slow pll *)
        ifase:=ifase+hi*0.1;     (* 0.2 on 32 size ifirfine *) 
        prndoppler:=prndoppler + hi*0.0001;   (* 0.0002 on 32 size ifirfine *)
      ELSE                       (* new sat so fast prn pll*)  
        ifase:=ifase+hi;         (* 1.0 on 32 size ifirfine *)
        prndoppler:=prndoppler + (hi-prndoppler)*0.1;
      END;
      ifase:=ifase+prndoppler;                (* pll with no phase delay avoid pseudorange bias on high doppler sats *)

      level:=level+(ABS(sumq)-level)*0.02;

----- carrier pll and squelch
      df:=fastatang2(sumi, sumq);
      dp:=df-lastph;
      IF dp>PI THEN dp:=dp-PI2 END;
      IF dp<-PI THEN dp:=dp+PI2 END;
      IF ABS(dp)<PI/2.0 THEN leadlag:=leadlag+dp*FREQMUL*10.0 END;
      lastph:=df;

      IF df<0.0 THEN df:=PI/2.0+df ELSE df:=df-PI/2.0 END; 

      squelch:=squelch*0.995+ABS(df);

      df:=30000.0*df;
      freq:=freq+df;

      leadlag:=leadlag+(freq-leadlag)*0.09;
      freq:=freq+(leadlag-freq)*0.15;
      freqi:=VAL(INTEGER, freq);                           (* save time *)
------carrier pll


--- shift sample time
      wp:=1;
      IF ifase<0.0 THEN
--WrStr(" P++++ ");
        wp:=2; 
        ifase:=VAL(REAL, IFIRFINE-1);
      ELSIF ifase>(IFIRFINE-1) THEN
--WrStr(" P---- ");
        wp:=0;
        ifase:=0.0;
      END; 
      ifasei:=VAL(CARDINAL, ifase);  
--- shift sample time

      sumi:=0.0;
      sumq:=0.0;
      sumqc:=0.0;
      sumqco:=0.0;
      IF msok>=SYNGOODMS THEN
        IF squelch>120.0 THEN    (* sat lost *)
          resetsat(prn);

IF verb THEN 
  WrInt(prn+1, 2);
  IF msok>SYNGOODMS THEN WrStrLn(": sync-lost");
  ELSE
    WrStr(":no sync found on "); WrFixed(freq/FREQMUL,0,1); WrStrLn("Hz");
  END;
END;

          insync:=AQUISIT;
        ELSIF squelch<80.0 THEN
          lastsync:=sampc; 
          scan:=FALSE;
          IF msok=SYNGOODMS THEN

IF verb THEN 
  WrInt(prn+1, 2); WrStr(":+++SYNC OK on "); WrFixed(freq/FREQMUL,0,1); WrStrLn("Hz");
END;
            INC(msok);
          END;
        END;
      ELSE INC(msok) END;  

    END;
  END;
END trace;


---- read different iq formats

PROCEDURE inreform(VAR b:ARRAY OF REAL):CARDINAL;
VAR i, bs, rs, wp:CARDINAL;
    res:INTEGER;
    ib:RECORD CASE :CARDINAL OF
                0:c:ARRAY[0..MAXINBUF-1] OF Complex;
               |1:i:ARRAY[0..MAXINBUF*4-1] OF INT16;
               |2:b:ARRAY[0..MAXINBUF*8-1] OF CARD8;
              END;
       END;
    p:POINTER TO ARRAY[0..65535] OF BYTE;
BEGIN
  bs:=(MAXINBUF*2);
  IF bs>(HIGH(b)+1)*2 THEN bs:=(HIGH(b)+1)*2 END;
  rs:=0;
  REPEAT
    p:=ADR(ib.b[rs]);
    res:=RdBin(iqfd, p^, bs-rs);
    IF res<=0 THEN RETURN 0 END;

    INC(rs, res);
  UNTIL rs>=bs;

  IF isize=1 THEN
    IF u8signed THEN
      FOR i:=0 TO rs-1 DO b[i]:=VAL(REAL, CAST(INT8, ib.b[i])) END;
    ELSE
      FOR i:=0 TO rs-1 DO b[i]:=VAL(REAL, VAL(INTEGER, ib.b[i])-128) END;
    END; 
    wp:=rs;

  ELSIF isize=2 THEN
    FOR i:=0 TO rs-1 DO b[i]:=VAL(REAL, ib.i[i]) END;
    wp:=ASH(rs, -1);

  ELSE
    MOVE(ADR(ib), ADR(b), rs);
    wp:=ASH(rs, -2);
  END;
  RETURN wp
END inreform;


---- iq samplerate conversation and interpolation

PROCEDURE doifir(fase:CARDINAL; VAR ui, uq:REAL);
VAR i,j,e:CARDINAL;
    t:REAL;
BEGIN
  i:=0;
  e:=IFIRLEN;
  IF fase>=IFIRFINE THEN i:=2; e:=IFIRLEN+2; DEC(fase, IFIRFINE) END;
  ui:=0.0;
  uq:=0.0;
  j:=fase*IFIRLEN;
  REPEAT
    t:=IFIRTAB[j];
    INC(j);
    ui:=ui+ifir[i]*t;
    INC(i);
    uq:=uq+ifir[i]*t;
    INC(i);
  UNTIL i>=e;
END doifir;


PROCEDURE allsat(phase:CARDINAL);
VAR ui, uq:REAL;
    ps:pSATDEMOD;
BEGIN
  ps:=psat;
  REPEAT
    IF ps^.insync=TRACE THEN
      doifir(VAL(CARDINAL, ps^.ifasei)+phase, ui, uq);
      trace(ps, ui, uq);

    ELSIF sampc MOD 2=0 THEN 
      doifir(phase, ui, uq);
      IF search(ps, ps^.fb, ui, uq) THEN    (* maybe sat found *)
        ps^.ifase:=0.0;
        ps^.insync:=TRACE;
        ps^.msok:=0;
        ps^.wp:=4;
      END;
    END;
    ps:=ps^.next;
  UNTIL ps=NIL;
  INC(sampc);
END allsat;


VAR 
    insampfine,
    insampstep,
    pinb,
    sinb,
    oldf,
    i, il:CARDINAL;
    ps:pSATDEMOD;

PROCEDURE getinsamp():BOOLEAN;
BEGIN
  IF pinb>=sinb THEN
    sinb:=inreform(iqbuf);
    IF sinb<2 THEN RETURN FALSE END;
    
    pinb:=0;
  END;
  MOVE(ADR(ifir[2]), ADR(ifir[0]), SIZE(ifir)-SIZE(ifir[0])*2);
  ifir[HIGH(ifir)-1]:=iqbuf[pinb];
  INC(pinb);
  ifir[HIGH(ifir)]:=iqbuf[pinb];
  INC(pinb);
  RETURN TRUE
END getinsamp;

BEGIN
  goldcode;


  MakeDDS(DDS);
  FILL(ADR(sat), 0C, SIZE(sat));
  FILL(ADR(almanach), 0C, SIZE(almanach));
  almfn:="";
  warmbootfn:="";
  maxalt:=50000.0;
  errsum:=0.0;
  Parms;

  warmbootfile(FALSE);
  initpos;

  genrev(FFTSIZE);
  
  psat:=NIL;
  FOR i:=0 TO maxsat-1 DO 
    ALLOCATE(ps, SIZE(ps^));
    IF ps=NIL THEN Error("out of memory") END;

    ps^.next:=psat;
    psat:=ps;
    ps^.insync:=AQUISIT;
    ps^.scan:=TRUE;
    ps^.lastsearch:=MAX(CARDINAL)-SEARCHTIME;
    FOR il:=0 TO HIGH(sat) DO
      IF il IN fixsat THEN
        ps^.fixprn:=TRUE;
        ps^.prn:=il;
        EXCL(fixsat, il); 
      END;
    END;
  END;


--FOR i:=0 TO HIGH(testf) DO testf[i]:=-1 END;


  iqfd:=OpenRead(iqfn);
  mkfir;
  IF iqfd<0 THEN Error("open iq file") END;


  upsample:=SAMPRATE>insamp;
  IF upsample THEN
    insampstep:=MAX(CARDINAL)-VAL(CARDINAL, (1.0-FLOAT(insamp)/SAMPRATE)*FLOAT(MAX(CARDINAL)));
  ELSE
    insampstep:=MAX(CARDINAL)-VAL(CARDINAL, (1.0-SAMPRATE/FLOAT(insamp))*FLOAT(MAX(CARDINAL)));
  END;
  
  IF upsample THEN
    LOOP
      oldf:=insampfine;
      INC(insampfine, insampstep);
      IF (insampfine<=oldf) & NOT getinsamp() THEN EXIT END;

      allsat(insampfine DIV (MAX(CARDINAL) DIV IFIRFINE+1));
    END;
  ELSE
    WHILE getinsamp() DO
      oldf:=insampfine;
      DEC(insampfine, insampstep);
      IF insampfine>=oldf THEN allsat(insampfine DIV (MAX(CARDINAL) DIV IFIRFINE+1)) END;
    END;
  END;

END gpssdr.


(*
70.77user 1.61system 1:39.76elapsed 72%CPU (0avgtext+0avgdata 0maxresident)k
51.82user 1.31system 1:10.57elapsed 75%CPU (0avgtext+0avgdata 0maxresident)k
52.26user 1.27system 1:17.12elapsed 69%CPU (0avgtext+0avgdata 0maxresident)k
52.31user 1.33system 1:16.86elapsed 69%CPU (0avgtext+0avgdata 0maxresident)k
52.02user 1.29system 1:16.61elapsed 69%CPU (0avgtext+0avgdata 0maxresident)k
45.91user 1.41system 1:09.40elapsed 68%CPU (0avgtext+0avgdata 0maxresident)k
48.17user 1.32system 1:16.91elapsed 64%CPU (0avgtext+0avgdata 0maxresident)k


VAR a,b,c,d,n,s:CARDINAL;
FOR s:=3 TO 11 DO
n:=0;
FOR a:=0 TO s DO
  FOR b:=a+1 TO s DO
    FOR c:=b+1 TO s DO
      FOR d:=c+1 TO s DO
        INC(n);
      END;
    END;
  END;
END;
WrInt(s,10); WrInt(n,10); WrStrLn("=m");
END;

FROM gps IMPORT GPS_ComputeSatellitePositionVelocityAzimuthElevationDoppler_BasedOnEphmerisData;


(*
wgs84(48.0*RAD, 13.0*RAD, 400.0, userpos.lat,userpos.long,userpos.alt);
wgs84r(userpos.lat,userpos.long,userpos.alt, fixpos.lat, fixpos.long, fixpos.alt);
WrFixed(fixpos.lat/RAD,5,1); WrStr(" ");
WrFixed(fixpos.long/RAD,5,1); WrStr(" ");
WrFixed(fixpos.alt,5,1); WrStrLn(" ");
*)

--WrFixed(egm96(48.251410*RAD, 13.037970*RAD, verb),2,10); WrStrLn("m");


---- limit real input > +-1.0 to INT16

PROCEDURE realint(x:REAL; VAR g:REAL):INT16;
VAR r:REAL;
BEGIN
  r:=x*g;
  IF ABS(r)>=32767.0 THEN
    g:=32767.0*32767.0/r;
    r:=x*g;
  END;
  RETURN VAL(INT16, r)
END realint;


(*
  FOR il:=0 TO HIGH(GOLD) DO
    a:=0.0;
    b:=0.0;
    FOR i:=1 TO HIGH(GOLD[0])-1 DO
      IF GOLD[il][i] = GOLD[il][i-1] THEN a:=a+1.0 END;
      IF GOLD[il][i] = GOLD[il][i+1] THEN b:=b+1.0 END;
    END; 
WrInt(il,2); WrFixed(a,2,10); WrFixed(b,2,10); WrStrLn("");
  END;

*)


 0    511.00    511.00
 1    511.00    511.00
 2    511.00    511.00
 3    512.00    511.00
 4    512.00    511.00
 5    511.00    510.00
 6    543.00    543.00
 7    480.00    479.00
 8    511.00    510.00
 9    511.00    511.00
10    511.00    511.00
11    511.00    511.00
12    511.00    511.00
13    511.00    511.00
14    544.00    543.00
15    510.00    510.00
16    543.00    543.00
17    511.00    511.00
18    511.00    511.00
19    511.00    511.00
20    543.00    543.00
21    480.00    479.00
22    511.00    511.00
23    543.00    543.00
24    511.00    511.00
25    511.00    511.00
26    512.00    511.00
27    511.00    510.00
28    511.00    511.00
29    511.00    511.00
30    511.00    511.00
31    512.00    511.00


         3         1=m
         4         5=m
         5        15=m
         6        35=m
         7        70=m
         8       126=m
         9       210=m
        10       330=m
        11       495=m


    IF solc>1 THEN
-- bubble sort
      j:=solc;
      REPEAT
        done:=TRUE; 
        i:=1;
        REPEAT
          IF sol[sol[i].idx].mnoise>sol[sol[i-1].idx].mnoise THEN
            j:=sol[i].idx;
            sol[i].idx:=sol[i-1].idx;
            sol[i-1].idx:=j;
            done:=FALSE;
          END;
          INC(i);
        UNTIL i>=j;
        DEC(j);
      UNTIL done;
--bubble sort
    END;

    i:=0; 
    msum:=0.0;
    WHILE (i<solc) & (sol[sol[i].idx].mnoise<=sol[sol[0].idx].mnoise*2.0) DO 
      WITH sol[sol[i].idx] DO
        n:=1.0/(sqr(mnoise)+1.0);
        mlat  :=mlat  + lat*n;
        mlong :=mlong + long*n;
        malt  :=malt  + alt*n;
        msum  :=msum  + n;
IF verb THEN 
  WrStr("{");WrInt(i,2); WrStr(" "); WrInt(sol[i].idx,2); WrStr(" ");WrFixed(mnoise,0,1); WrStr(" "); WrFixed(lat/RAD,5,1); WrStr(","); WrFixed(long/RAD,5,1); WrStr(",");WrFixed(alt,0,1); WrStrLn("}");
END;
      END;
      INC(i);
    END;
*)

PROCEDURE get4sats(s0,s1,s2,s3:CARDINAL;
                   p0,p1,p2,p3:LONGREAL;
                   VAR lat, long, heig, rx_clock_bias:LONGREAL):BOOLEAN;
BEGIN
  RETURN NAVIGATION_PerformClosedFormPositionSolution_FromPseuodrangeMeasurements(
   p0,p1,p2,p3,
   sat[s0].satpos.clkcorr, sat[s1].satpos.clkcorr, sat[s2].satpos.clkcorr, sat[s3].satpos.clkcorr,
   sat[s0].satpos.x, sat[s1].satpos.x, sat[s2].satpos.x, sat[s3].satpos.x,
   sat[s0].satpos.y, sat[s1].satpos.y, sat[s2].satpos.y, sat[s3].satpos.y,
   sat[s0].satpos.z, sat[s1].satpos.z, sat[s2].satpos.z, sat[s3].satpos.z,
   lat, long, heig, rx_clock_bias)>-1000;
END get4sats;

(*
PROCEDURE hdop(a, b, c, d:CARDINAL; r0, r1, r2, r3:LONGREAL; pos0-:POS; VAR sum:REAL):REAL;  (* amplifiction of pseudorange noise *)

  PROCEDURE pain(s:CARDINAL):REAL;
  VAR e:REAL;
  BEGIN
    e:=0.0;
    IF sat[s].satpos.azel THEN e:=0.15 END;
    RETURN e/sqr(max(0.02, sat[s].satpos.elevation))       (* low elevation effect m *)
           + math.pow(sat[s].prnnois,6.0)*50000.0          (* snr effect m *)
--           + sqr(max(0.0, sat[s].multipath-0.95))*10000.0 (* multipath effect *)
           + urai(almanach.ephem[s].ura)                   (* ura m *)
           + FLOAT(sat[s].bert)*5.0                        (* bit error in 1000bit/s stream *)
  END pain;


CONST TESTD=30.0;
VAR m, maxd:REAL;
    p   :POS;
    bias:LONGREAL;

BEGIN
WrStr("p=");
WrFixed(pain(a),1,1); WrStr(" ");
WrFixed(pain(b),1,1); WrStr(" ");
WrFixed(pain(c),1,1); WrStr(" ");
WrFixed(pain(d),1,1); WrStr(",");
  m:=0.0;
  IF get4sats(a,b,c,d, r0+TESTD, r1, r2, r3, p.lat, p.long, p.alt, bias) THEN m:=shortdist(p, pos0) END;
  maxd:=m;
WrFixed(m/TESTD,1,1); WrStr(" ");
  sum:=m*pain(a);
  m:=0.0;
  IF get4sats(a,b,c,d, r0, r1+TESTD, r2, r3, p.lat, p.long, p.alt, bias) THEN m:=shortdist(p, pos0) END;
  IF m>maxd THEN maxd:=m END;
WrFixed(m/TESTD,1,1); WrStr(" ");
  sum:=sum + m*pain(b);
  m:=0.0;
  IF get4sats(a,b,c,d, r0, r1, r2+TESTD, r3, p.lat, p.long, p.alt, bias) THEN m:=shortdist(p, pos0) END;
  IF m>maxd THEN maxd:=m END;
WrFixed(m/TESTD,1,1); WrStr(" ");
  sum:=sum + m*pain(c);
  m:=0.0;
  IF get4sats(a,b,c,d, r0, r1, r2, r3+TESTD, p.lat, p.long, p.alt, bias) THEN m:=shortdist(p, pos0) END;
  IF m>maxd THEN maxd:=m END;
WrFixed(m/TESTD,1,1); WrStr(" ");
  sum:=sum + m*pain(d);
  RETURN maxd*(1.0/TESTD)
END hdop;
*)


(*
  best:=MAX(REAL);
  WHILE satc>=3 DO

(*
    dops(pstart);
FOR i:=0 TO satc-1 DO
 WrStr("r3:"); WrInt(s[i].sn+1,2); WrStr(":"); WrFixed(s[i].r,1,1); WrStr("m ");
 WrFixed(s[i].dop,2,1);
 WrStrLn("");
END;
*)  
    p.lat :=pstart.lat;
    p.long:=pstart.long;
    p.alt :=pstart.alt;   
    offs:=rangesrest(p, s, satc, FALSE);

    IF offs<best THEN
      best:=offs;
      pstart.lat :=p.lat;
      pstart.long:=p.long;
      pstart.alt :=p.alt;
    END;

    IF satc>=4 THEN offs:=rangesrest(p, s, satc, TRUE) END;

    m:=-1.0;
    FOR i:=0 TO satc-1 DO
      IF s[i].dsum>m THEN m:=s[i].dsum; j:=i END;
    END;
    FOR i:=j+1 TO satc-1 DO s[i]:=s[i-1] END;
    DEC(satc);
  END;
  RETURN best<100.0
END d23fix;
*)


(*
PROCEDURE findbestpos(solc:CARDINAL; VAR sol:ARRAY OF SOLUTION; VAR upos:POS; timef:LONGREAL; dop:REAL):BOOLEAN; (* RAD *)
VAR i,cnt,one:CARDINAL;
    mlat, mlong, malt, msum, xmlat, xmlong, xmalt, xmsum, rlim, dt, lowpass, xlp:LONGREAL; 
    ddir, spd, moved, maxweight:REAL;
    ext:POS;
    pook:BOOLEAN; 
BEGIN
  pook:=FALSE;
  maxweight:=0.0;
  IF solc>0 THEN
    FOR i:=0 TO solc-1 DO    (* median of noise to minimize hopping of best solutions *)
      WITH sol[i] DO
        IF sat4ok THEN
          IF (s0<>s0o) OR (s1<>s1o) OR (s2<>s2o) OR (s3<>s3o) THEN (* new so start median from 1st value *)
            mnoise:=noise; 
            s0o:=s0; s1o:=s1; s2o:=s2; s3o:=s3;
          ELSE mnoise:=mnoise+(noise-mnoise)*0.05 END;
          weight:=1.0/(sqr(mnoise)+1.0);
          IF weight>maxweight THEN maxweight:=weight END;
        ELSE weight:=0.0 END;
      END;
    END;


    cnt:=0;
    FOR i:=0 TO solc-1 DO
      WITH sol[i] DO
        IF (solc=1) OR (weight*5.0<maxweight) & (hdil<5.0) THEN one:=i; INC(cnt) END; (* try 2dfix *)
      END;
    END;

    dt:=timef-kaldat.time;
    IF dt>5.0 THEN dt:=5.0 END;
    IF (kaldat.time>0.0) & (dt>0.1) THEN
--      dop:=100.0/(100.0+dop);
      IF nokalman OR (kaldat.okcnt=0) THEN
        rlim:=20000000.0;
        lowpass:=1.0;
      ELSE 
        ddir:=angdiff(upos.dir, kaldat.pos.dir);   (* direction change *)
--WrStr(" ddir=");WrFixed(ddir/RAD,1,1);
        ddir:=(1.0-ABS(ddir)*0.2);                 (* trust factor out of direction change *)
        spd:=min(kaldat.pos.speed, upos.speed);
        moved:=spd*dt*(1.0/EARTH);                 (* guess move way out of speed an dir *)
        ext.lat :=kaldat.pos.lat  + math.cos(kaldat.pos.dir)*moved;
        ext.long:=kaldat.pos.long + math.sin(kaldat.pos.dir)*math.cos(kaldat.pos.lat)*moved;
--WrStr(" x=");WrFixed(ext.lat/RAD,6,1);WrStr("/");WrFixed(ext.long/RAD,6,1);
--WrStr(" k=");WrFixed(kaldat.pos.lat/RAD,6,1);WrStr("/");WrFixed(kaldat.pos.long/RAD,6,1);
--WrStr(" u=");WrFixed(upos.lat/RAD,6,1);WrStr("/");WrFixed(upos.long/RAD,6,1);
--        dist:=shortdist(upos, ext);
        lowpass:=ddir*(kaldat.pos.speed+0.05)/(kaldat.pos.speed+2.0);  (* more lowpass if less movement *)
        rlim:=20.0 + dt*spd*2.0*ddir;                                  (* good if next pos in radius around predicted pos *)
IF verb2 THEN
WrStr(" dt=");WrFixed(dt,1,1);
WrStr(" dop=");WrFixed(dop,3,1);
WrStr(" spd=");WrFixed(spd,2,1);
WrStr(" moved=");WrFixed(moved*EARTH,2,1);
WrStr(" lp=");WrFixed(lowpass,2,1);
END;
--IF upos.lat=0.0 THEN rlim:=40000000.0; lowpass:=1.0; END;
      END;

      mlat:=0.0;
      mlong:=0.0;
      malt:=0.0;
      msum:=0.0;
      xmlat:=0.0;
      xmlong:=0.0;
      xmalt:=0.0;
      xmsum:=0.0;
      FOR i:=0 TO solc-1 DO
        WITH sol[i] DO
          IF (weight<>0.0) & ((cnt=1) OR (weight*5.0>=maxweight) & (hdil<5.0)) THEN

            IF (cnt=1) & (i=one) & d2altok THEN
              pos.alt:=d2alt; 
              IF    badest=s0 THEN pook:=d2fix(s1,s2,s3,r1,r2,r3,pos);
              ELSIF badest=s1 THEN pook:=d2fix(s0,s2,s3,r0,r2,r3,pos);
              ELSIF badest=s2 THEN pook:=d2fix(s0,s1,s3,r0,r1,r3,pos);
              ELSE                 pook:=d2fix(s0,s1,s2,r0,r1,r2,pos) END;
            END;

            IF (shortdist(pos, ext)<rlim) & (ABS(kaldat.pos.alt-pos.alt)<rlim*4.0) THEN
              mlat  :=mlat  + pos.lat*weight;
              mlong :=mlong + pos.long*weight;
              malt  :=malt  + pos.alt*weight;
              msum  :=msum  + weight;
              kaldat.xcnt:=0;
              pook:=TRUE;
IF verb THEN
 WrStr("{");WrInt(i,2); WrStr(" "); WrFixed(mnoise,0,1); WrStr(" "); WrFixed(pos.lat/RAD,5,1); WrStr(","); WrFixed(pos.long/RAD,5,1); WrStr(",");WrFixed(pos.alt,0,1); WrStrLn("}");
END;
            ELSE                                             (* out of aktual circle, look for other hotspot   *)
              IF shortdist(pos, kaldat.xpos)<rlim THEN INC(kaldat.xcnt) END;  (* inside new hotspot circle *)

              xmlat  :=xmlat  + pos.lat*weight;
              xmlong :=xmlong + pos.long*weight;
              xmalt  :=xmalt  + pos.alt*weight;
              xmsum  :=xmsum  + weight;
              weight:=0.0;                                   (* untag for speeds*)

IF verb2 THEN
 WrStr("[");WrInt(i,2); WrStr(" "); WrFixed(kaldat.xpos.lat/RAD,5,1); WrStr(","); WrFixed(kaldat.xpos.long/RAD,5,1); WrStr(",");WrFixed(kaldat.xpos.alt,0,1); WrStr(" xcnt="); WrInt(kaldat.xcnt,1); WrStrLn("]");
END;
            END;
          END;
        END;
      END;
      IF msum<>0.0 THEN
        msum:=1.0/msum;
        upos.lat :=upos.lat + (mlat *msum-upos.lat )*lowpass;
        upos.long:=upos.long+ (mlong*msum-upos.long)*lowpass;
        upos.alt :=upos.alt + (malt *msum-upos.alt )*lowpass;
      END;
      IF xmsum<>0.0 THEN
        xlp:=min(1.0, 2.0/(1.0+FLOAT(kaldat.xcnt)));
        xmsum:=1.0/xmsum;
        kaldat.xpos.lat :=kaldat.xpos.lat + (xmlat *xmsum-kaldat.xpos.lat )*xlp;
        kaldat.xpos.long:=kaldat.xpos.long+ (xmlong*xmsum-kaldat.xpos.long)*xlp;
        kaldat.xpos.alt :=kaldat.xpos.alt + (xmalt *xmsum-kaldat.xpos.alt )*xlp;
      END;
    END;
    IF kaldat.xcnt>=ORD((upos.lat<>0.0) OR (upos.long<>0.0))*13+2 THEN upos:=kaldat.xpos END;       (* switch to new hotspot, fast on first run *)
    kaldat.pos:=upos;
    kaldat.time:=timef;
  END;
  IF pook THEN INC(kaldat.okcnt) END;
  RETURN pook
END findbestpos;
*)


(*
PROCEDURE speeds(solc:CARDINAL; VAR sol:ARRAY OF SOLUTION; VAR res:POS);
VAR i, cnt:CARDINAL;
    climb, w, wcnt, maxw, dir, spd:REAL;
    clkbias:LONGREAL;
    pos1:POS;
BEGIN
  spd:=0.0;
  dir:=0.0;
  wcnt:=0.0;
  climb:=0.0;
  maxw:=0.0;
  IF solc>0 THEN
    FOR i:=0 TO solc-1 DO
      WITH sol[i] DO
--      IF weight<>0.0 THEN                             (* use only best solutions *)
          IF get4sats(s0, s1, s2, s3,
                r0 + ((LIGHT/FREQ)*sat[s0].mfreq(*clkdif*)-sat[s0].satpos.doppler),  (* shift pseudorange by meter of sat doppler *)
                r1 + ((LIGHT/FREQ)*sat[s1].mfreq(*clkdif*)-sat[s1].satpos.doppler),
                r2 + ((LIGHT/FREQ)*sat[s2].mfreq(*clkdif*)-sat[s2].satpos.doppler),
                r3 + ((LIGHT/FREQ)*sat[s3].mfreq(*clkdif*)-sat[s3].satpos.doppler),
                pos1.lat, pos1.long, pos1.alt, clkbias) THEN
            w:=1.0/sqr(hdil);
            spd:=spd + w*shortdist(pos, pos1);        (* true position - shifted by doppler *)

            IF w>maxw THEN
              maxw:=w;
              dir:=shortdir(pos, pos1)
            END;

            climb:=climb+w*(pos1.alt-pos.alt);
            wcnt:=wcnt+w;
          END;
--        END;
      END;
    END;
  END; 
  IF wcnt>0 THEN
    w:=1.0/wcnt;
    res.speed:=spd*w;     (* m/s *)
    res.clb:=climb*w;     (* m/s *)
    IF dir<0.0 THEN dir:=dir+PI2 END;
    res.dir:=dir;         (* RAD *)
  END;
END speeds;
*)


(*
PROCEDURE guesspos(VAR p:POS);
VAR i,c:CARDINAL;
    sp:POS;
BEGIN
  FILL(ADR(p), 0C, SIZE(p));
  c:=0;
  FOR i:=0 TO HIGH(sat) DO
    IF sat[i].insyn & sat[i].datasyn & sat[i].satok THEN
      wgs84r(sat[i].satpos.x, sat[i].satpos.y,  sat[i].satpos.z, sp.lat,sp.long,sp.alt);
      p.lat :=p.lat  + sp.lat;
      p.long:=p.long + sp.long;
      INC(c);
    END;
  END;
  IF c>0 THEN
    p.lat :=p.lat /LFLOAT(c);
    p.long:=p.long/LFLOAT(c);
WrStr(" g:[");WrFixed(p.lat*(1.0/RAD),5,1); WrStr(","); WrFixed(p.long*(1.0/RAD),5,1); WrStrLn("]");

  END;
END guesspos;
*)

(*
    nsat:=0;
--    umed.lat:=0.0; umed.long:=0.0; umed.alt:=0.0;
    minnoise:=1000000.0;
    nmeahdil:=1000.0;
    nsat:=0;
    FOR i:=0 TO HIGH(sat) DO
      IF sat[i].satok & sat[i].insyn & sat[i].datasyn & (sat[i].pmcnt=medcnt) THEN 
        mut[nsat]:=i;
        INC(nsat);
      END;
    END;
    med:=(LIGHT/SAMPRATE*PRNFINETUNE)/VAL(LONGREAL,medcnt);
    IF nsat>=4 THEN
      DEC(nsat);
      sa:=0;
      LOOP
        FOR sb:=sa+1 TO nsat DO
          FOR sc:=sb+1 TO nsat DO
            FOR sd:=sc+1 TO nsat DO
              WITH solutions[solcnt] DO
                s0:=mut[sa];
                s1:=mut[sb];
                s2:=mut[sc];
                s3:=mut[sd];
                r0:=sat[s0].prangem*med;
                r1:=sat[s1].prangem*med;
                r2:=sat[s2].prangem*med;
                r3:=sat[s3].prangem*med;
                sat4ok:=get4sats(s0, s1, s2, s3, r0, r1, r2, r3,
                      pos.lat, pos.long, pos.alt, bias) & (pos.alt>=MINALT) & (pos.alt<=maxalt);
                hdop(s0, s1, s2, s3, r0, r1, r2, r3, noise, hdil, badest);
                IF hdil<nmeahdil THEN nmeahdil:=hdil END;
                IF noise<minnoise THEN minnoise:=noise END;
IF verb THEN 
  WrStr("r:"); WrInt(solcnt,2); WrStr("(");
  WrInt(mut[sa]+1,2); WrStr("[");WrFixed(sat[mut[sa]].sdil,1,1); WrStr(",");WrFixed(sat[mut[sa]].snois,0,1); WrStr("],");
  WrInt(mut[sb]+1,2); WrStr("[");WrFixed(sat[mut[sb]].sdil,1,1); WrStr(",");WrFixed(sat[mut[sb]].snois,0,1); WrStr("],");
  WrInt(mut[sc]+1,2); WrStr("[");WrFixed(sat[mut[sc]].sdil,1,1); WrStr(",");WrFixed(sat[mut[sc]].snois,0,1); WrStr("],");
  WrInt(mut[sd]+1,2); WrStr("[");WrFixed(sat[mut[sd]].sdil,1,1); WrStr(",");WrFixed(sat[mut[sd]].snois,0,1); WrStr("]) hdil:");
  WrFixed(hdil,1,1); WrStr(" ");
  IF sat4ok THEN 
    WrStr("n:"); WrFixed(noise,0,1); WrStr(", ");
    WrFixed(pos.lat/RAD,5,1); WrStr(",");
    WrFixed(pos.long/RAD,5,1); WrStr(","); 
    WrFixed(pos.alt,0,1); 
  ELSE WrStr("sat4err ") END;
  WrStrLn("");
END;

                IF solcnt>=HIGH(solutions) THEN EXIT END;

                INC(solcnt);
              END;
            END; 
          END;
        END;
        IF sa>=nsat THEN EXIT END;

        INC(sa);
      END;

    ELSIF (nsat=3) & d2altok THEN
      upos:=fixpos;
      IF posok THEN upos:=userpos;       
      ELSIF (upos.lat=0.0) & (upos.long=0.0) THEN guesspos(upos) END;
      upos.alt:=d2alt; 
      upos.speed:=0.0;
      upos.clb:=0.0;
      d2ok:=d2fix(mut[0],mut[1],mut[2], sat[mut[0]].prangem*med,sat[mut[1]].prangem*med,sat[mut[2]].prangem*med, upos);
      IF d2ok THEN posok:=TRUE; userpos:=upos; END;
    END;
    IF solcnt>0 THEN
      IF NOT findbestpos(solcnt, solutions, userpos, utimef, minnoise) THEN solcnt:=0 END;
    END;
 
    IF solcnt>0 THEN
      posok:=TRUE;
      speeds(solcnt, solutions, userpos); 
--      IF manpos THEN posstat(egmmeter) END;
    END;
*)

(*
PROCEDURE get4sats(s1,s2,s3,s4:CARDINAL; p1,p2,p3,p4:LONGREAL;
                   VAR lat, long, heig, rx_clock_bias:LONGREAL):BOOLEAN;

TYPE SOLN=RECORD x, y, z:LONGREAL END;

VAR x1,x2,x3,x4,
    y1,y2,y3,y4,
    z1,z2,z3,z4  :LONGREAL;

  x12, x13, x14, (* 'xij' = 'xi' - 'xj' [m] *)
  y12, y13, y14,
  z12, z13, z14,
  p21, p31, p41, (* 'pij' = 'pi' - 'pj' [m] *)

  k1, k2, k3, (* coefficients *)
  c1, c2, c3,
  f1, f2, f3,
  m1, m2, m3,

  d1,   (* clock bias, solution 1 [m] *)
  d2,   (* clock bias, solution 2 [m] *)
  detA, (* determinant of A *)
  tmp1,
  tmp2,
  tmp3:LONGREAL;

  A:ARRAY[0..3] OF ARRAY[0..3] OF LONGREAL;
  D:ARRAY[0..3] OF ARRAY[0..3] OF LONGREAL; (* D is A^-1 * detA *)

  sol1, sol2:SOLN;

BEGIN
  x1:=sat[s1].satpos.x;
  x2:=sat[s2].satpos.x;
  x3:=sat[s3].satpos.x;
  x4:=sat[s4].satpos.x;
  y1:=sat[s1].satpos.y;
  y2:=sat[s2].satpos.y;
  y3:=sat[s3].satpos.y;
  y4:=sat[s4].satpos.y;
  z1:=sat[s1].satpos.z;
  z2:=sat[s2].satpos.z;
  z3:=sat[s3].satpos.z;
  z4:=sat[s4].satpos.z;


---apply the satellite clock corrections

  p1:=p1 + sat[s1].satpos.clkcorr;
  p2:=p2 + sat[s2].satpos.clkcorr;
  p3:=p3 + sat[s3].satpos.clkcorr;
  p4:=p4 + sat[s4].satpos.clkcorr;
  
  x12:= x1 - x2;
  x13:= x1 - x3;
  x14:= x1 - x4;

  y12:= y1 - y2;
  y13:= y1 - y3;
  y14:= y1 - y4;
  
  z12:= z1 - z2;
  z13:= z1 - z3;
  z14:= z1 - z4;

  p21:= p2 - p1;
  p31:= p3 - p1;
  p41:= p4 - p1;

  k1:= x12*x12 + y12*y12 + z12*z12 - p21*p21;
  k2:= x13*x13 + y13*y13 + z13*z13 - p31*p31;
  k3:= x14*x14 + y14*y14 + z14*z14 - p41*p41;

  A[0][0]:= 2.0*x12;
  A[1][0]:= 2.0*x13;
  A[2][0]:= 2.0*x14;

  A[0][1]:= 2.0*y12;
  A[1][1]:= 2.0*y13;
  A[2][1]:= 2.0*y14;

  A[0][2]:= 2.0*z12;
  A[1][2]:= 2.0*z13;
  A[2][2]:= 2.0*z14;

  tmp1:= A[1][1]*A[2][2] - A[2][1]*A[1][2];
  tmp2:= A[1][0]*A[2][2] - A[2][0]*A[1][2];
  tmp3:= A[1][0]*A[2][1] - A[2][0]*A[1][1];

  detA:= A[0][0]*tmp1 - A[0][1]*tmp2 + A[0][2]*tmp3;
  IF detA=0.0 THEN WrStr(" ----------ret1 "); RETURN FALSE END;


  D[0][0]:= tmp1;
  D[1][0]:=-tmp2;
  D[2][0]:= tmp3;

  D[0][1]:=-A[0][1]*A[2][2] + A[2][1]*A[0][2];
  D[1][1]:= A[0][0]*A[2][2] - A[2][0]*A[0][2];
  D[2][1]:=-A[0][0]*A[2][1] + A[2][0]*A[0][1];

  D[0][2]:= A[0][1]*A[1][2] - A[1][1]*A[0][2];
  D[1][2]:=-A[0][0]*A[1][2] + A[1][0]*A[0][2];
  D[2][2]:= A[0][0]*A[1][1] - A[1][0]*A[0][1];

  c1:= (D[0][0]*p21 + D[0][1]*p31 + D[0][2]*p41) * 2.0 / detA;
  c2:= (D[1][0]*p21 + D[1][1]*p31 + D[1][2]*p41) * 2.0 / detA;
  c3:= (D[2][0]*p21 + D[2][1]*p31 + D[2][2]*p41) * 2.0 / detA;

  f1:= (D[0][0]*k1 + D[0][1]*k2 + D[0][2]*k3) / detA;
  f2:= (D[1][0]*k1 + D[1][1]*k2 + D[1][2]*k3) / detA;
  f3:= (D[2][0]*k1 + D[2][1]*k2 + D[2][2]*k3) / detA;

  m1:= c1*c1 + c2*c2 + c3*c3 - 1.0;
  m2:= -2.0*( c1*f1 + c2*f2 + c3*f3 );
  m3:= f1*f1 + f2*f2 + f3*f3;

  tmp1:= m2*m2 - 4.0*m1*m3;
  IF tmp1 < 0.0 THEN RETURN FALSE END;   (* not good, there is no solution *)

  tmp1:=math.sqrt(tmp1);
  tmp2:=0.5 / m1;
  d1:= ( -m2 + tmp1) * tmp2;
  d2:= ( -m2 - tmp1) * tmp2;

--- two solutions
  sol1.x:= c1*d1 - f1 + x1;
  sol1.y:= c2*d1 - f2 + y1;
  sol1.z:= c3*d1 - f3 + z1;

  sol2.x:= c1*d2 - f1 + x1;
  sol2.y:= c2*d2 - f2 + y1;
  sol2.z:= c3*d2 - f3 + z1;

  tmp1:=math.sqrt( sol1.x*sol1.x + sol1.y*sol1.y + sol1.z*sol1.z);
  tmp2:=math.sqrt( sol2.x*sol2.x + sol2.y*sol2.y + sol2.z*sol2.z);

--- choose the correct solution based
--- on whichever solutino is closest to 
--- the Earth's surface
  tmp1:= ABS( tmp1 - 6371000.0 );
  tmp2:= ABS( tmp2 - 6371000.0 );

  IF  tmp2 < tmp1 THEN
    sol1:= sol2;
    d1:= d2;
  END;

  rx_clock_bias:=d1;

  wgs84r(sol1.x, sol1.y, sol1.z, lat, long, heig);
(*
  GEODESY_ConvertEarthFixedCartesianToGeodeticCurvilinearCoordinates(
    GEODESY_REFERENCE_ELLIPSE_WGS84,
    sol1.x, sol1.y, sol1.z,
    lat, long, heig );
*)

  RETURN TRUE
END get4sats;
*)

(*
PROCEDURE MakeDDSi(VAR dds:ARRAY OF INT16);
VAR i:CARDINAL;
    r:REAL; 
BEGIN
  r:=PI*2.0/FLOAT(HIGH(dds)+1);
  FOR i:=0 TO HIGH(dds) DO dds[i]:=VAL(INT16, 16383.9*sin(FLOAT(i)*r)) END;
END MakeDDSi;
*)

(*
PROCEDURE check(p-:POSL; us, satc:CARDINAL):LONGREAL;
VAR sum,x1,y1,z1:LONGREAL;
    i,j:CARDINAL;
BEGIN
  wgs84(p.lat, p.long, p.alt, x1, y1, z1);
  sum:=0.0;
  i:=us;
  WHILE i<=HIGH(sat) DO
    WITH sat[i].satpos DO sat[i].mydist:=dist(x, y, z, x1, y1, z1) - sat[i].range END;
    j:=us;
    WHILE j<>i DO
      sum:=sum + ABS(sat[i].mydist-sat[j].mydist);
      j:=sat[j].next;
    END;
    i:=sat[i].next;
  END;
  RETURN sum/LFLOAT(satc*(satc-1) DIV 2);   (* normalize to 1 pair *)
END check;
*)

(*
PROCEDURE hdop(a, b, c, d:CARDINAL; r0, r1, r2, r3:LONGREAL; VAR sum, maxd:REAL; VAR badsat:CARDINAL);  (* amplifiction of pseudorange noise *)

  PROCEDURE pain(s:CARDINAL):REAL;
  VAR e:REAL;
  BEGIN
    e:=0.0;
    IF sat[s].satpos.azel THEN e:=0.15 END;
    RETURN e/sqr(max(0.02, sat[s].satpos.elevation))                   (* low elevation effect m *)
           + math.pow(sat[s].prnnois,5.0)*10000.0+sat[s].prnnois*10.0  (* snr effect m *)
--           + sqr(max(0.0, sat[s].multipath-0.95))*10000.0            (* multipath effect *)
           + urai(almanach.ephem[s].ura)                               (* ura m *)
           + FLOAT(sat[s].bert)*5.0                                    (* bit error in 1000bit/s stream *)
  END pain;


CONST TESTD=10.0;
VAR m, maxn:REAL;
    p, p0        :POS;
    bias         :LONGREAL;


  PROCEDURE summ(s:CARDINAL; t0,t1,t2,t3:LONGREAL);
  BEGIN
    WITH sat[s] DO
      sdil:=100.0;
      IF get4sats(a,b,c,d, r0+t0, r1+t1, r2+t2, r3+t3, p.lat, p.long, p.alt, bias) THEN sdil:=shortdist(p, p0)*(1.0/TESTD) END;
      IF sdil>maxd THEN maxd:=sdil END;
      snois:=pain(s);
      m:=sdil*snois;
      sum:=sum + m;
      IF m>maxn THEN maxn:=m; badsat:=s END;
IF verb2 THEN WrFixed(sdil,1,1); WrStr("+"); WrFixed(snois,1,1); WrStr(" ") END;
    END;
  END summ;


BEGIN
IF verb2 THEN WrStr("hdop/+noise:") END;
  maxn:=0.0;
  maxd:=0.0;
  sum:=0.0;
  IF get4sats(a,b,c,d, r0, r1, r2, r3, p0.lat, p0.long, p0.alt, bias) THEN
    summ(a, TESTD,0.0,0.0,0.0);
    summ(b, 0.0,TESTD,0.0,0.0);
    summ(c, 0.0,0.0,TESTD,0.0);
    summ(d, 0.0,0.0,0.0,TESTD);
  ELSE maxd:=100.0; sum:=1000.0 END;
END hdop;


PROCEDURE d2fix(s0,s1,s2:CARDINAL; r0,r1,r2:LONGREAL; VAR pstart:POS):BOOLEAN;
CONST DIRS=16;
VAR m, d,do,ddo:LONGREAL;
    dir, bestdir:CARDINAL;
    po,p:POS;
    up:BOOLEAN;


  PROCEDURE check(m:LONGREAL; d:CARDINAL):LONGREAL;
  VAR d0, d1, d2, ux, uy, uz:LONGREAL;
  BEGIN
    d:=d*((HIGH(DDS)+1) DIV DIRS);
    p.long:=p.long + m*DDS[d];
    p.lat :=p.lat  + m*DDS[(d+(HIGH(DDS)+1) DIV 4) MOD (HIGH(DDS)+1)];
    wgs84(p.lat, p.long, p.alt, ux, uy, uz);
    d0:=dist(sat[s0].satpos.x,sat[s0].satpos.y,sat[s0].satpos.z, ux, uy, uz) - r0;
    d1:=dist(sat[s1].satpos.x,sat[s1].satpos.y,sat[s1].satpos.z, ux, uy, uz) - sat[s1].satpos.clkcorr + sat[s0].satpos.clkcorr-r1;
    d2:=dist(sat[s2].satpos.x,sat[s2].satpos.y,sat[s2].satpos.z, ux, uy, uz) - sat[s2].satpos.clkcorr + sat[s0].satpos.clkcorr-r2;
    RETURN ABS(d0-d1)+ABS(d0-d2)+ABS(d1-d2)
  END check;


BEGIN

  p:=pstart;
  m:=0.00005;
  dir:=0;
  po:=p;
  LOOP  
    do:=MAX(REAL);
    FOR dir:=0 TO DIRS-1 DO
      p:=po;
      d:=check(0.0000001,dir);
      IF d<do THEN do:=d; bestdir:=dir END;
    END;
WrStr("bd:"); WrInt(bestdir,1); WrStr(" "); 
    p:=po;
    d:=MAX(REAL); 
    REPEAT
      po:=p;
      do:=d;
      d:=check(m, bestdir);
WrStr("(");WrInt(bestdir,1);WrStr(",");WrFixed(d,0,1);WrStr(")");
    UNTIL d>do;
    p:=po;
    IF (d<1.0) OR (m<0.0000001) THEN EXIT END;

WrStr("-");
    m:=m*0.98;
  END;
  IF d<30.0 THEN pstart:=p; RETURN TRUE END;
  RETURN FALSE
END d2fix;
*)

(*
PROCEDURE posstat(egm:LONGREAL);
VAR d:LONGREAL;
BEGIN
  d:=shortdist(fixpos, userpos);
  IF d<100.0 THEN meddist:=meddist+(d-meddist)*0.01 END;
  d:=ABS(fixpos.alt-(userpos.alt-egm));
  IF d<500 THEN medalt:=medalt+(d-medalt)*0.01 END;
END posstat;
*)


(*

<* IF NOT __GEN_C__ THEN *>

or:=OpenRead("/tmp/n.txt");
--or:=OpenRead("/save/dxl/wav/gps/o7.txt");
--or:=OpenRead("/tmp/n.txt");
IF or>=0 THEN
  i:=0;
  res:=SET32{};
  sat[0].prn:=3;
  sat[0].insync:=TRACE;
  sat[1].insync:=UNUSED;
  WHILE (RdB.in(or, ch, 1)=1) & (ch>" ") DO
    bit50(0, ch="1");
    INC(sampc);
    IF sampc MOD 1500=0 THEN navigate END;
  END;
END;
WrStrLn("");
HALT;
<* END *>

*)
(*
PROCEDURE calcdop(s-:ARRAY OF USEDSAT; satc:CARDINAL);
VAR i,j,k:CARDINAL;
    x0,y0,z0,x1,y1,z1,x2,y2,z2:LONGREAL;
    w:REAL;
BEGIN
  wgs84(userpos.lat, userpos.long, userpos.alt, x0, y0, z0);
  FILL(ADR(bestdils), 0C ,SIZE(bestdils));
  IF satc>=2 THEN
WrStr("dops:");
    FOR i:=0 TO satc-1 DO
      FOR j:=i+1 TO satc-1 DO
        w:=angleofsats(x0,y0,z0,sat[s[i].sn].satpos.x,sat[s[i].sn].satpos.y,sat[s[i].sn].satpos.z,sat[s[j].sn].satpos.x,sat[s[j].sn].satpos.y,sat[s[j].sn].satpos.z);
WrFixed(w/RAD,1,1); WrStr(" ");
        w:=PI*0.5-ABS(ABS(w)-PI*0.5);
--        w:=w/sqrl(sat[s[i].sn].prnnois*sat[s[j].sn].prnnois+0.0000000001);
        k:=0;
        WHILE (k<=HIGH(bestdils)) & (w>bestdils[k].d) DO 
          IF k>0 THEN bestdils[k-1]:=bestdils[k] END;
          bestdils[k].d:=w; 
          bestdils[k].s0:=i;
          bestdils[k].s1:=j;
          INC(k) 
        END;
      END;
    END;
WrStr("/");
FOR i:=0 TO HIGH(bestdils) DO WrFixed(bestdils[i].d/RAD,1,1); WrStr(" ") END;
WrStrLn("");
  END; 
END calcdop;

PROCEDURE modpi(a:LONGREAL):LONGREAL;
BEGIN
  IF a<0.0 THEN a:=a+PI2 ELSIF a>PI2 THEN a:=a-PI2 END;
  RETURN a
END modpi;

PROCEDURE angleofsats(x0,y0,z0,x1,y1,z1,x2,y2,z2:LONGREAL):LONGREAL;
VAR d1, d2:LONGREAL;
BEGIN
  x1:=x1-x0; 
  y1:=y1-y0;
  z1:=z1-z0;
  d1:=math.sqrt(sqrl(x1) + sqrl(y1) + sqrl(z1));
  IF d1=0.0 THEN RETURN 0.0 END;

  d1:=1.0/d1;
  x2:=x2-x0;
  y2:=y2-y0;
  z2:=z2-z0;
  d2:=math.sqrt(sqrl(x2) + sqrl(y2) + sqrl(z2));
  IF d2=0.0 THEN RETURN 0.0 END;

  d2:=1.0/d2;
  d1:=0.5*math.sqrt(sqrl(x2*d2 - x1*d1) + sqrl(y2*d2 - y1*d1) + sqrl(z2*d2 - z1*d1));
  IF ABS(d1)>=1.0 THEN RETURN PI END;

  RETURN math.asin(d1)*2.0
END angleofsats;


*)
(*
PROCEDURE corrfalsest(us:CARDINAL; p-:POS);
VAR i,j:CARDINAL;
    x0,y0,z0:LONGREAL;
    m, max, d:LONGREAL;
BEGIN
  wgs84(p.lat, p.long, p.alt, x0, y0, z0);
  m:=0.0;
  j:=0;
  i:=us;
  WHILE i<=HIGH(sat) DO
    WITH sat[i] DO
      mydist:=dist(satpos.x, satpos.y, satpos.z, x0, y0, z0) - rangec;
      m:=m+mydist;  
      i:=next;
    END;
    INC(j);
  END;
  IF j>=4 THEN
    m:=m/LFLOAT(j);
    max:=-1.0;
    i:=us;
    WHILE i<=HIGH(sat) DO
      WITH sat[i] DO
        d:=ABS(m-mydist);
        IF d>max THEN
          max:=d;
          j:=i;
        END;
        i:=next;
      END;
    END;
    i:=us;
WrStr("corr:");
    WHILE i<=HIGH(sat) DO
      WITH sat[i] DO
        IF i=j THEN corrone:=corrone+(mydist-m-corrone)*0.1 ELSE corrone:=0.0 END;
WrInt(i+1,1); WrStr(":"); WrFixed(corrone,1,1); WrStr(" "); 
        i:=next;
      END;
    END;
WrStrLn("");
  END;
END corrfalsest;
*)

(*
PROCEDURE altfrom2ways(VAR po:POSL; VAR err:REAL);
CONST CHECKDIST=200.0;
VAR ph:POSL;
    f1, f2, a1:LONGREAL;
BEGIN
  IF po.alt<-0.0 THEN po.alt:=0.0 END;
  ph:=po;
  ph.alt:=ph.alt+CHECKDIST;
  f1:=1.0/(newton(ph, FALSE, 0.01, 10.0, 20.0)+0.001);   (* down to last alt *)
  a1:=ph.alt;
  ph:=po;
  ph.alt:=ph.alt-CHECKDIST;
  f2:=1.0/(newton(ph, FALSE, 0.01, 10.0, 20.0)+0.001);    (* up to last alt*)
  err:=ABS(a1-ph.alt);                                    (* 0 if same result else have bad dop *)
  po.alt:=(a1*f1 + ph.alt*f2)/(f1+f2);   (* weighted median of altitude from upper and lower *)
  f1:=newton(po, TRUE, 0.1, 1.0, -1.0);  (* fine adjust xy only*)
  IF err<1.0 THEN err:=f1 END;           (* most cases no err so use range reminders for fix quality *)

WrStr(" c2:[");WrFixed(po.lat*(1.0/RAD),5,1); WrStr(","); WrFixed(po.long*(1.0/RAD),5,1); WrStr(" "); WrFixed(po.alt,1,1); WrStr(" err="); WrFixed(err,1,1); WrStr(" f1="); WrFixed(f1,2,1);WrStr(" f2="); WrFixed(f2,2,1); WrStr(" a1="); WrFixed(a1,1,1);WrStr(" a2="); WrFixed(ph.alt,1,1);WrStr(" ");

END altfrom2ways;
*)

(*
PROCEDURE checkdoublefix(VAR po:POSL; VAR e:REAL);
CONST STEPDIST=250.0;
VAR ph, pb:POSL;
    r ,best, a:LONGREAL;
BEGIN
  e:=checkfix(po);
  IF e<200.0 THEN RETURN END;                     (* looks like no false altitude lock *)
             
  best:=MAX(REAL);                                (* we may have more than 1 solution *)
  a:=LFLOAT(MINALT)+STEPDIST;                     (* hard step thru altitude range to find best fix *)
  REPEAT
    ph:=po;
    ph.alt:=a;  
    r:=newton(ph, FALSE, 0.5, 5.0, -10.0);

WrStr(" r:"); WrFixed(r,1,1); WrStr(","); WrFixed(ph.alt,1,1);  

    IF r<best THEN best:=r; pb:=ph END;
    a:=a+STEPDIST;
  UNTIL a>maxalt-STEPDIST;
  po:=pb;
--  r:=newton(po, FALSE, 0.1, 10.0, -10.0);
  e:=checkfix(po);
END checkdoublefix;
*)

(*
PROCEDURE rangesupdown(VAR p:POSL; resol, startstep:REAL); 
VAR o,best,dir:LONGREAL;
    p1        :POSL;
BEGIN
  p1:=p;
  best:=MAX(REAL);
  dir:=startstep;
  LOOP
--WrStr("[");WrFixed(dir,2,1);WrStr(",");WrFixed(p.alt,1,1);WrStr("]");
    p.alt:=p1.alt+dir;
    IF p.alt>maxalt THEN p.alt:=maxalt-100.0; EXIT ELSIF p.alt<MINALT THEN p.alt:=MINALT+100.0; EXIT END;

INC(altfixc);
    o:=check(p);
    IF o<best THEN
      best:=o;
      p1:=p;
      IF best<resol THEN EXIT END;       (* has desired precision else go on same direction *)

    ELSE
      IF ABS(dir)<resol THEN EXIT END;   (* terminate possible forever loop *)

      dir:=dir*(-0.5);                   (* change direction an step finer *)
    END;
  END;
END rangesupdown;
*)

(*
PROCEDURE newton(VAR p:POSL; noalt:BOOLEAN; resol, maxtry, altstartstep:REAL):LONGREAL;
VAR offs, best, a, term:LONGREAL;
    ph:POSL;    
BEGIN
  best:=MAX(REAL);
  term:=0.0;
  LOOP
    ph:=p;
    offs:=aquisith(ph, resol);              (* best result with this alt *)
    IF offs>=best THEN EXIT END;

    best:=offs;
    p:=ph;
    IF noalt THEN EXIT END;

    a:=p.alt; 
    rangesupdown(p, resol, altstartstep);
--WrStr("[");WrFixed(a-p.alt,2,1); WrStr("]");
    IF (term<maxtry) & (ABS(a-p.alt)>term) THEN best:=MAX(REAL) END;      (* restart xy search with new alt *)
    term:=term+resol;
  END;
  RETURN best
END newton;
*)
PROCEDURE calcdil(up-:POS; us:CARDINAL);
CONST M=1.1;   (* does not work with 1.0, gcc leak? *)
VAR i,j:CARDINAL;
    r, r0, x0,y0,z0:LONGREAL;
    dd,da:REAL;
    p:POSL;
    pp:POS;
BEGIN
  nmeahdil:=0.0;
  nmeavdil:=0.0;


(*
  p.lat:=up.lat;
  p.long:=up.long;
  p.alt:=p.alt+200.0;
  r:=newton(p, FALSE, 0.1, -10.0);
  nmeavdil:=ABS(p.alt-up.alt);
  p.lat:=up.lat;
  p.long:=up.long;
  p.alt:=p.alt-200.0;
  r:=newton(p, FALSE, 0.1, -10.0);
  nmeavdil:=nmeavdil+ABS(p.alt-up.alt);
*)


  wgs84(up.lat, up.long, up.alt, x0, y0, z0);
  j:=0;
  i:=us;
  WHILE i<=HIGH(sat) DO
    WITH sat[i] DO
      range:=dist(satpos.x, satpos.y, satpos.z, x0, y0, z0);
      i:=next;
    END;
    INC(j);
  END;
  IF j>=4 THEN
    i:=us;
    WHILE i<=HIGH(sat) DO
      WITH sat[i] DO
        r0:=range;
        range:=r0+M;
        p.lat:=up.lat; p.long:=up.long; p.alt:=up.alt;
        r:=newton(p, FALSE, 0.01, 1.0, M);
        pp.lat:=p.lat; pp.long:=p.long;
        dd:=shortdist(pp, up)*(1.0/M);
        da:=ABS(p.alt-up.alt)*(1.0/M);
        IF dd>nmeahdil THEN nmeahdil:=dd END;
        IF da>nmeavdil THEN nmeavdil:=da END; 
        range:=r0;
        i:=next;
      END;
    END;
  END;
IF verb THEN WrStr("hdop="); WrFixed(nmeahdil,2,1); WrStr(" vdop="); WrFixed(nmeavdil,2,1); WrStrLn("") END;
END calcdil;
*)


  s:="     2.10           N: GPS NAV DATA                         RINEX VERSION / TYPE";
  Append(s, "GPSSDR V0.1         DXL                 ");
  Append(s, "GPS EPHEMERIDES TO RINEX");


#low elevaton bad
#ura>0 bad
speed over ground
#egm
check delay from prange median time
show samplerateerror
todo: leapsecond change
# egm96 correct?
predict kaldat hotspot ?

speed: use max-min doppler sats
preselect solutions to save cpu

#ASH( ,-1)  in GOLD[  DIV 2]
check freq false lock
# sound: spread sats over stereo base
# nmea: speed over ground, where is ground

# leapsecond for warmstart
# allow auto 2/3d fix 
-f f32 level?
dop calc if changed
# check fix alt
warmboot save only on new iodc
hdop nmea
-m 3 finds 2 sat
# clamp for fft peak detector
vdil too less


? 2dfix only after good 3dfix try different altitudes 
calcdop
? auto2d3d
*)