MODULE PZMultiScale EXPORTS Main;

(*
  Filter a curve with "a-posteriori" parametrization using multi-scale filtering 
*)

IMPORT  LR3;
FROM  LR3 IMPORT Dist;

IMPORT 
  PZTypes, PZProc, ParseParams, Process, FileRd, Wr, FileWr, Math,
  OSError, Thread, Stdio, Fmt, TextWr;

FROM PZTypes IMPORT
   FloatChain, FloatPos,  FourierChain;

FROM PZProc IMPORT
   ReadFloatChain, WriteFloatChain, InterpolatePoints, FFT;
  
FROM Stdio IMPORT stderr;
 
<* FATAL Wr.Failure, Thread.Alerted, OSError.E *>

TYPE
  Options = RECORD
      inFile: TEXT;     (* Input float chain file (without ".flc") *)
      outFile: TEXT;    (* Output float/curv chain file name (without ".flc") *)
      lNoise: LONGREAL; (* Min wave length to allow thru *)
      nIter: CARDINAL;  (* Number of iterations *)
    END;
 
PROCEDURE Main() =
  VAR
    k, n : CARDINAL; 
    lMin, lMax, step : LONGREAL; 
    pAnt, pNow : REF FloatChain;
    t : REF ARRAY OF LONGREAL; 
  BEGIN
    WITH
      o = GetOptions(),
      rp = ReadFloatChain(FileRd.Open(o.inFile & ".flc")),
      cs = rp.comments
    DO 
      pAnt := rp.chain; 
      t := ComputeInitialTimes(pAnt^);
      n := LAST(pAnt^);
      lMin := o.lNoise;
      lMax := 2.0d0 * lMin;
      k := 0;
      WHILE lMax < t[n]/ 2.0d0  DO 
        step := lMin / 4.0d0;
        pNow := ComputeFilteredCurve(pAnt,t^,lMin,lMax,o.nIter, step); 
        WriteFloatChain(FileWr.Open(o.outFile &  Fmt.Int(k) & ".flc"), 
                         cs & PZMultiScaleComments(o.inFile,
                                                   lMin, 
                                                   lMax,
                                                   t[n],
                                                   n+1), pNow^);
        INC(k);
        pAnt := pNow;
        t := ComputeInitialTimes(pAnt^);
        lMin := lMax;
        lMax := 2.0d0 * lMin; 
        n := LAST(pAnt^);
      END;   
    END (* with *)
  END Main;

PROCEDURE ComputeFilteredCurve(  p: REF FloatChain;
                                 VAR t: ARRAY OF LONGREAL;
                                 lMin : LONGREAL;
                                 lMax : LONGREAL;
                                 nIter : CARDINAL;
                                 step  : LONGREAL; 
                               ) : REF FloatChain =
  VAR
    result:  REF FloatChain;
    cont: CARDINAL; 
  BEGIN
    WITH
      n = NUMBER(p^),
      m = SelectFourierOrder(lMin,t[n]),
      W = NEW(REF ARRAY OF LONGREAL, m)^,
      u = NEW(REF FloatChain, m)^,
      U = NEW(REF FourierChain, 3,m)^
    DO 
      ComputeUniformlySpacedPoints(p^,t,u);
      FloatChainToFourierChain(u,U);
      Wr.PutText(stderr, "m :" & Fmt.Int(m) & " \n");
      WHILE  cont < nIter DO 
        ComputeUniformlySpacedPoints(p^,t,u);  
        Wr.PutText(stderr, "t[" & Fmt.Int(LAST(t)) & "]" & "=" 
          & Fmt.LongReal(t[n]) & " \n");
        FloatChainToFourierChain(u,U);
        ComputeSmoothBoxFilter(W,t[n], lMax, lMin);
        ApplyFilter(U,W); 
        FourierChainToFloatChain(U,u);
        RecomputeTimes(u,t);
        INC(cont);
      END; (* while  *) 
      result := InterpolateFilteredChain(u, t[n], step);
      RETURN result;
    END (* with *)
  END ComputeFilteredCurve;


