TYPE
  LONG = LONGREAL;

IMPORT LR3;
FROM Math IMPORT sqrt;

PROCEDURE MeanStep(READONLY c: PZLR3Chain.T): LONG =
  VAR s: LONG := 0.0d0;
  BEGIN
    FOR i := FIRST(c)+1 TO LAST(c) DO
      s := s + LR3.DistSqr(c[i], c[i-1])
    END;
    RETURN sqrt(s/FLOAT(NUMBER(c)-1, LONG))
  END MeanStep;
  


PROCEDURE ResampleChain(
    READONLY t: ARRAY OF LONG;
    tPeriod: LONG;
    READONLY p: PZLR3Chain.T;
    READONLY dp: PZLR3Chain.T;
    n: NAT;
    step: LONG
  ): REF PZLR3Chain.T =
  BEGIN
    <* ASSERT NUMBER(t) = NUMBER(p) *>
    WITH
      tStart = (tPeriod - FLOAT(n, LONG)*step)/2.0d0,
      (* Output samples: *)
      tq = NEW(REF ARRAY OF LONG, n)^,         (* Times of output samples. *)
      rq = NEW(REF PZLR3Chain.T, n), q  = rq^, (* Output samples. *)
      dq = NEW(REF PZLR3Chain.T, n)^           (* Velocities at output samples. *)
    DO
      <* ASSERT tStart >= step/2.0d0 *>
      (* Interpolate input samples: *)
      FOR i := 0 TO n-1 DO tq[i] := tStart + (FLOAT(i, LONG) + 0.5d0)*step END;
      HermiteTimeSample(t, tPeriod, p, dp, tq, q, dq);
      (* LinearTimeSample(t, tPeriod, p, tq, q); *)
      RETURN rq
    END
  END ResampleChain;

PROCEDURE PowerOfTwoNotGreaterThan(n: CARDINAL): CARDINAL =
  (* Largest power of two not greater than "n" *)
  VAR m: CARDINAL := 1;
  BEGIN
    <* ASSERT n > 0 *>
    WHILE m+m <= n DO m := m+m END;
    RETURN m
  END PowerOfTwoNotGreaterThan;

PROCEDURE GetSampleVelocities(
    READONLY t: ARRAY OF LONG;  (* Times of original samples. *)
    tPeriod: LONG;                    (* Time period of original curve. *)
    READONLY p: PZLR3Chain.T;   (* Original samples. *)
  ): REF PZLR3Chain.T =
  (*
    Estimates the veolcities at given sample points "p[i]" of
    a periodic curve, given also the curve's period "tPeriod"
    and the sample times "t[i]". *)
  BEGIN
    <* ASSERT NUMBER(t) = NUMBER(p) *>
    WITH
      n = NUMBER(p),
      rdp = NEW(REF PZLR3Chain.T, n), dp = rdp^
    DO
      EstimateVelocities(t, tPeriod, p, dp, PZGeo.EstimateVelocityC);
      RETURN rdp
    END
  END GetSampleVelocities;

PROCEDURE GetSampleEndPoints(
    READONLY t: ARRAY OF LONG;  (* Times of original samples. *)
    tPeriod: LONG;              (* Time period of original curve. *)
    READONLY p: PZLR3Chain.T;   (* Original samples. *)
    READONLY dp: PZLR3Chain.T;  (* Estimated velocities at original samples. *)
    n: NAT;                     (* Number of new samples. *)
    step: LONG;                 (* Sampling step. *)
  ): ARRAY [0..1] OF LR3.T =
  (*
    Returns two points on a curve that bracket the points returned
    by "ResampleChain", half a step away from the first and last samples. *)

  VAR u: ARRAY [0..1] OF LR3.T;
      tu: ARRAY [0..1] OF LONG;
      du: ARRAY [0..1] OF LR3.T;
  BEGIN
    <* ASSERT NUMBER(t) = NUMBER(p) *>
    WITH
      tStart = (tPeriod - FLOAT(n, LONG)*step)/2.0d0,
      tStop  = tPeriod - tStart
    DO
      <* ASSERT tStart >= step/2.0d0 *>
      tu[0] := tStart; tu[1] := tStop;
      HermiteTimeSample(t, tPeriod, p, dp, tu, u, du);
    END;
    RETURN u
  END GetSampleEndPoints;

