MODULE PZInvar EXPORTS Main;

(* Compute chain invariants *)

IMPORT LR3;
FROM LR3 IMPORT Norm, Dist;
FROM LR3Extras IMPORT Cross;

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

IMPORT PZLR3Chain, PZLRChain, PZSymbolChain;

FROM PZProc IMPORT Squeeze;
  
FROM Stdio IMPORT stderr;
 
<* FATAL Wr.Failure, Thread.Alerted, OSError.E *>

TYPE
  Options = RECORD
      inName: TEXT;         (* Input float chain file (without ".flv") *)
      outName: TEXT;        (* Output float/curv chain file name (without ".flc") *)
      epsilon: LONGREAL;    (* Estimated absolute curvature noise *)
      delta: LONGREAL;      (* Estimated relative curvature noise *)
    END;
   
PROCEDURE Main() =
  BEGIN
    WITH
      o = GetOptions(),
      rp = PZLR3Chain.Read(FileRd.Open(o.inName & ".flc")),
      p = rp.c^,
      rv = PZLR3Chain.Read(FileRd.Open(o.inName & ".flv")),
      v = rv.c^,
      ra = PZLR3Chain.Read(FileRd.Open(o.inName & ".fla")),
      a = ra.c^,
      cmt = rp.cmt & "\n" & PZInvarComments(o, NUMBER(p)),
      cv = InvariantChainFromLR3Chain(p, v, a)^,
      cvUnit = ra.unit,
      cvCmt = cmt & "\n" & "Curvature - computed at filtered curve points",
      L = ComputeLength(p)
    DO
      PZLRChain.Write(FileWr.Open(o.outName & ".fcv"), cvCmt, cv, 0.0d0, cvUnit);

      WITH
        cc =  QuantizeInvariantChain(cv, o.epsilon, o.delta)^,
        ccCmt = cmt & "\n" & "Coded Curvature - computed at filtered curve points"
      DO
        PZSymbolChain.Write(FileWr.Open(o.outName & ".cvc"), ccCmt, cc, L, o.epsilon, o.delta); 
      END;
    END
  END Main;


PROCEDURE ComputeLength(READONLY p: PZLR3Chain.T): LONGREAL =
  VAR  t: LONGREAL;
  BEGIN
    WITH N = NUMBER(p) DO 
      t := LR3.Dist(p[N-1], p[0]);
      FOR i := 1 TO N-1 DO
        WITH d = Dist(p[i], p[i-1]) DO t := t + d END;
      END;
      RETURN t
    END;
  END ComputeLength;


PROCEDURE InvariantChainFromLR3Chain(
    READONLY  p: PZLR3Chain.T;
    READONLY  v: PZLR3Chain.T; 
    READONLY  a: PZLR3Chain.T; 
  ): REF PZLRChain.T =
  (*
    Given n equally spaced points along a curve computes the corresponding 
    invariant (curvature) chain.
  *)
  VAR cmax : LONGREAL;
  BEGIN 
    WITH 
      NP = NUMBER(p),
      c = NEW(REF PZLRChain.T, NP)
    DO
      <* ASSERT NUMBER(v) = NP *>
      <* ASSERT NUMBER(a) = NP *>
      cmax := 0.0d0;
      FOR i:= 0 TO NP-1 DO 
        WITH 
          vel = Norm(v[i]), 
          cv = Cross(v[i],a[i])
        DO 
          c[i] := cv[2] / (vel * vel * vel);
        END;
      END;
      RETURN c
    END 
  END InvariantChainFromLR3Chain;


PROCEDURE QuantizeInvariantChain(
    READONLY  c: PZLRChain.T; 
    epsilon, delta: LONGREAL; 
  ): REF PZSymbolChain.T =
  (*
    Given the invariant chain, converts it to a coded
    curvature chain.
  *)
  BEGIN 
    WITH 
      NP = NUMBER(c),
      rcvc = NEW(REF PZSymbolChain.T, NP),
      cvc = rcvc^
    DO
     FOR i:= 0 TO NP-1 DO 
        WITH 
          h  = Squeeze(c[i], epsilon, delta),
          hs = MIN(+26.0d0, MAX(-26.0d0, h)),
          cs = ROUND(hs)
        DO
          IF cs < 0 THEN
            cvc[i] :=  VAL(ORD('a') - cs - 1, CHAR);
          ELSIF cs > 0 THEN 
            cvc[i] :=  VAL(ORD('A') + cs - 1, CHAR);
          ELSE 
            cvc[i] :=  VAL(ORD('0'),CHAR);  
          END
        END
      END;
      RETURN rcvc
    END 
  END QuantizeInvariantChain;


PROCEDURE GetOptions(): Options =
  VAR o: Options;
  BEGIN
    WITH
      pp = NEW(ParseParams.T).init(stderr)
    DO
      TRY
        pp.getKeyword("-inName");
        o.inName := pp.getNext();

        pp.getKeyword("-outName");
        o.outName := pp.getNext();
        
        pp.getKeyword("-epsilon");
        o.epsilon := pp.getNextLongReal();
        
        pp.getKeyword("-delta");
        o.delta := pp.getNextLongReal();
        
        pp.finish();                                       
      EXCEPT                                                            
      | ParseParams.Error =>                                              
          Wr.PutText(stderr, "Usage: PZInvar \\\n");
          Wr.PutText(stderr, "  -inName <name> \\\n");
          Wr.PutText(stderr, "  -outName <name> \\\n");
          Wr.PutText(stderr, "  -epsilon <number> -delta <number> \n");
        Process.Exit(1);
      END;
    END;
    RETURN o
    
  END GetOptions;


PROCEDURE PZInvarComments(READONLY o: Options; n : CARDINAL):TEXT=
  BEGIN 
    WITH 
      wr = NEW(TextWr.T).init()
    DO 
      Wr.PutText( wr, "PZInvar  inName: " & o.inName & "\n");
      Wr.PutText( wr, "         outName: " & o.outName & "\n");
      Wr.PutText( wr, "         epsilon: " & Fmt.LongReal(o.epsilon) & "\n");  
      Wr.PutText( wr, "         delta: " & Fmt.LongReal(o.delta) & "\n");  
      Wr.PutText( wr, "         n: " & Fmt.Int(n));  
      RETURN(TextWr.ToText(wr))
    END (* DO *);
  END PZInvarComments;


BEGIN
  Main()
END PZInvar.