PROCEDURE ComputeSmoothBoxFilter(VAR v: ARRAY OF LONGREAL; t, lMax, lMin: LONGREAL) = 
  BEGIN  
    WITH 
      N = NUMBER(v), 
      M = 0.50d0, 
      fmin = t / lMax,
      fmax = t / lMin
    DO  
      Wr.PutText(stderr, "fmin: " & Fmt.LongReal(fmin) & " \n") ;
      Wr.PutText(stderr, "fmax: " & Fmt.LongReal(fmax) & " \n") ;
      FOR i := 0 TO N-1 DO
        WITH
          f = MIN(i,N-i)
        DO
          IF f < TRUNC(fmin) THEN v[i] := 1.00d0
            ELSIF f > TRUNC(fmax) THEN v[i] := 0.00d0   
              ELSE
                v[i] := M * (  1.00d0 + 
                   Math.cos(FLOAT(Math.Pi, LONGREAL) * (FLOAT(f,LONGREAL)-fmin) 
                   / (fmax-fmin))) ;
                  (*  Wr.PutText(stderr, "f: " & Fmt.LongReal(f) & " \n")  *)     
          END
        END
      END  
    END
  END ComputeSmoothBoxFilter;
 
 
PROCEDURE FloatChainToFourierChain(
  READONLY  up : FloatChain;
  VAR UF : FourierChain) 
  =
  BEGIN 
    WITH 
      N = NUMBER(up),
      r = NEW(REF ARRAY OF LONGREAL, N)^
    DO 
      FOR i := 0 TO 2 DO 
        FOR j := 0 TO N-1 DO 
          r[j] := up[j,i]
        END;
        FFT(r,UF[i])
      END;
    END
  END FloatChainToFourierChain;  

 
PROCEDURE FourierChainToFloatChain(
  VAR UF : FourierChain;
  VAR up : FloatChain) 
  =
  VAR 
    r : REF ARRAY OF LONGREAL;   
  BEGIN 
    WITH 
      N = NUMBER(up)
    DO 
      r:= NEW(REF ARRAY OF LONGREAL, N);
      FOR i := 0 TO 2 DO 
        FFT(UF[i],r^);
        FOR j := 0 TO N-1 DO 
          up[j,i] := r[j];
        END
      END
    END
  END FourierChainToFloatChain;  

 
PROCEDURE ApplyFilter(
  VAR UF : FourierChain;
  READONLY W : ARRAY OF LONGREAL) =
  BEGIN 
    WITH 
      N = NUMBER(UF[0])   
    DO 
      FOR i:= 0 TO N-1 DO 
        FOR j := 0 TO 2 DO 
          (* Wr.PutText(stderr, "W: " & Fmt.LongReal(W[i]) & " \n"); *)
          UF[j,i] := UF[j,i] * W[i]  
        END
      END
    END    
 END ApplyFilter;

 
PROCEDURE RecomputeTimes(
  READONLY  u:  FloatChain;   
  VAR   t:  ARRAY OF LONGREAL)
  =  
  VAR k, j : CARDINAL;
      q, qAnt : FloatPos;
      d: LONGREAL;
  BEGIN   
    WITH 
      m = NUMBER(u),
      n = NUMBER(t)-1,
      delta = t[n] / FLOAT(m, LONGREAL) 
    DO
      Wr.PutText(stderr, "recomputing qi\n");
      q := u[0]; j:= 0; 
      <* ASSERT t[0] = 0.0d0 *>
      FOR i:= 1 TO n  DO 
        qAnt := q;
        k := TRUNC(t[i]/delta);
        IF k > m-1 THEN k:=m-1 END;
        q   := InterpolatePoints
                  (t[i], FLOAT(k,LONGREAL) * delta, u[(k MOD m)], 
                  FLOAT(k+1, LONGREAL) * delta, u[((k+1) MOD m)]);
        <* ASSERT k >= j *>
        IF k = j THEN
          d := Dist(qAnt,q)
        ELSE
          d := Dist(qAnt,u[((j+1) MOD m)]);
          j := j+1;
          WHILE j < k DO
            d := d + Dist(u[(j MOD m)], u[((j+1) MOD m)]);
            j := j+1
          END;
          d := d +  Dist(u[(j MOD m)], q) 
        END;
         t[i] := t[i-1] + d;
      END;
    END;  
  END RecomputeTimes;
 

