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

MODULE downsample;  (* shift frequency and change sampelrate of iq file *)

FROM SYSTEM IMPORT CARD16, INT16, CARD8, FILL, ADR, CAST, MOVE;
FROM osi IMPORT NextArg, WrStr, WrStrLn, WrCard,
                WrLn, Close, RdBin, WrBin, File, OpenRead, OpenWrite, Werr, WrFixed;
FROM aprsstr IMPORT StrToFix, StrToCard, Append, IntToStr, CardToStr, FixToStr;
FROM math IMPORT sin, cos;


CONST DDSSIZE=65536;
      SUBSAMPBITS=8;
      FIRMAX=4096;
      PI=3.1415926535;
      SUBSAMP=1<<SUBSAMPBITS;
      LF=12C;

TYPE SET32=SET OF [0..31];
     SET8 =SET OF [0..7];
     Complex=RECORD Im, Re:REAL END;
     FSAMP=Complex;

VAR as, fni, fno:ARRAY[0..4095] OF CHAR;
    bb:ARRAY[0..32767] OF CHAR;
    ib:ARRAY[0..32767] OF FSAMP;
    ob:ARRAY[0..32767] OF CARD8;
    prefill, fi, fo:INTEGER;
    eof, verb, shiftin, shiftout:BOOLEAN;
    fg, re, im, f, val, sr, srin, gain, outpeak:REAL; 
    vall:LONGREAL;
    pb, lenb, wp, rp, inform, outform, firlen, rl, fpend,
    phasereg, shiftstep, sampc, i, fp, rpend, samprate,
    statistic, limcnt, outsamps:CARDINAL;
    sum:Complex;
    DDS:ARRAY[0..DDSSIZE-1] OF RECORD si,co:REAL END;
    firraw, firtab:ARRAY[0..FIRMAX*SUBSAMP-1] OF REAL;

test:CARDINAL;


PROCEDURE Err(text:ARRAY OF CHAR);
BEGIN
  WrStr(text); WrStrLn(" error abort");
  HALT
END Err;


PROCEDURE WerrInt(n:INTEGER);
VAR s:ARRAY[0..20] OF CHAR;
BEGIN
  IntToStr(n, 8, s);
  Werr(s);
--  Werr(LF);
END WerrInt;


PROCEDURE mkfir(fg, gain:REAL; subsamp, flen:CARDINAL; VAR fir:ARRAY OF REAL);
VAR i, m:CARDINAL;
    u, w, l:REAL;
BEGIN
  flen:=flen*subsamp;
  w:=2.0*PI*fg/FLOAT(subsamp);
  m:=flen DIV 2;
  fir[m]:=1.0;
  FOR i:=1 TO flen DIV 2 DO
    u:=FLOAT(i)*w;
    u:=sin(u)/u;
    IF m+i<flen THEN fir[m+i]:=u END;
    fir[m-i]:=u;
  END;
  l:=fg*2.0*gain;                                                 (* gain compensation *) 
  u:=2.0/FLOAT(flen);
  FOR i:=0 TO flen-1 DO                                      (* hamming *)
    fir[i]:=l*fir[i]*(0.54+0.46*cos(PI*(FLOAT(i)*u-1.0)));
  END;
END mkfir;


PROCEDURE mkdds;
VAR i,q:CARDINAL;
    r:REAL;
BEGIN
  r:=2.0*PI/FLOAT(HIGH(DDS)+1);
  q:=(HIGH(DDS)+1) DIV 4;
  FOR i:=0 TO HIGH(DDS) DO 
    DDS[i].si:=sin(FLOAT(i)*r);
    DDS[(i+q) MOD (HIGH(DDS)+1)].co:=DDS[i].si;
  END;
END mkdds;


PROCEDURE WrStat;
VAR h,hh:ARRAY[0..100] OF CHAR;
    v:REAL;
BEGIN
  h:="Peak:";
  v:=outpeak;
  IF outform=1 THEN v:=v*256.0 ELSIF outform>1 THEN v:=v*(1.0/128.0) END;
  FixToStr(v, 3, hh); Append(h, hh);
  IF outform=0 THEN v:=v*(100.0/127.5);
  ELSIF outform=1 THEN v:=v*(100.0/32767.5) END;
  IF outform<=1 THEN
    Append(h, "(");
    FixToStr(v, 2, hh);
    Append(h, hh);
    Append(h, "%) Limited:");
    FixToStr(FLOAT(limcnt)*100.0/FLOAT(outsamps), 4, hh); 
    Append(h, hh);
    Append(h, "%");
  END;
  Append(h, LF);
  Werr(h);
  outpeak:=0.0;
  limcnt:=0;
  DEC(outsamps, statistic);
