MODULE PZCurve;

IMPORT LR3, PZLR3Chain, PZLRChain, PZProc, PZSmooth, PZGeo;
(* DEBUG
IMPORT Wr, Fmt;
FROM Stdio IMPORT stderr;
*)

REVEAL 
  T = Public BRANDED OBJECT
      v: REF PZLR3Chain.T;   (* "v[i]" is the velocity at time "t[i]". *) 
      a: REF PZLR3Chain.T;   (* A work vector, for certain procedures. *) 
    OVERRIDES
      setSamples := SetSamples;
      setLabels := SetLabels;
      setTimes := SetTimes;
      filter := Filter;
      sample := Sample;
      diff := Diff;
      copy := Copy;
    END;
    (*
      In this implementation, the curve is a closed cubic Hermite
      spline, that interpolates the point "c.p[i]" with velocity
      "c.v[i]" at time "c.t[i]", for "i IN [0..m-1]".
      
      The velocity "c.v[i]" and is automatically computed so as to
      minimize the velocity variation along the whole curve.
      
      A PZCurve.T includes also a vector "s[i]" giving the length along
      the curve from "c.t[i-1]" to "c.t[i]".  The total length of the
      curve is "c.length".
      
      The function "r" is defined by periodic linear interpolation
      of the values "c.r[i]". *)
           

PROCEDURE New(m: CARDINAL): T =
  BEGIN
    WITH
      c = NEW(T,
        m := m,
        t := NEW(REF PZLRChain.T, m),
        p := NEW(REF PZLR3Chain.T, m),
        r := NEW(REF PZLRChain.T, m),
        s := NEW(REF ARRAY OF LONG, m),
        v := NEW(REF PZLR3Chain.T, m),
        a := NEW(REF PZLR3Chain.T, m),
        tPeriod := 1.0d0,
        rPeriod := 1.0d0,
        length := 0.0d0
      ),
      p = c.p^,
      v = c.v^,
      t = c.t^,
      s = c.s^,
      r = c.r^
    DO
      FOR i := 0 TO m-1 DO
        p[i] := LR3.T{0.0d0, 0.0d0, 0.0d0};
        t[i] := FLOAT(i,LONG)/FLOAT(m,LONG);
        r[i] := t[i];
        s[i] := 0.0d0;
        v[i] := LR3.T{0.0d0, 0.0d0, 0.0d0};
      END;
      RETURN c
    END
  END New;

PROCEDURE Copy(c: T): T =
  BEGIN
    WITH
      m = c.m,
      d = NEW(T,
        m := m,
        t := NEW(REF ARRAY OF LONG, m),
        p := NEW(REF PZLR3Chain.T, m),
        s := NEW(REF ARRAY OF LONG, m),
        r := NEW(REF PZLRChain.T, m),
        v := NEW(REF PZLR3Chain.T, m),
        a := NEW(REF PZLR3Chain.T, m),
        length := c.length,
        tPeriod := c.tPeriod,
        rPeriod := c.rPeriod
      )
    DO
      d.t^ := c.t^;
      d.p^ := c.p^;
      d.s^ := c.s^;
      d.r^ := c.r^;
      d.v^ := c.v^;
      RETURN d
    END
  END Copy;

PROCEDURE Diff(c: T): T =
  BEGIN
    WITH
      d = New(c.m)
    DO
      d.t^ := c.t^; d.tPeriod := c.tPeriod;
      d.p^ := c.v^;
      d.r^ := c.r^; d.rPeriod := c.rPeriod;
      RecomputeVelocities(d);
      RecomputeLengths(d);
      RETURN d
    END
  END Diff;

PROCEDURE SetSamples(c: T; READONLY p: PZLR3Chain.T) =
  BEGIN
    <* ASSERT NUMBER(p) = c.m *>
    c.p^ := p;
    RecomputeVelocities(c);
    RecomputeLengths(c);
  END SetSamples;
  
PROCEDURE SetTimes(c: T; READONLY t: PZLRChain.T; tPeriod: LONG) =
  BEGIN
    <* ASSERT NUMBER(t) = c.m *>
    c.t^ := t;
    c.tPeriod := tPeriod;
    RecomputeVelocities(c);
    RecomputeLengths(c);
  END SetTimes;
  
PROCEDURE SetLabels(c: T; READONLY r: PZLRChain.T; rPeriod: LONG) =
  BEGIN
    <* ASSERT NUMBER(r) = c.m *>
    c.r^ := r;
    c.rPeriod := rPeriod;
  END SetLabels;
  
PROCEDURE RecomputeVelocities(c: T) =
  VAR tm, to, tp: LONG;
      im, io, ip: CARDINAL;
  BEGIN
    WITH
      m = c.m,
      p = c.p^,
      v = c.v^,
      t = c.t^,
      T = c.tPeriod
    DO
      <* ASSERT T > 0.0d0 *>
      im := m-1; tm := t[m-1] - T;
      io := 0; to := t[0];
      FOR i := 0 TO m-2 DO 
        ip := i+1; tp := t[i+1];
        v[i] := PZGeo.EstimateVelocityC(tm, p[im], to, p[io], tp, p[ip]);
        <* ASSERT v[i][0] + 1.0d0 > v[i][0] *>
        im := io; tm := to;
        io := ip; to := tp;
      END;
      ip := 0; tp := t[0] + T;
      v[m-1] := PZGeo.EstimateVelocityC(tm, p[im], to, p[io], tp, p[ip]);
      <* ASSERT v[m-1][0] + 1.0d0 > v[m-1][0] *>
    END
  END RecomputeVelocities;
  
