MODULE PZMergeClusters EXPORTS Main;

(* Combines a set of fragment clusters, rotated/shfted into a single cluster.  *)
(* Last edited on 2001-11-16 22:59:14 by stolfi *)

IMPORT ParseParams, Text, FileRd, FileWr, Wr, Fmt;
IMPORT Process, OSError, Thread, Stdio;
IMPORT LR2, LR3, LR4x4; 
IMPORT PZGeo, PZPose;  

FROM Stdio IMPORT stderr;
FROM Math IMPORT sin, cos;
FROM PZTypes IMPORT LONG, NAT;

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

TYPE
  Options = RECORD
      clusters: REF ClusterSpecList;  (* Cluster/pieces to be added. *)
      output: TEXT;                   (* Output filename, minus ".pos" extension. *)
    END;
    
  ClusterSpec = RECORD   (* Specifies a set of pieces to be added to the assembly. *)
      fragment: NAT;   (* A single piece, or LAST(NAT) if none. *)
      cluster: TEXT;   (* A pose file (without the ".pos" extension). *)
      turn: LONG;      (* Rotation angle in degrees. *)
      shift: LR2.T;    (* Displacement for the piece OR cluster. *)
    END;
    
  ClusterSpecList = ARRAY OF ClusterSpec;

PROCEDURE Main() =
  VAR nFrags: NAT;
  BEGIN
    WITH 
      o = GetOptions(),
      spec = o.clusters^,
      cluData = ReadClusters(spec)^
    DO
      nFrags := 0;
      FOR i := 0 TO LAST(cluData) DO
        INC(nFrags, NUMBER(cluData[i].pos^))
      END;
      WITH oinPose = NEW(REF PZPose.List, nFrags)^ DO
        nFrags := 0;
        FOR i := 0 TO LAST(cluData) DO
          WITH 
            si = spec[i],
            partPose = cluData[i].pos^,
            mi = ComputeMatrix(si.turn, si.shift)
          DO
            FOR k := 0 TO LAST(partPose) DO
              WITH P = joinPose[nFrags] DO
                P := partPose[k];
                (* Turn indeterminate poses into abslute ones: *)
                IF P.cvx[1] = P.cvx[0] THEN P.cvx[1] := LAST(NAT) END;
                IF P.cvx[1] = LAST(NAT) THEN 
                  P.m := LR4x4.Mul(P.m, mi)
                END;
                INC(nFrags)
              END
            END
          END
        END;
        WriteJoinedPoses(o.output, joinPose, "");
      END
    END
  END Main;  
  
PROCEDURE ComputeMatrix(turn: LONG; shift: LR2.T): LR4x4.T =
  CONST
    Degree = 0.0174532925199432d0;
  BEGIN
    WITH 
      theta = turn * Degree,
      u = LR3.T{1.0d0, 0.0d0, 0.0d0},
      v = LR3.T{cos(theta), sin(theta), 0.0d0},
      d = LR3.T{shift[0], shift[1], 0.0d0}
    DO
      RETURN LR4x4.Mul(PZGeo.Rotation(u, v), PZGeo.Translation(d))
    END
  END ComputeMatrix;

PROCEDURE WriteJoinedPoses(
    name: TEXT;
    READONLY pos: PZPose.List;
    cmt: TEXT;
  ) =
  BEGIN
    WITH fileName = name & ".pos" DO
      Wr.PutText(stderr, "writing " & fileName & "\n");
      WITH
        wr = FileWr.Open(fileName)
      DO
        PZPose.Write(wr, pos, cmt);
        Wr.Close(wr)
      END
    END
  END WriteJoinedPoses;
 
