(* Last edited on 1999-12-24 04:29:00 by hcgl *)  

      PROCEDURE Chop(i, j: NAT): BOOL =
        (*
          Tries to extract one or more segments from the sub-candidate
          defined by the pairs "mt[i..j]". The segments will be numbered
          starting from "nChops", which is incremented accordingly.
          Returns TRUE if at least one segment was generated.
        *)
        BEGIN
          WITH
            k = Midpoint(mt, i, j)
          DO
            IF mt[k][0] - mt[i][0] < hSteps 
            OR mt[k][1] - mt[i][1] < hSteps 
            OR mt[j][0] - mt[k][0] < hSteps 
            OR mt[j][1] - mt[k][1] < hSteps THEN
              RETURN FALSE
            ELSIF Chop(i,k) OR Chop(k,j) THEN
              RETURN TRUE
            ELSE
              WITH 
                nTag = Fmt.Pad(Fmt.Int(nChops), 2, '0'),
                name = candName & "-" & nTag,
                ctr = FindAlignment(a, b, mt[i], mt[k], mt[j], nSteps, maxAdjust),
                sa = ExtractSegmentShape(a, ctr[0], nSteps, name, "a", candComment)^,
                sb = ExtractSegmentShape(b, ctr[1], nSteps, name, "b", candComment)^,
                sm = ComputeMeanSignal(sa, sb)^,
                sd = ComputeDiffSignal(sa, sb)^
              DO 
                ProcessSignal(sd, name, "d", candComment);
                ProcessSignal(sm, name, "m", candComment);
              END;
              INC(nChops);
              INC(nOutCands);
              RETURN TRUE
            END
          END
        END Chop;
PROCEDURE Midpoint(
    READONLY mt: PZMatch.T;
    i, j: NAT;
  ): NAT =
  (*
    Finds an index "k" such that "s[k]  (s[i]+s[j])/2"
    where "s[r] = (mt[r][0] + mt[r][1]).
  *)
  VAR kMin, kMax: NAT;
  BEGIN
    WITH
      sm = ((mt[i][0] + mt[i][1]) + (mt[j][0] + mt[j][1])) DIV 2
    DO
      kMin := i; kMax := j;
      WHILE kMin < kMax DO 
        WITH
          sMin = mt[kMin][0] + mt[kMin][1],
          sMax = mt[kMax][0] + mt[kMax][1],
          r = FLOAT(sm - sMin, LONG)/FLOAT(sMax - sMin, LONG),
          k = MIN(kMin + FLOOR(r*FLOAT(kMax - kMin, LONG)), kMax-1),
          sk0 = mt[k][0] + mt[k][1],
          sk1 = mt[k+1][0] + mt[k+1][1]
        DO
          <* ASSERT sMin <= sm *>
          <* ASSERT sMax >= sm *>
          IF sm < sk0 THEN
            kMax := k
          ELSIF sm > sk1 THEN
            kMin := k+1
          ELSIF sm - sk0 < sk1 - sm THEN
            RETURN k
          ELSE
            RETURN k+1
          END
        END
      END;
      <* ASSERT kMin = kMax *>
      RETURN kMin
    END 
  END Midpoint;


