/* Plots histogram of sample discrepancies in a given set of candidates */
/* Last edited on 2015-01-20 16:44:40 by stolfilocal */

/*
  This program reads a set of candidates (not necessarily good
  ones), and outputs the histogram of the sample discrepancies
  (point distances and curvature differences, both plain and encoded/decoded).
  The histograms are written to files ending with "-Pt.plt",
  "-Cv.plt", and "-Sy.plt", respetively.

  If a candidate "c" has an explicit pairing "c.pm", the discrepancies
  are computed between the pairs of samples indicated by "pm".
  Otherwise one uses the pairing that covers the entire candidate "c"
  and minimizes the sum of squared distances time step length.
*/

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

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

#include <pz_symbol.h>
#include <pz_match.h>
#include <pz_geo.h>
#include <pz_candidate.h>
#include <pz_histogram.h>
#include <pz_double_chain.h>
#include <pz_r3_chain.h>
#include <pz_symbol_chain.h>
#include <pz_histogram.h>
#include <pz_types.h>

TYPE

  Options = struct ??? {
    char *input;         /* Input candidate file (without extensions). */
    unsigned maxCands;       /* Max num of candidates to consider. */
    char *chainDir;      /* Directory where to find chain files. */
    char *chainPrefix;   /* Input chain file name prefix. */
    unsigned band;           /* Nominal band width (for chain file names). */
    char *output;        /* Output plot file name (without extensions). */

      /* If any of these is zero, the corresponding plot is skipped: */
    double maxPtDist;     /* Maximum plotted point distance. */
    double maxCvDist;     /* Maximum plotted curvature difference. */
    double maxSyDist; /* Maximum plotted encoded curvature difference. */
  } ???;



int main(int argc, char **argv 
  )
  {

    { /* with */
      ??? o = pz_get_options(int argc, char **argv);
      /* do */

      { /* with */
        ??? candData = pz_candidate_read(open_read(o.input & ".can", TRUE));
        ??? cand = SUBARRAY(candData.c^, 0, MIN(o.maxCands, (candData.c^.ne)));
        ??? chainUsed = pz_candidate_chains_used(cand)^;
        /* do */
        if (( o.maxPtDist != 0.0e0 )){ 

          { /* with */
            ??? h = ComputePtHistogram(cand, chainUsed, o.maxPtDist, o);
            /* do */
            OutputHistogram(o.output, "Pt", h);
          };
        };
        if (( o.maxCvDist != 0.0e0 )){ 

          { /* with */
            ??? h = ComputeCvHistogram(cand, chainUsed, o.maxCvDist, o);
            /* do */
            OutputHistogram(o.output, "Cv", h);
          };
        };
        if (( o.maxSyDist != 0.0e0 )){ 

          { /* with */
            ??? h = ComputeSyHistogram(cand, chainUsed, o.maxSyDist, o);
            /* do */
            OutputHistogram(o.output, "Sy", h);
          };
        };
      };
    }
  } /* Main */


pz_histogram.T  ComputePtHistogram(
    VAR /*IO*/ cand: pz_candidate.List;
    bool_vec_t *chainUsed,
    double maxDist,
    Options *o 
  )
  /*
    Computes the histogram of squared distances between points.
    Also computes a packed pairing "c.pm" for each candidate,
    if it is missing one.
  */
  CONST NBins == 51; /* Bins in histograms */
  VAR rmt: REF pz_match_t;
  VAR rCost: REF pz_match_cost_matrix_t := NIL;
  {

    { /* with */
      chAllData = pz_r3_chain_read_all(
        prefix := o.chainPrefix, band := o.band, extension := ".flc";
        sel := chainUsed, dir := o.chainDir;
        header_only := FALSE, recenter := pz_ctr_NONE
      );
      ??? chData = chAllData.chData^;
      ??? h = pz_histogram.New(0.0e0, maxDist*maxDist, NBins)^;
      /* do */
      for (i = 0; i < (cand.ne ) ; i++){

        { /* with */ 
          ??? c = cand[i];
          ??? seg = c.seg;
          ??? cvxa = seg[0].cvx;
          ??? cvxb = seg[1].cvx;
          ??? a = pz_r3_chain_extract-segment(chData[cvxa].c^, seg[0])^;
          ??? b = pz_r3_chain_extract-segment(chData[cvxb].c^, seg[1])^;
          /* do */
          MatchGeometricChains(c, a, b, rmt, rCost);

          { /* with */
            ??? mt = rmt^;
            /* do */
            for (i = 0; i < (mt.ne ) ; i++){

              { /* with */ 
                ??? ia = mt[i][0], ib = mt[i][1];
                /* do */
                /*
                fprintf(stderr, "ia == " & FI(ia,4) & " ib == " & FI(ib,4) & "\n");
                fprintf(stderr, "a[ia] == " & FLR3(a[ia],6,2) & " b[ib] == " & FLR3(b[ib], 6,2) & "\n");
                */
                pz_histogram.Add(h, r3_dist_sqr(a[ia], b[ib]));
              };
            };
            if (( c.pm == NULL )){
              c.pm = pz_match_pack(mt);
            };
          };
        };
      };
      fprintf(stderr, "rms geometric distance == %s\n", FLR(sqrt(Avg(h)), 9,6) );
      return h;
    }
  } /* ComputePtHistogram */
  