PROCEDURE ReadClusters(
    READONLY spec: ARRAY OF ClusterSpec
  ): REF ARRAY OF PZPose.ReadData =
  BEGIN
    WITH 
      rdata = NEW(REF ARRAY OF PZPose.ReadData, NUMBER(spec)), data = rdata^
    DO
      FOR i := 0 TO LAST(spec) DO
        WITH 
          di = data[i],
          frag = spec[i].fragment,
          cluster = spec[i].cluster
        DO
          IF frag # LAST(NAT) THEN
            WITH pos = NEW(REF PZPose.List, 1) DO
              pos[0] := PZPose.T{cvx := PZPose.CurvePair{frag,LAST(NAT)}, m := LR4x4.Identity()};
              di := PZPose.ReadData{pos := pos, cmt := "fragment " & Fmt.Int(frag)}
            END;
            <* ASSERT Text.Empty(cluster) *>
          ELSE
            WITH fileName = cluster & ".pos" DO
              Wr.PutText(stderr, "reading " & fileName & " ...\n");
              WITH rd = FileRd.Open(fileName) DO
                di := PZPose.Read(rd)
              END
            END
          END
        END
      END;
      RETURN rdata
    END
  END ReadClusters;
      
PROCEDURE GetOptions(): Options =
  VAR o: Options;
  BEGIN
    WITH
      pp = NEW(ParseParams.T).init(stderr)
    DO
      TRY
        o.clusters := ParseClusterOptions(pp);
        
        pp.getKeyword("-output");
        o.output := pp.getNext();

        pp.finish();   
        
      EXCEPT                                                            
      | ParseParams.Error =>                                              
          Wr.PutText(stderr, "Usage: PZMergeClusters \\\n");
          Wr.PutText(stderr, "  [ -join \\\n");
          Wr.PutText(stderr, "      { fragment NUM | cluster NAME } \\\n");
          Wr.PutText(stderr, "      [ turn DEGREES ] [ shift DX DY ] \\\n");
          Wr.PutText(stderr, "  ]... \\\n");
          Wr.PutText(stderr, "  -output NAME \n");
        Process.Exit(1);
      END;
    END;
    RETURN o
  END GetOptions;
 
PROCEDURE ParseClusterOptions(pp: ParseParams.T): REF ClusterSpecList
  RAISES {ParseParams.Error} =
  VAR
    clusters := NEW(REF ClusterSpecList, 2);
    nClusters: NAT := 0;
  BEGIN
    WHILE pp.keywordPresent("-join") DO
      VAR spec: ClusterSpec;
      BEGIN 
        WITH what = pp.getNext() DO
          IF Text.Equal(what, "fragment") THEN
            spec.fragment := pp.getNextInt(0);
            spec.cluster := ""
          ELSIF Text.Equal(what, "cluster") THEN
            spec.fragment := LAST(NAT);
            spec.cluster := pp.getNext()
          ELSE
            pp.error("bad keyword \"" & what & "\"")
          END
        END;
        IF pp.testNext("turn") THEN
          spec.turn := pp.getNextLongReal()
        ELSE
          spec.turn := 0.0d0
        END;
        IF pp.testNext("shift") THEN
          spec.shift[0] := pp.getNextLongReal();
          spec.shift[1] := pp.getNextLongReal()
        ELSE
          spec.shift[0] := 0.0d0;
          spec.shift[1] := 0.0d0
        END;
        IF nClusters >= NUMBER(clusters^) THEN
          WITH newClusters = NEW(REF ClusterSpecList, 2*nClusters + 2) DO
            SUBARRAY(newClusters^, 0, nClusters) := clusters^;
            clusters := newClusters
          END
        END;
        clusters[nClusters] := spec;
        INC(nClusters)
      END
    END;
    IF nClusters < NUMBER(clusters^) THEN
      WITH trimClusters = NEW(REF ClusterSpecList, nClusters) DO
        trimClusters^ := SUBARRAY(clusters^, 0, nClusters);
        clusters := trimClusters
      END
    END;
    RETURN clusters
  END ParseClusterOptions;

BEGIN
  Main()
END PZMergeClusters.
(*
  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.
*)