PROCEDURE ComputeMatrix(
    READONLY a, b: PZLR3Chain.T;   (* Trimmed segments for the two candidates *)
    READONLY o: Options;
    step: LONG;
    lambda: LONG;
    plotName: TEXT;
    (*OUT*)
    VAR m: LR4x4.T;
    VAR mt: REF PZMatch.T;
    VAR mismatch: LONG;
    VAR length: LONG;
    VAR matchedLength: LONG;
    (*WORK*)
    VAR minAvgDist: REF LONGS;
    rCost: REF PZMatch.CostMatrix;
  ) =

  VAR f, f1: Value;
      nEvals: NAT;
      aMap: REF PZLR3Chain.T;
  BEGIN
    WITH
      ma = NUMBER(a),
      mb = NUMBER(b),
      Ra = Dist(a[0], a[ma-1])/2.0d0,
      Rb = Dist(b[0], b[mb-1])/2.0d0,
      scale = sqrt(2.0d0/3.0d0) * (Ra + Rb)/2.0d0,
      minLength = o.minLength - o.blurFactor * 2.0d0 * lambda,
      minChainEdges = MAX(0, FLOOR((2.0d0 * minLength)/step))
    DO

      PROCEDURE GoalFunction(READONLY m: LR4x4.T): LONG =
        (*
          Computes the image "aMap" of chain "a" by "m",
          the best matching "mt" of "aMap" with "b",
          and returns the mismatch.
        *)
        BEGIN
          aMap := PZLR3Chain.Map(a, m);
          ComputeMatch(aMap^, b, o,
            minChainEdges := minChainEdges,
            step := step,
            mt := mt,
            mismatch := mismatch,
            length := length,
            matchedLength := matchedLength,
            minAvgDist := minAvgDist,
            rCost := rCost
          );
          RETURN mismatch
        END GoalFunction;

      BEGIN
        (* Compute an initial guess for the placement matrix: *)
        m := ComputeInitialMatrix(a, b, o);

        (* Compute initial mapped chain, matching, and mismatch: *)
        f := GoalFunction(m);
        <* ASSERT f = mismatch *>

        (* Optimize matrix *)
        PZOptMatrix.Optimize(
          func := GoalFunction,
          scaleX := scale, scaleY := scale,
          maxEvals := o.maxEvals,
          nEvals := nEvals,
          m := m,
          f := f,
          plotName := plotName
        );

        (* Compute final mapped chain, matching, and mismatch: *)
        f1 := GoalFunction(m);
        <* ASSERT f1 = f *>
        <* ASSERT f = mismatch *>
      END
    END
  END ComputeMatrix;

PROCEDURE ComputeInitialMatrix(
    READONLY a, b: PZLR3Chain.T;
    READONLY o: Options;
  ): LR4x4.T =
  VAR m: LR4x4.T;
  BEGIN
    WITH na = NUMBER(a), nb = NUMBER(b) DO
      IF o.dontGuess THEN
        m := Identity();
      ELSE
        m := ComputeTransformationMatrix(a[0], a[na-1], b[0], b[nb-1]);
      END;
      (* Perturbe initial matrix *)
      IF o.perturbMatrix THEN
        m := PerturbMatrix (m, dTheta := 0.1d0, dX := 1.0d0, dY := 1.0d0)
      END;
    END;
    RETURN m
  END ComputeInitialMatrix;

PROCEDURE ComputeTransformationMatrix(READONLY a1, b1, a2, b2 : LR3.T): LR4x4.T =
  BEGIN
    WITH
      c1 = PZGeo.SegMid(a1, b1),
      c2 = PZGeo.SegMid(a2, b2),
      u  = PZGeo.SegDir(a1, b1),
      v  = PZGeo.SegDir(a2, b2),
      T1 = PZGeo.Translation(LR3.Neg(c1)),
      T2 = PZGeo.Translation(c2),
      R =  PZGeo.Rotation(u, v)
    DO
      RETURN Mul(Mul(T1, R), T2);
    END;
  END ComputeTransformationMatrix;

PROCEDURE PerturbMatrix(VAR m: LR4x4.T; dTheta, dX, dY: LONG): LR4x4.T =
  VAR n: LR4x4.T := LR4x4.Identity();
  BEGIN
    WITH
      cs = cos(dTheta),
      sn = sin(dTheta)
    DO
      <* ASSERT cs # 0.0d0 OR sn # 0.0d0 *>
      n[1,1] := cs;
      n[1,2] := sn;
      n[2,1] := -sn;
      n[2,2] := cs;
      n[0,1] := dX;
      n[0,2] := dY;
    END;
    RETURN LR4x4.Mul(m, n)
  END PerturbMatrix;

