MODULE RandCurvDist EXPORTS Main;

IMPORT  LR3;
FROM  LR3 IMPORT Dist, Dir;

IMPORT  LR4x4;
FROM  LR4x4 IMPORT Mul, Identity;

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

FROM PZTypes IMPORT
   PZLR3Chain.T,PZIntChain.T,CodedChain;

FROM PZProc IMPORT
  PZLR3Chain.Read, PZLR3Chain.Match,SegDist1,PZLR3Chain.Map, PZMatrix.Write;
   
FROM Stdio IMPORT stderr;   
FROM PZTypes IMPORT LONG, INT, NAT, BOOL;

<* FATAL Wr.Failure, Thread.Alerted, OSError.E *>

TYPE
  MismatchOptions = PZMismatch.Options;
  Options = RECORD
    inFile: TEXT;       (* Input float chain file (without ".flc") *)
    outFile: TEXT;      (* Output float/curv chain file name (without ".flc") *)
    lStart : CARDINAL;
    lEnd : CARDINAL;
    step : CARDINAL;
    nFrag : CARDINAL;
    nMax : CARDINAL;
  END;
  
  
PROCEDURE Main() =
  VAR d: LONGREAL;
      match : REF PZIntChain.T;
      nTimes : CARDINAL; 
      fName2, fName1 : TEXT;
      cont1, cont2 : CARDINAL;
  BEGIN 
    WITH 
      o = GetOptions(),
      r = NEW(Random.Default).init(),
      wrFile = FileWr.Open(o.outFile & ".plt")
    DO   
      FOR length:=o.lStart TO o.lEnd BY o.step DO 
        nTimes := 0;
        REPEAT
          cont1 :=  r.integer(0,o.nFrag);
          fName1 := o.inFile & "-" & Fmt.Pad(Fmt.Int(cont1), 4, '0');
          REPEAT 
            cont2 :=  r.integer(0,o.nFrag); 
          UNTIL cont2 # cont1;
          fName2 := o.inFile & "-" & Fmt.Pad(Fmt.Int(cont2), 4, '0');
          Wr.PutText(stderr, fName1 & "\n");
          Wr.PutText(stderr, fName2 & "\n");
          WITH 
            c1 = PZLR3Chain.Read(FileRd.Open(fName1 & ".flc")),
            c2 = PZLR3Chain.Read(FileRd.Open(fName2 & ".flc")),
            sc1 = ComputeRandomSubFloatChain(c1.chain^,FLOAT(length,LONGREAL)),
            n1 = NUMBER(sc1^),
            sc2 = ComputeRandomSubFloatChain(c2.chain^,FLOAT(length,LONGREAL)),
            n2 = NUMBER(sc2^),
            m = ComputeTransformationMatrix(sc1[0], sc1[n1-1], sc2[0], sc2[n2-1]),
            sc3 = PZLR3Chain.Map(sc2^ ,m)
          DO   
            PZMatrix.Write(stderr, m); 
            PZLR3Chain.Match(sc1^,sc3^,SegDist1,d,match);
          END;
          Wr.PutText(wrFile, Fmt.Int(length)); Wr.PutText(wrFile, "  ");
          Wr.PutText(wrFile, Fmt.LongReal(d));  Wr.PutText(wrFile, "\n");
          Wr.Flush(wrFile);
          INC(nTimes); 
        UNTIL nTimes = o.nMax; 
      END; 
    END;
  END Main;


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("-lStart");
        o.lStart := pp.getNextInt();
        
        pp.getKeyword("-lEnd");
        o.lEnd := pp.getNextInt();
        
        pp.getKeyword("-step");
        o.step := pp.getNextInt();
        
        pp.getKeyword("-nFrag");
        o.nFrag := pp.getNextInt();
        
        pp.getKeyword("-nMax");
        o.nMax := pp.getNextInt();
        
        pp.finish();                                       
      EXCEPT                                                            
      | ParseParams.Error =>                                              
          Wr.PutText(stderr, "Usage: RandCurvDist \\\n");
          Wr.PutText(stderr, "   -inFile   <fname> \\\n");
          Wr.PutText(stderr, "   -outFile  <fname> \\\n");   
          Wr.PutText(stderr, "   -lStart   <number> \\\n"); 
          Wr.PutText(stderr, "   -lEnd     <number> \\\n"); 
          Wr.PutText(stderr, "   -step     <number> \\\n");
          Wr.PutText(stderr, "   -nFrag    <number> \\\n");
          Wr.PutText(stderr, "   -nMax     <number> \\\n");
        Process.Exit(1);
      END;
    END;
    RETURN o
  END  GetOptions;


 <*  UNUSED  *>    
  PROCEDURE GenerateCodedChain(length : INTEGER): REF CodedChain = 
    VAR cc : REF CodedChain;
  BEGIN 
    WITH  r = NEW(Random.Default).init() DO 
      cc := NEW(REF CodedChain, length);
      FOR i := 0 TO length-1 DO 
        cc[i] :=  r.integer(0,2);
      END; 
    END;  
    RETURN(cc);
  END GenerateCodedChain;


  PROCEDURE   ComputeRandomSubFloatChain (READONLY c: PZLR3Chain.T; length: LONGREAL;): 
    REF PZLR3Chain.T =
    VAR sc : REF PZLR3Chain.T;
  BEGIN  
    WITH 
      n = NUMBER(c),  
      r = NEW(Random.Default).init(),
      i = r.integer(0,n-1) 
    DO   
      sc := SendChainGreaterThanLength(c,length,i);
      RETURN sc;
    END;  
  END ComputeRandomSubFloatChain;
 

  PROCEDURE ComputeTransformationMatrix(READONLY a1, b1, a2, b2 : LR3.T): LR4x4.T = 
  BEGIN
    WITH 
      c1 = CalculateOrigin(a1,b1),
      c2 = CalculateOrigin(a2,b2),
      u  = CalculateDir(a1,b1),
      v  = CalculateDir(b2,a2),
      T1 = BuildMatrixT(-c1[0],-c1[1]),
      T2 = BuildMatrixT(c2[0],c2[1]),
      R =  BuildMatrixR(u,v)
    DO 
      RETURN Mul(Mul(T1,R),T2); 
    END;
  END ComputeTransformationMatrix;


 PROCEDURE  CalculateDir(READONLY p,q : LR3.T): LR3.T =
  VAR r: LR3.T;
  BEGIN
    r[0] := q[0] - p[0];
    r[1] := q[1] - p[1];
    r[2] := q[2] - p[2];
    RETURN Dir(r);
  END CalculateDir;
 
 
 PROCEDURE BuildMatrixT(x,y : LONGREAL): LR4x4.T = 
 VAR m : LR4x4.T; 
 BEGIN
   m :=  Identity();
   m[0,1] := x;
   m[0,2] := y;
   RETURN m;
 END BuildMatrixT;
 
 
 PROCEDURE BuildMatrixR(u,v : LR3.T): LR4x4.T =
 VAR m : LR4x4.T; 
 BEGIN
   m :=  Identity();
   WITH 
     cos = v[0]*u[0] + v[1] * u[1],
     sin = v[1]*u[0] - u[1] * v[0] 
   DO 
      m[1,1] := cos;
      m[1,2] := sin;
      m[2,1] := -sin;
      m[2,2] := cos;
   END;
   RETURN m;
 END BuildMatrixR;
 
 
  PROCEDURE CalculateOrigin( READONLY p0: LR3.T; READONLY p1: LR3.T): LR3.T =
    VAR  o: LR3.T;
  BEGIN 
    FOR i := 0 TO 2 DO  
      o[i] := (p1[i] + p0[i]) / 2.0d0;
    END;
    RETURN o  
  END CalculateOrigin;



  PROCEDURE SendChainGreaterThanLength(
    READONLY q : PZLR3Chain.T; 
    length : LONGREAL; 
    s : CARDINAL; 
  ): REF PZLR3Chain.T =
    VAR d: LONGREAL;
        i: CARDINAL;  
  BEGIN 
    WITH 
      n = NUMBER(q)
    DO 
      d := 0.0d0; i:= 0;
      REPEAT  
        INC(i);
        d := Dist(q[(i+s) MOD n] , q[(i+s-1) MOD n]) + d;
        Wr.PutText(stderr, "d : " & Fmt.LongReal(d) &  "\n"); 
        UNTIL d > length OR i = n;
      WITH 
        a = NEW(REF PZLR3Chain.T, i)
      DO 
        FOR k:=s TO (s+i-1) MOD n DO 
          a[k-s] := q[k];
        END;
        RETURN a;
      END;   
    END;
 END SendChainGreaterThanLength;


BEGIN
  Main()
END RandCurvDist.