MODULE PZTestMatch EXPORTS Main;

IMPORT Wr, Thread, ParseParams, Process;
IMPORT PSPlot, LR3;
FROM PSPlot IMPORT Color;
IMPORT PZLR3Chain;
IMPORT PZMatch;
FROM Stdio IMPORT stderr;
FROM Math IMPORT sqrt, sin, cos, atan2;
 
<* FATAL Wr.Failure, Thread.Alerted *>

TYPE 
  
  Options = RECORD
      chainA: ChainData;
      chainB: ChainData;
      outName: TEXT;
      step: LONGREAL;
      maxShift: INTEGER;
      maxDist: LONGREAL;
      cutDist: LONGREAL;        (* Max avg curvature distance for accepting cand. *)
      minLength: LONGREAL;      (* Minimum length of matching segments. *)
    END;
  
  ChainData = RECORD
      ini, fin: LR3.T;
      curvature: LONGREAL;
    END;

PROCEDURE DoIt() =
  BEGIN
    WITH
      o = GetOptions(),
      ps = NEW(PSPlot.PSFile).open(o.outName & ".ps"),
      a = MakePolygonal(o.chainA, o.step)^,
      b = MakePolygonal(o.chainB, o.step)^,
      step = o.step,
      match = MatchPolygonals(
        a, b, 
        o.maxShift,
        o.maxDist, 
        o.cutDist,
        step
      )^
    DO
      ps.beginPage();
        ps.beginDrawing(xSize := 150.0d0, ySize := 150.0d0);
          DrawMatch(ps, a, b, match);
        ps.endDrawing();
      ps.endPage();
      ps.close()     
    END
  END DoIt;
  
PROCEDURE GetOptions(): Options =

  PROCEDURE GetChainData(pp: ParseParams.T): ChainData RAISES{ParseParams.Error} =
    VAR d: ChainData;
    BEGIN
      d.ini[0] := pp.getNextLongReal();
      d.ini[1] := pp.getNextLongReal();
      d.ini[2] := 0.0d0;
      d.fin[0] := pp.getNextLongReal();
      d.fin[1] := pp.getNextLongReal();
      d.ini[2] := 0.0d0;
      d.curvature := pp.getNextLongReal();
      RETURN d
    END GetChainData;
    
  VAR o: Options;
  BEGIN
    WITH
      pp = NEW(ParseParams.T).init(stderr)
    DO
      TRY
        pp.getKeyword("-outName");
        o.outName := pp.getNext();
        
        pp.getKeyword("-maxShift");
        o.maxShift := pp.getNextInt(0);
        
        pp.getKeyword("-step");
        o.step := pp.getNextLongReal();
       
        pp.getKeyword("-maxDist");
        o.maxDist := pp.getNextLongReal(0.0d0);
        
        pp.getKeyword("-cutDist");
        o.cutDist := pp.getNextLongReal(0.0d0);
        
        pp.getKeyword("-chainA");
        o.chainA := GetChainData(pp);
        
        pp.getKeyword("-chainB");
        o.chainB := GetChainData(pp);
        
        pp.finish();                                       
      EXCEPT                                                            
      | ParseParams.Error =>                                              
          Wr.PutText(stderr, "Usage: PZRefineCands \\\n");
          Wr.PutText(stderr, "  -outName NAME \\\n");
          Wr.PutText(stderr, "  -step NUMBER  \\\n");
          Wr.PutText(stderr, "  -maxShift NUM \\\n");
          Wr.PutText(stderr, "  -maxDist NUM \\\n");
          Wr.PutText(stderr, "  -cutDist NUMBER \\\n"); 
          Wr.PutText(stderr, "  -chainA X1 Y1 X2 Y2 CURVATURE \\\n");
          Wr.PutText(stderr, "  -chainB X1 Y1 X2 Y2 CURVATURE \n");
        Process.Exit(1);
      END;
    END;
    RETURN o
  END GetOptions;

PROCEDURE DrawMatch(
    f: PSPlot.File; 
    READONLY a, b: PZLR3Chain.T; 
    READONLY match: PZMatch.T;
  ) =
  BEGIN
    f.gridLines();
    f.setScale(PSPlot.Axis.Y, -5.0d0, 5.0d0);
    f.setScale(PSPlot.Axis.X, -5.0d0, 5.0d0);
    
    f.setLineSolid();
    
    f.setLineWidth(0.15);
    f.setLineColor(Color{0.0, 0.25, 1.0});
    f.coordLine(PSPlot.Axis.X, 0.0d0);
    f.coordLine(PSPlot.Axis.Y, 0.0d0);
    
    f.setLineDashed(ARRAY OF REAL{2.0, 1.0}, 1.0);
    f.setLineColor(Color{0.5, 0.5, 0.5});
    f.setGridSize(10,10);
    f.gridLines(); 

    (* Draw paths: *)
    f.setLineWidth(0.50);
    f.setLineColor(Color{0.0, 0.0, 0.0});
    f.setLineDashed(ARRAY OF REAL{}, 0.0);
    DrawPolygonal(f, a);
    DrawPolygonal(f, b);
    
    (* Draw matching: *)
    f.setLineWidth(0.25);
    f.setLineColor(Color{1.0, 0.0, 0.0});
    FOR i:= 0 TO LAST(match) DO 
      WITH ia = match[i][0], ib = match[i][1] DO
        f.segment(a[ia][0], a[ia][1], b[ib][0], b[ib][1])
      END
    END;
  END DrawMatch; 
  
