/* Computes the velocity and acceleration of a curve. */
/* Last edited on 2015-01-20 16:44:05 by stolfilocal */
    
/*
  This program takes an arbitrary curve "c" and outputs 
  two curves which are its first and second derivatives. 
*/

#include <stdio.h>

#include <argparser.h>

#include <vsignal.h>
#include <pz_r3_chain.h>
#include <pz_types.h>
 
/* 

    !!! FIND THE PROPER PLACE FOR THIS !!! 
    
    The curve is `almost always' G1-smooth, i.e. the tangent
    direction is defined at all times, except for a zero-measure
    subset of configurations of the samples {p[i]}.

    The curve {c.p(t)} is defined so that {c.p(t) == c.p[i]} when
    {t == c.t[i]}, for {i IN [0..c.m-1]}; and also {c(t +
    c.tPeriod) == c(t)} for all {t}.  The nodes (sampling times)
    {t[i]} are a strictly increasing sequence in {[0 _
    c.tPeriod)}.

    For each time value {t}, the curve also assigns a numeric
    label {c.r(t)}, which is a monotonically increasing
    continuous function of reals to reals, such that
    {c.r(t + c.tPeriod) == c.r(t) + c.rPeriod}, for  all {t}.
    The label value at time {c.t[i]} is {c.r[i]}, for all {i}.

    The set of points traversed by the curve does not depend on
    the times {c.t[i]} or the labels {c.r[i]}. */

void pz_curve_setSamples(pz_curve_t *c, r3_vec_t *p);
  /* Modifies the curve {c} so that it goes through point {p[i]},
    for {i} in {[0..c.m-1]}.  Requires {(p.ne) == c.m}.
    Recomputes the lengths {c.s[i]}.  Preserves the times
    {c.t[i]} and the labels {c.r[i]}. */

void pz_curve_setLabels(pz_curve_t *c, double_vec_t *r, double rPeriod);
  /* Modifies the labeling function of the curve {c} so that it
    has period {rPeriod} and value {r[i]} at time {c.t[i]}, for
    all {i} in {[0..c.m-1]}.  Preserves the positions {c.p[i]},
    sampling times {c.t[i]}, and lengths {c.s[i]}.  Requires
    {(r.ne) == c.m}. */

TYPE

  Options = struct ??? {
    char *input;      /* Input curve file name (without ".flc"). */
    char *output;     /* Output velocity/acceleration file name (without extension). */
    double velUnit;    /* Quantization unit for velocity. */
    double accUnit;    /* Quantization unit for acceleration. */
    bool_t verbose; /* TRUE to make noise while working. */
  } ???;
    
TYPE

  CurveData = struct ??? {
    char *cmt;
    pz_curve.T curve;
  } ???;


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

    { /* with */
      ??? o = pz_get_options(int argc, char **argv);
      ??? inputFileName = txtcat(o.input , ".flc");
      ??? rp = ReadCurve(inputFileName, o.verbose);
      ??? c = rp.curve;
      ??? v = c.diff();
      ??? a = v.diff();
      ??? cCmt = txtcat(rp.cmt & "pz_compute_vel_acc:\n" & "  input file = " & inputFileName , "\n");
      /* do */ 
      WriteCurve(o.output & ".flv", cCmt, v, o.velUnit);
      WriteCurve(o.output & ".fla", cCmt, a, o.accUnit);;
    }
  } /* Main */
  

CurveData  ReadCurve(
    char *fileName, 
    bool_t verbose 
  )
  {
    fprintf(stderr, "reading file %s ...\n", fileName );

    { /* with */
      ??? chd = pz_r3_chain_read(open_read(fileName, TRUE), header_only := FALSE, recenter := pz_ctr_NONE);
      ??? p = chd.c^;
      ??? m = (p.ne);
      ??? c = pz_curve.New(m);
      /* do */
      c.setSamples(p);
      c.uniformize();
      if (( verbose )){
        fprintf(stderr, "curve length == %s\n", FLR(c.tPeriod,8,5) );;
      };
      /* Use the natural parameter values of the original curve as labels: */
      c.setLabels(c.t^, c.tPeriod);
      return CurveData{cmt = chd.cmt, curve = c};
    }
  } /* ReadCurve */
  

void WriteCurve(
    char *fileName,
    char *cmt,
    pz_curve.T *c,
    double unit 
  ) 
  {
    fprintf(stderr, "writing file %s ...\n", fileName );

    { /* with */
      ??? adjUnit = pz_r3_chain_adjust_unit(unit, c.p^);
      /* do */
      pz_r3_chain_write(open_write(fileName, TRUE), cmt, c.p^, unit = adjUnit);
    }
  } /* WriteCurve */


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, "-velUnit");
        o.velUnit = argparser_get_next_double(pp, 0.000000001e0, (double.ne - 1));
        
        argparser_get_keyword(pp, "-accUnit");
        o.accUnit = argparser_get_next_double(pp, 0.000000001e0, (double.ne - 1));
        
        o.verbose = argparser_keyword_present(pp, "-verbose");
        
        argparser_finish(pp);                                       
      
      EXCEPT                                                            
      | ParseParams.Error ==>                                              
          fprintf(stderr, "Usage: pz_compute_vel_acc \\\n");
          fprintf(stderr, "  -input NAME \\\n");
          fprintf(stderr, "  -output NAME \\\n");
          fprintf(stderr, "  -velUnit NUM -accUnit NUM \\\n");
          fprintf(stderr, "  [ -verbose ] \n");
        Process.Exit(1);;
      };;
    };
    return o
    
  } /* GetOptions */


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

{



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