PROCEDURE GetOptions(): Options =
  VAR o: Options;
  BEGIN
    WITH
      pp = NEW(ParseParams.T).init(stderr)
    DO
      TRY
        pp.getKeyword("-inFile");
        o.inFile := pp.getNext();

        pp.getKeyword("-outFile");
        o.outFile := pp.getNext();
        
        pp.getKeyword("-lNoise");
        o.lNoise := pp.getNextLongReal();
        
        pp.getKeyword("-nIter");
        o.nIter := pp.getNextInt();
        
        pp.finish();                                       
      EXCEPT                                                            
      | ParseParams.Error =>                                              
          Wr.PutText(stderr, "Usage: PZMultiScale \\\n");
          Wr.PutText(stderr, "  -inFile <name> \\\n");
          Wr.PutText(stderr, "  -outFile <name> \\\n");
          Wr.PutText(stderr, "  -lNoise <number> \\\n");
          Wr.PutText(stderr, "  -nIter <number> \n");
        Process.Exit(1);
      END;
    END;
    RETURN o
    
  END GetOptions;


PROCEDURE  SelectFourierOrder(lMin, length : LONGREAL ): CARDINAL = 
  BEGIN
     RETURN(FindPowGreaterThanN(4.00d0*length/lMin)) 
  END  SelectFourierOrder;


PROCEDURE ComputeInitialTimes(READONLY q : FloatChain) : REF ARRAY OF LONGREAL=
  VAR d: LONGREAL;
      t: REF ARRAY OF LONGREAL;
  BEGIN 
    WITH 
      n = NUMBER(q)
    DO 
      t := NEW(REF ARRAY OF LONGREAL,n+1);
      d := 0.0d0; t[0] := 0.00d0; 
      FOR i :=1 TO n DO   
        d := Dist(q[i-1] , q[i MOD n]);
        (* Wr.PutText(stderr, "d : " & Fmt.LongReal(d) &  "\n"); *)
        t[i] := d + t[i-1] 
      END;
      Wr.PutText(stderr, "t : " & Fmt.LongReal(t[n]) &  "\n");
      RETURN t
    END
   END ComputeInitialTimes; 
  

PROCEDURE  ComputeUniformlySpacedPoints(
   READONLY p : FloatChain;
   READONLY t : ARRAY OF LONGREAL;
   VAR  u : FloatChain;
   )
   = 
  VAR   r: LONGREAL;
        k : CARDINAL; 
  BEGIN 
    WITH  
      n = NUMBER(p),
      m = NUMBER(u), 
      delta = t[n] / FLOAT(m,LONGREAL)
    DO 
    Wr.PutText(stderr, "delta :=" & Fmt.LongReal(delta)  & "\n");
    r := 0.0d0; k:=0; 
    u[0] := p[0];
    FOR j:= 1 TO m-1 DO 
      r:= r + delta;
      (* Wr.PutText(stderr, "r :=" & Fmt.LongReal(r)  & " "); *)
      WHILE t[k MOD n+1] <= r  DO 
        k:= k+1; 
      END;
       u[j] := InterpolatePoints(r,t[(k-1) MOD n+1],p[(k-1) MOD n], 
                                             t[k MOD n+1], p[k MOD n]);
     END;
   END (*WITH *) 
 END  ComputeUniformlySpacedPoints;
 

PROCEDURE FindPowGreaterThanN(n : LONGREAL): INTEGER   =
  VAR x : LONGREAL;
  BEGIN 
    x:= 1.00d0;
    WHILE Math.pow(2.00d0,x) <= n DO x := x + 1.00d0 END;
      RETURN ROUND(Math.pow(2.00d0,x))
  END FindPowGreaterThanN;