PROCEDURE DrawPolygonal(f: PSPlot.File; READONLY a: PZLR3Chain.T) =
  BEGIN
    FOR i:= 1 TO LAST(a) DO 
      f.segment(a[i-1][0], a[i-1][1], a[i][0], a[i][1])
    END
  END DrawPolygonal;

PROCEDURE MakePolygonal(READONLY c: ChainData; step: LONGREAL): REF PZLR3Chain.T =
  BEGIN
    IF ABS(c.curvature) < 1.0d-6 THEN 
      RETURN MakeSegment(c, step)
    ELSE
      RETURN MakeArc(c, step)
    END
  END MakePolygonal;
  
PROCEDURE MakeSegment(READONLY c: ChainData; step: LONGREAL): REF PZLR3Chain.T =
  BEGIN
    WITH
      length = LR3.Dist(c.ini, c.fin),
      nSteps = MAX(1, ROUND(length/step)),
      r = NEW(REF PZLR3Chain.T, nSteps+1),
      a = r^
    DO
      FOR i := 0 TO nSteps DO 
        WITH 
          s = FLOAT(i,LONGREAL)/FLOAT(nSteps, LONGREAL),
          t = 1.0d0 - s
        DO
          a[i] := LR3.T{
            s*c.ini[0] + t*c.fin[0],
            s*c.ini[1] + t*c.fin[1],
            s*c.ini[2] + t*c.fin[2]
          }
        END
      END;
      RETURN r
    END
  END MakeSegment;
  
PROCEDURE MakeArc(READONLY c: ChainData; step: LONGREAL): REF PZLR3Chain.T =
  VAR ti, tf: LONGREAL;
  CONST 
    Pi = 3.14159265358979323844d0;
    TwoPi = 6.28318530717958647688d0;
  BEGIN
    WITH
      radius = 1.0d0/c.curvature,
      m = LR3.Scale(0.5d0, LR3.Add(c.fin, c.ini)),
      d = LR3.Scale(0.5d0, LR3.Sub(c.fin, c.ini)),
      dn = LR3.Norm(d),
      f = dn/radius,
      scale = radius/dn * sqrt(MAX(0.0d0, 1.0d0 - f*f)),
      e = LR3.Scale(scale, LR3.T{-d[1], +d[0], 0.0d0}),
      center = LR3.Add(m, e),
      ci = LR3.Sub(c.ini, center),
      cf = LR3.Sub(c.fin, center)
    DO
      ti := atan2(ci[1], ci[0]);
      tf := atan2(cf[1], cf[0]);
      WHILE ABS(tf-ti) > Pi DO
        IF tf < ti THEN tf := tf + TwoPi ELSE tf := tf - TwoPi END
      END;
      
      WITH
        nSteps = MAX(1, ROUND(radius * (tf - ti) / step)),
        r = NEW(REF PZLR3Chain.T, nSteps+1),
        a = r^
      DO
        FOR i := 0 TO nSteps DO 
          WITH t = ti + (tf - ti)*FLOAT(i,LONGREAL)/FLOAT(nSteps, LONGREAL) DO
            a[i] := LR3.T{
              center[0] + ABS(radius) * cos(t),
              center[1] + ABS(radius) * sin(t),
              0.0d0
            }
          END
        END;
        RETURN r
      END
    END
  END MakeArc;
  
PROCEDURE MatchPolygonals(
    READONLY a, b: PZLR3Chain.T;
    maxShift: CARDINAL;
    maxDist: LONGREAL;
    cutDist: LONGREAL;
    step : LONGREAL;
  ): REF PZMatch.T =
  VAR 
    match: REF PZMatch.T;
    mismatch: LONGREAL;
    nMatched: CARDINAL; 
    xCenter, yCenter: CARDINAL;
    minAvgDist: REF ARRAY OF LONGREAL;
    XYCost: REF PZMatch.CostMatrix := NIL;
  BEGIN
    WITH
      NA = NUMBER(a),
      NB = NUMBER(b),
      ac = (NA-1) DIV 2,
      bc = (NB-1) DIV 2,
      intLoc = ac+bc,
      intAlign = ac-bc,
      maxAlign = +NA-1,
      minAlign = -NB+1,
      maxValidShift = MIN(maxAlign - intAlign, intAlign - minAlign)
    DO
      PZLR3Chain.OutMatch(
        a, b,
        loc := FLOAT(intLoc, LONGREAL),
        align := FLOAT(intAlign, LONGREAL),
        maxShift := FLOAT((MIN(maxShift, maxValidShift) DIV 2) * 2, LONGREAL),
        maxDist := maxDist,
        cutDist := cutDist,
        step := step,
        minChainEdges := 0,
        (*OUT*) mismatch := mismatch,
        (*OUT*) nMatched := nMatched,
        (*OUT*) match := match,
        (*OUT*) xCenter := xCenter,
        (*OUT*) yCenter := yCenter,
        (*OUT*) minAvgDist := minAvgDist,
        (*WORK*) XYCost := XYCost
      );
      RETURN match
    END
  END MatchPolygonals;

BEGIN
  DoIt();
END PZTestMatch.