MODULE PZMatch;

IMPORT Wr, Thread, Fmt; (*DEBUG*)
FROM Stdio IMPORT stderr; (*DEBUG*)

TYPE
  Direction = [-1..+1]; (* -1 = prefix, +1 = suffix *)

PROCEDURE MC(READONLY p, q: Pair; maxCost: LONGREAL): LONGREAL =
  BEGIN
    IF p[0] = q[0] OR p[1] = q[1] THEN 
      RETURN maxCost*0.5d0
    ELSE
      RETURN maxCost
    END
  END MC;

PROCEDURE Match(
    NX, NY: CARDINAL;
    cost: StepCostFunction;
    maxCost : LONGREAL;
    VAR (*OUT*) totCost: LONGREAL;
    VAR (*OUT*) match: REF T;
  ) =
  VAR v1, v2, v3: LONGREAL;
      k: CARDINAL;
  BEGIN
    WITH
      rm = NEW(REF CostMatrix, NX, NY),
      m = rm^,
      np = NX+NY-1,
      rp = NEW(REF T, np),
      p = rp^
    DO
      (* Fill the cost matrix: *)
      (*
        We set "m[ix,iy]" with the cost of the cheapest partial 
        matching that begins with "(0,0)" and ends with "(ix,iy)"
      *)
      m[0,0] := 0.0d0;
      FOR ix := 1 TO NX-1 DO 
        m[ix,0] := m[ix-1,0] + MIN(cost(Pair{ix-1, 0}, Pair{ix, 0}), maxCost*0.5d0);
      END;
      FOR iy := 1 TO NY-1 DO 
        m[0,iy] := m[0,iy-1] + MIN(cost(Pair{0, iy-1}, Pair{0, iy}), maxCost*0.5d0);
      END;
      FOR ix := 1 TO NX-1 DO 
        FOR iy := 1 TO NY-1 DO
          v1 := m[ix-1,iy] + MIN(cost(Pair{ix-1, iy}, Pair{ix, iy}), maxCost*0.5d0);
          v2 := m[ix,iy-1] + MIN(cost(Pair{ix, iy-1}, Pair{ix, iy}), maxCost*0.5d0);
          v3 := m[ix-1,iy-1] + MIN(cost(Pair{ix-1, iy-1}, Pair{ix, iy}), maxCost);
          m[ix,iy] := MIN(MIN(v1,v2), v3);
        END
      END; 

      (* Compute the optimum matching: *)
      (*
        We trace back from "m[nx-1,ny-1]" to "m[0,0]", reconstructing the 
        choice that resulted in the best matching at each step:
      *)
      k := np-1;
      p[k] := Pair{NX-1,NY-1};
      WHILE p[k][0] > 0 OR p[k][1] > 0 DO
        WITH
          ix = p[k][0],
          iy = p[k][1]
        DO
          IF ix = 0 THEN
            p[k-1] := Pair{ix, iy-1}
          ELSIF iy = 0 THEN
            p[k-1] := Pair{ix-1, iy}
          ELSE
            WITH
              v0 = m[ix,iy],
              v1 = m[ix-1,iy] + MIN(cost(Pair{ix-1, iy}, Pair{ix, iy}), maxCost*0.5d0),
              v2 = m[ix,iy-1] + MIN(cost(Pair{ix, iy-1}, Pair{ix, iy}), maxCost*0.5d0),
              v3 = m[ix-1,iy-1] + MIN(cost(Pair{ix-1, iy-1}, Pair{ix, iy}), maxCost)
            DO
              IF v0 = v3 THEN 
                p[k-1] := Pair{ix-1, iy-1}
              ELSIF v0 = v1 THEN
                p[k-1] := Pair{ix-1, iy}
              ELSIF v0 = v2 THEN
                p[k-1] := Pair{ix, iy-1}
              ELSE
                <* ASSERT FALSE *>
              END
            END
          END
        END;
        DEC(k);
      END; (* while *)
      totCost := m[NX-1,NY-1] - m[0,0];
      match := Trim(p, 0, np)
    END;
  END Match;