PROCEDURE InterpolateFilteredChain(
  READONLY u:FloatChain; 
  T:LONGREAL; 
  epsilon:LONGREAL;
  ) : REF FloatChain =
  VAR j : CARDINAL;
      alpha : LONGREAL;
  BEGIN 
    WITH
     m = NUMBER(u),
     gamma = T /  FLOAT(m,LONGREAL),
     n = ROUND(T/epsilon),
     epsilonlinha = T / FLOAT(n,LONGREAL),
     rq = NEW(REF FloatChain,n),  
     q  = rq^ 
    DO
      q[0] := u[0];
      FOR i:=1 TO n-1 DO
        j:= TRUNC(FLOAT(i,LONGREAL) * epsilonlinha /gamma);
        (* q[i] estara' entre q[i] e q[i+1] *)
        alpha := FLOAT(i,LONGREAL) * epsilonlinha - FLOAT(j,LONGREAL) * gamma;
        q[i] := InterpolateFourPoints(alpha, 
          -1.0d0, u[(j-1) MOD m],
          00.0d0, u[j MOD m], 
          +1.0d0, u[(j+1)MOD m],
          +2.0d0, u[(j+2)MOD m]
        )  
      END;
      RETURN rq
    END    
  END InterpolateFilteredChain;


PROCEDURE InterpolateFourPoints(
  alpha: LONGREAL;
  t1: LONGREAL; READONLY p1: LR3.T;
  t2: LONGREAL; READONLY p2: LR3.T;
  t3: LONGREAL; READONLY p3: LR3.T;
  t4: LONGREAL; READONLY p4: LR3.T;
  ): LR3.T =
  BEGIN
    WITH
      p12 = InterpolatePoints(alpha, t1, p1, t2, p2),
      p23 = InterpolatePoints(alpha, t2, p2, t3, p3),
      p34 = InterpolatePoints(alpha, t3, p3, t4, p4),
      p123 = InterpolatePoints(alpha, t1, p12, t3, p23),
      p234 = InterpolatePoints(alpha, t2, p23, t4, p34),
      p1234 = InterpolatePoints(alpha, t1, p123, t4, p234)
    DO
      RETURN p1234
    END
  END InterpolateFourPoints;

<* UNUSED *>
PROCEDURE WriteChainPowerSpectrum(
  wr :Wr.T;
  READONLY c: FourierChain;
  READONLY T: LONGREAL;
  )=
  VAR  
    g: LONGREAL;
    lambda : LONGREAL;
  BEGIN  
    WITH 
      n = NUMBER(c[0]),  
      n2 = n DIV 2 
    DO 
      Wr.PutText(wr, Fmt.LongReal(T)); Wr.PutText(wr, "  ");
      FOR j:= 0 TO 2 DO 
        Wr.PutText(wr, Fmt.LongReal(c[j,0]));  Wr.PutText(wr, "  ");
      END;
      Wr.PutText(wr, "\n");
      FOR i := 1 TO n2 - 1  DO
        lambda := T / FLOAT(i,LONGREAL);
        Wr.PutText(wr, Fmt.LongReal(lambda)); Wr.PutText(wr, "  ");
        FOR j := 0 TO 2 DO 
          g := c[j,i]*c[j,i] + c[j,n-i]*c[j,n-i]; 
          Wr.PutText(wr, Fmt.LongReal(g));  Wr.PutText(wr, "  ")
        END; 
        Wr.PutText(wr, "\n")
      END;
      Wr.PutText(wr, Fmt.LongReal(T/FLOAT(n2,LONGREAL))); Wr.PutText(wr, "  ");
      FOR j:=0 TO 2 DO 
        Wr.PutText(wr, Fmt.LongReal(c[j,n2]));  Wr.PutText(wr, "  ")
      END;
      Wr.Flush(wr)
    END    
  END WriteChainPowerSpectrum;

   
PROCEDURE PZMultiScaleComments(READONLY inFile : TEXT;
                               READONLY lMin ,lMax, tn : LONGREAL;
                               READONLY n : CARDINAL):TEXT=
BEGIN 
  WITH 
    wr = NEW(TextWr.T).init()
  DO 
    Wr.PutText( wr, "PZMultiScale  inFile: " & inFile & "\n");
    Wr.PutText( wr, "              lMin: " & Fmt.LongReal(lMin) & "\n");
    Wr.PutText( wr, "              lMax: " & Fmt.LongReal(lMax) & "\n");
    Wr.PutText( wr, "              t[n]: " & Fmt.LongReal(tn) & "\n");
    Wr.PutText( wr, "              n: "    & Fmt.Int(n)& "\n");  
    RETURN(TextWr.ToText(wr))
  END (* DO *);
END PZMultiScaleComments;

 
BEGIN
  Main()
END PZMultiScale.

(********************************************************************)

    
PROCEDURE  GiveBackInitialSextuples(
   READONLY  a: CurvChain; 
   READONLY  b: CurvChain;
   ia : CARDINAL;
   ib : CARDINAL; 
   cutDist: CARDINAL;
  ): REF ListSextuple =
  VAR 
    r : REF ListSextuple;
    n  : CARDINAL := 0;
    nt : CARDINAL := 0;
    ltupla : REF ListSextuple;
  BEGIN 
    WITH 
      Na = NUMBER(a),
      Nb = NUMBER(b),
      m = ComputeDistanceMatrixFromCurvChain(a,b) 
    DO
      Wr.PutText(stderr, "GiveBackInitialSextuples \n");  
      ltupla := NEW(REF ListSextuple, Na * Nb * NMatch ); 
      nt := NUMBER(ltupla^); 
      FOR i := 0 TO Na-1  DO 
        FOR j:= 0 TO Nb-1  DO 
          IF m[i,j] < cutDist THEN 
           (* Wr.PutText(stderr, "m[" &  Fmt.Int(i) & ","& Fmt.Int(j) & "] :" 
                & Fmt.Int(m[i,j]) & " \n");
            *)
            IF n >= nt  THEN 
              WITH ttupla = NEW(REF ListSextuple, 2 * nt) DO 
                SUBARRAY(ttupla^,0,n) := SUBARRAY(ltupla^,0,n);
                ltupla := ttupla; 
                nt := NUMBER(ltupla^);
              END;
            END;  
            Wr.PutText(stderr, "n :" & Fmt.Int(n) & " \n");
            WITH sext = ltupla^[n] DO
              sext[0].k := ia;
              sext[1].k := ib;
              FOR k:=0 TO 1  DO  
                sext[k].i := FLOAT(i,LONGREAL)-0.5d0; 
                sext[k].j := FLOAT(j,LONGREAL)+0.5d0;
              END;
            END;
            INC(n);
          END;  
        END; 
      END;  
      r := NEW(REF ListSextuple,n); 
      SUBARRAY(r^,0,n) :=  SUBARRAY(ltupla^,0,n);
      RETURN r
    END;
  END  GiveBackInitialSextuples; 
 
 
 PROCEDURE ConcatenateList(x: REF ListSextuple;  READONLY y: ListSextuple; 
   VAR noc: CARDINAL )=   
   BEGIN 
     WITH 
       nx = NUMBER(x^),
       ny = NUMBER(y)
     DO
       Wr.PutText(stderr, "noc :" & Fmt.Int(noc) & " \n");
       Wr.PutText(stderr, "nx :" & Fmt.Int(nx) & " \n");
       Wr.PutText(stderr, "ny :" & Fmt.Int(ny) & " \n");
       IF noc + ny > nx THEN 
         WITH nlist = NEW(REF ListSextuple, 2*noc + ny) DO 
            SUBARRAY(nlist^,0,noc) := SUBARRAY(x^,0,noc);
            x := nlist;
         END;
       END;  
       SUBARRAY(x^,noc,ny) := SUBARRAY(y,0,ny);
       noc := noc+ny;
     END;
   END  ConcatenateList;


PROCEDURE PutInTheArray ( 
   ltupla : REF ListSextuple;
   VAR nt : CARDINAL;
   ia : LONGREAL;
   ja : LONGREAL;
   ib : LONGREAL;
   jb : LONGREAL;
   ka : CARDINAL;
   kb : CARDINAL; 
  )=
  BEGIN 
    WITH n = NUMBER(ltupla^)  DO 
      IF nt >= n  THEN 
        WITH ttupla = NEW(REF ListSextuple, 2*nt) DO
          SUBARRAY(ttupla^,0,n) := SUBARRAY(ltupla^,0,n);
          ltupla := ttupla;         
        END;   
        ltupla[nt,0].k := ka;  ltupla[nt,1].k := kb;   
        ltupla[nt,0].i := ia;  ltupla[nt,1].i := ib;
        ltupla[nt,0].j := ja;  ltupla[nt,1].j := jb;
      END;
    END;  
  END PutInTheArray;



(**** main ****)

FOR i := 0 TO n-1 DO 
        FOR j := 0 TO n-1 DO 
          IF i # j  THEN 
            tempMatch := GiveBackInitialSextuples(re[i]^, re[j]^, i,j, o.cutDist); 
            IF NUMBER(tempMatch^) > 0 THEN 
             ConcatenateList(basisMatch, tempMatch^, nb); 
            END; 
          END;
        END;
      END;