END WrStat;

PROCEDURE WrSamp(s:REAL);
VAR n:INTEGER;
BEGIN
  IF statistic<>0 THEN
    INC(outsamps);
    IF ABS(s)>outpeak THEN outpeak:=ABS(s) END;
    IF outsamps>=statistic THEN WrStat END;
  END;

  IF outform=0 THEN
    IF s>127.99 THEN s:=127.99; INC(limcnt) ELSIF s<-127.99 THEN s:=-127.99; INC(limcnt) END;
    ob[wp]:=TRUNC(s+128.0);
    INC(wp);
  ELSIF outform=1 THEN
    s:=s*256.0;
    IF s>32766.0 THEN
      s:=32766.0;
      INC(limcnt);
    ELSIF s<-32766.0 THEN
      s:=-32766.0;
      INC(limcnt);
    END;
    n:=VAL(INTEGER, s);
    ob[wp]:=n;
    INC(wp);
    ob[wp]:=ASH(n, -8);
    INC(wp);
  ELSE
    s:=s*(1.0/128.0);         (* float is -1 to 1 *)
    n:=CAST(CARDINAL, s);
    ob[wp]:=n;
    INC(wp);
    ob[wp]:=ASH(n, -8);
    INC(wp);
    ob[wp]:=ASH(n, -16);
    INC(wp);
    ob[wp]:=ASH(n, -24);
    INC(wp);
  END;
  IF wp>HIGH(ob) THEN WrBin(fo, ob, wp); wp:=0; END;
END WrSamp;


PROCEDURE inword():REAL;
VAR i:INTEGER;
    c,e:CARDINAL;
    r:REAL;
BEGIN
  IF prefill>0 THEN DEC(prefill); RETURN 0.0 END;

  IF pb>=lenb THEN
    i:=RdBin(fi, bb, SIZE(bb));
    IF i<=0 THEN
      IF prefill>=-VAL(INTEGER, firlen-1) THEN DEC(prefill) ELSE eof:=TRUE END;
      RETURN 0.0

    END;
    lenb:=i;
    pb:=0;
  END;
  IF inform=0 THEN INC(pb); RETURN VAL(REAL, ORD(bb[pb-1]))-127.5;
  ELSIF inform=1 THEN INC(pb, 2); RETURN VAL(REAL, CAST(INT16, ASH(ORD(bb[pb-1]),8)+ORD(bb[pb-2])))*(1.0/256.0);
  ELSE
    INC(pb, 4);
    c:=ASH(ORD(bb[pb-1]),24)+ASH(ORD(bb[pb-2]),16)+ASH(ORD(bb[pb-3]),8)+ORD(bb[pb-4]);
    e:=c DIV 1000000H;
    IF e=07FH THEN RETURN 127.0 ELSIF e=0FFH THEN RETURN -127.0 END;

    r:=CAST(REAL, c);
    IF r>1.0 THEN r:=1.0 ELSIF r<-1.0 THEN r:=-1.0 END;
    RETURN r*127.0
  END;
END inword;


PROCEDURE StrToFixL(VAR x:LONGREAL; s-:ARRAY OF CHAR):BOOLEAN;
CONST M=MAX(LONGREAL)*0.01;
VAR i:CARDINAL;
    p:LONGREAL;
    neg, ok:BOOLEAN;
BEGIN
  i:=0;
  neg:=FALSE;
  ok:=FALSE;
  WHILE (i<=HIGH(s)) & (s[i]=" ") DO INC(i) END;
  IF s[i]="-" THEN neg:=TRUE; INC(i) END;
  x:=0.0;
  WHILE (i<=HIGH(s)) & (s[i]>="0") & (s[i]<="9") DO
    IF x<M THEN x:=x*10.0 + LFLOAT(ORD(s[i])-ORD("0")) END;
    INC(i);
    ok:=TRUE;
  END;
  IF (i<=HIGH(s)) & (s[i]=".") THEN
    INC(i);
    p:=0.1;
    WHILE (i<=HIGH(s)) & (s[i]>="0") & (s[i]<="9") DO
      x:=x + p*LFLOAT(ORD(s[i])-ORD("0"));
      p:=p*0.1;
      INC(i);
      ok:=TRUE;
    END;
  END;
  IF neg THEN x:=-x END;
  RETURN ok & ((i>HIGH(s)) OR (s[i]=0C))