PROCEDURE RecomputeLengths(c: T) =
  VAR ta, tb: LONG;
      ia, ib: CARDINAL;
      len: LONG := 0.0d0;
  BEGIN
    WITH
      m = c.m,
      p = c.p^,
      v = c.v^,
      t = c.t^,
      s = c.s^,
      T = c.tPeriod
    DO
      ia := m-1; ta := t[m-1] - T;
      FOR i := 0 TO m-1 DO 
        ib := i; tb := t[i];
        WITH
          hs = PZGeo.HermiteCurveLength(ta, p[ia], v[ia], tb, p[ib], v[ib]),
          es = LR3.Dist(p[ia], p[ib])
        DO
          s[i] := 0.5d0 * (es + MIN(1.6d0*es, MAX(es, hs)))
        END;
        len := len + s[i];
        ia := ib; ta := tb;
      END;
      c.length := len
    END
  END RecomputeLengths;
  
PROCEDURE Filter(c: T; d: T; sigma: LONG) =
  BEGIN 
    WITH
      ct = c.t^,
      m = c.m,
      T = d.tPeriod,
      arg = NEW(REF PZLRChain.T, m)^
    DO
      <* ASSERT T > 0.0d0 *>
      (* Temporary hack: filters the *linear* interpolation of the points "p[i]". *)
      FOR i := 0 TO m-1 DO
        ct[i] := T * FLOAT(i, LONG)/FLOAT(m, LONG)
      END;
      c.tPeriod := d.tPeriod;
      PZLRChain.ArgsFromValues(d.t^, d.tPeriod, ct, arg);
      PZLRChain.ValuesFromArgs(d.r^, d.rPeriod, arg, c.r^);
      c.rPeriod := d.rPeriod;
      PZSmooth.Polygonal(sigma, d.p^, d.t^, d.tPeriod, 0.0d0, c.p^, c.v^, c.a^);
      RecomputeVelocities(c);
      RecomputeLengths(c);
    END;
  END Filter;

PROCEDURE Sample(
    c: T; 
    READONLY t: PZLRChain.T; 
    VAR p, v: PZLR3Chain.T;
    VAR r: PZLRChain.T;
  ) =
  BEGIN
    PZLR3Chain.HermiteTimeSample(c.t^, c.tPeriod, c.p^, c.v^, t, p, v);
    SampleLabels(c.t^, c.tPeriod, c.r^, c.rPeriod, t, r)
  END Sample;
  
PROCEDURE SampleLabels(
    READONLY ct: PZLRChain.T;   (* Times of input samples. *) 
    READONLY T: LONG;           (* Time period *)
    READONLY cr: PZLRChain.T;   (* Label values at times "t[i]". *)
    READONLY R: LONG;           (* Label period *)
    READONLY t: PZLRChain.T;    (* Times of output samples. *)
    VAR r: PZLRChain.T;         (* OUT: labels at times "t[j]". *) 
  ) =
  VAR k: CARDINAL;
      ta, tb: LONG;
      ra, rb: LONG;
  BEGIN 
    WITH  
      m = NUMBER(ct),
      n = NUMBER(t)
    DO
      <* ASSERT NUMBER(cr) = m *>
      <* ASSERT NUMBER(r) >= n *>
      k:=0; 
      ta := ct[0]; tb := ct[1]; 
      ra := cr[0]; rb := cr[1];
      FOR j:= 0 TO n-1 DO 
        (* Invariant: "ta = ct[k]", "tb = ct[k+1]", with periodic wrap-around *)
        (* Invariant: "ra = cr[k]", "rb = cr[k+1]", with periodic wrap-around *)
        WITH tau = ReduceToPeriod(t[j], ct[0], ct[0] + T) DO
          WHILE tau < ta DO 
            k := k - 1; 
            WITH km = k MOD m, kq = FLOAT(k DIV m, LONG) DO
              tb := ta; ta := ct[km] + T * kq;
              rb := ra; ra := cr[km] + R * kq;
            END;
          END;
          WHILE tau > tb DO 
            k := k + 1; 
            WITH km = (k+1) MOD m, kq = FLOAT((k+1) DIV m, LONG) DO
              ta := tb; tb := ct[km] + T * kq;
              ra := rb; rb := cr[km] + R * kq;
            END;
          END;
          r[j] := PZProc.Interpolate(tau, ta, ra, tb, rb);
        END;
      END;
    END
  END SampleLabels;
   
PROCEDURE ReduceToPeriod(t, tLo, tHi: LONG): LONG =
  (*
    Reduces "t" to the range "[tLo _ tHi)" modulo "T = tHi - tLo". *)
  BEGIN
    <* ASSERT tLo < tHi *>
    IF t < tLo OR t >= tHi THEN 
      WITH T = tHi - tLo DO
        WITH k = FLOOR((t - tLo)/T) DO t := t - FLOAT(k,LONG)*T END;
        WHILE t < tLo DO t := t + T END;
        WHILE t >= tHi DO t := t - T END;
      END
    END;
    RETURN t
  END ReduceToPeriod;

BEGIN
END PZCurve.