MODULE PZTestMinimizer EXPORTS Main;

IMPORT Minimizer, CoordMinimizer;
IMPORT BrentUniMin;

FROM Minimizer IMPORT Point, Gradient, Value;

IMPORT  LR3;
FROM  LR3 IMPORT Dir;

IMPORT  LR4x4;
FROM  LR4x4 IMPORT Mul, Identity;

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

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

FROM PZProc IMPORT
  PZLR3Chain.Write, PZLR3Chain.Read, PZLR3Chain.Match, SegDist2, PZLR3Chain.Map, 
  PZMatrix.Write, ChangeFloatChain, PZPlot.PSMatchingWithCaption;
   
FROM Stdio IMPORT stderr;   

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

TYPE
  Options = RECORD
    inFile1: TEXT;       (* Input OPEN float chain file (without ".flc") *)
    start1:  LONGREAL; 
    stop1:  LONGREAL;
    inFile2: TEXT;       (* Input OPEN float chain file (without ".flc") *)
    start2:  LONGREAL; 
    stop2:  LONGREAL;
    maxEvals: CARDINAL; 
    noGuess : BOOLEAN; 
  END;
  
  Reg  = RECORD 
        start : CARDINAL;
        len : CARDINAL; 
  END; 
  
  RegInf = ARRAY OF Reg; 
  
PROCEDURE Main() =
  VAR d: LONGREAL;
      match : REF PZIntChain.T;
      m : LR4x4.T;
      comments : TEXT;
      aMap: REF PZLR3Chain.T;
  BEGIN 
    WITH 
      o = GetOptions(),
      rra = PZLR3Chain.Read(FileRd.Open(o.inFile1 & ".flc")), 
      sizea = NUMBER(rra.chain^),
      starta = ROUND(o.start1 * FLOAT(sizea, LONGREAL)), 
      na = ROUND((o.stop1 - o.start1) * FLOAT(sizea, LONGREAL)) MOD (sizea+1),
      rrb = PZLR3Chain.Read(FileRd.Open(o.inFile2 & ".flc")),
      sizeb = NUMBER(rrb.chain^),
      startb = ROUND(o.start2 * FLOAT(sizeb, LONGREAL)), 
      nb = ROUND((o.stop2 - o.start2) * FLOAT(sizeb, LONGREAL)) MOD (sizeb+1),
      reg =  NEW(REF RegInf, 2)^, 
      x = NEW(REF Point, 3)^,
      ra = GetSubChain( rra.chain^, na, starta, reg[0] ),
      
      (* ComputeRandomSubFloatChain(rra.chain^, o.length, reg[0]), *)
      
      a = ra^,
      rb = GetSubChain( rrb.chain^, nb, startb, reg[1] ),
      
      (* ComputeRandomSubFloatChain(rrb.chain^, o.length, reg[1]), *)
      
      b  = rb^
    DO   
      PZLR3Chain.Write(stderr, "a: ", a);
      PZLR3Chain.Write(stderr, "b: ", b);
      ChangeFloatChain(rb); 
      comments := PZMinimizerComments(o, reg);
      IF o.noGuess THEN  m := Identity();
        ELSE m := ComputeTransformationMatrix(a[0], a[na-1], b[0], b[nb-1]);
      END;  
      PZMatrix.Write(stderr, m); 
      x[0] := m[0, 1];
      x[1] := m[0, 2];
      x[2] := Math.atan2(m[1, 2], m[1, 1]);
      ComputeMappedChainDist(a, b, aMap, x, d, match);
      PZPlot.PSMatchingWithCaption(o.inFile1 & "-" & o.inFile2 & "-a.ps", 
          match^, aMap^, b, comments & "                 d      : " & Fmt.LongReal(d));
      BrentMinimizer(a, b, o.maxEvals, d, match, x);
      ComputeMappedChainDist(a, b, aMap, x, d, match);
      PZPlot.PSMatchingWithCaption(o.inFile1 & "-" & o.inFile2 & ".ps", 
          match^, aMap^, b, comments & "                 d      : " & Fmt.LongReal(d));
    END;  
  END Main;

PROCEDURE ComputeMappedChainDist(
    READONLY a, b: PZLR3Chain.T;
    VAR aMap: REF PZLR3Chain.T;
    READONLY x: LR3.T;
    VAR d: LONGREAL;
    VAR match: REF PZIntChain.T;
  ) =
  VAR m: LR4x4.T;
  BEGIN
    m := BuildMatrixT(x[0], x[1]);
    m[1, 1] := Math.cos(x[2]);
    m[1, 2] := Math.sin(x[2]);
    m[2, 1] := -m[1, 2];
    m[2, 2] := m[1, 1];
    aMap := PZLR3Chain.Map(a, m);
    PZLR3Chain.Match(aMap^, b, SegDist2, d, match, removeDanglingEnds := TRUE );
  END ComputeMappedChainDist; 

