MODULE PZGetStraightSegs EXPORTS Main;

(* Generate list of straight segments in symbolic chains *)
(* Last edited on 1999-09-04 13:23:56 by hcgl *)

(* 
  Looks for contour segments that have low average curvature
  in the current scale, and a specified minimum length.
  
  These segments are broadened to include the adjacent blurred
  corners.  Remember to use a negative blurFactor when mapping these
  segments to other scales.
*)

IMPORT ParseParams, Process, FileWr, Wr, OSError, Thread, Stdio, Fmt;

IMPORT PZSymbolChain, PZSymbol, PZSegment;

FROM Stdio IMPORT stderr;
FROM PZTypes IMPORT LONG, LONGS, NAT, BOOLS;

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

CONST
  ExpectedSegsPerCurve = 1;

TYPE
  Options = RECORD
      chainDir: TEXT;         (* Directory where to find curve data *)
      chainPrefix: TEXT;      (* Invariant chain file name prefix. *)
      nCurves: NAT;           (* Number of curves. *)
      output: TEXT;           (* Output segment file name. *)
      band: NAT;              (* Filtering "band" (wavelength parameter). *)
      step: LONG;             (* Sampling step size. *)
      minLength: LONG;        (* Min length of straight segments. *)
      minCurvature: LONG;     (* Min curvature to consider matching. *)
      blurFactor: LONG;       (* Corner broadening factor. *)
      printSegs: NAT;         (* Max segments to print. *)
    END;

TYPE
  VTable = PZSymbol.Table;

PROCEDURE Main() =
  BEGIN
    WITH
      o = GetOptions(),
      lambda = FLOAT(o.band, LONG),
      sel = SelectAll(o.nCurves)^,
      chData = PZSymbolChain.ReadAll(o.chainPrefix, o.band, ".cvc", sel, 
        headerOnly := FALSE, dir := o.chainDir
      ), 
      dch = PZSymbol.MakeDecodeTable(chData.sigmaMin)^,
      ech = PZSymbol.MakeErrorVarTable(chData.sigmaMin)^,
      seg = ComputeStraightSegments(
        chData.chain^,
        dch := dch,
        ech := ech,
        minCurvature := o.minCurvature,
        minLength := EnsurePositive(o.minLength - 2.0d0 * o.blurFactor * lambda),
        step := o.step,
        (* Extend the result to cover the blurred corners too: *)
        extraLength := 4.0d0 * o.blurFactor * lambda
      )^
    DO
      <* ASSERT chData.sigmaMin = chData.sigmaMax *>
      WITH wr = FileWr.Open(o.output & ".seg") DO
        PZSegment.Write(wr, SegComment(o), seg)
      END;
      PrintSegments(chData.chain^, seg, o.band, o.printSegs);
    END;
  END Main;
  
PROCEDURE EnsurePositive(segLength: LONG): LONG =
  BEGIN
    IF segLength < 0.0d0 THEN
      Wr.PutText(stderr, "warning: requested segment length too small for this scale\n");
      Wr.Flush(stderr);
      Process.Exit(1);
    END;
    RETURN MAX(0.0d0, segLength)
  END EnsurePositive;

PROCEDURE SelectAll(n: NAT): REF BOOLS = 
  BEGIN
    WITH
      rb = NEW(REF BOOLS, n), b = rb^
    DO
      FOR i := 0 TO n-1 DO b[i] := TRUE END;
      RETURN rb
    END
  END SelectAll;