PROCEDURE OutMatch(
    NX, NY: CARDINAL;
    stepCost: StepCostFunction;
    maxCost : LONGREAL; 
    L, A: INTEGER; 
    shiftMin, shiftMax: INTEGER;
    shiftCost: ShiftCostFunction;
    mismatch: MismatchFunction;
    VAR (*OUT*) mBest: LONGREAL;
    VAR (*OUT*) match: REF T;
    VAR (*OUT*) xCenter, yCenter: CARDINAL;
    VAR (*OUT*) minTotCost: REF ARRAY OF LONGREAL;
    VAR (*WORK*) XYCost: REF CostMatrix;
  ) =
  BEGIN 
    <* ASSERT NX > 0 *>
    <* ASSERT NY > 0 *>
    (* (Re)allocate "XYCost" matrix: *)
    IF XYCost = NIL 
    OR NUMBER(XYCost^) < NX
    OR NUMBER(XYCost[0]) < NY 
    THEN 
      XYCost := NEW(REF CostMatrix, NX, NY)
    END;
    
    minTotCost := NEW(REF ARRAY OF LONGREAL, NX+NY-1);
    
    WITH 
      LMin = 0,
      LMax = (NX-1)+(NY-1),
      cost = XYCost^,
      mtc = NEW(REF T, NX+NY-1)^,
      minTotCost = minTotCost^,
      curMinCost = NEW(REF ARRAY OF LONGREAL, LMax + 1)^ 
        (* 
          "curMinCost[k]" is the min cost for any matching from 
          the current center to any pair "(x,y)" with "x+y = k".
          
          Note that "curMinCost[k]" is indexed by the location
          of the distal pair, whereas "minTotCost" is indexed
          by the *length* of the match.
        *)
    DO 
    
      FOR k := 0 TO LAST(minTotCost) DO minTotCost[k] := LAST(LONGREAL) END;
     
      TYPE Step = RECORD cost: LONGREAL; xFrom, yFrom: CARDINAL END;
      
      PROCEDURE CheapestStep(
          READONLY p: Pair; 
          dir: Direction; 
          xMin,yMin: CARDINAL; 
          xMax,yMax: CARDINAL;
        ): Step =
        (*
          Inner loop of dynamic programming in direction "dir".
          Computes the cheapest path ending at "p" and coming from one of the
          three neighboring pairs in the direction "-dir".
          The cost array nominally extends from "[xMin,yMin]" to "[xMax,yMax]".
        *)
        VAR ct: LONGREAL;
            f: Pair;
        BEGIN
          IF dir = -1 THEN
            (* Prefix of path, ends at "(xMax,yMax)": *)
            IF p[0] = xMax AND p[1] = yMax THEN ct := 0.0d0 ELSE ct := LAST(LONGREAL) END
          ELSIF dir = +1 THEN
            (* Suffix of path, begins at "(xMin,yMin)": *)
            IF p[0] = xMin AND p[1] = yMin THEN ct := 0.0d0 ELSE ct := LAST(LONGREAL) END
          ELSE
            <* ASSERT FALSE *>
          END;
          f := p;
          WITH
            x = p[0],
            y = p[1],
            x1 = x - dir, 
            y1 = y - dir
          DO
            IF  x1 >= xMin AND x1 <= xMax
            AND y1 >= yMin AND y1 <= yMax THEN
              WITH 
                q = Pair{x1,y1},
                c = cost[x1,y1] + MIN(stepCost(q, p), maxCost)
              DO
                IF c < ct THEN ct := c; f := q END;
              END
            END;
            IF x1 >= xMin AND x1 <= xMax THEN
              WITH
                q = Pair{x1,y},
                c = cost[x1,y] + MIN(stepCost(q, p), maxCost*0.5d0)
              DO
                IF c < ct THEN ct := c; f := q END;
              END
            END;
            IF y1 >= yMin AND y1 <= yMax THEN
              WITH 
                q = Pair{x,y1},
                c = cost[x,y1] + MIN(stepCost(q, p), maxCost*0.5d0)
              DO
                IF c < ct THEN ct := c; f := q END;
              END
            END;
          END;
          RETURN Step{cost := ct, xFrom := f[0], yFrom := f[1]}
        END CheapestStep;

      PROCEDURE ExtractMinCostPath(
          p: Pair;
          dir: Direction; 
          xMin,yMin: CARDINAL; 
          xMax,yMax: CARDINAL;
          VAR m: CARDINAL;
          VAR mtc: T;
        ) =
        (*
          Extracts result of dynamic programming in direction "dir".
          Extracts from "cost" the cheapest path whose "dir" end is at "p".
          The path is appended to the "mtc" path, which is assumed to have "m" pairs.
          The cost array nominally extends from "[xMin,yMin]" to "[xMax,yMax]".
        *)
        VAR xFin, yFin: CARDINAL;
            kIni: CARDINAL;
        BEGIN
          (* Find end of path: *)
          IF dir = -1 THEN 
            (* Prefix of path, ends at "(xMax,yMax)": *)
            xFin := xMax; yFin := yMax;
          ELSIF dir = +1 THEN 
            (* Suffix of path, begins at "(xMin,yMin)": *)
            xFin := xMin; yFin := yMin;
            <* ASSERT m > 0 *>
            <* ASSERT mtc[m-1] = Pair{xMin,yMin} *>
          ELSE
            <* ASSERT FALSE *>
          END;
          
          (* Extract path, store it in "mtc[kIni..k]": *)
          IF m = 0 THEN
            kIni := 0;
          ELSE
            kIni := m-1;
            (* If "dir = -1" then we should have "mtc[kIni] = p" *)
            (* If "dir = +1", that will be true only after reversing the match. *) 
          END;
          mtc[kIni] := p;
          m := kIni + 1;
          WHILE p[0] # xFin OR p[1] # yFin DO
            WITH step = CheapestStep(p, dir, xMin, yMin, xMax, yMax) DO
              p[0] := step.xFrom; p[1] := step.yFrom;
              mtc[m] := p;
              INC(m)
            END
          END;
          IF dir = +1 THEN
            ReverseMatch(SUBARRAY(mtc, kIni, m - kIni))
          END; 
        END ExtractMinCostPath;
    
      PROCEDURE ReverseMatch(VAR mtc:  T) = 
        BEGIN
          FOR i := 0 TO NUMBER(mtc) DIV 2 - 1 DO
            WITH j = LAST(mtc) - i DO
              VAR t := mtc[i]; BEGIN mtc[i] := mtc[j]; mtc[j] := t END
            END
          END
        END ReverseMatch;

      VAR 
        paBest, pbBest: Pair;
        LaBest, LbBest: CARDINAL; 
        oldmBest : LONGREAL;
        m: CARDINAL;
      BEGIN
        m := 0;
        mBest := LAST(LONGREAL);
        <* ASSERT L MOD 2 = A MOD 2 *>
        <* ASSERT shiftMin <= shiftMax *>
        <* ASSERT shiftMin MOD 2 = 0 *>
        <* ASSERT shiftMax MOD 2 = 0 *>
        (* DebugIntPair("NX,NY:", NX,NY); *)
        (* DebugIntPair("L,A:", L, A); *)
        FOR shift := shiftMin TO shiftMax BY 2 DO 
          WITH
            xc = (L+(A+shift)) DIV 2,
            yc = (L-(A+shift)+1) DIV 2,
            Lc = xc + yc,
            Ac = xc - yc,
            curShiftCost = shiftCost(shift)
          DO
            <* ASSERT shift = Ac - A *>
            (* DebugIntPair("xc,yc:", xc, yc); *)
            IF xc >= 0 AND xc < NX AND yc >= 0 AND yc < NY THEN
              FOR i := LMin TO LMax DO curMinCost[i] := LAST(LONGREAL) END;
              (* Backwards dynamic programming *)
              FOR xa := xc TO 0 BY -1 DO 
                FOR ya := yc TO 0 BY -1 DO
                  cost[xa,ya] := CheapestStep(Pair{xa,ya},-1,0,0,xc,yc).cost;
                  WITH La = xa+ya DO curMinCost[La] := MIN(curMinCost[La], cost[xa,ya]) END
                END;  
              END;
              (* Forward dynamic programming *)
              FOR  xb := xc TO NX-1 DO 
                FOR yb := yc TO NY-1 DO
                  cost[xb,yb] := CheapestStep(Pair{xb,yb},+1,xc,yc,NX-1,NY-1).cost;
                  WITH Lb = xb+yb DO curMinCost[Lb] := MIN(curMinCost[Lb], cost[xb,yb]) END
                END;  
              END;
              (* Look for minimum mismatch path: *)
              oldmBest := mBest;
              FOR La:= LMin TO Lc DO
                FOR Lb := Lc TO LMax DO
                  WITH
                    pathCost = curMinCost[La] + curMinCost[Lb],
                    totCost = pathCost + curShiftCost,
                    totChainSteps = Lb - La,
                    mm = mismatch(totCost, totChainSteps)
                  DO
                    minTotCost[totChainSteps] := 
                      MIN(minTotCost[totChainSteps], pathCost);
                    IF mm < mBest THEN 
                      mBest := mm; LaBest := La; LbBest := Lb;
                    END
                  END
                END;
              END;
              (* DebugIntPair("LaBest,LbBest:", LaBest,LbBest); *)
              (* Did we find a better solution? *)
              IF mBest < oldmBest THEN
                (* Save the center pair and the "LCost" vector: *)
                xCenter := xc;
                yCenter := yc;
                
                (* Save the matching before destroying the matrix: *)
                (* Find start of prefix: *)
                FOR xa := MAX(0,LaBest-yc) TO MIN(xc,LaBest) DO
                  WITH ya = LaBest - xa DO
                    IF curMinCost[LaBest] = cost[xa,ya] THEN paBest := Pair{xa,ya} END
                  END;
                END;
                (* Find end of suffix: *)
                FOR xb:= MAX(xc,LbBest-NY+1) TO MIN(NX-1,LbBest-yc) DO
                  WITH yb = LbBest - xb DO
                    IF curMinCost[LbBest] = cost[xb,yb] THEN pbBest := Pair{xb,yb} END
                  END;
                END;
                    
                (* Extract matching: *)
                m := 0;
                ExtractMinCostPath(paBest, -1, 0, 0, xc, yc, m, mtc);
                ExtractMinCostPath(pbBest, +1, xc,yc, NX-1, NY-1, m, mtc);
              END;
            END
          END
        END;
        (* Return the best match: *)
        <* ASSERT m > 0 *>
        match := Trim(mtc, 0, m);
      END;
    END;
  END OutMatch; 

