MODULE TestLength EXPORTS Main;

(* Tests the functions PZGeo.HermiteCurveLength and EstimateVelocity{A,B,C} *)
(* Last edited on 1999-08-15 09:04:36 by hcgl *)

IMPORT Math, PSPlot, Wr, Fmt, LR3, Thread, Random;
IMPORT PZTypes, PZGeo;
FROM Stdio IMPORT stderr;
FROM PZTypes IMPORT LONG, LONGS, NAT, BOOL;

TYPE 
  Curve = PROCEDURE(t: LONG): LR3.T;
  Times = LONGS;
  Points = ARRAY OF LR3.T;
  Velocities = ARRAY OF LR3.T;
  VelEstimator = PROCEDURE (
      a: LONG; READONLY pa: LR3.T; 
      b: LONG; READONLY pb: LR3.T; 
      c: LONG; READONLY pc: LR3.T;
    ): LR3.T;

CONST TwoPi = 2.0d0*3.141592653589793d0;

<* FATAL Wr.Failure, Thread.Alerted *>

PROCEDURE Main() =
  BEGIN
    WITH
      vel = ARRAY [0..2] OF VelEstimator{
        PZGeo.EstimateVelocityQ,
        PZGeo.EstimateVelocityL,
        PZGeo.EstimateVelocityC
      },
      velName = ARRAY [0..2] OF TEXT{"Q", "L", "C"}
    DO
      FOR k := 0 TO 2 DO
        FOR tJitter := FALSE TO TRUE DO
          FOR pJitter := FALSE TO TRUE DO
            DoTests(
              Circle, "Circle", TRUE,
              vel[k], velName[k],
              uniform := NOT tJitter,
              exact := TwoPi,
              perturb := pJitter
            );
            DoTests(
              Sinusoid, "Sinusoid", FALSE,
              vel[k], velName[k],
              uniform := NOT tJitter,
              exact := 8.96024877070d0,  (* Maple sez so *)
              perturb := pJitter
            );
          END
        END
      END
    END
  END Main;
  
PROCEDURE DoTests(
    curve: Curve; curveName: TEXT;
    closed: BOOL;
    vel: VelEstimator; velName: TEXT;
    uniform: BOOL;
    exact: LONG;
    perturb: BOOL;
  ) =
  VAR n: NAT;
  BEGIN
    WITH
      uniformName = ARRAY BOOL OF TEXT{"jittered", "uniform"}[uniform],
      perturbName = ARRAY BOOL OF TEXT{"exact", "perturbed"}[perturb],
      closedName = ARRAY BOOL OF TEXT{"open", "closed"}[closed],
      testName = curveName & "-" & velName & "-" & uniformName & "-" & perturbName,
      noise = ARRAY BOOL OF LONG{0.00d0, 0.01d0}[perturb]
    DO
      Wr.PutText(stderr, "curve = " & curveName & "\n");
      Wr.PutText(stderr, "closed = " & closedName & "\n");
      Wr.PutText(stderr, uniformName & "\n");
      Wr.PutText(stderr, perturbName & "\n");
      Wr.PutText(stderr, "velocity estimator = " & velName & "\n");
      Wr.PutText(stderr, 
        "exact length = " & 
        Fmt.Pad(Fmt.LongReal(exact, Fmt.Style.Fix, 6), 12) & "\n"
      );
      Wr.PutText(stderr, 
        "position noise = " & 
        Fmt.Pad(Fmt.LongReal(noise, Fmt.Style.Fix, 6), 12) & "\n"
      );
      Wr.PutText(stderr, "\n");
      n := 128;
      WHILE n > 0 DO
        WITH
          t = PickTimes(n, uniform)^,
          p = PickPoints(curve, closed, t, noise)^,
          v = EstVelocities(t, p, vel, closed)^
        DO
          PrintTest(t, p, v);
          IF n = 32 THEN 
            WITH runName = testName & "-" & Fmt.Pad(Fmt.Int(n), 3, '0') DO
              PlotTest(runName, t, p, v, 50)
            END
          END;
        END;
        n := n DIV 2
      END;
    END
  END DoTests;

PROCEDURE PickTimes(n: NAT; uniform: BOOL): REF Times =
  (*
    Returns "n+1" increasing times "t[0..n]",
    with "t[1] = 0" and "t[n] = 1". *)
  BEGIN
    WITH
      rt = NEW(REF Times, n+1), t = rt^,
      step = 1.0d0/FLOAT(n, LONG),
      rnd = NEW(Random.Default).init(fixed := TRUE)
    DO
      t[0] := 0.0d0;
      FOR i := 1 TO n-1 DO 
        t[i] := step*FLOAT(i, LONG);
        IF NOT uniform THEN 
          t[i] := t[i] + step * rnd.longreal(-0.499d0, +0.499d0)
        END
      END;
      t[n] := 1.0d0;
      RETURN rt
    END
  END PickTimes;