PROCEDURE GetOptions(): Options =
  VAR o: Options;
  BEGIN
    WITH
      pp = NEW(ParseParams.T).init(stderr)
    DO
      TRY
        IF pp.keywordPresent("-chainDir") THEN
          o.chainDir := pp.getNext()
        ELSE
          o.chainDir := "."
        END;

        pp.getKeyword("-chainPrefix");
        o.chainPrefix := pp.getNext();

        pp.getKeyword("-nCurves");
        o.nCurves := pp.getNextInt();

        pp.getKeyword("-band");
        o.band := pp.getNextInt();

        pp.getKeyword("-step");
        o.step := pp.getNextLongReal(0.0d0);

        pp.getKeyword("-output");
        o.output := pp.getNext();

        pp.getKeyword("-minLength");
        o.minLength := pp.getNextLongReal(0.0d0);

        pp.getKeyword("-blurFactor");
        o.blurFactor := pp.getNextLongReal(0.0d0);

        pp.getKeyword("-minCurvature");
        o.minCurvature := pp.getNextLongReal(0.0d0);

        IF pp.keywordPresent("-printSegs") THEN
          o.printSegs := pp.getNextInt(1)
        ELSE
          o.printSegs := 1000
        END;

        pp.finish();
      EXCEPT
      | ParseParams.Error =>
          Wr.PutText(stderr, "Usage: PZGetStraightSegs \\\n");
          Wr.PutText(stderr, "  [ -chainDir DIR ] -chainPrefix NAME \\\n");
          Wr.PutText(stderr, "  -nCurves NUMBER \\\n");
          Wr.PutText(stderr, "  -band NUMBER -step NUMBER \\\n");
          Wr.PutText(stderr, "  -output NAME \\\n");
          Wr.PutText(stderr, "  -minLength NUMBER  -blurFactor NUMBER \\\n");
          Wr.PutText(stderr, "  -minCurvature NUMBER \\\n");
          Wr.PutText(stderr, "  [ -printSegs NUMBER ]\n");
        Process.Exit(1);
      END;
    END;
    RETURN o
  END GetOptions;

PROCEDURE ComputeStraightSegments(
    READONLY chain : ARRAY OF PZSymbolChain.ReadData;
    READONLY dch: VTable;              (* Curvature decoding table. *)
    READONLY ech: VTable;              (* Quantization noise variances. *)
    minCurvature: LONG;      (* Min curvature to consider *)
    minLength: LONG;         (* Min length of straight segment (already shrunk) *)
    step: LONG;              (* Sampling step *)
    extraLength: LONG;       (* How much to extend the result *)
  ): REF PZSegment.List =
  VAR
    s: REF PZSegment.List;
    nSegs : NAT := 0;
  BEGIN
    WITH
      nChains = NUMBER(chain)
    DO
      s := NEW(REF PZSegment.List, nChains * ExpectedSegsPerCurve);
      FOR k := 0 TO nChains-1 DO
        WITH
          c = chain[k].c^,
          minSteps = FLOOR(minLength / step),
          halfStretch = FLOOR(0.5d0 * extraLength / step )
        DO
          ProcessChain(
            c, k,
            s, nSegs,
            dch := dch,
            ech := ech,
            minSteps := minSteps,
            minCurvature := minCurvature,
            halfStretch := halfStretch
          )
        END;
      END;
      WITH st = NEW(REF PZSegment.List, nSegs) DO
        st^ :=  SUBARRAY(s^, 0, nSegs);
        RETURN st;
      END;
    END;
  END ComputeStraightSegments;

