/* Combines a set of fragment clusters, rotated/shfted into a single cluster.  */
/* Last edited on 2015-01-20 16:46:56 by stolfilocal */

#include <stdio.h>
#include <string.h>
#include <math.h>

#include <argparser.h>
#include <r2.h>
#include <r3.h>
#include <r4x4.h>

#include <pz_geo.h>
#include <pz_pose.h>
#include <pz_types.h>


TYPE

  Options = struct ??? {
    ClusterSpecList *clusters;  /* Cluster/pieces to be added. */
    char *output; /* Output filename, minus ".pos" extension. */
  } ???;
    

  ClusterSpec = struct ??? {
    /* Specifies a set of pieces to be added to the assembly. */
    unsigned fragment;   /* A single piece, or (unsigned.ne - 1) if none. */
    char *cluster;   /* A pose file (without the ".pos" extension). */
    double turn;      /* Rotation angle in degrees. */
    LR2.T shift; /* Displacement for the piece OR cluster. */
  } ???;
    
  ClusterSpecList == ARRAY OF ClusterSpec;


int main(int argc, char **argv 
  )
  VAR nFrags: unsigned;
  {

    { /* with */ 
      ??? o = pz_get_options(int argc, char **argv);
      ??? spec = o.clusters^;
      ??? cluData = ReadClusters(spec)^;
      /* do */
      nFrags = 0;
      for (i = 0; i < (cluData.ne ) ; i++){
        INC(nFrags, (cluData[i].pos^.ne));
      };

      { /* with */
        ??? oinPose = NEW(REF pz_pose.List, nFrags)^;
        /* do */
        nFrags = 0;
        for (i = 0; i < (cluData.ne ) ; i++){

          { /* with */ 
            ??? si = spec[i];
            ??? partPose = cluData[i].pos^;
            ??? mi = ComputeMatrix(si.turn, si.shift);
            /* do */
            for (k = 0; k < (partPose.ne ) ; k++){

              { /* with */
                ??? P = joinPose[nFrags];
                /* do */
                P = partPose[k];
                /* Turn indeterminate poses into abslute ones: */
                if (( P.cvx[1] == P.cvx[0] )){ P.cvx[1] = (unsigned.ne - 1) ;};
                if (( P.cvx[1] == (unsigned.ne - 1) )){ 
                  P.m = LR4x4.Mul(P.m, mi);
                };
                nFrags++;
              };
            };
          };
        };
        WriteJoinedPoses(o.output, joinPose, "");;
      };
    }
  } /* Main */  
  

r4x4_t  ComputeMatrix(
    double turn, 
    LR2.T shift 
  )
  CONST
    Degree == 0.0174532925199432e0;
  {

    { /* with */ 
      ??? theta = turn * Degree;
      ??? u = r3_t{1.0e0, 0.0e0, 0.0e0};
      ??? v = r3_t{cos(theta), sin(theta), 0.0e0};
      ??? d = r3_t{shift[0], shift[1], 0.0e0};
      /* do */
      return LR4x4.Mul(pz_geo.Rotation(u, v), pz_geo.Translation(d));
    }
  } /* ComputeMatrix */


void WriteJoinedPoses(
    char *name,
    pz_pose.List *pos,
    char *cmt 
  )
  {

    { /* with */
      ??? fileName = txtcat(name , ".pos");
      /* do */
      fprintf(stderr, "writing %s\n", fileName );

      { /* with */
        ??? wr = open_write(fileName, TRUE);
        /* do */
        pz_pose.Write(wr, pos, cmt);
        fclose(wr);
      };
    }
  } /* WriteJoinedPoses */
 

