/* Compute curve invariants */
/* Last edited on 2015-01-20 16:43:58 by stolfilocal */

#include <stdio.h>

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

#include <pz_r3_chain.h>
#include <pz_double_chain.h>
#include <pz_types.h>


TYPE

  Options = struct ??? {
    char *input;   /* Input velocity and acceleration files (without ".flv"/".fla") */
    char *output;  /* Output curvature file name (without ".flc") */
    double lambda; /* Filtering scale */
  } ???;


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

    { /* with */
      ??? o = pz_get_options(int argc, char **argv);
      ??? rv = ReadChain(o.input & ".flv");
      ??? v = rv.c^;
      ??? ra = ReadChain(o.input & ".fla");
      ??? a = ra.c^;
      ??? cmt = rv.cmt & "\n" & ra.cmt & "\n" & PZComputeCurvatureComments(o, (v.ne));
      ??? cv = InvariantChainFromLR3Chain(v, a)^;
      /* do */

      { /* with */
        ??? cvUnit = ra.unit;
        ??? cvCmt = txtcat(cmt & "\n" , "Curvature - computed at filtered curve points");
        /* do */
        pz_double_chain_write(open_write(o.output & ".fcv", TRUE), cvCmt, cv, 0.0e0, cvUnit);
      };;
    }
  } /* Main */
  

pz_r3_chain_read_data  ReadChain(
    char *fileName 
  )
  {
    fprintf(stderr, "reading %s\n", fileName );
    return pz_r3_chain_read(
      open_read(fileName, TRUE),
      header_only = FALSE,
      recenter = pz_ctr_NONE
    )
  } /* ReadChain */


pz_double_chain_t *InvariantChainFromLR3Chain(
    pz_r3_chain_t *v,
    pz_r3_chain_t *a 
  )
  /*
    Given velocities and accelerations at various 
    points along a curve, computes the corresponding
    local curvatures. */
  VAR cmax : double;
  {
    affirm((v.ne) == (a.ne) );

    { /* with */
      ??? NP = (v.ne);
      ??? c = pz_double_chain_new(NP);
      /* do */
      cmax = 0.0e0;
      for (i = 0; i <= NP-1 ; i++){

        { /* with */
          ??? vel = Norm(v[i]);
          ??? cv = Cross(v[i],a[i]);
          /* do */
          c[i] = cv[2] / (vel * vel * vel);;
        };;
      };
      return c;
    }
  } /* InvariantChainFromLR3Chain */


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);

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

        argparser_get_keyword(pp, "-lambda");
        o.lambda = argparser_get_next_double(pp);

        argparser_finish(pp);
      EXCEPT
      | ParseParams.Error ==>
          fprintf(stderr, "Usage: pz_compute_curvature \\\n");
          fprintf(stderr, "  -input FILE \\\n");
          fprintf(stderr, "  -output FILE \\\n");
          fprintf(stderr, "  -lambda NUMBER \n");
        Process.Exit(1);;
      };;
    };
    return o

  } /* GetOptions */


char *PZComputeCurvatureComments(
    Options *o, 
    unsigned nSamples 
  )
  {

    { /* with */
      ??? wr = NEW(TextFILE *).init();
      /* do */
      fprintf( wr, "pz_compute_curvature:\n");
      fprintf(wr, "  input: %s\n", o.input );
      fprintf(wr, "  output: %s\n", o.output );
      fprintf(wr, "  lambda: %s\n", Fmt.LongReal(o.lambda) );
      fprintf( wr, "  nSamples: " & Fmt.Int(nSamples));
      return(TextFILE *oText(wr));
    } /* ){ */;
  } /* PZComputeCurvatureComments */

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