-- DynFitImpl.mesa
-- Last edited by Michael Plass August 23, 1982 9:18 am
--Maureen Stone March 19, 1984 5:01:48 pm PST
-- Michael Plass 30-Sep-81

DIRECTORY
Vector,
Complex,
DynFit,
Seq;

DynFitImpl: CEDAR PROGRAM IMPORTS Vector, Complex EXPORTS DynFit =
BEGIN OPEN Seq;
PointNumber: TYPE = DynFit.PointNumber;

-- These global variables hold the samples

NearestZ: PUBLIC PROCEDURE [z: ComplexSequence, v:Complex.Vec] RETURNS [PointNumber] =
 BEGIN
 closest:PointNumber ← 0;
 best:REAL ← 10e+20;
 n:NAT ← z.length;
 IF z#NIL THEN
  FOR i:NAT IN [0..n) DO
   dd:REAL = Complex.SqrAbs[Complex.Sub[v,z[i]]];
   IF dd < best THEN {best ← dd; closest ← i}
   ENDLOOP;
 RETURN[closest];
 END;

LeastSquares: PROCEDURE [numberOfPoints: REAL,
       sum, sumSqr: Complex.Vec,
       sumSqrAbs: REAL] RETURNS [REAL] =
 BEGIN -- finds the sum of the squares of the distances of a set of points to a line, where the line is chosen such that this sum is minimized. The points are specified as complex numbers, and this routine requires their (complex) sum and the sum of their (complex) squares, as well as the sum of the squares of their absolute values. For the best numerical stability, the origin should be chosen as close to the center of mass of the points as is practical.
 OPEN Complex;
 RETURN[(sumSqrAbs - SqrAbs[sum]/numberOfPoints
   - Abs[Sub[sumSqr,Vector.Div[Sqr[sum],numberOfPoints]]])/2]
 END;

BestLine: PUBLIC PROCEDURE [z: ComplexSequence, firstPoint, lastPoint: PointNumber]
   RETURNS [r: DynFit.Line] =
 BEGIN -- finds the line that minimizes the sum of the squares of the distances to the points. The equation of the resulting line is given by centerOfMass + t*direction, where t is a real-valued parameter.
 OPEN Complex;
 n:NAT ← z.length;
 f:NAT = IF firstPoint<=lastPoint THEN firstPoint ELSE lastPoint;
 l:NAT = lastPoint + firstPoint - f;
 m:NAT ← l - f + 1;
 a:Complex.Vec ← [0,0];
 v:Complex.Vec ← [0,0];
 IF m > n THEN m ← n;
 FOR i:NAT IN [f..l] DO
   a ← Add[a,z[i]];
   ENDLOOP;
 a ← Vector.Div[a,m];
 FOR i:NAT IN [f..l] DO
   v ← Add[v,Sqr[Sub[z[i],a]]];
   ENDLOOP;
 v ← Vector.Div[v,m];
 r.centerOfMass ← a;
 r.direction ← SqRt[v];
 END;

tryAll: BOOLEAN ← FALSE;

FitSegments: PUBLIC PROCEDURE [z: ComplexSequence, tolerance: REAL]
    RETURNS [segments: DynFit.Segments, totalBadness: REAL] =
 BEGIN
 n:NAT ← z.length;
 bestPrev:NatSequence = NEW[NatSequenceRec[n]];
 segCount:NatSequence = NEW[NatSequenceRec[n]];
 totSqrD:RealSequence = NEW[RealSequenceRec[n]];
 IF n<2 THEN RETURN [NIL,0];
 bestPrev[0] ← 0;
 segCount[0] ← 0;
 totSqrD[0] ← 0.0;
 FOR m:NAT IN [1..n) DO
  sum,sumSqr: Complex.Vec ← [0,0];
  sumSqrAbs: REAL ← 0;
  b:REAL ← 0;
  lowbad:REAL ← 1.0E+20;
  bestl:NAT;
  s:REAL ← 0;
  sOpt:REAL;
  sBound:REAL ← 0.001;
  FOR l:NAT DECREASING IN [0..m) WHILE tryAll OR s<=sBound DO
   deltaZ:Complex.Vec = Complex.Sub[z[l],z[m]];
   sum ← Complex.Add[sum,deltaZ];
   sumSqr ← Complex.Add[sumSqr,Complex.Sqr[deltaZ]];
   sumSqrAbs ← sumSqrAbs + Complex.SqrAbs[deltaZ];
   s ← LeastSquares[m-l+1,sum,sumSqr,sumSqrAbs];
   b ← s + totSqrD[l] + tolerance * (segCount[l] + 1);
   sBound ← sBound + tolerance;
   IF b<lowbad THEN
    {lowbad𡤋 bestl←l; sOpt←s}
   ENDLOOP;
  totSqrD[m] ← sOpt + totSqrD[bestl];
  segCount[m] ← segCount[bestl] + 1;
  bestPrev[m] ← bestl;