ARRAY *OF pz_pose.ReadData  ReadClusters(
    ARRAY *OF ClusterSpec spec 
  )
  {

    { /* with */ 
      ??? rdata = NEW(ARRAY *OF pz_pose.ReadData, (spec.ne)), data = rdata^;
      /* do */
      for (i = 0; i < (spec.ne ) ; i++){

        { /* with */ 
          ??? di = data[i];
          ??? frag = spec[i].fragment;
          ??? cluster = spec[i].cluster;
          /* do */
          if (( frag != (unsigned.ne - 1) )){

            { /* with */
              ??? pos = NEW(REF pz_pose.List, 1);
              /* do */
              pos[0] = pz_pose.T{cvx = pz_pose.CurvePair{frag,(unsigned.ne - 1)}, m = LR4x4.Identity()};
              di = pz_pose.ReadData{pos = pos, cmt = "fragment " & Fmt.Int(frag)};
            };
            affirm(Text.Empty(cluster) );
          }else{

            { /* with */
              ??? fileName = txtcat(cluster , ".pos");
              /* do */
              fprintf(stderr, "reading %s ...\n", fileName );

              { /* with */
                ??? rd = open_read(fileName, TRUE);
                /* do */
                di = pz_pose.Read(rd);
              };
            };
          };
        };
      };
      return rdata;
    }
  } /* ReadClusters */
      

Options  pz_get_options(int argc, char **argv 
  )
  VAR o: Options;
  {
    argparser_t *pp = argparser_new(stderr, argc, argv);
    argparser_set_help(pp, PROG_NAME " version " PROG_VERS ", usage:\n" PROG_HELP);
    argparser_set_info(pp, PROG_INFO);
    argparser_process_help_info_options(pp);
    

    { /* with */
      /* do */
      TRY
        o.clusters = ParseClusterOptions(pp);
        
        argparser_get_keyword(pp, "-output");
        o.output = argparser_get_next(pp);

        argparser_finish(pp);   
        
      EXCEPT                                                            
      | ParseParams.Error ==>                                              
          fprintf(stderr, "Usage: pz_merge_clusters \\\n");
          fprintf(stderr, "  [ -join \\\n");
          fprintf(stderr, "      { fragment NUM | cluster NAME } \\\n");
          fprintf(stderr, "      [ turn DEGREES ] [ shift DX DY ] \\\n");
          fprintf(stderr, "  ]... \\\n");
          fprintf(stderr, "  -output NAME \n");
        Process.Exit(1);;
      };;
    };
    return o
  } /* GetOptions */
 

ClusterSpecList *ParseClusterOptions(
    arparser_t pp 
  )
  RAISES {ParseParams.Error} =
  VAR
    clusters = NEW(REF ClusterSpecList, 2);
    nClusters: unsigned = 0;
  {
    while ( argparser_keyword_present(pp, "-join") ){
      VAR spec: ClusterSpec;
      { 

        { /* with */
          ??? what = argparser_get_next(pp);
          /* do */
          if (( Text.Equal(what, "fragment") )){
            spec.fragment = argparser_get_next_int(pp, 0);
            spec.cluster = ""
          }else if (( Text.Equal(what, "cluster") )){
            spec.fragment = (unsigned.ne - 1);
            spec.cluster = argparser_get_next(pp)
          }else{
            argparser_error(pp, "bad keyword \"" & what & "\"");
          };
        };
        if (( argparser_keyword_present_next(pp, "turn") )){
          spec.turn = argparser_get_next_double(pp)
        }else{
          spec.turn = 0.0e0;
        };
        if (( argparser_keyword_present_next(pp, "shift") )){
          spec.shift[0] = argparser_get_next_double(pp);
          spec.shift[1] = argparser_get_next_double(pp)
        }else{
          spec.shift[0] = 0.0e0;
          spec.shift[1] = 0.0e0;
        };
        if (( nClusters >= (clusters^.ne) )){

          { /* with */
            ??? newClusters = NEW(REF ClusterSpecList, 2*nClusters + 2);
            /* do */
            SUBARRAY(newClusters^, 0, nClusters) = clusters^;
            clusters = newClusters;
          };
        };
        clusters[nClusters] = spec;
        nClusters++;
      };
    };
    if (( nClusters < (clusters^.ne) )){

      { /* with */
        ??? trimClusters = NEW(REF ClusterSpecList, nClusters);
        /* do */
        trimClusters^ = SUBARRAY(clusters^, 0, nClusters);
        clusters = trimClusters;
      };
    };
    return clusters
  } /* ParseClusterOptions */

{
/*
  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.
*/