void MatchGeometricChains(
    VAR /*IO*/ c: pz_candidate_t;
    VAR /*IO*/ a, b: pz_r3_chain_t;
    REF *pz_match_t rmt  /* (OUT) */
    REF *pz_match_cost_matrix_t rCost, /* (WORK) */
  )
  /*
    Translates and rotates the chains "a" and "b" so as to minimize
    the average distance between them.  Also returns a pairing "rmt^"
    between their samples.
    
    If candidate "c" has an explicit pairing "c.pm", that pairing is
    used to adjust the two chains, and is returned in "rmt".
    
    If the candidate has no explicit pairing, the chains are
    positioned by pairing their endpoints only.  (This simple method
    is probably better than any fancier schema, since candidates
    without explicit pairings are probably created by hand and
    checked visually with "pz_draw_cands" in which case the simple
    alignment by endpoints is just what the user intended.)  */
  VAR avgDist: double;
  {
    
    /* Get the initial pairing: */
    if (( c.pm != NULL )){
      rmt = pz_match_unpack(c.pm^);
      AlignMatchedPoints(a, b, rmt^)
    }else{
      MakeHorizontal(a);
      MakeHorizontal(b);
      pz_r3_chain_match_find_best(
        /*IN:*/
        a, b,
        maxDistSqr = (double.ne - 1),
        removeUnmatchedEnds = FALSE,
        /*OUT:*/
        mt = rmt,
        avgDist = /*OUT*/ avgDist,
        length = c.length,
        matchedLength = c.matchedLength,
        /*WORK:*/
        rCost = rCost
      );
      c.mismatch = c.length * avgDist*avgDist;;
    };
  } /* MatchGeometricChains */
  

void MakeHorizontal(
    pz_r3_chain_t *a 
  )
  /*
    Translates and rotates a chain "a" so that its 
    end-points are on the X-axis, at equal distances
    from the origin. */
  {

    { /* with */ 
      ??? p = a[0];
      ??? q = a[(a.ne - 1)];
      ??? ctr = pz_geo.SegMid(p, q);
      ??? u = pz_geo.SegDir(p, ctr);
      ??? disp = (r3_t){-ctr[0],-ctr[1],-ctr[2]};
      ??? trn = pz_geo.Translation(disp); /* -ctr */
      ??? rot = pz_geo.Rotation(u, r3_t{1.0e0, 0.0e0, 0.0e0});
      ??? map = LR4x4.Mul(trn,rot);
      /* do */
      pz_r3_chain_do_map(a, map);
    };     
  } /* MakeHorizontal */  
  

