MODULE PZFindClusters EXPORTS Main;

(* Indentifies clusters of fragments, and computes cluster-relative poses. *)
(* Last edited on 2001-11-15 18:47:25 by stolfi *)

IMPORT ParseParams, FileRd, FileWr, Wr, Fmt;
IMPORT Process, OSError, Thread, Stdio;
IMPORT LR2, LR4x4;
IMPORT PZLR3Chain, PZCandidate, PZPose;

FROM Stdio IMPORT stderr;
FROM PZTypes IMPORT LONG, INT, NAT, NATS, BOOLS;

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

TYPE
  Options = RECORD
      (* Input data specs: *)
      input: TEXT;             (* Input candidate file name *)
      chainDir: TEXT;          (* Directory where chains can be found. *)
      chainPrefix: TEXT;       (* Chain file name prefix *)
      band: NAT;               (* Nominal band width for file names *)
      extension: TEXT;         (* Extension for geometric chains (".flc" usually) *)

      (* Parameters for pose computation: *)
      firstCand: NAT;          (* Index of first candidate to consider. *)
      lastCand: NAT;           (* Index of last candidate to consider. *)
      angTolerance: LONG;      (* Max inconsistency in angle (radians). *)
      linTolerance: LONG;      (* Max position inconsistency (pixels). *)

      (* Output specs: *)
      output: TEXT;            (* Output plot file (without ".ps"/".eps") *)
      minClusterSize: NAT;     (* Minimum number of fragments in output cluster. *)
    END;

PROCEDURE Main() =
  BEGIN
    WITH
      o = GetOptions()
    DO
      WITH
        inpData = ReadCands(o.input),
        cand = inpData.c^,
        lambda = inpData.lambda,
        chainUsed = ChainsReallyUsed(cand, o)^,
        chAllData = PZLR3Chain.ReadAll(
          o.chainPrefix, o.band, o.extension, sel := chainUsed,
          dir := o.chainDir, headerOnly := FALSE, centralize := TRUE
        ),
        chData = chAllData.chData^,
        candPose = ComputeCandPoses(cand, o.firstCand, o.lastCand, chData)^,
        kind = NEW(REF ARRAY OF PZPose.EdgeKind, NUMBER(cand))^,
        joinPose = NEW(REF PZPose.List, NUMBER(chData))^,
        clusterNum = NEW(REF NATS, NUMBER(chData))^,
        cluster = PZPose.FindClusters(
          candPose, o.firstCand, o.lastCand,
          o.angTolerance, o.linTolerance,
          kind, joinPose, clusterNum
        )^
      DO
        FOR i := 0 TO LAST(cluster) DO
          WITH clu = cluster[i]^ DO
            IF NUMBER(clu) >= o.minClusterSize THEN
              WriteClusterPoses(o.output, i, joinPose, clu, "");
            END
          END
        END;
        WriteGraphs(o.output, cand, kind, lambda, "")
      END
    END;
 END Main;

PROCEDURE ReadCands(name: TEXT): PZCandidate.ReadData =
  BEGIN
    WITH fileName = name & ".can" DO
      Wr.PutText(stderr, "reading " & fileName & "\n");
      RETURN PZCandidate.Read(FileRd.Open(fileName))
    END
  END ReadCands;
  
PROCEDURE ChainsReallyUsed(
    READONLY cand: PZCandidate.List;
    READONLY o: Options;
  ): REF BOOLS =
  BEGIN
    WITH nCands = MIN(o.lastCand, LAST(cand)) - o.firstCand + 1 DO
      RETURN PZCandidate.ChainsUsed(SUBARRAY(cand, o.firstCand, nCands))
    END
  END ChainsReallyUsed;

PROCEDURE ComputeCandPoses(
    READONLY cand: PZCandidate.List;
    firstCand, lastCand: NAT;
    READONLY chData: ARRAY OF PZLR3Chain.ReadData;
  ): REF PZPose.List =
  (*
    For each candidate "c = cand[i]" in the specified range, computes
    the 4x4 Euclidean transformation matrix "candPose[i]" that, when
    applied to the segment "c.seg[0]", makes it coincide (approximately)
    with the segment "c.seg[1]". *)
  VAR m: ARRAY [0..1] OF LR4x4.T;  (* Mapping matrices *)
  BEGIN
    WITH
      rp = NEW(REF PZPose.List, NUMBER(cand)), candPose = rp^
    DO
      FOR iCand := 0 TO LAST(cand) DO
        WITH
          ci = cand[iCand],
          P = candPose[iCand]
        DO
          IF iCand >= firstCand AND iCand <= lastCand THEN
            FOR j := 0 TO 1 DO
              WITH
                s = ci.seg[j],
                f = s.cvx
              DO
                P.cvx[j] := f;
                m[j] := PZPose.PlaceSegment(
                  s, chain := chData[f].c^,
                  tilt := 0.0d0, shift := LR2.T{0.0d0,0.0d0},
                  flip := FALSE
                )
              END
            END;
            P.m := LR4x4.Mul(m[0], LR4x4.Inv(m[1]))
          ELSE
            P.m := LR4x4.Identity()
          END
        END
      END;
      RETURN rp
    END
  END ComputeCandPoses;