PROCEDURE PickPoints(
    curve: Curve;
    closed: BOOL;
    READONLY t: Times; 
    noise: LONG;
  ): REF Points =
  (*
    Samples the curve "curve(t)" at times "t[0..LAST]",
    perturbs the samples with the given amount of noise.
    If "closed=TRUE" ensures that "p[n] = p[0]". *)
  BEGIN
    WITH
      n = NUMBER(t) - 1,
      rp = NEW(REF Points, n+1), p = rp^,
      rnd = NEW(Random.Default).init(fixed := TRUE)
    DO
      FOR i := 0 TO n-1 DO 
        p[i] := LR3.Add(curve(t[i]), Jitter(rnd, noise))
      END;
      IF closed THEN 
        p[n] := p[0]
      ELSE
        p[n] := LR3.Add(curve(t[n]), Jitter(rnd, noise))
      END;
      RETURN rp
    END;
  END PickPoints;
  
PROCEDURE Jitter(rnd: Random.T; noise: LONG): LR3.T =
  VAR v: LR3.T;
  BEGIN
    REPEAT
      v[0] := rnd.longreal(-1.0d0, +1.0d0);
      v[1] := rnd.longreal(-1.0d0, +1.0d0);
      v[2] := rnd.longreal(-1.0d0, +1.0d0);
    UNTIL LR3.NormSqr(v) <= 1.0d0;
    v[0] := v[0] * noise;
    v[1] := v[1] * noise;
    v[2] := v[2] * noise;
    RETURN v
  END Jitter;

PROCEDURE EstVelocities(
    READONLY t: Times; 
    READONLY p: Points; 
    est: VelEstimator;
    closed: BOOL;
  ): REF Velocities =
  (*
    Estimates the velocities of a curve at times "t[0..LAST]",
    given the corresponding positions "p[0..LAST]".
    If "closed=TRUE" assumes "p[0] = p[LAST]" and 
    continuity there; otherwise uses free-end estimators
    at "p[0]" and "p[n]". *)
  BEGIN
    <* ASSERT NUMBER(p) = NUMBER(t) *>
    WITH
      n = NUMBER(t) - 1,
      rv = NEW(REF Velocities, n+1), v = rv^
    DO
      IF closed THEN
        <* ASSERT LR3.DistSqr(p[n], p[0]) = 0.0d0 *>
        FOR ib := 0 TO n-1 DO
          WITH 
            ia = (ib-1) MOD n, da = FLOAT((ib-1) DIV n, LONG),
            ic = (ib+1) MOD n, dc = FLOAT((ib+1) DIV n, LONG)
          DO
            v[ib] := est(t[ia]+da, p[ia], t[ib], p[ib], t[ic]+dc, p[ic])
          END;
        END;
        v[n] := v[0]
      ELSE
        FOR ib := 0 TO n DO
          WITH ia = MAX(0,ib-1), ic = MIN(n,ib+1) DO
            v[ib] := est(t[ia], p[ia], t[ib], p[ib], t[ic], p[ic])
          END
        END
      END;
      RETURN rv
    END;
  END EstVelocities;

PROCEDURE PrintTest(
    READONLY t: Times;      (* Sampling times *)
    READONLY p: Points;     (* Corresponding sample points *)
    READONLY v: Velocities; (* Corresponding estimated velocities *)
  ) =
  <* FATAL Wr.Failure, Thread.Alerted *>
  BEGIN
    <* ASSERT NUMBER(p) = NUMBER(t) *>
    <* ASSERT NUMBER(v) = NUMBER(t) *>
    WITH
      n = NUMBER(t) - 1,
      cs = CubicLength(t, p, v),
      ps = PolyLength(p),
      ms = 0.5d0 * (ps + cs)
    DO
      Wr.PutText(stderr, "n = " & Fmt.Pad(Fmt.Int(n), 5));
      Wr.PutText(stderr, "  cubic length = " & 
        Fmt.Pad(Fmt.LongReal(cs, Fmt.Style.Fix, 6), 12)
      );
      Wr.PutText(stderr, "  poly length = " & 
        Fmt.Pad(Fmt.LongReal(ps, Fmt.Style.Fix, 6), 12)
      );
      Wr.PutText(stderr, "  average = " & 
        Fmt.Pad(Fmt.LongReal(ms, Fmt.Style.Fix, 6), 12)
      );
      Wr.PutText(stderr, "\n")
    END
  END PrintTest;
  