void AlignMatchedPoints(
    pz_r3_chain_t *a,
    pz_r3_chain_t *b, 
    pz_match_t *mt 
  )
  /* 
    Translates and rotates the two chains so as to minimize the 
    sum of the squared distances from "a[mt[i,0]]" to "b[mt[i,1]]",
    weighted by the length of the steps of "mt" adjacent
    to "mt[i]". */
    

  double  PairWeight(
    unsigned i 
  )
    /* 
      The weight of the pair "mt[i]", defined as half of the weight of
      the adjacent steps of "mt", where a full step has weight 1 and a
      half-step has weight 1/2. */
    VAR s: double := 0.0e0;
    {
      if (( i > FIRST(mt) )){ 
        if (( mt[i-1,0] == mt[i,0] ) || ( mt[i-1,1] == mt[i,1] )){
          s = s + 0.25e0
        }else{
          s = s + 0.5e0;
        };
      };
      if (( i < (mt.ne - 1) )){ 
        if (( mt[i+1,0] == mt[i,0] ) || ( mt[i+1,1] == mt[i,1] )){
          s = s + 0.25e0
        }else{
          s = s + 0.5e0;
        };
      };
      return s
    } /* PairWeight */
  
  {
    /* Bring the barycenters of "a" and "b" "(0,0)": */
    VAR aSum, bSum: r3_t := r3_t{0.0e0, ..}; 
        wSum: double = 0.0e0;
    {
      /* Compute the weighted barycenters of "a" and "b": */
      for (i = 0; i < (mt.ne ) ; i++){

        { /* with */ 
          ??? ia = mt[i][0], ib = mt[i][1];
          ??? w = PairWeight(i);
          /* do */
          aSum = r3_mix(1.0e0, aSum, w, a[ia]);
          bSum = r3_mix(1.0e0, bSum, w, b[ib]);
          wSum = wSum + w;
        };
      };

      /* Translate the chains: */

      { /* with */
        ??? aBar = r3_scale(1.0e0/wSum, aSum);
        /* do */
        for (i = 0; i < (a.ne ) ; i++){ a[i] = r3_sub(a[i], aBar) ;};
      };

      { /* with */
        ??? bBar = r3_scale(1.0e0/wSum, bSum);
        /* do */
        for (i = 0; i < (b.ne ) ; i++){ b[i] = r3_sub(b[i], bBar) ;};
      };;
    };

    /* Rotate "b" to minimize the discrepancy with "a": */
    VAR s, c: double := 0.0e0;
    {
      /* 
        Now the sum of the weighted distances squared is a sinusoid
        of the form "K - 2*(c*cos(theta) + s*sin(theta))" where
        "theta" is the rotation of "b".
      */
      /* Compute the coeficients "c" and "s": */
      for (i = 0; i < (mt.ne ) ; i++){

        { /* with */ 
          ??? ia = mt[i][0];
          ??? xa = a[ia][0], ya = a[ia][1];
          ??? ib = mt[i][1];
          ??? xb = b[ib][0], yb = b[ib][1];
          ??? w = PairWeight(i);
          /* do */
          c = c + w * (xa*xb + ya*yb);
          s = s - w * (xa*yb - ya*xb);;
        };
      };

      if (( c != 0.0e0 ) || ( s != 0.0e0 )){
        /* Rotate the chain "b" by the angle of (c,s) */

        { /* with */
          ??? r = sqrt(c*c + s*s), c = c/r, s = s/r;
          /* do */
          for (i = 0; i < (b.ne ) ; i++){

            { /* with */
              ??? x = b[i][0];
              ??? y = b[i][1];
              ??? xr = x*c - y*s;
              ??? yr = x*s + y*c;
              /* do */
              x = xr; y = yr;
            };
          };
        };
      };
    }
  } /* AlignMatchedPoints */


pz_histogram.T  ComputeCvHistogram(
    pz_candidate.List *cand,
    bool_vec_t *chainUsed,
    double maxDist,
    Options *o 
  )
  CONST NBins == 51; /* Bins in histograms */
  {

    { /* with */
      chAllData = pz_double_chain_read_all(
        prefix := o.chainPrefix, band := o.band, extension := ".fcv";
        sel := chainUsed, dir := o.chainDir, header_only := FALSE
      );
      ??? chData = chAllData.chData^;
      ??? h = pz_histogram.New(0.0e0, maxDist*maxDist, NBins)^;
      /* do */
      for (i = 0; i < (cand.ne ) ; i++){

        { /* with */ 
          ??? c = cand[i];
          ??? seg = c.seg;
          ??? mt = GetCandPairing(c)^;
          ??? cvxa = seg[0].cvx;
          ??? cvxb = seg[1].cvx;
          ??? a = pz_double_chain_extract_segment(chData[cvxa].c^, seg[0], curvature := TRUE)^;
          ??? b = pz_double_chain_extract_segment(chData[cvxb].c^, seg[1], curvature := TRUE)^;
          /* do */
          for (i = 0; i < (mt.ne ) ; i++){

            { /* with */ 
              ??? ia = mt[i][0], ib = mt[i][1];
              ??? d = a[ia] - b[ib];
              /* do */
              pz_histogram.Add(h, d*d);
            };
          };
        };
      };
      fprintf(stderr, "rms curvature difference == %s\n", FLR(sqrt(Avg(h)), 9,6) );
      return h;
    }
  } /* ComputeCvHistogram */