PROCEDURE BrentMinimizer(
    READONLY a, b : PZLR3Chain.T; 
    mE : CARDINAL;  
    VAR d : LONGREAL; 
    match : REF PZIntChain.T;
    VAR x : Point;
  ) = 
  VAR 
    f : Value;
  BEGIN
    WITH 
      brent = NEW(BrentUniMin.T), 
      cmin =  NEW(CoordMinimizer.T).setUniMin(brent).setBudget(5).setSize(3), 
      g    =  NEW(REF Gradient, 3)^
    DO
 
      PROCEDURE Evaluate(READONLY x: Point; VAR f: Value; VAR g: Gradient;)=
        VAR aMap: REF PZLR3Chain.T;
        BEGIN
          ComputeMappedChainDist(a, b, aMap, x, d, match);
          f := d;  
          Wr.PutText(stderr, "f: " & Fmt.LongReal(f));  Wr.PutText(stderr, "\n");
        END Evaluate;
     
      BEGIN 
        Evaluate(x, f, g);

        cmin.debug := TRUE; 
        
        cmin.minimize(
          eval := Evaluate,     (* Evaluates the goal function. *)
          x := x,               (* In: initial guess, out: minimum point. *) 
          f := f,               (* In/out: function value at "x" *)
          g := g,               (* In/out: gradient of "f" at "x" *)
          dist := 1.0d-1,       (* Guessed distance from optimum *)
          flat := 0.0d0,        (* Stopping criterion *)
          tol := 1.0d-8,        (* Guessed radius of optimum region *)
          maxEvals := mE,       (* Max number of "eval"s allowed *)
          trace := NIL          (* A path reporting procedure *)
        );
        Evaluate(x, f, g);
        d := f;
      END;
    END;
  END BrentMinimizer;


PROCEDURE GetOptions(): Options =
  VAR o: Options;
  BEGIN
    WITH
      pp = NEW(ParseParams.T).init(stderr)
    DO
      TRY
        pp.getKeyword("-inFile1");
        o.inFile1 := pp.getNext();
        
        o.start1 := pp.getNextLongReal(0.0d0, 1.0d0);
        o.stop1 := pp.getNextLongReal(0.0d0, 1.0d0);
        
        pp.getKeyword("-inFile2");
        o.inFile2 := pp.getNext();
        
        o.start2 := pp.getNextLongReal(0.0d0, 1.0d0);
        o.stop2 := pp.getNextLongReal(0.0d0, 1.0d0);
        
        pp.getKeyword("-maxEvals");
        o.maxEvals := pp.getNextInt(); 

        IF pp.keywordPresent("-noGuess") THEN
          o.noGuess := TRUE;  
        ELSE
          o.noGuess := FALSE 
        END;
        
        pp.finish();                                       
      EXCEPT                                                            
      | ParseParams.Error =>                                              
          Wr.PutText(stderr,  "Usage: PZTestMinimizer \\\n");
          Wr.PutText(stderr,  "   -inFile1   <fname> <start1> <stop1> \\\n");
          Wr.PutText(stderr,  "   -inFile2   <fname> <start2> <stop2>  \\\n");
          Wr.PutText(stderr,  "   -maxEvals  <cardinal> \\\n");
          Wr.PutText(stderr,  "   [ -noGuess ]          \\\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 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(a2, b2), 
      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 GetSubChain(
    READONLY q : PZLR3Chain.T; 
    length : CARDINAL; 
    s : CARDINAL; 
    VAR reg : Reg;
  ): REF PZLR3Chain.T =
  BEGIN 
    WITH 
      n = NUMBER(q)
    DO 
      WITH 
        a = NEW(REF PZLR3Chain.T, length)
      DO 
        FOR k:=0 TO length-1 DO 
          a[k] := q[(k+s) MOD n];
        END;
        reg.start := s; 
        reg.len := NUMBER(a^);
        RETURN a;
      END;   
    END;
 END GetSubChain;


PROCEDURE PZMinimizerComments(
  READONLY o: Options; 
  READONLY reg: RegInf; 
  ):TEXT=
BEGIN 
  WITH 
    wr = NEW(TextWr.T).init()
  DO 
    Wr.PutText( wr, "PZTestMinimizer  inFile1: " & o.inFile1 & "\n");
    Wr.PutText( wr, "                 start  : " & Fmt.LongReal(o.start1) & "\n"); 
    Wr.PutText( wr, "                 stop   : " & Fmt.LongReal(o.stop1) & "\n"); 
    Wr.PutText( wr, "                 istart : " & Fmt.Int(reg[0].start) & "\n");
    Wr.PutText( wr, "                 length : " & Fmt.Int(reg[0].len) & "\n\n");
    Wr.PutText( wr, "----------------------------------------------------\n");
    Wr.PutText( wr, "                 inFile2: " & o.inFile2 & "\n");
    Wr.PutText( wr, "                 start  : " & Fmt.LongReal(o.start2) & "\n"); 
    Wr.PutText( wr, "                 stop   : " & Fmt.LongReal(o.stop2) & "\n"); 
    Wr.PutText( wr, "                 istart : " & Fmt.Int(reg[1].start) & "\n");
    Wr.PutText( wr, "                 length : " & Fmt.Int(reg[1].len) & "\n\n");
    RETURN(TextWr.ToText(wr))
  END (* DO *);
END PZMinimizerComments;
 
BEGIN
  Main()
END PZTestMinimizer.