PROCEDURE Trim(READONLY m: T; start, length: CARDINAL): REF T =
  BEGIN
    WITH r = NEW(REF T, length) DO
      r^ := SUBARRAY(m, start, length);
      RETURN r
    END   
  END Trim;
 
PROCEDURE RemoveDanglingEnds(READONLY m: T): REF T =
  VAR ini, fin: CARDINAL; 
  BEGIN
    ini := 0; fin := LAST(m);
    WITH
      ix = m[0][0],
      iy = m[0][1]
    DO
      WHILE (ini < fin) AND (
          (m[ini][0] = ix AND m[ini+1][0] = ix) OR 
          (m[ini][1] = iy AND m[ini+1][1] = iy)
        )
      DO
        INC(ini)
      END
    END;
    WITH
      ix = m[LAST(m)][0],
      iy = m[LAST(m)][1]
    DO
      WHILE (ini < fin) AND (
          (m[fin][0] = ix AND m[fin-1][0] = ix) OR 
          (m[fin][1] = iy AND m[fin-1][1] = iy)
        )
      DO
        DEC(fin)
      END
    END;
    RETURN Trim(m, ini, fin-ini+1)    
  END RemoveDanglingEnds;
  
  
PROCEDURE RemoveUnmatchedEnds(
   READONLY m: T; 
   cost: StepCostFunction; 
   maxCost: LONGREAL
  ): REF T =
  
  VAR ini, fin: CARDINAL; 
  BEGIN
    ini := 0; fin := LAST(m);
    WHILE ini < fin 
    AND cost(m[ini], m[ini+1]) >= MC(m[ini], m[ini+1], maxCost)
    DO
      INC(ini)
    END;
    WHILE ini < fin
    AND cost(m[fin-1], m[fin]) >= MC(m[fin-1], m[fin], maxCost)
    DO
      DEC(fin)
    END;
    RETURN Trim(m, ini, fin-ini+1)    
  END RemoveUnmatchedEnds;
    
    