PROCEDURE RemoveLinearTrend(
    VAR q: PZLR3Chain.T;
    READONLY u0, u1: LR3.T;
  ) =
  (*
    Subtracts from each sample "q[i]" a corresponding point "u[i]"
    on the line from "u0" to "u1".  The points "u[i]"
    are equally spaced, with half a step at each end. *)
  BEGIN
    WITH n = NUMBER(q), fn = FLOAT(n, LONG) DO
      FOR i := 0 TO n-1 DO
        WITH
          s = (FLOAT(i,LONG) + 0.5d0)/fn,
          ui = PZGeo.LinearInterpolate(s, 0.0d0, u0, 1.0d0, u1)
        DO
          q[i] := LR3.Sub(q[i], ui)
        END
      END
    END
  END RemoveLinearTrend;

PROCEDURE WriteOriginalChains(
    READONLY a, b, m, d: PZLR3Chain.T;
    prefix: TEXT;
    iCand: NAT
  ) =
  BEGIN
    WITH
      file = prefix & "-" & FI(iCand, 6)
    DO
      (* Write original chains, mapped for best match: *)
      WITH
        wr = FileWr.Open(name & "-a.flc"),
        comment = "Original A chain (mapped) for candidate " & Fmt.Int(iCand)
      DO
        PZLR3Chain.Write(wr, comment, a, Unit);
      END;
      WITH
        wr = FileWr.Open(name & "-b.flc"),
        comment = "Original B chain for candidate " & Fmt.Int(iCand)
      DO
        PZLR3Chain.Write(wr, comment, b, Unit);
      END;
      WITH
        wr = FileWr.Open(name & "-m.flc"),
        comment = "Mean chain for candidate " & Fmt.Int(iCand)
      DO
        PZLR3Chain.Write(wr, comment, m, Unit);
      END;
      WITH
        wr = FileWr.Open(name & "-d.flc"),
        comment = "Error chain for candidate " & Fmt.Int(iCand)
      DO
        PZLR3Chain.Write(wr, comment, d, Unit);
      END;
    END
  END WriteOriginalChains;