PROCEDURE ProcessChain(
    READONLY c: PZSymbolChain.T;
    k: NAT;
    VAR s: REF PZSegment.List;
    VAR nSegs: NAT;
    READONLY dch: VTable;       (* Curvature decoding table *)
    READONLY ech: VTable;       (* Quantization noise variances. *)
    minSteps: NAT;              (* Minimum number of steps in segment. *)
    minCurvature: LONG;         (* Min curvature to consider *)
    halfStretch: NAT;           (* Number of steps to add at each end *)
  ) =
  (*
    Consider the set of indices "ic" of the chain "c".
    This set is a priodic sequence with period "mc=NUMBER(c)".

    We compute a boolean vector "ok", such that "ok[ic]" tells whether
    the the "minSteps+1" elements beginning with element "r" would be
    an acceptable segment. Specifically, "ok[ic] = TRUE" iff the root
    mean of the squared curvatures "c[ic+k]^2" is less than
    "minCurvature".  All averages are taken for "k" ranging in
    [0..minSteps]".

    The segments are then found by identifying the maximal substrings
    of consecutive "TRUE"s in the "ok" vector.  Finally, "halfStretch"
    steps are added at each end of every segment found.
  *)
  VAR c2Sum: LONG;
  BEGIN
    WITH
      mc = NUMBER(c),
      nOld = nSegs + 0,
      (* Properties of a minimal-size segment: *)
      minSamples = FLOAT(minSteps + 1, LONG),
      minTotValueSqr = minSamples * minCurvature*minCurvature,
      (* Work vectors: *)
      c2s = NEW(REF LONGS, mc)^,  (* Accumulated "c[ia]^2" *)
      ok = NEW(REF BOOLS, mc)^
    DO
      Wr.PutText(stderr, "chain: " & Fmt.Int(k) & "[" & Fmt.Int(mc) & "]");
      Wr.PutText(stderr, " minSteps: " & Fmt.Int(minSteps));
      (*
        Accumulates squared values,
        so that "c2s[r] = SUM { c[k]^2 : k IN [0..r] }".
      *)
      c2Sum := 0.0d0;
      FOR r := 0 TO mc-1 DO
        WITH
          cv = dch[c[r]], e2 = ech[c[r]]
        DO
          c2Sum := c2Sum + cv*cv + e2; c2s[r] := c2Sum;
        END;
      END;
      (*
        Mark in "ok" all positions "r" along the diagonal
        that are the beginnings of substrings, consisting OF
        "minSteps+1" consecutive samples, which have
        sufficiently small root mean square curvatures:
      *)
      FOR r := 0 TO mc-1 DO
        WITH
          rLo = r - 1,
          rmLo = rLo MOD mc,
          qLo = FLOAT(rLo DIV mc, LONG),

          rHi = r + minSteps,
          rmHi = rHi MOD mc,
          qHi = FLOAT(rHi DIV mc, LONG),

          c2Tot = (c2s[rmHi] + qHi*c2Sum) - (c2s[rmLo] + qLo*c2Sum)
        DO
          ok[r] := c2Tot < minTotValueSqr
        END;
      END;
      BreakChain(
        c, k,
        ok,
        s, nSegs,
        dch := dch, ech := ech,
        minSteps := minSteps,
        minCurvature := minCurvature, 
        halfStretch := halfStretch
      );
      Wr.PutText(stderr,"  nSegs: " & Fmt.Int(nSegs));
      Wr.PutText(stderr," (+" & Fmt.Int(nSegs - nOld) & ")\n");
    END;
  END ProcessChain;