--  IF JaMFnsDefs.GetJaMBreak[] THEN {n←m+1; EXIT};
  ENDLOOP;
 totalBadness ← totSqrD[n-1] + tolerance * (segCount[n-1] + 1);
 -- Now record the answer
  BEGIN
  p:NAT;
  m:NAT ← segCount[n-1] + 1;
  segments ← NEW[DynFit.SegmentSequenceRec[m]];
  p ← n-1;
  FOR i:NAT DECREASING IN [0..m) DO
   segments[i] ← p;
   p ← bestPrev[p]
   ENDLOOP  
  END
 END;

SampleSegments: PUBLIC PROCEDURE [segments: DynFit.Segments, z: ComplexSequence]
 RETURNS [newSamples: ComplexSequence, len: NAT] =
 BEGIN -- Make new samples from the segmentation info.
 ENABLE ABORTED => GOTO Quit;
 epsilon: REAL ← 1.0;
 Sample: PROCEDURE [v: Complex.Vec] =
  BEGIN
  newSamples[len] ← v;
  len ← len + 1;
  END;
 newSamples ← NEW [ComplexSequenceRec[segments.length*2]];
 len ← 0;
 FOR m:NAT DECREASING IN (0..segments.length) DO
  OPEN Complex; -- uses Vec,Add,Sub,Mul,Abs
  first:NAT ← segments[m-1];
  last:NAT ← segments[m];
  line:DynFit.Line ← BestLine[z,first,last];
  towardsLast:Vec ← Sub[z[last],line.centerOfMass];
  t:Vec = -- tangent vector directed roughly toward last point
   Vector.Mul[line.direction,Vector.Dot[line.direction,towardsLast]];
  s:REAL = Abs[t];
  unit:Vec = IF s>0 THEN Vector.Div[t,s] ELSE t;
  normal:Vec = Mul[unit,[0,1]];
  max,min:REAL ← 0; -- extreme components along tangent
  oldNormalComp,oldTangentComp:REAL ← 0;
  FOR i:NAT IN [first..last] DO
   zi:Vec = Sub[z[i],line.centerOfMass];
   tangentComp:REAL = Vector.Dot[zi,unit];
   normalComp:REAL = Vector.Dot[zi,normal];
   candidate:REAL ← 0;
   IF normalComp < epsilon THEN candidate ← tangentComp
   ELSE IF oldNormalComp*normalComp < 0 THEN -- find crossing
     candidate ← (normalComp*oldTangentComp
        - oldNormalComp*tangentComp)
       / (normalComp - oldNormalComp);
   IF candidate < min THEN min ← candidate;
   IF candidate > max THEN max ← candidate;
   oldNormalComp ← normalComp;
   oldTangentComp ← tangentComp;
   ENDLOOP;
  Sample[Add[line.centerOfMass,Vector.Mul[unit,max]]];
  Sample[Add[line.centerOfMass,Vector.Mul[unit,min]]];
  ENDLOOP;
 EXITS Quit => {}
 END;

END.

Michael Plass August 23, 1982 9:18 am. Added check for jam break. Removed CountSamples and SamplesToZ.