pz_histogram.T  ComputeSyHistogram(
    pz_candidate.List *cand,
    bool_vec_t *chainUsed,
    double maxDist,
    Options *o 
  )
  CONST NBins == 53; /* Bins in histograms */
  {

    { /* with */
      chAllData = pz_symbol_chain_read_all(
        prefix := o.chainPrefix, band := o.band, extension := ".cvc";
        sel := chainUsed, dir := o.chainDir, header_only := FALSE
      );
      ??? chData = chAllData.chData^;
      ??? h = pz_histogram.New(0.0e0, maxDist*maxDist, NBins)^;
      ??? curvSigma = chAllData.sigmaMax;
      ??? decode = pz_symbol_make_decode_table(curvSigma)^;
      ??? errorVar = pz_symbol_make_error_var_table(curvSigma)^;
      /* do */
      affirm(chAllData.sigmaMax == chAllData.sigmaMin );
      for (i = 0; i < (cand.ne ) ; i++){

        { /* with */ 
          ??? c = cand[i];
          ??? seg = c.seg;
          ??? mt = GetCandPairing(c)^;
          ??? cvxa = seg[0].cvx;
          ??? cvxb = seg[1].cvx;
          ??? a = pz_symbol_chain_extract_segment(chData[cvxa].c^, seg[0])^;
          ??? b = pz_symbol_chain_extract_segment(chData[cvxb].c^, seg[1])^;
          /* do */
          for (i = 0; i < (mt.ne ) ; i++){

            { /* with */ 
              ??? ia = mt[i][0], ib = mt[i][1];
              ??? d2 = pz_symbol_dist_sqr(a[ia],b[ib];
                complement := FALSE, decode := decode, errorVar := errorVar
              );
              /* do */
              pz_histogram.Add(h, d2);
            };
          };
        };
      };
      fprintf(stderr, "rms curvature difference == %s\n", FLR(sqrt(Avg(h)), 9,6) );
      return h;
    }
  } /* ComputeSyHistogram */


pz_match_t *GetCandPairing(
    pz_candidate_t *c 
  )
  {
    if (( c.pm != NULL )){
      return pz_match_unpack(c.pm^)
    }else{
      return pz_match_most_perfect(c.seg[0].ns, c.seg[1].ns);
    };
  } /* GetCandPairing */


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
        argparser_get_keyword(pp, "-input");
        o.input = argparser_get_next(pp);

        if (( argparser_keyword_present(pp, "-maxCands") )){
          o.maxCands = argparser_get_next_int(pp, 1)
        }else{
          o.maxCands = (unsigned.ne - 1);
        };

        if (( argparser_keyword_present(pp, "-chainDir") )){
          o.chainDir = argparser_get_next(pp)
        }else{
          o.chainDir = ".";
        };

        argparser_get_keyword(pp, "-chainPrefix");
        o.chainPrefix = argparser_get_next(pp);

        argparser_get_keyword(pp, "-band");
        o.band = argparser_get_next_int(pp);

        if (( argparser_keyword_present(pp, "-maxPtDist") )){
          o.maxPtDist = argparser_get_next_double(pp, 0.000001e0)
        }else{
          o.maxPtDist = 0.0e0;
        };

        if (( argparser_keyword_present(pp, "-maxCvDist") )){
          o.maxCvDist = argparser_get_next_double(pp, 0.000001e0)
        }else{
          o.maxCvDist = 0.0e0;
        };

        if (( argparser_keyword_present(pp, "-maxSyDist") )){
          o.maxSyDist = argparser_get_next_double(pp, 0.000001e0)
        }else{
          o.maxSyDist = 0.0e0;
        };

        argparser_get_keyword(pp, "-output");
        o.output = argparser_get_next(pp);

        argparser_finish(pp);
      EXCEPT
      | ParseParams.Error ==>
          fprintf(stderr,  "Usage: pz_dist_sqr_histogram \\\n");
          fprintf(stderr,  "   -input  FNAME [ -maxCands NUMBER ] \\\n");
          fprintf(stderr,  "   [ -chainDir  NAME ] -chainPrefix  NAME \\\n");
          fprintf(stderr,  "   -band  unsigned \\\n");
          fprintf(stderr,  "   -output  FNAME \\\n");
          fprintf(stderr,  "   [ -maxPtDistSqr NUM ] \\\n");
          fprintf(stderr,  "   [ -maxCvDistSqr NUM ] \\\n");
          fprintf(stderr,  "   [ -maxSyDistSqr NUM ] \n");
          Process.Exit(1);;
      };;
    };
    return o;
  }  GetOptions;


void OutputHistogram(
    char *output, 
    char *typeTag,
    pz_histogram.T *h 
  )
  {

    { /* with */
      ??? fileName = txtcat(output & "-" & typeTag , ".plt");
      /* do */
      fprintf(stderr, "writing %s\n", fileName );

      { /* with */
        ??? wr = open_write(fileName, TRUE);
        /* do */
        pz_histogram.Output(wr, h);
        fclose(wr);
      };
    }
  } /* OutputHistogram */

<*UNUSED);

char *FI(
    int x, 
    unsigned w 
  ) 
  {
    return Fmt.Pad(Fmt.Int(x), w)
  } /* FI */


char *FLR(
    double x, 
    w,p: unsigned 
  ) 
  {
    return Fmt.Pad(Fmt.LongReal(x, style = Fmt.Style.Fix, prec = p), w)
  } /* FLR */

<*UNUSED);

char *FLR3(
    r3_t x, 
    w,p: unsigned 
  ) 
  {
    return "( " & FLR(x[0],w,p) & " " & FLR(x[1],w,p) & " " & FLR(x[2],w,p) & " )"
  } /* FLR3 */

{

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