PROCEDURE BreakChain(
    READONLY c: PZSymbolChain.T;
    k: NAT;
    READONLY ok: BOOLS;
    VAR s: REF  PZSegment.List;
    VAR  nSegs:  NAT;
    READONLY dch: VTable;       (* Curvature decoding table *)
    READONLY ech: VTable;       (* Quantization noise variances. *)
    minSteps: NAT;              (* Minimum number of steps in segment. *)
    minCurvature: LONG;         (* Min curvature to consider *)
    halfStretch: NAT;           (* Number of steps to add at each end *)
  ) =
  VAR
    t: NAT;
    count, tStart, tIni, tLen: NAT;
    inx: NAT;
  BEGIN
    WITH
      mc = NUMBER(c),
      maxSteps = mc-1
    DO
      <* ASSERT mc > 1 *>
      (* Look for a TRUE element "ok[tStart]" right after a FALSE element: *)
      t := 0;
      WHILE ok[t MOD mc] = TRUE AND t < mc DO INC(t) END;
      IF t = mc THEN
        (* All TRUE *)
        tStart := 0
      ELSE
        inx := 0;
        WHILE ok[t MOD mc] = FALSE AND inx < mc DO INC(t); INC(inx) END;
        IF inx = mc THEN (* All FALSE *) RETURN END;
        tStart := t MOD mc;
      END;
      (* Collect runs of TRUEs": *)
      t := tStart;
      REPEAT
        (* Here begins a new run *)
        tIni := t;
        count := 0;
        REPEAT
          INC(t);
          INC(count);
        UNTIL ok[t MOD mc] = FALSE OR count = mc;

        (* Compute end of segment, clipping to "maxSteps": *)
        WITH
          nSteps = count - 1 + minSteps,
          excess = MAX(0, nSteps - maxSteps)
        DO
          tIni := (tIni + excess DIV 2) MOD mc;
          tLen := MIN(maxSteps, nSteps) + 1;
        END;

        (* Remove high-curvature points at either end: *)
        TrimCrookedEnds(c, minCurvature*minCurvature, dch, ech, tIni, tLen);

        (* Extend segment in both directions, clipping to "maxSteps": *)
        tIni := (tIni - halfStretch) MOD mc;
        tLen := tLen + 2*halfStretch;
        WITH
          excess = MAX(0, tLen - 1 - maxSteps)
        DO
          tIni := (tIni + excess DIV 2) MOD mc;
          tLen := MIN(maxSteps + 1, tLen);
        END;

        (* Finally, store segment if long enough: *)
        IF tLen - 1 >= minSteps THEN

          (* Make room for a new segment: *)
          IF nSegs >= NUMBER(s^) THEN
            WITH st  = NEW(REF PZSegment.List, nSegs * 2) DO
              SUBARRAY(st^, 0, NUMBER(s^)) := s^;
              s := st;
            END;
          END;

          (* Save this run as a new segment: *)
          s[nSegs] := PZSegment.T{
            cvx := k,
            tot := mc,
            ini := tIni MOD mc,
            ns := tLen,
            rev := FALSE
          };
          INC(nSegs)
        END;
        WHILE ok[t MOD mc] = FALSE DO INC(t) END;
      UNTIL (t MOD mc) = tStart
    END;
  END BreakChain;

PROCEDURE TrimCrookedEnds(
    READONLY c: PZSymbolChain.T;
    minCurvSqr: LONG;           (* Min curvature to be non-straight, squared. *)
    READONLY dch: VTable;       (* Curvature decoding table *)
    READONLY ech: VTable;       (* Quantization noise variances. *)
    VAR (*IO*) tIni, tLen: NAT;
  ) =
  VAR tFin := tIni + tLen - 1;
  BEGIN
    WITH
      mc = NUMBER(c)
    DO
      WHILE tIni < tFin DO
        WITH
          ic = tIni MOD mc,
          cvi = dch[c[ic]], e2i = ech[c[ic]],

          fc = tFin MOD mc,
          cvf = dch[c[fc]], e2f = ech[c[fc]]
        DO
          IF cvi*cvi + e2i >= minCurvSqr THEN
            INC(tIni)
          ELSIF cvf*cvf + e2f >= minCurvSqr THEN
            DEC(tFin)
          ELSE
            tLen := tFin - tIni + 1;
            RETURN
          END
        END
      END
    END
  END TrimCrookedEnds;

PROCEDURE PrintSegments (
    READONLY chain: ARRAY OF PZSymbolChain.ReadData;
    READONLY seg : PZSegment.List;
    band: NAT;
    maxSegs: NAT;
  ) =
  BEGIN
    IF maxSegs = 0 THEN RETURN END;
    Wr.PutText(stderr, "=== band = " & Fmt.Int(band) & "===\n");
    FOR i := 0 TO MIN(maxSegs, NUMBER(seg))-1 DO
      WITH s = seg[i] DO
        Wr.PutText(stderr, "seg[" & Fmt.Int(i) & "]\n");
        PZSegment.Print(stderr, s);
        WITH ch = chain[s.cvx].c DO
          IF ch # NIL THEN
            <* ASSERT NUMBER(ch^) = s.tot *>
            PZSymbolChain.PrintSegment(stderr, ch^, s)
          END
        END
      END
    END;
    Wr.Flush(stderr);
  END PrintSegments;

PROCEDURE SegComment(READONLY o: Options): TEXT =
  BEGIN
    RETURN
      "PZGetStraightSegs\n" &
      "    chainPrefix " & o.chainPrefix & "\n" &
      "    nCurves = " & Fmt.Int(o.nCurves) & "\n" &
      "    band = " & Fmt.Int(o.band) & "\n" &
      "    minCurvature = " & Fmt.LongReal(o.minCurvature) & "\n" &
      "    minLength = " & Fmt.LongReal(o.minLength) & "\n" &
      "    blurFactor = " & Fmt.LongReal(o.blurFactor) & "\n"
  END SegComment;

BEGIN
  Main()
END PZGetStraightSegs.