MODULE PZComputeVelAcc EXPORTS Main;

(* Computes the velocity and acceleration of a curve. *)
(* Last edited on 1999-11-08 05:39:34 by hcgl *)
    
(*
  This program takes an arbitrary curve "c" and outputs 
  two curves which are its first and second derivatives. 
*)

IMPORT ParseParams, Process, FileRd, Wr, FileWr;
IMPORT OSError, Thread, Stdio, Fmt;
IMPORT PZCurve, PZLR3Chain;

FROM Stdio IMPORT stderr;
FROM PZTypes IMPORT LONG, LONGS, NAT, BOOL;
 
<* FATAL Wr.Failure, Thread.Alerted, OSError.E *>

TYPE
  Options = RECORD
      input: TEXT;      (* Input float chain file name (without ".flc"). *)
      output: TEXT;     (* Output float chain file name (without extension). *)
      velUnit: LONG;    (* Quantization unit for velocity. *)
      accUnit: LONG;    (* Quantization unit for acceleration. *)
      verbose: BOOL;    (* TRUE to make noise while working. *)
    END;
    
TYPE
  CurveData = RECORD
      cmt: TEXT;
      curve: PZCurve.T
    END;

PROCEDURE Main() =
  BEGIN
    WITH
      o = GetOptions(),
      inputFileName = o.input & ".flc",
      rp = ReadCurve(inputFileName, o.verbose),
      c = rp.curve,
      v = c.diff(),
      a = v.diff(),
      cCmt = rp.cmt & "PZComputeVelAcc:\n" & "  input file = " & inputFileName & "\n"
    DO 
      WriteCurve(o.output & ".flv", cCmt, v, o.velUnit);
      WriteCurve(o.output & ".fla", cCmt, a, o.accUnit);
    END
  END Main;
  
PROCEDURE ReadCurve(fileName: TEXT; verbose: BOOL): CurveData =
  VAR L: LONG;
  BEGIN
    Wr.PutText(stderr, "reading file " & fileName & " ...\n");
    WITH
      chd = PZLR3Chain.Read(FileRd.Open(fileName), headerOnly := FALSE),
      p = chd.c^,
      m = NUMBER(p),
      c = PZCurve.New(m),
      s = NEW(REF LONGS, m)^
    DO
      c.setSamples(p);
      (* Initial times are lengths along the polygonal: *)
      L := PZLR3Chain.LinearLengths(p, s);
      IF verbose THEN
        Wr.PutText(stderr, "polygonal length = " & FLR(L,8,5) & "\n");
      END;
      c.setTimes(s, L);
      (* Recompute lengths along interpolated curve: *)
      c.uniformize();
      IF verbose THEN
        Wr.PutText(stderr,"adjusted curve length = " & FLR(c.tPeriod,8,5) & "\n");
      END;
      (* Use the natural parameter values of the original curve as labels: *)
      c.setLabels(c.t^, c.tPeriod);
      RETURN CurveData{cmt := chd.cmt, curve := c}
    END
  END ReadCurve;
  
PROCEDURE WriteCurve(
    fileName: TEXT;
    cmt: TEXT;
    READONLY c: PZCurve.T;
    unit: LONG;
  ) = 
  BEGIN
    Wr.PutText(stderr, "writing file " & fileName & " ...\n");
    PZLR3Chain.Write(
      FileWr.Open(fileName), 
      cmt,
      c.p^,
      unit := unit
    )
  END WriteCurve;

PROCEDURE GetOptions(): Options =
  VAR o: Options;
  BEGIN
    WITH
      pp = NEW(ParseParams.T).init(stderr)
    DO
      TRY
        pp.getKeyword("-input");
        o.input := pp.getNext();
        
        pp.getKeyword("-output");
        o.output := pp.getNext();
        
        pp.getKeyword("-velUnit");
        o.velUnit := pp.getNextLongReal(0.000000001d0, LAST(LONG));
        
        pp.getKeyword("-accUnit");
        o.accUnit := pp.getNextLongReal(0.000000001d0, LAST(LONG));
        
        o.verbose := pp.keywordPresent("-verbose");
        
        pp.finish();                                       
      
      EXCEPT                                                            
      | ParseParams.Error =>                                              
          Wr.PutText(stderr, "Usage: PZComputeVelAcc \\\n");
          Wr.PutText(stderr, "  -input NAME \\\n");
          Wr.PutText(stderr, "  -output NAME \\\n");
          Wr.PutText(stderr, "  -velUnit NUM -accUnit NUM \\\n");
          Wr.PutText(stderr, "  [ -verbose ] \n");
        Process.Exit(1);
      END;
    END;
    RETURN o
    
  END GetOptions;

PROCEDURE FLR(x: LONG; width, prec: NAT): TEXT =
  BEGIN
    RETURN Fmt.Pad(Fmt.LongReal(x, prec := prec, style := Fmt.Style.Fix), width)
  END FLR;

BEGIN
  Main()
END PZComputeVelAcc.



(*
  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.
*)