END StrToFixL;


BEGIN
  statistic:=0;
  outpeak:=0.0;
  outsamps:=0;
  limcnt:=0;

  inform:=0;
  outform:=0;
  firlen:=64;
  fg:=0.0;
  gain:=1.0;
  shiftstep:=0;
  sr:=0.0;
  samprate:=65536;
  LOOP
    NextArg(as);
    IF as[0]=0C THEN EXIT END;

    IF (as[0]="-") & (as[1]<>0C) & (as[2]=0C) THEN

      IF as[0]="-" THEN
        IF as[1]="i" THEN NextArg(fni);

        ELSIF as[1]="o" THEN  NextArg(fno);
  
        ELSIF as[1]="s" THEN
          NextArg(as);
          IF NOT StrToFix(val, as) THEN Err("-s <shift>") END;
          IF (val>0.5) OR (val<-0.5) THEN  Err("shift range +-0.5 (of sampelrate)") END;
          shiftstep:=VAL(INTEGER, val*4294967150.0);

        ELSIF as[1]="r" THEN
          NextArg(as);
          IF NOT StrToFixL(vall, as) THEN Err("-r <ratio>") END;
          IF (vall<0.02) OR (vall>5.0) THEN  Err("ratio range 0.001..5.0 (of input sampelrate)") END;
          samprate:=VAL(INTEGER, 16777216.0/vall);
          sr:=vall;

        ELSIF as[1]="a" THEN
          NextArg(as);
          IF NOT StrToFix(val, as) THEN Err("-a <aliasingfilter>") END;
          IF (val<=0.0) OR (val>1.0) THEN  Err("filter range 0.0..1.0 (of input sampelrate)") END;
          fg:=val*0.5;

        ELSIF as[1]="l" THEN
          NextArg(as);
          IF NOT StrToCard(as, firlen) THEN Err("-l <fir length>") END;
          IF (firlen>FIRMAX) OR (firlen<4) THEN Err("max fir length 4096") END;

        ELSIF as[1]="F" THEN
          NextArg(as);
          IF (as[0]="u") & (as[1]="8") THEN inform:=0;
          ELSIF (as[0]="i") & (as[1]="1") & (as[2]="6") THEN inform:=1;
          ELSIF (as[0]="f") & (as[1]="3") & (as[2]="2") THEN inform:=2;
          ELSE Err("input formats u8 i16 f32") END;

        ELSIF as[1]="f" THEN
          NextArg(as);
          IF (as[0]="u") & (as[1]="8") THEN outform:=0;
          ELSIF (as[0]="i") & (as[1]="1") & (as[2]="6") THEN outform:=1;
          ELSIF (as[0]="f") & (as[1]="3") & (as[2]="2") THEN outform:=2;
          ELSE Err("output formats u8 i16 f32") END;

        ELSIF as[1]="g" THEN
          NextArg(as);
          IF NOT StrToFix(gain, as) THEN Err("-g <factor>") END;

        ELSIF as[1]="v" THEN
          NextArg(as);
          IF NOT StrToCard(as, statistic) THEN Err("-v <cnt>") END;

        ELSIF as[1]="h" THEN
          WrStrLn("");
          WrStrLn(" shift frequency and up/downsample iq-file");
          WrStrLn(" -a <aliasingfilter> manual set antialiasing filter 0.0..1.0 else automatic");
          WrStrLn(" -F <input format>   u8 i16 f32 (u8)");
          WrStrLn(" -f <output format>  u8 i16 f32 (u8)");
          WrStrLn(" -g <factor>         gain (1.0)");
          WrStrLn(" -h");
          WrStrLn(" -i <infile>         iq file");
          WrStrLn(" -l <fir length>     fir length 4..4096 with linear more cpu usage (64)");
          WrStrLn(" -o <outfile>        iq file"); 
          WrStrLn(" -r <ratio>          output to input sampelrate 0.02..5.0 (1.0)");
          WrStrLn(" -s <shift>          shift frequency before resample -0.5..0.5 of iq rate (0.0)");   
          WrStrLn(" -v <cnt>            show output level, overdrives on stderr every <cnt> output sample (0)");
          HALT

        ELSE Err(as) END;
    
      ELSE Err("use -h") END; 
    ELSE Err("use -h") END;
  END;

  srin:=sr;
  IF srin>1.0 THEN srin:=1.0 END;
  IF (fg=0.0) & (srin<>0.0) & (firlen>=8) THEN fg:=srin*0.5-1.2/FLOAT(firlen) END;   (* auto aliasingfilter *)
  IF fg<=0.005 THEN
    IF sr<>0.0 THEN Err("(auto) aliasing fail, set filter manual (-a) and/or use longer filter (-l) ") END;
    fg:=0.005;                                            (* like pass thru *)
  END;

  fi:=OpenRead(fni); 
  IF fi<0 THEN Err("iq file open") END;

  fo:=OpenWrite(fno);
  IF fo<0 THEN Err("iq file write") END;

  shiftout:=FALSE;
  shiftin:=FALSE;
  IF shiftstep>0 THEN
    mkdds;
    IF sr>1.0 THEN shiftout:=TRUE ELSE shiftin:=TRUE END;
  END;

  mkfir(fg, gain, SUBSAMP, firlen, firraw);
  FOR i:=0 TO firlen*SUBSAMP-1 DO firtab[i]:=firraw[i MOD firlen*SUBSAMP + SUBSAMP-1-i DIV firlen] END;