PROCEDURE ComputeMatch(
    READONLY a, b: PZLR3Chain.T;
    READONLY o: Options;
    minChainEdges: NAT;
    step: LONG;
    (*OUT*)
    VAR mt: REF PZMatch.T;
    VAR mismatch: LONG;
    VAR length: LONG;
    VAR matchedLength: LONG;
    (*WORK*)
    VAR minAvgDist: REF LONGS;
    VAR rCost: REF PZMatch.CostMatrix;
  ) =
  BEGIN
    WITH
      NA = NUMBER(a),
      NB = NUMBER(b),
      ctr = PickCenter(a, b),
      mtOld = PZMatch.T{ctr}
    DO
      IF minChainEdges <= (NA-1)+(NB-1) THEN
        IF minAvgDist = NIL OR NUMBER(minAvgDist^) < NA + NB - 1 THEN
          minAvgDist := NEW(REF LONGS, NA + NB - 1)
        END;
        PZLR3Chain.RefineMatch(
          a, b,
          step := step,
          mtOld := mtOld,
          maxAdjust := CEILING(o.maxShift / step),
          critDistSqr := o.mp.critDistSqr,
          maxDistSqr := o.mp.maxDistSqr,
          (*OUT*)
          mt := mt,
          mismatch := mismatch,
          length := length,
          matchedLength := matchedLength,
          (*WORK*)
          minAvgDist := SUBARRAY(minAvgDist^, 0, NA+NB-1),
          rCost := rCost
        );
        (* If the matching has zero length, provide a useful "mismatch": *)
        IF mt = NIL OR NUMBER(mt^) < 2 OR length <= 0.0d0 THEN 
          <* ASSERT mismatch = 0.0d0 *>
          WITH
            ac = (NA-1) DIV 2,
            bc = IndexNearestPoint(b, a[ac])
          DO
            mismatch := LR3.DistSqr(a[ac], b[bc]);
          END
        END
      ELSE
        mismatch := LAST(LONG);
        length := 0.0d0;
        matchedLength := 0.0d0;
        mt := NEW(REF PZMatch.T, 1);
        mt[0] := ctr
      END
    END;
  END ComputeMatch;

PROCEDURE PickCenter(READONLY a, b: PZLR3Chain.T): PZMatch.Pair =
  BEGIN
    WITH
      ai = IndexNearestPoint(a, b[0]),
      af = IndexNearestPoint(a, b[LAST(b)]),
      bi = IndexNearestPoint(b, a[0]),
      bf = IndexNearestPoint(b, a[LAST(a)])
    DO
      RETURN PZMatch.Pair{(ai + af) DIV 2, (bi + bf) DIV 2}
    END
  END PickCenter;

PROCEDURE IndexNearestPoint(READONLY b: PZLR3Chain.T; p: LR3.T ): NAT =
  VAR index : NAT;
      mindist :  LONG;
  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;

(***************************************************)


              o := o,
              step := step,
              lambda := lambda,
              plotName := "",
              mt := mt,
              mismatch := mismatch,
              length := candLength,
              matchedLength := matchedLength,
              (*WORK*)
              minAvgDist := minAvgDist,
              rCost := rCost
            Wr.PutText(stderr, "# mismatch = " &  Fmt.LongReal(mismatch) & "\n");
            mt := PZMatch.RemoveDanglingEnds(mt^);
            cam := PZLR3Chain.Map(ca, m);
        
        
      
      
      ComputeMatrix(
        ca, cb, o,
        step := step,
        lambda := lambda,
        plotName := "",
        (*OUT*)
        m := m,
        mt := mt,
        mismatch := mismatch,
        length := candLength,
        matchedLength := matchedLength,
        (*WORK*)
        minAvgDist := minAvgDist,
        rCost := rCost
      );
      Wr.PutText(stderr, "# mismatch = " &  Fmt.LongReal(mismatch) & "\n");
      mt := PZMatch.RemoveDanglingEnds(mt^);
      cam := PZLR3Chain.Map(ca, m);