PROCEDURE WriteReparametrizedChains(
    READONLY a, b, m, d: PZLR3Chain.T;
    prefix: TEXT;
    iCand: NAT
  ) =
  CONST
    MinLambda = 1.0d0;      (* Smallest interesting wavelength. *)
    Step = MinLambda/2.0d0; (* Sampling step to use. *)
    MinSamples = 64;        (* Minimum samples to consider match. *)
  VAR
    Lm: LONG;      (* Length of mean chain. *)
    Lt: LONG;
  BEGIN
    WITH
      nm = NUMBER(match),
      na = NUMBER(a),
      nb = NUMBER(b),
      tm = NEW(REF ARRAY OF LONG, nm)^,  (* Times for mean chain samples. *)
      ta = NEW(REF ARRAY OF LONG, na)^,  (* Times for "a" samples. *)
      tb = NEW(REF ARRAY OF LONG, nb)^,  (* Times for "b" samples. *)
      wa = NEW(REF ARRAY OF NAT, na)^,   (* Num pairs for "a" samples. *)
      wb = NEW(REF ARRAY OF NAT, nb)^    (* Num pairs for "b" samples. *)
    DO
      (* Compute the time of each sample, as

      FOR ia := 0 TO na-1 DO ta[ia] := 0.0d0; wa[ia] := 0 END;
      FOR ib := 0 TO nb-1 DO tb[ib] := 0.0d0; wb[ib] := 0; END;

      FOR im := 0 TO nm-1 DO
        WITH
          ia = match[im,0],
          ib = match[im,1]
        DO
          IF im = 0 THEN
            Lm := 0.0d0
          ELSE
            WITH
              d0 = (m[im,0] - m[im-1,0]),
              d1 = (m[im,1] - m[im-1,1]),
              dL = Math.sqrt(d0*d0 + d1*d1)
            DO
              Lm := Lm + dL;
            END
          END;
          tm[im] := Lm;
          ta[ia] := ta[ia] + tm[im]; INC(wa[ia]);
          tb[ib] := tb[ib] + tm[im]; INC(wb[ib]);
        END;
      END;
      Lm := Lm + Lm/FLOAT(nm-1, LONG);

      Lt := 0.0d0;
      FOR ia := 0 TO na-1 DO
        IF wa[ia] > 0 THEN Lt := ta[ia]/FLOAT(wa[ia], LONG) END;
        ta[ia] := Lt
      END;
      Lt := 0.0d0;
      FOR ib := 0 TO nb-1 DO
        IF wb[ib] > 0 THEN Lt := tb[ib]/FLOAT(wb[ib], LONG) END;
        tb[ib] := Lt
      END;

    WITH
      candTag = FI(iCand, 6)
    DO
        WITH maxSamples = FLOOR(Lm/Step) DO
          IF maxSamples < MinSamples THEN
            Wr.PutText(stderr, "Matched part of candidate " &
              Fmt.Int(iCand) & " is too short.\n"
            );
          ELSE
            WITH
              n = PowerOfTwoNotGreaterThan(maxSamples),
              dm = GetSampleVelocities(tm, Lm, m)^,
              u = GetSampleEndPoints(tm, Lm, m, dm, n, Step)
            DO
              (* Write resampled chains: *)
              WITH
                da = GetSampleVelocities(ta, Lm, a)^,
                as = ResampleChain(ta, Lm, a, da, n, Step)^,
                wr = FileWr.Open(o.outName & "-" & candTag & "-As.flc"),
                comment = "Resampled A chain (mapped and sheared) for candidate " 
                  & Fmt.Int(iCand)
              DO
                RemoveLinearTrend(as, u[0], u[1]);
                PZLR3Chain.Write(wr, comment, as, Unit);
              END;
              WITH
                db = GetSampleVelocities(tb, Lm, b)^,
                bs = ResampleChain(tb, Lm, b, db, n, Step)^,
                wr = FileWr.Open(o.outName & "-" & candTag & "-Bs.flc"),
                comment = "Resampled B chain (sheared) for candidate " & Fmt.Int(iCand)
              DO
                RemoveLinearTrend(bs, u[0], u[1]);
                PZLR3Chain.Write(wr, comment, bs, Unit);
              END;
              WITH
                ms = ResampleChain(tm, Lm, m, dm, n, Step)^,
                wr = FileWr.Open(o.outName & "-" & candTag & "-ms.flc"),
                comment = "Resampled mean chain for candidate " & Fmt.Int(iCand)
              DO
                RemoveLinearTrend(ms, u[0], u[1]);
                PZLR3Chain.Write(wr, comment, ms, Unit);
              END;
              WITH
                de = GetSampleVelocities(tm, Lm, e)^,
                es = ResampleChain(tm, Lm, e, de, n, Step)^,
                wr = FileWr.Open(o.outName & "-" & candTag & "-es.flc"),
                comment = "Resampled geometric error for candidate " & Fmt.Int(iCand)
              DO
                PZLR3Chain.Write(wr, comment, es, Unit);
              END;
            END
          END
        END
      END
    END;
    Wr.PutText( stderr, ".");
  END WriteReparametrizedChains;

PROCEDURE IndexNearestPoint(READONLY b: PZLR3Chain.T; p: LR3.T ): CARDINAL =
  VAR index : CARDINAL;
      mindist :  LONGREAL;
  BEGIN
     WITH
       m = NUMBER(b),
       mHalf = (m-1) DIV 2
     DO
       index := 0; mindist := Dist(b[0],p);
       FOR i:= mHalf TO 1 BY -1  DO
         WITH d = Dist(b[i],p) DO
           IF d <  mindist THEN index := i; mindist := d END;
         END
       END;
       FOR i:= mHalf+1 TO m-1 DO
         WITH d = Dist(b[i],p) DO
           IF d <  mindist THEN index := i; mindist := d END;
         END
       END;
     END;
     RETURN index
 END IndexNearestPoint;