PROCEDURE PlotTest(
    testName: TEXT;
    READONLY t: Times;      (* Sampling times *)
    READONLY p: Points;     (* Corresponding sample points *)
    READONLY v: Velocities; (* Corresponding estimated velocities *)
    m: NAT;
  ) =
  BEGIN
    <* ASSERT NUMBER(p) = NUMBER(t) *>
    <* ASSERT NUMBER(v) = NUMBER(t) *>
    WITH
      q = CubicSample(t, p, v, m)^,
      f = NEW(PSPlot.EPSFile).open(testName & ".eps")
    DO
      f.setScale(PSPlot.Axis.X, -2.0d0, +2.0d0);
      f.setScale(PSPlot.Axis.Y, -2.0d0, +2.0d0);
      FOR i := 0 TO LAST(q)-1 DO 
        WITH a = q[i], b = q[i+1] DO 
          f.segment(a[0], a[1], b[0], b[1])
        END;
      END;
      f.close()
    END
  END PlotTest;
  
PROCEDURE CubicLength(
    READONLY t: Times;      (* Sampling times *)
    READONLY p: Points;     (* Corresponding sample points *)
    READONLY v: Velocities; (* Corresponding estimated velocities *)
  ): LONG =
  (*
    Computes the length of curve "curve(t)" between "t[0]" and "t[LAST]",
    using PZGeo.HermiteCurveLength. *)
  (* <* FATAL Wr.Failure, Thread.Alerted *> *)
  VAR s: LONG := 0.0d0;
  BEGIN
    <* ASSERT NUMBER(p) = NUMBER(t) *>
    <* ASSERT NUMBER(v) = NUMBER(t) *>
    WITH
      n = NUMBER(t) - 1
    DO
      FOR ia := 0 TO n-1 DO 
        WITH 
          ib = ia+1,
          ds = PZGeo.HermiteCurveLength(t[ia], p[ia], v[ia], t[ib], p[ib], v[ib])
        DO
          s := s + ds
        END
      END
    END;
    RETURN s
  END CubicLength;

PROCEDURE PolyLength(
    READONLY p: Points;     (* Corresponding sample points *)
  ): LONG =
  (*
    The length of the polygonal "p[0..LAST]". *)
  VAR s: LONG := 0.0d0;
  BEGIN
    WITH
      n = NUMBER(p) - 1
    DO
      FOR i := 1 TO n DO 
        WITH Pa = p[i-1], Pb = p[i], ds = LR3.Dist(Pa, Pb) DO
          s := s + ds
        END;
      END;
    END;
    RETURN s
  END PolyLength;

PROCEDURE CubicSample(
    READONLY t: Times;      (* Sampling times *)
    READONLY p: Points;     (* Corresponding sample points *)
    READONLY v: Velocities; (* Corresponding estimated velocities *)
    m: NAT; 
  ): REF Points =
  (*
    Samples the Hermite spline curve "(t,p,v)[0..LAST]"
    at "m+1" equally spaced points. *)
  VAR ia: NAT;
      vj: LR3.T;
  BEGIN
    <* ASSERT NUMBER(p) = NUMBER(t) *>
    <* ASSERT NUMBER(v) = NUMBER(t) *>
    WITH
      n = NUMBER(t) - 1,
      rq = NEW(REF Points, m+1), q = rq^
    DO    
      ia := 0;
      FOR j := 0 TO m DO 
        WITH 
          tau = FLOAT(j, LONG)/FLOAT(m, LONG)
        DO
          WHILE ia < n AND t[ia+1] < tau DO ia := ia+1 END;
          <* ASSERT tau >= t[ia] AND tau <= t[ia+1] *>
          WITH ib = ia + 1 DO
            PZGeo.HermiteInterpolate(tau, 
              t[ia], p[ia], v[ia], t[ib], p[ib], v[ib], q[j], vj
            )
          END
        END
      END;
      RETURN rq
    END;
  END CubicSample;

PROCEDURE Circle(t: LONG): LR3.T =
  BEGIN
    WITH
      theta = TwoPi*t,
      c = Math.cos(theta),
      s = Math.sin(theta)
    DO
      RETURN LR3.T{c, s, 0.0d0}
    END
  END Circle;

PROCEDURE Sinusoid(t: LONG): LR3.T =
  BEGIN
    WITH
      theta = 1.5d0*TwoPi*t,
      c = Math.cos(theta),
      s = Math.sin(theta)
    DO
      RETURN LR3.T{t, s, c*c}
    END
  END Sinusoid;

BEGIN
  Main()
END TestLength.