--  FOR i:=0 TO firlen*SUBSAMP-1 DO firtab[i]:=firraw[(firlen-1-i MOD firlen)*SUBSAMP + SUBSAMP-1-i DIV firlen] END;

  prefill:=firlen;
  lenb:=0;
  pb:=0;
  eof:=FALSE;
  wp:=0;
  rp:=0;
  phasereg:=0;
  wp:=0;
  sampc:=0;
  rl:=0;
  REPEAT
    REPEAT
      WITH ib[rl] DO 
        re:=inword();
        im:=inword();
--shift
        IF shiftin THEN
          WITH DDS[phasereg DIV 65536] DO
            Re:=re*si - im*co;
            Im:=re*co + im*si;
          END;  
          INC(phasereg, shiftstep);
--shift
        ELSE Re:=re; Im:=im END;
      END;
      INC(rl);
    UNTIL eof OR (rl>HIGH(ib));

--fir
    
    rp:=0;
    REPEAT
      sum.Re:=0.0;
      sum.Im:=0.0;
      fp:=ASH(sampc,SUBSAMPBITS-24) MOD SUBSAMP * firlen;    (* interpolate *)
--WrStr("+");
--WrCard(rp+firlen-1,6);
--WrCard(rl,6);
--WrFixed(ib[rp+firlen-1].Re, 2, 8);
--WrStr("-");
      FOR i:=rp TO rp+firlen-1 DO 
        f:=firtab[fp];
        INC(fp);
        sum.Re:=sum.Re+ib[i].Re*f;
        sum.Im:=sum.Im+ib[i].Im*f;
      END;
--shift
      IF shiftout THEN
        WITH DDS[phasereg DIV 65536] DO
          WrSamp(sum.Re*si - sum.Im*co);
          WrSamp(sum.Re*co + sum.Im*si);
        END;
        INC(phasereg, shiftstep);
      ELSE  
--shift
--IF NOT ODD(test) THEN
        WrSamp(sum.Re);
        WrSamp(sum.Im);
--END;
--INC(test); 
      END;
      INC(sampc, samprate);
      INC(rp, ASH(sampc,-24));
      sampc:=CAST(CARDINAL, CAST(SET32, sampc)*SET32{0..23});
    UNTIL rp+firlen>=rl;
--fir
--WrStr("M");
--WrCard(rp,6);
    rl:=HIGH(ib)+1-rp;
--  MOVE(ADR(ib[rp]), ADR(ib), rl*SIZE(FSAMP));
    MOVE(ADR(ib[rp]), ADR(ib), firlen*SIZE(FSAMP));
  UNTIL eof;
  IF wp>0 THEN WrBin(fo, ob, wp) END;                            (* flush outbuf *)
  IF (statistic<>0) & (outsamps>0) THEN WrStat END;

  Close(fi);
  Close(fo);


END downsample.