PROCEDURE MatchCost(
    READONLY m: T; 
    cost: StepCostFunction;
    maxCost: LONGREAL;
  ): LONGREAL = 
  VAR totCost : LONGREAL := 0.0d0;
  BEGIN
    FOR i := 0 TO LAST(m)-1 DO 
      WITH p0 = m[i], p1 = m[i+1] DO
        totCost := totCost + MIN(MC(p0, p1, maxCost), cost(p0, p1))
      END
    END;
    RETURN totCost
  END MatchCost;
  
PROCEDURE NMatched(
    READONLY m: T; 
    cost: StepCostFunction; 
    maxCost: LONGREAL;
  ): CARDINAL = 
  VAR nMatched: CARDINAL := 0;
  BEGIN
    FOR i := 0 TO LAST(m)-1 DO 
      WITH p0 = m[i], p1 = m[i+1] DO
        IF cost(p0, p1) < MC(p0, p1, maxCost) THEN INC(nMatched) END
      END
    END;  
    RETURN nMatched
  END NMatched;
  
PROCEDURE LengthMatched(
    READONLY m: T; 
    length: LengthFunction;
    cost: StepCostFunction; 
    maxCost: LONGREAL;
  ): LONGREAL = 
  VAR lengthMatched: LONGREAL := 0.0d0;
  BEGIN
    FOR i := 0 TO LAST(m)-1 DO 
      WITH p0 = m[i], p1 = m[i+1] DO
        IF cost(p0, p1) < MC(p0, p1, maxCost) THEN
          lengthMatched := lengthMatched + length(p0, p1)
        END
      END
    END;  
    RETURN lengthMatched
  END LengthMatched;
  
<*UNUSED*>
PROCEDURE DebugIntPair(title: TEXT; x, y: INTEGER) =
  <* FATAL Wr.Failure, Thread.Alerted *>
  BEGIN
    Wr.PutText(stderr, title);
    Wr.PutText(stderr, " ");
    Wr.PutText(stderr, Fmt.Int(x));
    Wr.PutText(stderr, " ");
    Wr.PutText(stderr, Fmt.Int(y));
    Wr.PutText(stderr, "\n");
  END DebugIntPair;

BEGIN 
END PZMatch.