PROCEDURE WriteClusterPoses(
    name: TEXT;
    clusterId: NAT;
    READONLY joinPose: PZPose.List;
    READONLY frag: NATS;
    cmt: TEXT;
  ) =
  (*
    Writes out a file contaning the poses "pos[frag[j]]" 
    for a set of fragments whose numbers are given in "frag[j]". *)
  BEGIN
    WITH fileName = name & "-" & FIZ(clusterId,4) & ".pos" DO
      Wr.PutText(stderr, "writing " & fileName & "\n");
      WITH
        wr = FileWr.Open(fileName),
        pos = NEW(REF PZPose.List, NUMBER(frag))^
      DO
        FOR j := 0 TO LAST(frag) DO pos[j] := joinPose[frag[j]] END;
        PZPose.Write(wr, pos, cmt);
        Wr.Close(wr)
      END
    END
  END WriteClusterPoses;

PROCEDURE WriteGraphs(
    name: TEXT;
    READONLY cand: PZCandidate.List;
    READONLY kind: ARRAY OF PZPose.EdgeKind;
    lambda: LONG;
    cmt: TEXT;
  ) =
  BEGIN
    WriteSelectedCands(
      name & "-tree",
      cand, kind, PZPose.EdgeKind.Tree, lambda,
      cmt & "\n\n  [candidates of the spanning forest]"
    );
    WriteSelectedCands(
      name & "-ok",
      cand, kind, PZPose.EdgeKind.Ok, lambda, 
      cmt & "\n\n  [redundant candidates]"
    );
    WriteSelectedCands(
      name & "-bad",
      cand, kind, PZPose.EdgeKind.Bad, lambda,
      cmt & "\n\n  [inconsistent edges]"
    )
  END WriteGraphs;

PROCEDURE WriteSelectedCands(
    name: TEXT;
    READONLY cand: PZCandidate.List;
    READONLY kind: ARRAY OF PZPose.EdgeKind;
    which: PZPose.EdgeKind;
    lambda: LONG;
    cmt: TEXT;
  ) =
  VAR nSel: NAT := 0;
  BEGIN
    WITH fileName = name & ".can" DO
      Wr.PutText(stderr, "writing " & fileName & "\n");
      (* Count selected cands: *)
      FOR i := 0 TO LAST(cand) DO
        IF kind[i] = which THEN INC(nSel) END
      END;
      WITH
        wr = FileWr.Open(fileName),
        selCand = NEW(REF PZCandidate.List, nSel)^
      DO
        (* Collect selected cands: *)
        nSel := 0;
        FOR i := 0 TO LAST(cand) DO
          IF kind[i] = which THEN selCand[nSel] := cand[i]; INC(nSel) END
        END;
        PZCandidate.Write(wr, cmt, selCand, lambda);
        Wr.Close(wr)
      END
    END
  END WriteSelectedCands;

PROCEDURE GetOptions(): Options =
  VAR o: Options;
  BEGIN
    WITH
      pp = NEW(ParseParams.T).init(stderr)
    DO
      TRY
        pp.getKeyword("-input");
        o.input := pp.getNext();

        IF pp.keywordPresent("-chainDir") THEN
          o.chainDir := pp.getNext()
        ELSE
          o.chainDir := "."
        END;

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

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

        IF pp.keywordPresent("-extension") THEN
          o.extension:= pp.getNext();
        ELSE
          o.extension := ".flc";
        END;

        (* Pose computation options: *)

        IF pp.keywordPresent("-maxCands") THEN
          o.firstCand := 0;
          o.lastCand := pp.getNextInt(1) - 1
        ELSIF pp.keywordPresent("-candsRange") THEN
          o.firstCand := pp.getNextInt(0);
          o.lastCand := pp.getNextInt(o.firstCand)
        ELSE
          o.firstCand := 0;
          o.lastCand := 239
        END;

        IF pp.keywordPresent("-angTolerance") THEN
          o.angTolerance := pp.getNextLongReal(0.0d0)
        ELSE
          o.angTolerance := 0.05d0
        END;

        IF pp.keywordPresent("-linTolerance") THEN
          o.linTolerance := pp.getNextLongReal(0.0d0)
        ELSE
          o.linTolerance := 0.05d0
        END;

        (* Output options: *)

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

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

        pp.finish();

      EXCEPT
      | ParseParams.Error =>
          Wr.PutText(stderr, "Usage: PZFindClusters \\\n");
          Wr.PutText(stderr, "  -input NAME \\\n");
          Wr.PutText(stderr, "  [ -chainDir DIR ] -chainPrefix NAME \\\n");
          Wr.PutText(stderr, "  -band NUMBER [ -extension EXT ] \\\n");
          Wr.PutText(stderr, "  [ -maxCands NUMBER | -candsRange FIRSTNUM LASTNUM ] \\\n");
          Wr.PutText(stderr, "  [ -angTolerance NUMBER ] [ -linTolerance NUMBER ] \\\n");
          Wr.PutText(stderr, "  -output NAME [ -minClusterSize NUM ] \n");
        Process.Exit(1);
      END;
    END;
    RETURN o
  END GetOptions;


PROCEDURE FIZ(x: INT; width: NAT): TEXT =
  BEGIN
    RETURN Fmt.Pad(Fmt.Int(x), width, '0')
  END FIZ;
<*UNUSED*>
PROCEDURE FLR(x: LONG; width, prec: NAT): TEXT =
  BEGIN
    RETURN Fmt.Pad(Fmt.LongReal(x, Fmt.Style.Fix, prec), width)
  END FLR;

BEGIN
  Main()
END PZFindClusters.
(*
  Copyright © 2001 Universidade Estadual de Campinas (UNICAMP).
  Authors: Helena C. G. Leitão and Jorge Stolfi.

  This file can be freely distributed, used, and modified, provided
  that this copyright and authorship notice is preserved, and that any
  modified versions are clearly marked as such.

  This software has NO WARRANTY of correctness or applicability for
  any purpose. Neither the authors nor their employers chall be held
  responsible for any losses or damages that may result from its use.
*)